Inverse Kinematics Solver Service

Jump to: navigation , search

Inverse Kinematics Solver Service

The robot software provides a simple Position Inverse Kinematics (IK) Solver as a ROS Service. Specify a desired Cartesian coordinate and orientation for an Endpoint (in world x,y,z space), and the solver will return a set of joint angles that will get the arm there - or return an Invalid state if the pose is unreachable. An optional seed to start looking for solutions around can be specified as a joint angle configuration.

Note that the service takes an array of requests, and similarly returns an array of results with corresponding indexes. Similarly, optional seeds for each Endpoint request can be provided in a corresponding array of the same size as the requests array. This multiple array structure is a common structure used in ROS and explained in the sensor_msgs/JointState message.

SolvePositionIK Service [srv]
/ExternalTools/<side>/PositionKinematicsNode/IKService (intera_core_msgs/SolvePositionIK.srv)

  • Request
    • pose_stamp (Required) - an array of geometry_msgs/PoseStamped requests, each defining the Cartesian <x,y,z> Position and Quaternion orientation of the Endpoint in the /base TF frame.
    • (Optional) seed_angles - an array of joint configuration seeds to start searching for solutions around, corresponding to each request in pose_stamp, where each individual seed is defined as a JointState message with the joint names and positions filled for all the arm Joints (so seed_angles is an array of arrays).
    • (Optional) seed_mode - specify the IK solver strategy for what types of seeds to use. By default (SEED_AUTO), the solver will iterate through the different strategies, starting with the user provided seed if it exists, until a solution is found. However, by explicitly setting the seed_mode to a specific mode, the solver will only return whether it was able to find a solution using that type of seed. The different mode types are defined as constants in the message (default: SEED_AUTO):
          uint8 SEED_AUTO    = 0
          uint8 SEED_USER    = 1
          uint8 SEED_CURRENT = 2
          uint8 SEED_NS_MAP  = 3
      
  • Response
    • result_type will be a corresponding array of uint8's, which will be 0 if no valid joint solution was found for the given Endpoint request; otherwise, it will be non-zero and correspond to the type of seed eventually used that gave a valid solution. These values correspond to the constants in the message used to specify seed_mode.
    • isValid will be a corresponding array of booleans which will be True if a valid joint solution was found for the given Endpoint request.
    • joints will contain joint solutions corresponding to each Endpoint request. Note that you should always check the result_type field to see if a valid joint solution was found, as invalid joint solutions can be returned in this array.

This service allows users to solve for basic IK joint angle solutions or perform workspace validation. The service will use a number of seeding strategies to find a solution. These strategies can be specified explicitly or just use the default to find the first solution in the possible methods. Optionally a Joint configuration can optionally be provided as a seed to the Solver.

Example:

# Seeded request call to the service
$ rosservice call /ExternalTools/right/PositionKinematicsNode/IKService "pose_stamp:
- header:
    seq: 0
    stamp: now
    frame_id: 'base'
  pose:
    position:
      x=0.450628752997,
      y=0.161615832271,
      z=0.217447307078,
    orientation:
      x=0.704020578925,
      y=0.710172716916,
      z=0.00244101361829,
      w=0.00194372088834,
seed_angles:
- header: auto
  name: ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
  position: [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
seed_mode: 0"
# Returns:
joints: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs: 0
      frame_id: ''
    name: ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
    position: [0.9594468009963417, -0.20372936772965106, -1.0393484209059132, 1.47406455200751, -1.855751853200498, -1.0697047509927082, 1.8756467201212745]
    velocity: []
    effort: []
isValid: [True]
result_type: [1]