rt_ikinestanfpaul - compute all solutions for the inverse kinematics of a Stanford like robot arm using the Paul's algebraic method[view code]
| alpha | A | theta | D | joint type | |
| link 1 | -%pi/2 | 0 | arbitrary | arbitrary | R |
| link 2 | %pi/2 | 0 | arbitrary | not 0 | R |
| link 3 | 0 | 0 | 0 or -%pi/2 | arbitrary | P |
| link 4 | -%pi/2 | 0 | arbitrary | 0 | R |
| link 5 | %pi/2 | 0 | arbitrary | 0 | R |
| link 6 | 0 | 0 | arbitrary | not 0 | R |
This function returns all the 2 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 Stanford like manipulators, that is R-R-P-R-R-R serial chains with a sperichal wrist. The use of a symbolic solution means that it executes over XX times faster than rt_ikine() for a Stanford arm solution.
A further advantage is that rt_ikinestanfpaul() 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 Stanford arm can
// be solved, the following code could be used.
// load Stanford arm parameters
exec <PATH>/models/rt_stanford.sce;
// end-effector location for a generic joint state vector
q = [-0.565, 0.628, 0.754, -1.047, 0.628, -1.131],
T06 = rt_fkine(stanf, q),
// solve the IKP
Q = rt_ikinestanfpaul(stanf, T06),
// test solutions
T06 - rt_fkine(stanf, Q(1,:)),
T06 - rt_fkine(stanf, Q(2,:)),
// plot solutions
stanf_1 = rt_robot(stanf); stanf_1.name = "Stanford Arm (1)"; // rob copy
stanf_2 = rt_robot(stanf); stanf_2.name = "Stanford Arm (2)"; // rob copy
h0 = scf(0); a0 = h0.children;
a0.tight_limits = "on"; a0.rotation_angles = [66, -61];
h1 = scf(1); a1 = h1.children;
a1.tight_limits = "on"; a1.rotation_angles = [66, -61];
scf(0); rt_plot(stanf_1, Q(1,:)); // Q(1,:)
scf(1); rt_plot(stanf_2, Q(2,:)); // Q(2,:)
// solve the IKP for a Stanford arm with a modified base frame config.
stanf_1.base = rt_transl(0.5, 0, 0); // modify stanf_1 base
T06 = rt_fkine(stanf_1, q), // compute end-effector location
Q = rt_ikinestanfpaul(stanf_1, T06), // solve the IKP
T06 - rt_fkine(stanf_1, Q(1,:)), // test solutions
T06 - rt_fkine(stanf_1, Q(2,:)),
// solution's feasibilty in presence of joint limits
stanf_1.qlim = [ -%pi/2, %pi/2;.. // set joint coordinate limits
-%pi/2, %pi/2;.. // (compatibles with q)
0, 1;..
-%pi/2, %pi/2;..
-%pi/2, %pi/2;..
-%pi/2, %pi/2];
stanf_1.islimit(Q(1,:).'), // workable joint state vector
stanf_1.islimit(Q(2,:).'), // 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_ikinestanfpaul() 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, 0.62, 0.7, -1.04, -0.62, -1.4], // q5 in (-%pi, 0)
T06 = rt_fkine(stanf, q), // compute end-effector location
Q = rt_ikinestanfpaul(stanf, 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_ikinestanfpaul() 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.
q = [-0.45, 1.25, 0.7, 1.4, 0, -1]; // wrist-singular config.
T06 = rt_fkine(stanf, q), // compute end-effector location
Q = rt_ikinestanfpaul(stanf, T06), // q does not appear in Q
q = [-0.45, 0, 0.7, 1.4, 0.65, -1]; // elbow singularity
T06 = rt_fkine(stanf, q), // This config doesn't lead to
Q = rt_ikinestanfpaul(stanf, 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_ikine560paul, 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.