Inverse manipulator kinematics
Q = ikine(R, T) Q = ikine(R, T, Q0, OPTIONS) Q = ikine(R, T, Q0, M, OPTIONS)
key/value
use pseudo-inverse instead of Jacobian transpose
set the maximum iteration count (default 1000)
set the tolerance on error norm (default 1e-6)
set step size gain (default 1)
disable variable step size
show number of iterations for each point
show state at each iteration
plot iteration state versus time
Q = ikine(R,T)
is the joint coordinates corresponding to the robot
end-effector pose T which is a homogenenous transform.
Q = ikine(R,T, Q0, OPTIONS)
specifies the initial estimate of the joint
coordinates.
Q = ikine(R, T, Q0, M, OPTIONS)
specifies the initial estimate of the joint
coordinates and a mask matrix. For the case where the manipulator
has fewer than 6 DOF the solution space has more dimensions than can
be spanned by the manipulator joint coordinates. In this case
the mask matrix M specifies the Cartesian DOF (in the wrist coordinate
frame) that will be ignored in reaching a solution. The mask matrix
has six elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value should be 0 (for ignore) or 1.
The number of non-zero elements should equal the number of manipulator DOF.
For example when using a 5 DOF manipulator rotation about the wrist z-axis
might be unimportant in which case M = [1 1 1 1 1 0]
.
In all cases if T is 4x4xM it is taken as a homogeneous transform sequence and ikine() returns the joint coordinates corresponding to each of the transforms in the sequence. Q is MxN where N is the number of robot joints. The initial estimate of Q for each time step is taken as the solution from the previous time step.
Solution is computed iteratively.
Solution is sensitive to choice of initial gain. The variable step size logic (enabled by default) does its best to find a balance between speed of convergence and divergence.
Some experimentation might be required to find the right values of tol, ilimit and alpha.
The pinv option sometimes leads to much faster convergence.
The tolerance is computed on the norm of the error between current and desired tool pose. This norm is computed from distances and angles without any kind of weighting.
The inverse kinematic solution is generally not unique, and depends on the initial guess Q0 (defaults to 0).
The default value of Q0 is zero which is a poor choice for most manipulators (eg. puma560, twolink) since it corresponds to a kinematic singularity.
Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically, like ikine6s or ikine3.
This approach allows a solution to obtained at a singularity, but the joint angles within the null space are arbitrarily assigned.
Joint offsets, if defined, are added to the inverse kinematics to generate Q.