Tutorial 5: Using the MPC Planner¶

📋 Implementation Status: This tutorial describes the planned MPC planner functionality. The MPC planner architecture is fully specified and documented, but the implementation is currently in progress. This document serves as both a specification for developers and a preview of upcoming features for users.

Model Predictive Control (MPC) enables real-time reactive trajectory planning that can adapt to dynamic obstacles and disturbances.

What is MPC?¶

MPC (Model Predictive Control) is a closed-loop control strategy that:

Predicts future states over a time horizon
Optimizes control actions to reach the goal
Executes only the first control step
Repeats continuously until goal is reached

MPC vs Classic Planning¶

Aspect	Classic Planner	MPC Planner
Loop	Open-loop	Closed-loop
Planning	Once before execution	Continuous during execution
Adaptability	Fixed trajectory	Real-time adaptation
Obstacles	Static only	Static + dynamic
Disturbances	Requires replanning	Automatic correction
GPU Usage	Burst	Sustained
Latency	50-200ms	1-10ms per iteration

Prerequisites¶

Before starting this tutorial:

Complete Tutorial 1: First Trajectory
Ensure you have:
- NVIDIA GPU with CUDA support
- curobo_ros installed and configured
- Robot simulator or real robot

Step 1: Launch with MPC Planner¶

Using Launch File¶

Create a launch file that uses MPC by default:

# mpc_demo.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
 
def generate_launch_description():
    return LaunchDescription([
        Node(
            package='curobo_ros',
            executable='curobo_trajectory_planner',
            name='trajectory_planner',
            parameters=[{
                'default_planner': 'mpc',  # Use MPC planner
                'robot_type': 'doosan_m1013',
                'time_dilation_factor': 0.5,
 
                # MPC-specific parameters
                'mpc_convergence_threshold': 0.01,  # meters
                'mpc_max_iterations': 1000,
                'mpc_horizon': 10,
                'mpc_control_frequency': 100  # Hz
            }],
            output='screen'
        )
    ])

Launch it:

ros2 launch curobo_ros mpc_demo.launch.py

Using Command Line¶

Alternatively, launch with default settings and switch to MPC:

# Terminal 1: Launch unified planner
ros2 run curobo_ros curobo_trajectory_planner
 
# Terminal 2: Switch to MPC
ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 1}"

Step 2: Configure MPC Parameters¶

MPC has several tunable parameters:

Convergence Threshold¶

How close to the goal before stopping (in meters):

ros2 param set /unified_planner mpc_convergence_threshold 0.01

Smaller values (0.001): Higher precision, longer execution
Larger values (0.05): Faster completion, lower precision
Recommended: 0.01-0.02 for most tasks

Max Iterations¶

Maximum number of MPC iterations:

ros2 param set /unified_planner mpc_max_iterations 1000

More iterations: Can handle complex scenes, slower
Fewer iterations: Faster but may not converge
Recommended: 500-1000 for dynamic environments

MPC Horizon¶

Planning horizon length (number of future steps):

ros2 param set /unified_planner mpc_horizon 10

Longer horizon: Better long-term planning, more computation
Shorter horizon: Reactive, less computational cost
Recommended: 5-15 steps

Control Frequency¶

How often MPC replans (Hz):

ros2 param set /unified_planner mpc_control_frequency 100

Higher frequency: More reactive, more GPU usage
Lower frequency: Less smooth, more efficient
Recommended: 50-100 Hz for real-time control

Step 3: Generate and Execute with MPC¶

Using Python API¶

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from curobo_ros.planners import PlannerFactory
from geometry_msgs.msg import Pose
import numpy as np
 
class MPCDemoNode(Node):
    def __init__(self):
        super().__init__('mpc_demo_node')
 
        # Create MPC planner
        self.planner = PlannerFactory.create_planner(
            'mpc',
            self,
            config_wrapper
        )
 
        self.get_logger().info('MPC planner initialized')
 
    def plan_and_execute(self):
        # Define start state (current robot position)
        start_state = self.get_current_joint_state()
 
        # Define goal pose
        goal_pose = Pose()
        goal_pose.position.x = 0.5
        goal_pose.position.y = 0.3
        goal_pose.position.z = 0.4
        goal_pose.orientation.w = 1.0
 
        # Plan with MPC
        config = {
            'convergence_threshold': 0.01,
            'max_iterations': 1000
        }
 
        result = self.planner.plan(start_state, goal_pose, config)
 
        if result.success:
            self.get_logger().info('MPC planning succeeded')
 
            # Execute (MPC will continuously replan)
            success = self.planner.execute(robot_context, goal_handle)
 
            if success:
                self.get_logger().info('MPC execution completed')
        else:
            self.get_logger().error(f'MPC planning failed: {result.message}')
 
