rt_ikine - compute the inverse kinematics for a generic serial n-link manipulator[view code]
This function returns the joint coordinates for the manipulator described by the object robot whose end-effector homogeneous transform is given by T. Note that the robot's base can be arbitrarily specified within the robot object.
If T is a homogeneous transform (m = 1) then a row vector of joint coordinates is returned. The estimate for the first step is q0 if this is given else is zeros(n,1).
If T is a homogeneous transform trajectory (m > 1) then each row in q will be a vector of joint coordinates corresponding to the last subscript of T. The estimate for the first step is q0 if this is given else is zeros(n,1). The initial estimate of q for each time step is taken as the solution from the previous time step.
Note that the inverse kinematic solution is generally not unique, and depends on the initial value q0 (which defaults to zeros(1,n)).
For the case of a manipulator with fewer than 6 DOF it is not possible for the end-effector to satisfy the end-effector pose specified by an arbitrary homogeneous transform. This tipically leads to non-convergence in rt_ikine. A solution is to specify a 6-element weighting vector, M, whose elements are 0, for those Cartesian DOF that are uncostrained and 1 otherwise. The elements correspond to translation along the X-, Y- and Z-axes and rotation about the X-, Y- and Z-axes respectively. For example, a 5-axis manipulator may be incapable of independantly controlling rotation about the end-effector's Z-axis. In this case M = [1, 1, 1, 1, 1, 0] would enable a solution in which the end-effector adopted the pose Texcept for the end-effector rotation. The number of non-zero elements should equal the number of robot's DOF.
// IKP (Inverse Kinematic Problem) for a Puma 560. Note that for such // manipulators you should use specific inverse kinematic solutions // derived symbolically (rt_ikine560() and rt_ikine560paul()). exec <PATH>/models/rt_puma560.sce; // load Puma 560 parameters // T is a homogeneous transform q = [0, -%pi/4, -%pi/4, 0, %pi/8, 0]; // joint state vector T = rt_fkine(p560, q); // compute end-effector pose tic(); Q = rt_ikine(p560, T); toc(), // solve the IKP tic(); Q = rt_ikine560(p560, T); toc(), // use a symbolic solution tic(); Q = rt_ikine560paul(p560, T); toc(), // compute all solutions // T is a trajectory t = [0:.056:2]; // create a time vector T1 = rt_transl(0.6, -0.5, 0.0), // define the start point T2 = rt_transl(0.4, 0.5, 0.2), // and destination T = rt_ctraj(T1, T2, length(t)); // compute a Cartesian path Q = rt_ikine(p560, T); // solve the IKP // To show how the IKP for a simple 3R planar (lying on X-Y plane) can // be solved, the following code could be used. // Note that this manipulator has less than 6 DOF therefore a mask should // be specified. The robot is able to move its end-effector to any // position in the X-Y plane (inside its reachable workspace) and // orientation (about the Z-axis), so the mask will be // M = [M_position, M_orientation] where M_position = [1, 1, 0] and // M_orientation = [0, 0, 1]. // build a simple 3R planar and its mask. Then compute the end-effector // pose l = rt_link([0, 1, 0, 0, 0], "stand"); r = rt_robot(list(l,l,l), "3R plan."); M = [1, 1, 0, 0, 0, 1]; // mask q = [-0.5, 0.85, -0.22]; // generic joint state vect. T = rt_fkine(r, q); // end-effector pose // solve the IKP: // a) when q0 = [0, 0, 0], returned joint angles are in non-minimum form // and differ from values in q Q = rt_ikine(r, T, [0, 0, 0], M); Q1 = modulo(Q, 2*%pi*[1, 1, 1]); // adjust joint values // b) when q0 = [-0.2, 0, 0], returned solution is equal to q Q = rt_ikine(r, T, [-0.2, 0, 0], M); // Q == q // test solutions rt_fkine(r, Q) - rt_fkine(r, Q1), // T(Q) == T(Q1) // plot solutions r_1 = rt_robot(r); r_1.name = "3R plan. (1)"; // robot copy h0 = scf(0); a0 = h0.children; a0.tight_limits = "on"; a0.rotation_angles = [0, -90]; scf(0); rt_plot(r, Q); rt_plot(r_1, Q1);
Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically.
The returned joint angles may be in non-minimum form, i.e. q + 2*n*%pi.
This approach allows a solution to be obtained at a singularity, but the joint coordinates whitin the null space are arbitrarily assigned.
Note that the dimensional units for the last column of the T 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.
rt_ikine560, rt_ikine560paul, rt_ikinestanfpaul, rt_fkine, rt_tr2diff, rt_robot, robot/rt_plot,
Corke, P.I. "A Robotics Toolbox for MATLAB", IEEE Robotics and Automation Magazine, Volume 3(1), March 1996, pp. 24-32
S. Chiaverini, L. Sciavicco and B. Siciliano, "Control of robotic systems through singularities", in Proc. Int. Workshop on Nonlinear and Adaptive Control: Issues in Robotics, (C. C. de Wit ed.), Springer-Verlag, 1991.