Trajectory Optimizer Tutorial#

Note

Windows Support: The TrajectoryOptimizer is not currently available on Windows.

This tutorial demonstrates how to use the TrajectoryOptimizer class in the cuMotion integration to plan collision-free, kinematically constrained trajectories to configuration space or task space targets.

By the end of this tutorial, you’ll understand:

How to create and configure the TrajectoryOptimizer
How to plan trajectories to configuration space targets
How to plan trajectories to task-space targets
How to execute optimized trajectories
How to configure optimization parameters

Prerequisites

Review the Robot Configuration tutorial to understand how to load robot configurations.
Review the Trajectory Planning and Execution tutorial to understand the Trajectory interface.
Review the World Interface tutorial to understand how to set up CumotionWorldInterface.

To follow along with the tutorial, you can search and enable the cuMotion Examples extension within your running Isaac Sim instance. Within the isaacsim.robot_motion.cumotion.examples extension, there is a fully functional example of trajectory optimization including collision-free planning and trajectory execution.

Key Concepts#

The TrajectoryOptimizer provides global optimization-based trajectory planning that:

Plans collision-free trajectories: Uses obstacle information from CumotionWorldInterface to avoid collisions
Respects kinematic constraints: Generates trajectories that respect joint limits, velocity, and acceleration constraints
Supports multiple target types: Can plan to configuration space targets, task space targets, or task space goalsets
Returns Trajectory objects: Optimized trajectories implement the Trajectory interface, enabling use with TrajectoryFollower

The optimizer requires a CumotionWorldInterface instance to provide obstacle information for collision avoidance. The World Interface tutorial shows how to set up a CumotionWorldInterface, often using WorldBinding for convenience.

Initializing the Optimizer#

First, set up a CumotionWorldInterface as described in the World Interface tutorial. Once you have a CumotionWorldInterface, you can create the optimizer:

    # Create trajectory optimizer
    # Use controlled_joint_names (not dof_names) to match cuMotion's expected joint space
    robot_joint_space = cumotion_robot.controlled_joint_names
    optimizer = TrajectoryOptimizer(
        cumotion_robot=cumotion_robot,
        robot_joint_space=robot_joint_space,
        cumotion_world_interface=world_binding.get_world_interface(),
    )

The optimizer needs:

Robot configuration: A cuMotion robot configuration (retrieved via load_cumotion_supported_robot()). See the Robot Configuration tutorial for details on loading robot configurations.
World interface: A CumotionWorldInterface instance
Joint space: The controlled joint names (use cumotion_robot.controlled_joint_names, not articulation.dof_names)

Accessing cuMotion Parameters#

The optimizer provides access to the underlying cuMotion trajectory optimizer configuration for parameter modification:

    # Get the underlying cuMotion config
    config = optimizer.get_trajectory_optimizer_config()

    # Modify parameters using set_param
    config.set_param(
        "enable_self_collision",
        cumotion.TrajectoryOptimizerConfig.ParamValue(False),
    )

For a complete list of available parameters and their usage, see the cuMotion Python and C++ API documentation.

Updating World State#

The world interface must be updated before planning if obstacles or the robot base have moved:

    # Update world state before planning
    world_binding.get_world_interface().update_world_to_robot_root_transforms(articulation.get_world_poses())
    world_binding.synchronize_transforms()

Planning to Configuration Space Targets#

The optimizer can plan to configuration space targets using cumotion.TrajectoryOptimizer.CSpaceTarget:

    # Get initial and target configurations
    # Use default configuration as starting point (ensures valid configuration)
    q_initial = cumotion_robot.robot_description.default_cspace_configuration()
    q_initial[0] = -np.pi / 2
    q_initial[1] = -np.pi / 8

    # Create target configuration by modifying default
    q_target = cumotion_robot.robot_description.default_cspace_configuration()
    q_target[0] = np.pi / 2
    q_target[1] = -np.pi / 3

    # Create CSpaceTarget with target configuration
    # Path constraints must be explicitly set (even if to none())
    cspace_target = cumotion.TrajectoryOptimizer.CSpaceTarget(
        q_target,
        translation_path_constraint=cumotion.TrajectoryOptimizer.CSpaceTarget.TranslationPathConstraint.none(),
        orientation_path_constraint=cumotion.TrajectoryOptimizer.CSpaceTarget.OrientationPathConstraint.none(),
    )

    # Plan trajectory
    trajectory = optimizer.plan_to_goal(q_initial, cspace_target)

    if trajectory is None:
        print("Planning failed! Check warnings for failure status.")
        # Common issues:
        # - Start/goal configurations outside joint limits
        # - Start/goal configurations in collision
        # - Insufficient optimization parameters
    else:
        print(f"Planning succeeded! Trajectory duration: {trajectory.duration}")

