Manipulation Motion Planning

Isaac SDK provides manipulation motion planning for robotics arms using the MultiJointLqrPlanner and MultiJointController components:

• MultiJointLqrPlanner: Produces a simple, smooth plan to move a list of joints in a robotic arm from a starting state to a target state. Each joint is treated independently. The LQR solver automatically adjusts the time to find a valid trajectory that does not exceed the minimum and maximum speed and acceleration constraints. It also ensures that the last state in the trajectory is the received target.

  This component only outputs a new plan when the target has changed. Use the speed_max, speed_min, acceleration_max, and acceleration_min config parameters to ensure the plan is feasible for the robot.

• MultiJointController: Interpolates a trajectory received from the planner to determine the current command to send. This codelet can output either a joint position or speed command based on the control_mode config. The interpolation is based on the timestamps in the trajectory and the current tick time–with a small look-ahead, which can be configured with the command_delay config parameter, to account for latency in the control loop.

Both codelets require a kinematic_tree config parameter, which must refer to a kinematic-tree file. The codelets use the kinematic-tree configuration to retrieve the number of joints and their names for parsing and serializing CompositeProto messages. See the Manipulation Kinematics documentation for more information on creating a kinematic-tree file.

Isaac SDK provides a subgraph that integrates the MultiJointLqrPlanner, MultiJointController and KinematicTree components. You can find this subgraph at packages/planner/apps/multi_joint_lqr_control.subgraph.json. The subgraph provides the following interface edges:

• state (input): “subgraph/interface/joint_state”

• target (input): “subgraph/interface/joint_target”

• command (output): “subgraph/interface/joint_command”

Manipulation-related components use CompositeProto messages to communicate. A CompositeProto with rank-1 tensor is used for a single state or command. The following example shows a state for two joints:

            

            
CompositeProto: {
  "schema": [
    {"entity": "shoulder", "element_type": Float64, "measure": Position},
    {"entity": "shoulder", "element_type": Float64, "measure": Speed},
    {"entity": "elbow", "element_type": Float64, "measure": Position},
    {"entity": "elbow", "element_type": Float64, "measure": Speed}
  ],
  "schema_hash": "...",
  "values": {
    "element_type": Float64,
    "sizes": [19],
    "dataBufferIndex": 0
  }
}
buffers: [[0.7, 0.3, -0.4, 0.1]]
        

A CompositeProto with rank-2 tensor represents a trajectory. The first dimension of the tensor is the number of timesteps. The timestamp for each timestep is part of the schema. The following example shows a trajectory for two joints with two timesteps at t=0 and t=0.1:

            

            
CompositeProto: {
  "schema": [
    {"entity": "timestamp", "element_type": Float64, "measure": Time},
    {"entity": "joint1", "element_type": Float64, "measure": Position},
    {"entity": "joint2", "element_type": Float64, "measure": Position},
    {"entity": "joint1", "element_type": Float64, "measure": Speed},
    {"entity": "joint2", "element_type": Float64, "measure": Speed}
  ],
  "schema_hash": "...",
  "values": {
    "element_type": Float64,
    "sizes": [10 , 4],
    "dataBufferIndex": 0
  }
}
buffers: [[[0, 0.3, 0.7, -0.4, 0.1], [0.1, 0.32, 0.72, -0.3, 0.15]]]