def main():
    rclpy.init()
    node = MPCDemoNode()
    node.plan_and_execute()
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()

Using ROS 2 Services/Actions¶

# 1. Generate trajectory with MPC
ros2 service call /unified_planner/generate_trajectory curobo_msgs/srv/TrajectoryGeneration \
  "{
    goal_pose: {
      position: {x: 0.5, y: 0.3, z: 0.4},
      orientation: {w: 1.0}
    },
    use_current_state: true
  }"
 
# 2. Execute with MPC (will continuously replan)
ros2 action send_goal /unified_planner/execute_trajectory curobo_msgs/action/SendTrajectory "{}"

Step 4: Using the MpcMove Action¶

What is MpcMove?¶

The MpcMove action provides a streamlined interface for MPC-based motion execution. Unlike the two-step process of generate_trajectory + execute_trajectory, MpcMove combines planning and execution into a single action call.

Key Features:

Single Action Call: No need for separate planning and execution
Real-time Feedback: Provides continuous joint command updates and progress
Closed-loop Control: MPC continuously replans during execution
Reactive: Automatically adapts to obstacles and disturbances

MpcMove vs generate_trajectory + execute_trajectory¶

Aspect	generate_trajectory + execute_trajectory	MpcMove Action
API Calls	Two separate calls	Single action call
Planning	Generates full trajectory upfront	Continuous replanning during execution
Feedback	Trajectory waypoints, then execution status	Real-time joint commands + progression
Use Case	Static environments, reproducible paths	Dynamic environments, reactive control
Complexity	More steps to coordinate	Simpler single-action interface

MpcMove Action Interface¶

Action Name: /unified_planner/mpc_move Action Type: curobo_msgs/action/MpcMove

Goal:

geometry_msgs/Pose target_pose  # Target end-effector pose

Result:

bool success                     # True if goal reached
string message                   # Status message or error details

Feedback:

sensor_msgs/JointState joint_command  # Current joint target
float32 step_progression              # Progress toward goal (0.0 to 1.0)

Example 1: Basic MpcMove Usage¶

# Send goal to MPC action server
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {w: 1.0}}}" \
  --feedback

Expected Feedback Output:

# Feedback updates continuously during execution
feedback:
  joint_command:
    position: [-0.234, 0.567, -1.234, 0.890, -0.456, 0.123]
  step_progression: 0.15  # 15% complete

feedback:
  joint_command:
    position: [-0.245, 0.578, -1.245, 0.901, -0.467, 0.134]
  step_progression: 0.32  # 32% complete

...

feedback:
  joint_command:
    position: [-0.289, 0.634, -1.312, 0.978, -0.523, 0.189]
  step_progression: 0.98  # 98% complete

Result:

result:
  success: true
  message: 'MPC reached goal pose successfully'

Example 2: MpcMove with Different Orientations¶

# Move with end-effector pointing downward
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.6, y: 0.0, z: 0.3}, \
    orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}}" \
  --feedback

Example 3: Monitor Progress Without Feedback¶

# Send goal without continuous feedback (simpler output)
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.45, y: -0.2, z: 0.5}, orientation: {w: 1.0}}}"

# In another terminal, monitor action status
ros2 action list
ros2 action info /unified_planner/mpc_move

Example 4: Cancel MpcMove During Execution¶

# Terminal 1: Start MPC move
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.8, y: 0.4, z: 0.6}, orientation: {w: 1.0}}}" \
  --feedback

# Terminal 2: Cancel the action (e.g., if obstacle appears)
# Get the goal ID from Terminal 1 output, then:
ros2 action send_goal --cancel /unified_planner/mpc_move

