4. Motion Planning#

In this tutorial, we will show you how to use MetaSim to plan a motion for a robot. Note here, we can use the pyroki or curobo package to plan the motion. If you haven’t installed it, please refer to our pyroki installation guide or curobo installation guide.

Common Usage#

python get_started/4_motion_planning.py  --sim <simulator>

you can also render in the headless mode by adding --headless flag. By using this, there will be no window popping up and the rendering will also be faster.

By running the above command, you will plan a motion for a robot and it will automatically record a video.

IsaacSim#

python get_started/4_motion_planning.py  --sim isaacsim

Isaac Gym#

python get_started/4_motion_planning.py  --sim isaacgym

Mujoco#

# For mac users, replace python with mjpython.
python get_started/4_motion_planning.py  --sim mujoco --headless

Note that we find the non-headless mode of Mujoco is not stable. So we recommend using the headless mode.

Genesis#

python get_started/4_motion_planning.py  --sim genesis

Note that we find the headless mode of Genesis is not stable. So we recommend using the non-headless mode.

Sapien#

python get_started/4_motion_planning.py  --sim sapien3

Pybullet#

python get_started/4_motion_planning.py  --sim pybullet

You will get the following videos:

IsaacSim

Isaac Gym

Mujoco

Genesis

Sapien

PyBullet

IK Solver Usage#

MetaSim provides a unified IK solver that supports both curobo and pyroki backends for end-effector control. The IK solver is designed to work with multiple environments simultaneously and provides two main functions for end-effector control.

Backend Options#

The IK solver supports two backends:

  • curobo: High-performance CUDA-accelerated IK solver with collision checking

  • pyroki: JAX-based differentiable IK solver (default)

1. solve_ik_batch() - Compute Arm Joint Positions#

This function solves inverse kinematics for a batch of target end-effector poses, returning only the arm joint positions (excluding finger/gripper joints).

from metasim.utils.ik_solver import setup_ik_solver

# Initialize IK solver
ik_solver = setup_ik_solver(robot_cfg, solver="pyroki")  # or "curobo"
# IK solver expects original joint order, but state uses alphabetical order
reorder_idx = handler.get_joint_reindex(scenario.robots[0].name)
inverse_reorder_idx = [reorder_idx.index(i) for i in range(len(reorder_idx))]
curr_robot_q = obs.robots[scenario.robots[0].name].joint_pos[:, inverse_reorder_idx]
# Solve IK for multiple environments
q_solution, ik_success = ik_solver.solve_ik_batch(
    ee_pos_target=target_positions,    # (B, 3) - target EE positions
    ee_quat_target=target_quaternions, # (B, 4) - target EE quaternions (wxyz)
    seed_q=curr_robot_q         # (B, n_dof) - seed configs (required for curobo)
)

# q_solution: (B, n_dof_ik) - arm joint positions only
# ik_success: (B,) - boolean mask indicating successful IK solutions, pyroki does not need this

2. compose_joint_action() - Combine Arm + Gripper#

This function combines the arm joint positions from IK with gripper positions to create the complete joint command. It can return either a tensor or action dictionaries.

# Convert binary gripper command to joint widths
gripper_widths = process_gripper_command(
    gripper_binary=gripper_open_close,  # (B,) or (B, 1) - binary gripper state
    robot_cfg=robot_cfg,
    device=device
)

# Option 1: Return tensor in alphabetical order (default)
actions_tensor = ik_solver.compose_joint_action(
    q_solution=q_solution,           # (B, n_dof_ik) - arm joint positions from IK
    gripper_widths=gripper_widths,   # (B, ee_n_dof) - gripper joint positions
    current_q=current_joint_state,   # (B, n_robot_dof) - optional current state
    return_dict=False                # Default: return tensor
)
# q_full: (B, n_robot_dof) - complete joint command in alphabetical order

# Option 2: Return action dictionaries directly
actions_dict = ik_solver.compose_joint_action(
    q_solution=q_solution,
    gripper_widths=gripper_widths,
    current_q=current_joint_state,
    return_dict=True                 # Return action dictionaries
)
# actions: list of action dictionaries for env execution

Note on Joint Ordering:

  • Tensor output (return_dict=False): Joints are ordered alphabetically, including end-effector joints

  • Dictionary output (return_dict=True): Joints maintain the original dictionary order from robot configuration

Example for Franka robot:

# Tensor order (alphabetical):
# [finger1, finger2, joint1, joint2, joint3, joint4, joint5, joint6, joint7]

# Dictionary order (original, with keys):
# [joint1, joint2, joint3, joint4, joint5, joint6, joint7, finger1, finger2]

Complete End-Effector Control Example#

import torch
from metasim.utils.ik_solver import setup_ik_solver, process_gripper_command

# Setup
ik_solver = setup_ik_solver(robot_cfg, solver="pyroki")
num_envs = 4
device = torch.device("cuda")

# Target end-effector poses
target_positions = torch.randn(num_envs, 3, device=device)  # (B, 3)
target_quaternions = torch.randn(num_envs, 4, device=device)  # (B, 4) wxyz
gripper_commands = torch.randint(0, 2, (num_envs,), device=device)  # (B,) binary

# Step 1: Solve IK for arm joints
ik_solver = setup_ik_solver(robot_cfg, solver="pyroki")  # or "curobo"
# IK solver expects original joint order, but state uses alphabetical order
reorder_idx = handler.get_joint_reindex(scenario.robots[0].name)
inverse_reorder_idx = [reorder_idx.index(i) for i in range(len(reorder_idx))]
curr_robot_q = obs.robots[scenario.robots[0].name].joint_pos[:, inverse_reorder_idx]

q_arm, ik_success = ik_solver.solve_ik_batch(
    ee_pos_target=target_positions,
    ee_quat_target=target_quaternions,
    seed_q=curr_robot_q 
)

# Step 2: Process gripper commands
gripper_widths = process_gripper_command(
    gripper_binary=gripper_commands,
    robot_cfg=robot_cfg,
    device=device
)

# Step 3: Compose full joint command and create actions directly
actions = ik_solver.compose_joint_action(
    q_solution=q_arm,
    gripper_widths=gripper_widths,
    return_dict=True  # Return action dictionaries directly
)

Backend-Specific Notes#

Curobo Backend:

  • Better performance for parallel environments

  • Requires CUDA installation

Pyroki Backend (Default):

  • JAX-based differentiable solver

  • Lighter weight installation

  • Good for research and prototyping

This IK solver design separates arm control (IK solving) from gripper control, making it easy to implement end-effector-based manipulation tasks across multiple environments.