Version: 8

Model Predictive Control (MPC)

Model Predictive Control (MPC) is a powerful technique for optimizing robot motion in real-time by solving an optimization problem over a prediction horizon. MoveIt Pro provides a framework for creating custom MPC Behaviors that can optimize multiple goals simultaneously through residual functions.

This how-to guide demonstrates how to create a custom MPC Behavior for a pose tracking application as well as a custom residual function to penalize joint motion.

note

This is an advanced topic and will likely require integration support from PickNik engineers.

danger

This is experimental and only guaranteed to work in simulation for now. Use caution when testing on hardware.

Overview

MPC in MoveIt Pro uses MuJoCo for physics simulation and optimization. The optimization problem aims to minimize a weighted sum of residuals. In this implementation, residuals are used to represent the error which is minimized through a gradient descent optimization function.

For more information please reference the paper and the code that our MoveIt Pro MPC implementation is based on: "Predictive Sampling: Real-time Behavior Synthesis with MuJoCo", mujoco_mpc.

Prerequisites

Before creating custom MPC Behaviors, ensure that:

Your robot configuration package has a MuJoCo model configured (.xml file)
You are familiar with creating custom Behaviors

Creating a Simplified MuJoCo Model

While MPC can technically be performed using any MuJoCo scene as the model, it is highly recommended to create a simplified MuJoCo model to improve optimization speed. This can be built from scratch, or by starting from an existing MuJoCo .xml scene and removing all unnecessary elements. This model should typically be limited to only the robot kinematics. As an example, here is the simplified lab_sim MuJoCo model used for the Behaviors in this document. (As compared to the full lab_sim scene).

Creating a Custom MPC Behavior

An MPC Behavior inherits from MPCBehaviorBase and defines the set of residual functions it will optimize. Here's how to create one:

Step 1: Create a custom behavior from the UI

Set the Behavior Name to "custom_mpc_behavior", the Behavior Type to "SyncActionNodeWithSharedResources" for realtime control, and the Description to "My custom MPC Behavior."

Creating the custom MPC behavior

This will create a ROS2 package with the following structure

custom_mpc_behavior
├── behavior_plugin.yaml
├── CMakeLists.txt
├── custom_mpc_behavior_plugin_description.xml
├── include
│   └── custom_mpc_behavior
│       └── custom_mpc_behavior.hpp
├── package.xml
├── src
│   ├── custom_mpc_behavior.cpp
│   └── register_behaviors.cpp
└── test
    ├── CMakeLists.txt
    └── test_behavior_plugins.cpp

Step 2: Overwrite the Header File

Overwrite the custom_mpc_behavior.hpp header file with the following contents:

#pragma once

#include "moveit_pro_mpc/residual_functions/joint_acceleration.hpp"
#include "moveit_pro_mpc/residual_functions/joint_velocity.hpp"
#include "moveit_pro_mpc/residual_functions/site_tracking.hpp"
#include "moveit_studio_behavior_interface/mpc_behavior_base.hpp"
// Include other residual function headers as needed

namespace custom_mpc_behavior
{

/**
 * @brief Custom MPC Behavior for tracking a target pose.
 *
 * This behavior uses Model Predictive Control to track a target pose
 * while optimizing for smooth motion and respecting joint limits.
 */
class CustomMpcBehavior : public moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>
{
public:
  CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config,
                    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);

  static BT::PortsList providedPorts();

  /**
   * @brief Define the residuals used by this MPC behavior.
   * @return A tuple of name-residual pairs.
   */
  static auto getResiduals()
  {
    return std::make_tuple(std::make_pair("site_tracking", moveit_pro_mpc::SiteTrackingResidualFunction()),
                           std::make_pair("velocity", moveit_pro_mpc::VelocityResidualFunction()),
                           std::make_pair("acceleration", moveit_pro_mpc::AccelerationResidualFunction()));
  }

  static BT::KeyValueVector metadata();
};

}  // namespace custom_mpc_behavior

The key component is the getResiduals() method, which returns a tuple of string-residual pairs. Each pair consists of:

A name for the residual (used to create Behavior Tree ports)
An instance of the residual function

For this example we will be incorporating three residual functions:

A site tracking residual to minimize the distance between the end effector pose and an arbitrary pose we wish to track.
A velocity residual to limit the velocity of the robot joints.
An acceleration residual to limit the acceleration of the robot joints.

Step 3: Overwrite the Implementation File

Overwrite the corresponding custom_mpc_behavior.cpp file with the following contents:

#include "custom_mpc_behavior/custom_mpc_behavior.hpp"
#include "moveit_studio_behavior_interface/metadata_fields.hpp"

namespace
{
inline constexpr auto kDescription =
    "Custom MPC Behavior that tracks a target pose while optimizing for smooth motion.";
}