Understanding Feedback: step_progression¶

The step_progression field indicates how close the robot is to the goal:

0.0: Starting position
0.5: Halfway to goal
1.0: Goal reached

Progress Interpretation:

Linear motion: Progression reflects Cartesian distance traveled
Complex paths: Progression may be non-linear due to obstacle avoidance
Near obstacles: Progression may slow down (more MPC iterations needed)

Understanding Feedback: joint_command¶

The joint_command field provides the current joint targets being sent to the robot:

joint_command:
  position: [j1, j2, j3, j4, j5, j6]  # Joint positions in radians
  velocity: []                         # Not populated
  effort: []                           # Not populated

Use Cases for joint_command Feedback:

Monitor robot joint state during execution
Verify robot is following MPC commands
Log joint trajectories for analysis
Detect anomalies (e.g., joint limits)

When to Use MpcMove¶

Use MpcMove when:

You need simple, single-call interface for motion
Environment has dynamic obstacles
You want real-time feedback on progress
Reactive control is more important than trajectory reproducibility

Use generate_trajectory + execute_trajectory when:

You need to inspect/modify trajectory before execution
Trajectory must be saved or replayed
Environment is static (more efficient with classic planner)
You need full trajectory waypoints upfront

Combining MpcMove with Obstacle Management¶

MpcMove automatically adapts to obstacles in the world configuration:

# 1. Add obstacle
ros2 service call /unified_planner/add_object curobo_msgs/srv/AddObject \
  "{name: 'wall', type: 0, dimensions: {x: 0.1, y: 1.0, z: 1.5}, \
    pose: {position: {x: 0.4, y: 0.0, z: 0.5}, orientation: {w: 1.0}}}"

# 2. Send MpcMove goal (will automatically avoid wall)
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.6, y: 0.3, z: 0.4}, orientation: {w: 1.0}}}" \
  --feedback

# 3. MPC continuously replans to avoid the wall during execution

Adding Obstacles Mid-Execution¶

One of MPC’s key strengths is handling obstacles that appear during motion:

# Terminal 1: Start MPC move to distant goal
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.7, y: 0.5, z: 0.5}, orientation: {w: 1.0}}}" \
  --feedback

# Terminal 2: Add obstacle mid-execution (after 2 seconds)
sleep 2
ros2 service call /unified_planner/add_object curobo_msgs/srv/AddObject \
  "{name: 'dynamic_box', type: 0, dimensions: {x: 0.15, y: 0.15, z: 0.15}, \
    pose: {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {w: 1.0}}}"

# Observe: MPC automatically avoids the new obstacle without stopping!

Troubleshooting MpcMove¶

Issue 1: Action Immediately Fails¶

Symptoms:

result:
  success: false
  message: 'Goal pose unreachable or violates constraints'

Solutions:

# Check if goal is reachable using IK service (warmup first if not done)
ros2 service call /unified_planner/warmup_ik curobo_msgs/srv/WarmupIK "{batch_size: 1}"
ros2 service call /unified_planner/ik curobo_msgs/srv/Ik \
  "{pose: {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {w: 1.0}}}"

# If IK fails, adjust goal pose to be within workspace

Issue 2: MPC Not Converging¶

Symptoms:

result:
  success: false
  message: 'MPC failed to converge after max iterations'

Solutions:

# Increase max iterations
ros2 param set /unified_planner mpc_max_iterations 2000

# Relax convergence threshold
ros2 param set /unified_planner mpc_convergence_threshold 0.02

# Try again
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove "{...}"

Issue 3: Slow Progress (step_progression not advancing)¶

Symptoms: Feedback shows progression stuck at same value

Possible Causes:

Collision with obstacle blocking path
Joint limits preventing motion
Goal pose nearly unreachable

Solutions:

# Check for collision distance
ros2 service call /unified_planner/get_collision_distance curobo_msgs/srv/GetCollisionDistance \
  "{joint_state: {position: [j1, j2, j3, j4, j5, j6]}}"

# If collision detected, remove or adjust obstacles
ros2 service call /unified_planner/remove_object curobo_msgs/srv/RemoveObject "{name: 'obstacle_name'}"

# Or adjust goal pose

Issue 4: Action Never Completes¶