The plan_to_goal() method returns a CumotionTrajectory if planning succeeds, or None if planning fails. The trajectory implements the Trajectory interface.

Planning to Task-Space Targets#

The optimizer can also plan to task-space targets using cumotion.TrajectoryOptimizer.TaskSpaceTarget. Task-space targets require converting from Isaac Sim world frame to the robot base frame:

    # Get target cube pose in world frame
    target_positions, target_orientations = target_cube.get_world_poses()
    target_position_world = target_positions.numpy()[0]
    target_orientation_world = target_orientations.numpy()[0]  # quaternion wxyz

    # Get robot base transform
    robot_base_positions, robot_base_orientations = articulation.get_world_poses()

    # Convert target pose from world frame to robot base frame
    target_pose_base = isaac_sim_to_cumotion_pose(
        position_world_to_target=target_position_world,
        orientation_world_to_target=target_orientation_world,
        position_world_to_base=robot_base_positions,
        orientation_world_to_base=robot_base_orientations,
    )

    # Create TaskSpaceTarget
    task_space_target = cumotion.TrajectoryOptimizer.TaskSpaceTarget(
        translation_constraint=cumotion.TrajectoryOptimizer.TranslationConstraint.target(target_pose_base.translation),
        orientation_constraint=cumotion.TrajectoryOptimizer.OrientationConstraint.terminal_target(
            target_pose_base.rotation
        ),
    )

    # Use default configuration as starting point (ensures valid configuration)
    q_initial = cumotion_robot.robot_description.default_cspace_configuration()
    q_initial[0] = -np.pi / 2
    q_initial[1] = -np.pi / 8

    # Plan trajectory
    trajectory = optimizer.plan_to_goal(q_initial, task_space_target)

    if trajectory is None:
        print("Planning failed! Check warnings for failure status.")
        # Common issues:
        # - Start configuration outside joint limits or in collision
        # - Target pose unreachable
        # - Insufficient optimization parameters
    else:
        print(f"Planning succeeded! Trajectory duration: {trajectory.duration}")

The cuMotion integration includes helper functions (like isaac_sim_to_cumotion_pose()) to convert between Isaac Sim world-frame representation (position, quaternion tuple) and cuMotion types in the robot base frame, since cuMotion works entirely in the base frame.

Optional: Checking for Collisions#

Before planning, you can check if the initial or target configurations are in collision:

    # Create robot-world inspector for collision checking
    robot_world_inspector = cumotion.create_robot_world_inspector(
        cumotion_robot.robot_description,
        world_binding.get_world_interface().world_view,
    )

    # Ensure world view is up to date before collision checks
    world_binding.get_world_interface().world_view.update()

    if robot_world_inspector.in_self_collision(q_initial) or robot_world_inspector.in_collision_with_obstacle(
        q_initial
    ):
        print("Initial configuration is in collision!")
        return False

    if robot_world_inspector.in_self_collision(q_target) or robot_world_inspector.in_collision_with_obstacle(q_target):
        print("Target configuration is in collision!")
        return False

    return True

Executing Trajectories#

Once you have a trajectory, you can execute it in two ways:

Note

These trajectory objects are not statefully tied to the Articulation class in Isaac Sim. Therefore, the user must generally make sure to design trajectories that start at the current configuration space of the robot, if they intend to immediately execute that trajectory.

Direct execution: Sample the trajectory at each time step and apply the joint states directly to the articulation:

    # Sample the trajectory at the current time
    target_state = trajectory.get_target_state(t)
    if target_state is not None and target_state.joints.positions is not None:
        # Apply directly to the articulation
        articulation.set_dof_position_targets(
            positions=target_state.joints.positions,
            dof_indices=target_state.joints.position_indices,
        )

