IKine

RTSX Help: Compute inverse kinematics iteratively

IKine

This function computes inverse kinemetics for a general robot model numerically

Syntax

  • q = Ikine(robot, T, options)

Input Arguments

  • robot –a robot model
  • T — a 4×4 (or 4x4xns) homogeneous transformation matrix representing tool frame w.r.t base

Output Arguments

  • q –a 1 x nq (or ns x nq) vector (matrix) of joint variable values, where nq = number of joint variables, ns = number of sequences.

Options

Note : options in [ ] must be passed to the function in pairs.

  • ‘pinv’ — use pseudo-inverse instead of Jacobian transpose
  • ‘novarstep’ — disable variable step size
  • ‘verbose’ — show number of iterations for each point
  • ‘verbose=2’ — show state at each iteration
  • ‘plot’ — plot iteration states versus time (cannot be used when T is a 3D matrix.)
  • [ ‘q0’, [1 x nq vector]] — initial estimate of joint variable values
  • [ ‘m’, [1 x 6 mask vector]] –mask vector
  • [ ‘ilimit’, value ] –set the maximum iteration count (default 1000)
  • [ ‘tol’, value ] –set the tolerance on error norm (default 1e-6)
  • [ ‘alpha’, value ] –set step size gain (default 1)

Description

IKine( ) iterates an inverse kinematic solution for a general robot model that does not have a closed-form solution. The execution is quite slow and may diverge for bad choice of parameters and initial joint variable ettimates. 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). Joint offsets, if defined, are added to the inverse kinematics to generate Q.

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 vector m specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask vector 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].

Examples

-->exec('./models/mdl_puma560.sce',-1);  // create robot name p560
// with joint variable vectors q_n
-->q_n
 q_n  =
    0.    0.7853982    3.1415927    0.    0.7853982    0.   
// Compute forward kinematics from q_n
-->T=fkine(p560, q_n)
 T  =
    0.    0.    1.    0.5963031  
    0.    1.    0.  - 0.15005    
  - 1.    0.    0.  - 0.0143543  
    0.    0.    0.    1.         
// Compute inverse kinematics 
--> q_i = ikine(p560, T)
..........................................................................................................................
..........................................................................................................................
..........................................................................................................................
.........................
 q_i  =  
- 0.0000009  - 0.8335316    0.0939530    0.0000014  - 0.8312176  - 0.0000010   
// forcing a solution with initial joint variable estimates
-->q_i = ikine(p560, T, 'q0', [0 0 3 0 0 0])
...........................................................................................................................
...........................................................................................................................
..........................................................................................................................
........... 
q_i  =  
- 0.0000009    0.7853971    3.1415954  - 0.0000016    0.7853970    0.0000012

See Also

  • FKine — Compute forward kinematics from robot model
  • Ikine6s — Compute an inverse kinematics solution for a robot with 6 revolute joints such as Puma 560