Symptoms: Progression reaches 0.95-0.98 but never completes

Possible Causes:

Convergence threshold too tight
Goal pose requires precision beyond MPC capability

Solutions:

# Relax convergence threshold
ros2 param set /unified_planner mpc_convergence_threshold 0.02

# Or use classic planner for final precise positioning
ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 0}"
ros2 service call /unified_planner/generate_trajectory ...

Monitoring MpcMove Performance¶

View Action Status¶

# List all active actions
ros2 action list -t

# Get detailed info about MpcMove action
ros2 action info /unified_planner/mpc_move

# Output shows:
# - Action server status (ACTIVE/IDLE)
# - Number of active goals
# - Clients connected

Echo Feedback in Real-Time¶

# View feedback without sending a goal
ros2 action send_goal /unified_planner/mpc_move curobo_msgs/action/MpcMove \
  "{target_pose: {position: {x: 0.5, y: 0.3, z: 0.4}, orientation: {w: 1.0}}}" \
  --feedback | tee mpc_feedback.log

# Logs all feedback to file for later analysis

Monitor Joint Commands¶

# In another terminal during MpcMove execution:
ros2 topic echo /joint_states

# Compare commanded joints (from feedback) vs actual joints (from topic)

Step 5: Observe MPC Behavior¶

In RViz¶

Visualization:
- Ghost robot shows predicted trajectory
- Trajectory updates in real-time
- Can see MPC adapting to obstacles

Add dynamic obstacle while robot is moving:

ros2 service call /unified_planner/add_object curobo_msgs/srv/AddObject \
  "{name: 'dynamic_box', type: 0, dimensions: {x: 0.2, y: 0.2, z: 0.2}, \
    pose: {position: {x: 0.4, y: 0.2, z: 0.3}, orientation: {w: 1.0}}}"

Observe: MPC automatically avoids new obstacle without stopping!

Monitor MPC Performance¶

# Monitor MPC iterations
ros2 topic echo /unified_planner/mpc_stats
 
# Expected output:
# iterations: 87
# convergence: 0.009  # meters from goal
# computation_time: 8.5  # ms
# success: true

Step 6: Advanced MPC Usage¶

Dynamic Obstacle Avoidance¶

MPC excels at avoiding obstacles that appear during execution:

def demo_dynamic_avoidance():
    # Start MPC execution
    planner.execute(robot_context, goal_handle)
 
    # Simulate dynamic obstacle appearing
    time.sleep(2.0)  # Let robot start moving
 
    # Add obstacle in robot's path
    add_collision_object("moving_obstacle", [0.5, 0.3, 0.4])
 
    # MPC automatically avoids it without stopping!

Disturbance Rejection¶

MPC can correct for external disturbances:

# Start MPC execution
planner.execute(robot_context, goal_handle)
 
# Simulate disturbance (push robot)
def apply_disturbance():
    # MPC detects deviation and corrects automatically
    pass

Tracking Moving Targets¶

MPC can track moving goal positions:

class MovingTargetMPC:
    def __init__(self):
        self.planner = PlannerFactory.create_planner('mpc', node, config)
        self.timer = node.create_timer(0.1, self.update_goal)
 
    def update_goal(self):
        # Update goal position (e.g., tracking moving object)
        new_goal = self.get_moving_target_pose()
 
        # MPC will automatically adjust trajectory
        self.planner.update_goal(new_goal)

Step 7: Tuning MPC for Your Application¶

High-Speed Reactive Control¶

For fast, reactive movements:

config = {
    'convergence_threshold': 0.02,  # Slightly relaxed
    'max_iterations': 500,          # Fewer iterations
    'control_frequency': 200,       # Very high frequency
    'horizon': 5                    # Short horizon
}

High-Precision Tasks¶

For precise positioning:

config = {
    'convergence_threshold': 0.005,  # Tight tolerance
    'max_iterations': 2000,          # More iterations
    'control_frequency': 50,         # Lower frequency OK
    'horizon': 15                    # Longer horizon
}

Constrained Environments¶

For cluttered spaces with many obstacles:

config = {
    'convergence_threshold': 0.01,
    'max_iterations': 1500,
    'voxel_size': 0.005,  # Fine collision checking
    'horizon': 10
}