Using TrajectoryFollower: Use the TrajectoryFollower controller from the Motion Generation API to integrate trajectory execution into a larger control system:

    # Create TrajectoryFollower controller
    follower = TrajectoryFollower()

    # Set the trajectory
    follower.set_trajectory(trajectory)

    # Get current estimated state
    joint_state = JointState.from_name(
        robot_joint_space=robot_joint_space,
        positions=(robot_joint_space, articulation.get_dof_positions()),
        velocities=(robot_joint_space, articulation.get_dof_velocities()),
    )
    estimated_state = RobotState(joints=joint_state)

    # Reset the follower (sets start time)
    follower.reset(estimated_state, None, t)

    # In your control loop, call forward to get desired state
    desired_state = follower.forward(estimated_state, None, t)
    while desired_state is not None:
        if desired_state.joints.positions is not None:
            # Apply desired state to robot
            articulation.set_dof_position_targets(
                positions=desired_state.joints.positions,
                dof_indices=desired_state.joints.position_indices,
            )
        # Update simulation
        simulation_app.update()
        # Update estimated state and time for next iteration
        estimated_state = RobotState(
            joints=JointState.from_name(
                robot_joint_space=robot_joint_space,
                positions=(robot_joint_space, articulation.get_dof_positions()),
                velocities=(robot_joint_space, articulation.get_dof_velocities()),
            )
        )
        t += step  # Advance time
        desired_state = follower.forward(estimated_state, None, t)

The TrajectoryFollower follows the standard controller interface (reset and forward) and can be composed with other controllers in the Motion Generation API. For more details on trajectory execution, see the Trajectory Planning and Execution tutorial.

Note

The scenario.py example uses Articulation.set_dof_positions() to directly set physics state for perfect demonstration of the planned trajectory. Real robots require controllers to follow joint targets instead, and should use one of the aforementioned methods to execute the trajectory.

Example Usage#

Note

To experiment with this tutorial interactively, see the scenario.py file in the isaacsim.robot_motion.cumotion.examples extension at isaacsim/robot_motion/cumotion/examples/trajectory_optimizer/scenario.py.

The following videos demonstrate trajectory optimization as shown in the isaacsim.robot_motion.cumotion.examples extension.

The first video shows trajectory optimization planning trajectories to configuration space and task space targets while avoiding obstacles in the scene.

../_images/isim_6.0_full_tut_viewport_trajectory_optimizer_normal_usage.webp — Trajectory optimizer planning trajectories to configuration space and task space targets while avoiding obstacles#

The second video demonstrates adding a new obstacle to the scene, resetting the world state, and running the trajectory optimizer again. Objects are added in the same way as described in the World Interface tutorial. Inside the scenario.py file, a new CumotionWorldInterface is created for every plan. This is not generally necessary if objects are not being added or removed from the scene.

../_images/isim_6.0_full_tut_viewport_trajectory_optimizer_adding_sphere.webp — Adding an obstacle, resetting the cumotion world, and running the trajectory optimizer again#

Summary#

This tutorial demonstrated:

Optimizer Initialization: Creating the TrajectoryOptimizer with a CumotionWorldInterface for obstacle awareness
Parameter Access: Accessing underlying cuMotion objects for advanced configuration
World Updates: Keeping world state synchronized for accurate planning
Configuration Space Planning: Planning trajectories to configuration space targets using CSpaceTarget
Task-Space Planning: Planning trajectories to task-space targets using TaskSpaceTarget
Collision Checking: Optionally checking for collisions before planning
Trajectory Execution: Executing optimized trajectories using the Trajectory interface

The TrajectoryOptimizer provides global, optimization-based trajectory planning that generates smooth, collision-free trajectories while respecting kinematic constraints.

Note

This tutorial covers the basic usage of the TrajectoryOptimizer for configuration space and task-space targets. The cuMotion library provides many additional capabilities including task-space goalsets, path constraints, axis constraints, and advanced optimization parameters. For complete documentation of all available features, see the official cuMotion Python and C++ API documentation.

Next Steps#

World Interface tutorial - World state management
RMPflow tutorial - Reactive control
Trajectory Generator tutorial - Time-optimal trajectory generation
Graph Planner tutorial - Sampling-based path planning
cuMotion library documentation - Advanced parameter configuration and task space planning