rt_ikine560paul - compute all solutions for the inverse kinematics of a Puma 560 like robot arm using the Paul's algebraic method[view code]
| alpha | A | theta | D | joint type | |
| link 1 | %pi/2 | 0 | arbitrary | 0 | R |
| link 2 | 0 | not 0 | arbitrary | 0 | R |
| link 3 | + or - %pi/2 | arbitrary | arbitrary | arbitrary | R |
| link 4 | + or - %pi/2 | 0 | arbitrary | not 0 | R |
| link 5 | + or - %pi/2 | 0 | arbitrary | 0 | R |
| link 6 | 0 | 0 | arbitrary | arbitrary | R |
This function returns all the 4 joint coordinates corresponding to the end-effector homogeneous transform T06. The robot's base can be arbitrarily specified within the robot object. Joint coordinates are computed using the symbolic solution of algebraic method described by Paul for Puma 560 like robots, that is all revolute 6DOF arms with a sperichal wrist. The use of a symbolic solution means that it executes over XX times faster than rt_ikine() for a Puma 560 solution.
A further advantage is that rt_ikine560paul() can identify singular configurations which lead to infinitely many solutions for the inverse kinematics problem and it informs the user about them displaying a warning.
// To show how the IKP (Inverse Kinematic Problem) for a Puma 560 can
// be solved, the following code could be used.
// load Puma 560 parameters
exec <PATH>/models/rt_puma560.sce;
// end-effector location for a generic joint state vector
q = [0.2, -1.35, 0.65, 0, 1.4, 2.55],
T06 = rt_fkine(p560, q),
// solve the IKP
Q = rt_ikine560paul(p560, T06),
// test solutions
T06 - rt_fkine(p560, Q(1,:)),
T06 - rt_fkine(p560, Q(2,:)),
T06 - rt_fkine(p560, Q(3,:)),
T06 - rt_fkine(p560, Q(4,:)),
// plot solutions
p560_1 = rt_robot(p560); p560_1.name = "Puma 560 (1)"; // robot copy
p560_2 = rt_robot(p560); p560_2.name = "Puma 560 (2)"; // robot copy
p560_3 = rt_robot(p560); p560_3.name = "Puma 560 (3)"; // robot copy
p560_4 = rt_robot(p560); p560_4.name = "Puma 560 (4)"; // robot copy
h0 = scf(0); a0 = h0.children;
a0.tight_limits = "on"; a0.rotation_angles = [73, -38];
h1 = scf(1); a1 = h1.children;
a1.tight_limits = "on"; a1.rotation_angles = [73, -38];
h2 = scf(2); a2 = h2.children;
a2.tight_limits = "on"; a2.rotation_angles = [73, -38];
h3 = scf(3); a3 = h3.children;
a3.tight_limits = "on"; a3.rotation_angles = [73, -38];
scf(0); rt_plot(p560_1, Q(1,:)); // ELBOW DOWN, SHOULDER FORE
scf(1); rt_plot(p560_2, Q(2,:)); // ELBOW UP, SHOULDER FORE
scf(2); rt_plot(p560_3, Q(4,:)); // ELBOW DOWN, SHOULDER AFT
scf(3); rt_plot(p560_4, Q(3,:)); // ELBOW UP, SHOULDER AFT
// solve the IKP for a Puma 560 with a modified base frame config.
p560_1.base = rt_transl(0.5, 0, 0); // modify p560_1 base
T06 = rt_fkine(p560_1, q), // compute end-effector location
Q = rt_ikine560paul(p560_1, T06), // solve the IKP
T06 - rt_fkine(p560_1, Q(1,:)), // test solutions
T06 - rt_fkine(p560_1, Q(2,:)),
T06 - rt_fkine(p560_1, Q(3,:)),
T06 - rt_fkine(p560_1, Q(4,:)),
// solution's feasibilty in presence of joint limits
p560_1.qlim = [ -%pi/2, %pi/2;.. // set joint coordinate limits
-%pi/2, %pi/2;.. // (compatibles with q)
-%pi/2, %pi/2;..
-%pi/2, %pi/2;..
-%pi/2, %pi/2;..
0, 2*%pi];
p560_1.islimit(Q(1,:).'), // workable joint state vector
p560_1.islimit(Q(2,:).'), // joint limits exceeded!
p560_1.islimit(Q(3,:).'), // joint limits exceeded!
p560_1.islimit(Q(4,:).'), // joint limits exceeded!
// Note that, in both cases, the starting joint state vector (the state
// vector with which T06 has been computed) has been returned as a
// solution to the IKP (Q(1,:)). However, there are many cases in which
// this can not occur.
// Following examples are intended to illustrate some of these
// situations.
// When the starting joint state vector has q5 belonging to (-%pi, 0)
// radians it will be not returned as a solution, since the configuration
// of last three joints of robot coincides with a set of Euler angles and
// rt_ikine560paul() computes the solution reducing the range to whom q5
// belongs to (0, %pi) radians.
// (See help of rt_tr2eul() function for further details).
q = [0.2, -1.35, 0.65, 0, -1.4, 2.55], // q5 in (-%pi, 0)
T06 = rt_fkine(p560, q), // compute end-effector location
Q = rt_ikine560paul(p560, T06), // solve the IKP:
// Q(1,4:6) is different from
// q(4:6), but they are
// equivalent in their effects
// When the starting joint state vector is a singular configuration for
// the robot then it could not be returned as a solution to the IKP.
// Note that rt_ikine560paul() can identify singular configurations which
// lead to infinitely many solutions for the IKP and it informs you about
// them displaying a warning.
// You can use the following code to experience yourself with this.
L = p560_2.links;
L(3).A = 0; // Puma 560 without..
L(3).D = 0; // ..shoulder offset
q = [0, 0.3926991, %pi/4, 0, 0.2, 1]; // shoulder-singular config.
T06 = rt_fkine(p560_2, q), // compute end-effector location
Q = rt_ikine560paul(p560_2, T06), // Q(1,:) == q
q = [0.85, -0.5, %pi/2, 0, 0.2, 1]; // elbow-shoulder singularity
T06 = rt_fkine(p560_2, q), // compute end-effector location
Q = rt_ikine560paul(p560_2, T06), // q does not appear in Q
q = [0.85, -0.5, -%pi/2, 0, 0.2, 1]; // elbow singularity
T06 = rt_fkine(p560_2, q), // This config doesn't lead to
Q = rt_ikine560paul(p560_2, T06), // infinitely many solutions,
// therefore no warning is shown
Note that the dimensional units for the last column of the T06 matrix must agree with the dimensional units used in the robot object robot. The units can be whatever you choose (metres, inches, cubits or furlongs) but they must be consistent. The toolbox definitions rt_puma560 and rt_stanford all use SI units with dimensions in metres.
This function will not work for robot objects that use the modified Denavit-Hartenberg convention.
rt_drivebot, rt_ikine, rt_ikine560, rt_ikinestanfpaul, rt_fkine, rt_robot, rt_tr2eul, robot/rt_plot,
R. P. Paul, Robot Manipulators: Mathematics, Programming and Control. Cambridge, Massachusetts: MIT Press, 1981.
L. Sciavicco, B. Siciliano, Modelling and Control of Robot Manipulators. 2nd Edition, Springer-Verlag Advanced Textbooks in Control and Signal Processing Series, London, UK, 2000.