Common Issues and Solutions¶

Issue 1: MPC Not Converging¶

Symptom:

Warning: MPC failed to converge after 1000 iterations

Solutions:

Increase max_iterations:

ros2 param set /unified_planner mpc_max_iterations 2000

Check goal is reachable:

# Test with classic planner first
ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 0}"

Relax convergence threshold:

ros2 param set /unified_planner mpc_convergence_threshold 0.02

Issue 2: Jerky Motion¶

Symptom: Robot movement is not smooth, oscillates

Solutions:

Decrease control frequency:

ros2 param set /unified_planner mpc_control_frequency 50

Increase horizon length:

ros2 param set /unified_planner mpc_horizon 15

Tune time_dilation_factor:

ros2 param set /unified_planner time_dilation_factor 0.3  # Slower, smoother

Issue 3: High GPU Usage¶

Symptom: GPU at 100%, system slow

Solutions:

Reduce control frequency:

ros2 param set /unified_planner mpc_control_frequency 30

Shorten horizon:

ros2 param set /unified_planner mpc_horizon 5

Use classic planner for static environments:

ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 0}"

Issue 4: Slow Reaction to Obstacles¶

Symptom: MPC doesn’t avoid obstacles quickly enough

Solutions:

Increase control frequency:

ros2 param set /unified_planner mpc_control_frequency 150

Reduce collision voxel size (finer detection):

ros2 param set /unified_planner voxel_size 0.005

Adjust MPC weights (favor obstacle avoidance):

config['obstacle_weight'] = 10.0  # Higher priority

Comparing MPC vs Classic¶

Experiment: Static Environment¶

# Test 1: Classic planner
ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 0}"
ros2 service call /unified_planner/generate_trajectory ...
# Observe: Fast planning, smooth execution, predictable
 
# Test 2: MPC planner
ros2 service call /unified_planner/set_planner curobo_msgs/srv/SetPlanner "{planner_type: 1}"
ros2 service call /unified_planner/generate_trajectory ...
# Observe: Continuous replanning, slightly slower, adaptive

Result: In static environments, Classic is more efficient

Experiment: Dynamic Environment¶

# Add obstacle mid-execution
 
# With Classic planner:
# - Robot must stop
# - Replan required
# - Execution resumes
 
# With MPC planner:
# - No stop needed
# - Automatic avoidance
# - Continuous execution

Result: In dynamic environments, MPC is superior

Best Practices¶

1. Choose the Right Planner¶

Use Classic when:
- Environment is static
- Speed/efficiency is critical
- Trajectory must be reproducible
- Limited GPU resources
Use MPC when:
- Environment has moving obstacles
- Real-time adaptation needed
- Disturbance rejection required
- High-precision tracking

2. Start with Default Parameters¶

# Good defaults for most applications
config = {
    'convergence_threshold': 0.01,
    'max_iterations': 1000,
    'control_frequency': 100,
    'horizon': 10
}

Tune only if needed!

3. Monitor Performance¶

Always monitor MPC performance:

ros2 topic echo /unified_planner/mpc_stats

Look for:

Iterations: Should be < max_iterations
Convergence: Should approach threshold
Computation time: Should be < 1/frequency

4. Warm-Up the Planner¶

Pre-warm MPC for faster first execution:

# At startup
planner = PlannerFactory.create_planner('mpc', node, config)
planner.warmup()  # Initializes GPU kernels

5. Fallback to Classic¶

If MPC fails, fallback to Classic:

result = mpc_planner.plan(start, goal, config)
if not result.success:
    self.get_logger().warn('MPC failed, falling back to Classic')
    classic_planner = PlannerFactory.create_planner('classic', node, config)
    result = classic_planner.plan(start, goal, config)

Next Steps¶

Constrained Planning - Custom constraints
Unified Planner Concepts - Deep dive into architecture
Performance Optimization - GPU optimization

References¶

MPC Overview
cuRobo Documentation
Unified Planner Architecture - Overall planner framework design
MPC Implementation Guide - Technical guide for developers

For Developers: If you’re implementing the MPC planner, see the MPC Implementation Guide for detailed technical specifications, code templates, and testing strategies.