IKine6s

RTSX Help: Compute inverse kinematics for Puma 560 robot

IKine6s

This function computes inverse kinemetics for a robot model with 6 revolute joints and having a spherical wrist, such as Puma 560.

Syntax

  • q = Ikine6s(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 : Configuration codes below can be put together in one string

  • ‘l’ — arm to the left (default)
  • ‘r’ — arm to the left
  • ‘u’ — elbow up (default)
  • ‘d’ — elbow down
  • ‘n’ — wrist not flipped (default)
  • ‘f’ — wrist flipped (rotated by 180 deg)

Description

IKine6s( ) computes joint coordinates corresponding to the robot end-effector pose T represented by the homogenenous transform. This is a analytic solution for a 6-axis robot with a spherical wrist (such as the Puma 560). The inverse kinematic solution is generally not unique, and depends on the configuration string. Joint offsets, if defined, are added to the inverse kinematics to generate q.

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 = ikine6s(p560,T)
 q_i  =
    2.6485612  - 3.9269908    0.0939558    2.5325594    0.9743496    0.3733996  
 // even q_i not equal q_n, they render the same tool frame
-->fkine(p560, q_i) 
 ans  =
    0.    0.    1.    0.5963031  
    0.    1.    0.  - 0.15005    
  - 1.    0.    0.  - 0.0143543  
    0.    0.    0.    1.         
 // force a solution with configuration code
-->q_i = ikine6s(p560,T,'ru')
 q_i  =
    0.    0.7853982    3.1415927    0.    0.7853982    0.

See Also

  • FKine — Compute forward kinematics from robot model
  • Ikine — Compute an inverse kinematics solution for a robot model numerically