namespace custom_mpc_behavior
{

CustomMpcBehavior::CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config,
                                     const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
  : MPCBehaviorBase(name, config, shared_resources)
{
}

BT::PortsList CustomMpcBehavior::providedPorts()
{
  return moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>::providedPorts();
}

BT::KeyValueVector CustomMpcBehavior::metadata()
{
  return { { moveit_studio::behaviors::kSubcategoryMetadataKey, "Motion - Execute" },
           { moveit_studio::behaviors::kDescriptionMetadataKey, kDescription } };
}

}  // namespace custom_mpc_behavior

Step 4: Build Your Package

Build your user workspace:

moveit_pro build user_workspace

Step 5: Create an Objective Using Your New Custom Behavior

Launch MoveIt Pro using:

moveit_pro run -c lab_sim

Duplicate the "MPC Pose Tracking" Objective as a starting point for the new "Custom MPC Pose Tracking" Objective:

Duplicating the "MPC Pose Tracking" Objective

Replace the "MPCPoseTracking" Behavior with the new "CustomMpcBehavior" so the Behavior Tree beneath the "Parallel" node looks like the following:

"Custom MPC Pose Tracking" Objective Behavior Tree

Update the following ports in "CustomMpcBehavior":

Set the acceleration weight to 0.
Set the gripper_site_name parameter to pinch.
Set the max_velocity_goal parameter to 0.
Set mujoco_model to description/simple_scene.xml.
Set mujoco_model_package to lab_sim.
Set the site_tracking weight to 1.0.
Set target_pose to {stamped_pose}.
Set target_twist to {stamped_twist}.
Set the velocity weight to 0.

The ports should look like the following:

CustomMPCBehavior Ports

Run the "Custom MPC Pose Tracking" Objective, the end effector should track the moving site, but it might overshoot a little initially because it is moving so quickly.

Change the weight on the velocity residual to 0.1 and run the Objective again. Notice how this causes the robot to slow down and not overshoot as much, but it also means the robot cannot keep up as well with the pose. Reset the velocity weight to 0.0 before moving on.

Creating a Custom Residual Function

Residual functions define the optimization goals. Here's how to create a custom residual function that minimizes the motion of a specific joint in the robot model.

Step 1: Create the Header File

Create minimize_joint_motion_residual.hpp in the custom_mpc_behavior/include/custom_mpc_behavior directory:

#pragma once

#include "moveit_pro_mpc/residual_functions/residual_function_interface.hpp"
#include "mujoco/mjdata.h"
#include "mujoco/mjmodel.h"
#include "tl_expected/expected.hpp"

namespace custom_residuals
{

/**
 * @brief Custom residual function for minimizing motion of a joint.
 *
 * This residual penalizes any motion of a specified joint from its initial value.
 * Useful for keeping specific joints stationary during motion.
 */
class MinimizeJointMotionResidualFunction final : public moveit_pro_mpc::ResidualFunctionInterface
{
public:
  /**
   * @brief Get the number of dimensions for this residual.
   * @return Number of dimensions (1 for a single joint value).
   */
  [[nodiscard]] int getNumDims() const override
  {
    return 1;
  }

  /**
   * @brief Validate residual parameters and save initial joint value.
   * @return Success or error message.
   */
  tl::expected<bool, std::string> validateResidualParameters() override;

  /**
   * @brief Apply the residual function to compute motion from initial joint value.
   * @param model The MuJoCo model.
   * @param data The MuJoCo data at the current timestep.
   * @param residual Output array for residual values.
   */
  void applyResidual(const mjModel& model, const mjData& data, double* residual) const override;

  /**
   * @brief Define parameters that can be set from Behavior Tree ports.
   * @return Tuple of parameter name-pointer pairs.
   */
  [[nodiscard]] auto getResidualParameters()
  {
    return std::make_tuple(std::make_pair("limited_motion_joint_name", &joint_name_));
  }

private:
  /** @brief Name of the joint in the MuJoCo model whose motion should be minimized. */
  std::string joint_name_;

  /** @brief Index of the joint in the MuJoCo model (cached during validation). */
  int joint_index_ = -1;

  /** @brief Initial value of the joint (saved during validation). */
  mjtNum initial_joint_value_ = 0.0;
};

}  // namespace custom_residuals

Step 2: Implement the Residual Function

Create minimize_joint_motion_residual.cpp in the custom_mpc_behavior/src directory:

#include "custom_mpc_behavior/minimize_joint_motion_residual.hpp"
#include "fmt/format.h"
#include "mujoco/mujoco.h"

namespace custom_residuals
{

tl::expected<bool, std::string> MinimizeJointMotionResidualFunction::validateResidualParameters()
{
  // Look up the joint by name in the MuJoCo model
  joint_index_ = mj_name2id(model, mjOBJ_JOINT, joint_name_.c_str());
  if (joint_index_ == -1)
  {
    return tl::make_unexpected(fmt::format("Joint '{}' does not exist in the MuJoCo model.", joint_name_));
  }

  // Save the initial value of the joint from initial_data
  // Get the joint address in qpos (joint position data)
  int qpos_addr = model->jnt_qposadr[joint_index_];
  initial_joint_value_ = initial_data->qpos[qpos_addr];

  return true;
}

void MinimizeJointMotionResidualFunction::applyResidual(const mjModel& /*model*/, const mjData& data,
                                                        double* residual) const
{
  // Get the joint address in qpos
  int qpos_addr = model->jnt_qposadr[joint_index_];

  // Get current joint value from MuJoCo data
  const mjtNum current_joint_value = data.qpos[qpos_addr];

  // Compute difference from initial joint value (residual 0)
  residual[0] = current_joint_value - initial_joint_value_;
}

}  // namespace custom_residuals

Step 3: Incorporate the Residual Function into Your MPC Behavior

To use your custom MinimizeJointMotionResidualFunction in your MPC Behavior, you need to update the behavior's header file to include it in the getResiduals() method.

Update the custom_mpc_behavior.hpp file to include your custom residual:

#pragma once

#include "moveit_pro_mpc/residual_functions/joint_acceleration.hpp"
#include "moveit_pro_mpc/residual_functions/joint_velocity.hpp"
#include "moveit_pro_mpc/residual_functions/site_tracking.hpp"
#include "moveit_studio_behavior_interface/mpc_behavior_base.hpp"
// Include your custom residual function header
#include "custom_mpc_behavior/minimize_joint_motion_residual.hpp"

namespace custom_mpc_behavior
{

/**
 * @brief Custom MPC Behavior with MinimizeJointMotion residual.
 */
class CustomMpcBehavior : public moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>
{
public:
  CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config,
                    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);

  static BT::PortsList providedPorts();

  /**
   * @brief Define the residuals used by this MPC behavior.
   * @return A tuple of name-residual pairs.
   */
  static auto getResiduals()
  {
    return std::make_tuple(std::make_pair("site_tracking", moveit_pro_mpc::SiteTrackingResidualFunction()),
                           std::make_pair("velocity", moveit_pro_mpc::VelocityResidualFunction()),
                           std::make_pair("acceleration", moveit_pro_mpc::AccelerationResidualFunction()),
                           // Add your custom residual function
                           std::make_pair("minimize_joint_motion",
                                          custom_residuals::MinimizeJointMotionResidualFunction()));
  }

  static BT::KeyValueVector metadata();
};

}  // namespace custom_mpc_behavior

Step 4: Update CMakeLists.txt to Build the Residual Function

To ensure your custom residual function is compiled and linked with your MPC Behavior, update the custom_mpc_behavior/CMakeLists.txt file to include the new source file.

Add src/minimize_joint_motion_residual.cpp to the list of source files in the add_library() command:

cmake_minimum_required(VERSION 3.22)
project(custom_mpc_behavior CXX)

find_package(moveit_pro_package REQUIRED)
moveit_pro_package()

set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib)
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
  find_package(${package} REQUIRED)
endforeach()

add_library(
  custom_mpc_behavior
  SHARED
  src/custom_mpc_behavior.cpp
  src/minimize_joint_motion_residual.cpp  # Add this line
  src/register_behaviors.cpp)
target_include_directories(
  custom_mpc_behavior
  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
target_link_libraries(custom_mpc_behavior PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})

# Install the plugin description file
pluginlib_export_plugin_description_file(moveit_studio_behavior_interface
                                         custom_mpc_behavior_plugin_description.xml)

# Install headers
install(DIRECTORY include/ DESTINATION include)

# Install the library
install(
  TARGETS custom_mpc_behavior
  EXPORT custom_mpc_behaviorTargets
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

After updating the CMakeLists.txt file, rebuild your workspace:

moveit_pro build user_workspace

Once you rebuild your workspace, your MPC Behavior will automatically expose:

A weight port named minimize_joint_motion for tuning the residual weight
A parameter port named limited_motion_joint_name for specifying which joint to keep stationary

Step 5: Update the Custom Behavior Ports for the New Residual

Find and update the following new ports in "CustomMpcBehavior" from the new residual:

Set the limited_motion_joint_name parameter to linear_rail_joint.
Set the minimize_joint_motion weight to 5.

Run the Objective again and notice how the motion of the linear rail joint is restricted. Try increasing the weight on the minimize_joint_motion residual to something large like 50 and run the Objective again. Notice how the robot can no longer track the position of the site because the linear rail joint no longer moves but still attempts to track the orientation of the site using the other joints.

Overview​

Prerequisites​

Creating a Simplified MuJoCo Model​

Creating a Custom MPC Behavior​

Step 1: Create a custom behavior from the UI​

Step 2: Overwrite the Header File​

Step 3: Overwrite the Implementation File​

Step 4: Build Your Package​

Step 5: Create an Objective Using Your New Custom Behavior​

Creating a Custom Residual Function​

Step 1: Create the Header File​

Step 2: Implement the Residual Function​

Step 3: Incorporate the Residual Function into Your MPC Behavior​

Step 4: Update CMakeLists.txt to Build the Residual Function​

Step 5: Update the Custom Behavior Ports for the New Residual​