Custom mpc behavior and residual

src/custom_mpc_behavior/CMakeLists.txt

@@ -0,0 +1,59 @@
+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 moveit_pro_mpc)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+  custom_mpc_residuals
+  SHARED
+  src/site_tracking.cpp)
+target_include_directories(
+  custom_mpc_residuals
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(custom_mpc_residuals moveit_pro_mpc::mpc_problem moveit_pro_mpc::mpc_residuals)
+ament_target_dependencies(custom_mpc_residuals
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+add_library(
+  custom_mpc_behavior
+  SHARED
+  src/custom_mpc_behavior.cpp
+  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 custom_mpc_residuals)
+ament_target_dependencies(custom_mpc_behavior
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS custom_mpc_behavior custom_mpc_residuals
+  EXPORT custom_mpc_behaviorTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(custom_mpc_behavior)
+endif()
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+  moveit_studio_behavior_interface custom_mpc_behavior_plugin_description.xml)
+
+ament_export_targets(custom_mpc_behaviorTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/custom_mpc_behavior/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    custom_mpc_behavior:
+      - "custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader"

src/custom_mpc_behavior/custom_mpc_behavior_plugin_description.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="custom_mpc_behavior">
+  <class
+    type="custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader"
+    base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>

src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include "custom_mpc_behavior/site_tracking.hpp"
+#include "moveit_pro_mpc/residual_functions/cartesian_acceleration.hpp"
+#include "moveit_pro_mpc/residual_functions/cartesian_velocity.hpp"
+#include "moveit_pro_mpc/residual_functions/force_torque.hpp"
+#include "moveit_pro_mpc/residual_functions/joint_acceleration.hpp"
+#include "moveit_pro_mpc/residual_functions/joint_velocity.hpp"
+#include "moveit_studio_behavior_interface/mpc_behavior_base.hpp"
+
+namespace custom_mpc_behavior
+{
+/**
+ * @brief TODO(...)
+ */
+class CustomMpcBehavior : public moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>
+{
+public:
+  /**
+   * @brief Constructor for the custom_mpc_behavior behavior.
+   * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+   * this Behavior is created within a new behavior tree.
+   * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+   * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+   * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+   * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+   * Behavior is created within a new behavior tree.
+   * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+   * the initialize() function is called, so these classes should not be used within the constructor.
+   */
+  CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config,
+                    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
+
+  /**
+   * @brief Implementation of the required providedPorts() function for the custom_mpc_behavior Behavior.
+   * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+   * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+   * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+   * used internally by the behavior tree.
+   * @return custom_mpc_behavior does not expose any ports, so this function returns an empty list.
+   */
+  static BT::PortsList providedPorts();
+
+  /**
+   * @brief Define the residuals used by this MPC behavior.
+   * @details This method allows the use of a MPCProblem instance to add residual-specific ports.
+   * @return A BT::PortsList containing the required ports for the behavior.
+   */
+  static auto getResiduals()
+  {
+    return std::make_tuple(std::make_pair("site_tracking", custom_mpc_behavior::SiteTrackingResidualFunction()),
+                           std::make_pair("velocity", moveit_pro_mpc::VelocityResidualFunction()),
+                           std::make_pair("acceleration", moveit_pro_mpc::AccelerationResidualFunction()),
+                           std::make_pair("force_torque", moveit_pro_mpc::ForceTorqueResidualFunction()),
+                           std::make_pair("cartesian_velocity", moveit_pro_mpc::CartesianVelocityResidualFunction()),
+                           std::make_pair("cartesian_acceleration",
+                                          moveit_pro_mpc::CartesianAccelerationResidualFunction()));
+  }
+
+  /**
+   * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+   * subcategory, in the MoveIt Studio Developer Tool.
+   * @return A BT::KeyValueVector containing the Behavior metadata.
+   */
+  static BT::KeyValueVector metadata();
+};
+}  // namespace custom_mpc_behavior

src/custom_mpc_behavior/include/custom_mpc_behavior/site_tracking.hpp

@@ -0,0 +1,49 @@
+// Copyright 2025 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#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_mpc_behavior
+{
+
+/**
+ * \brief Residual functions for tracking a moving pose.
+ */
+class SiteTrackingResidualFunction final : public moveit_pro_mpc::ResidualFunctionInterface
+{
+public:
+  [[nodiscard]] int getNumDims() const override;
+
+  tl::expected<bool, std::string> validateResidualParameters() override;
+
+  void applyResidual(const mjModel& model, const mjData& data, double* residual) const override;
+
+  [[nodiscard]] auto getResidualParameters()
+  {
+    return std::make_tuple(std::make_pair("gripper_site_name", &site_tip_),
+                           std::make_pair("target_pose", &target_pose_),
+                           std::make_pair("target_twist", &target_twist_));
+  }
+
+private:
+  /** @brief The robot end effector site in the MuJoCo model to be used for tracking.*/
+  std::string site_tip_;
+  /** @brief The target pose to track. Must be in world frame.*/
+  geometry_msgs::msg::PoseStamped target_pose_;
+  /** @brief The target twist to track. Must be in world frame.*/
+  geometry_msgs::msg::TwistStamped target_twist_;
+
+  int site_tip_ind_ = 1;
+};
+
+}  // namespace custom_mpc_behavior

src/custom_mpc_behavior/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<package format="3">
+  <name>custom_mpc_behavior</name>
+  <version>0.0.0</version>
+  <description>Custom MPC behavior</description>
+
+  <maintainer email="[email protected]">MoveIt Pro User</maintainer>
+  <author email="[email protected]">MoveIt Pro User</author>
+
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_pro_package</build_depend>
+
+  <depend>moveit_studio_behavior_interface</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+  <buildtool_depend>python3-colcon-common-extensions</buildtool_depend>
+</package>

src/custom_mpc_behavior/src/custom_mpc_behavior.cpp

@@ -0,0 +1,25 @@
+#include "../include/custom_mpc_behavior/custom_mpc_behavior.hpp"
+
+#include <custom_mpc_behavior/custom_mpc_behavior.hpp>
+
+#include "spdlog/spdlog.h"
+
+namespace custom_mpc_behavior
+{
+CustomMpcBehavior::CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config,
+                                     const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
+  : moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>(name, config, shared_resources)
+{
+}
+
+BT::PortsList CustomMpcBehavior::providedPorts()
+{
+  return moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>::providedPorts();
+}
+
+BT::KeyValueVector CustomMpcBehavior::metadata()
+{
+  return { { "description", "Custom MPC behavior" }, { "subcategory", "Motion - Execute" } };
+}
+
+}  // namespace custom_mpc_behavior

src/custom_mpc_behavior/src/register_behaviors.cpp

@@ -0,0 +1,24 @@
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_studio_behavior_interface/behavior_context.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
+
+#include <custom_mpc_behavior/custom_mpc_behavior.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace custom_mpc_behavior
+{
+class CustomMpcBehaviorBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+  void registerBehaviors(
+      BT::BehaviorTreeFactory& factory,
+      [[maybe_unused]] const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
+  {
+    moveit_studio::behaviors::registerBehavior<CustomMpcBehavior>(factory, "CustomMpcBehavior", shared_resources);
+  }
+};
+}  // namespace custom_mpc_behavior
+
+PLUGINLIB_EXPORT_CLASS(custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader,
+                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

src/custom_mpc_behavior/src/site_tracking.cpp

@@ -0,0 +1,92 @@
+// Copyright 2025 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include "custom_mpc_behavior/site_tracking.hpp"
+
+#include "fmt/format.h"
+#include "mujoco/mujoco.h"
+
+namespace custom_mpc_behavior
+{
+
+int SiteTrackingResidualFunction::getNumDims() const
+{
+  return 6;
+}
+
+tl::expected<bool, std::string> SiteTrackingResidualFunction::validateResidualParameters()
+{
+  site_tip_ind_ = mj_name2id(model, mjOBJ_SITE, site_tip_.c_str());
+  if (site_tip_ind_ == -1)
+  {
+    return tl::make_unexpected(
+        fmt::format("The specified {} '{}' does not exist in the MuJoCo model.", "site_tip_", site_tip_));
+  }
+
+  if ("world" != target_pose_.header.frame_id || "world" != target_twist_.header.frame_id)
+  {
+    return tl::make_unexpected(fmt::format("The specified `{}` does not match the frame_id of either pose or twist "
+                                           "inputs. The frame of the pose input is {} and the twist input is {}.",
+                                           "site_tip_", target_pose_.header.frame_id, target_twist_.header.frame_id));
+  }
+
+  return true;
+}
+
+void SiteTrackingResidualFunction::applyResidual(const mjModel& /*model*/, const mjData& data, double* residual) const
+{
+  mjtNum target_position_velocity[3] = {};
+  mjtNum target_angular_velocity[3] = {};
+  mjtNum target_position[3] = {};
+  mjtNum target_quat[4] = {};
+  target_position[0] = target_pose_.pose.position.x;
+  target_position[1] = target_pose_.pose.position.y;
+  target_position[2] = target_pose_.pose.position.z;
+  target_quat[0] = target_pose_.pose.orientation.w;
+  target_quat[1] = target_pose_.pose.orientation.x;
+  target_quat[2] = target_pose_.pose.orientation.y;
+  target_quat[3] = target_pose_.pose.orientation.z;
+  target_position_velocity[0] = target_twist_.twist.linear.x;
+  target_position_velocity[1] = target_twist_.twist.linear.y;
+  target_position_velocity[2] = target_twist_.twist.linear.z;
+  target_angular_velocity[0] = target_twist_.twist.angular.x;
+  target_angular_velocity[1] = target_twist_.twist.angular.y;
+  target_angular_velocity[2] = target_twist_.twist.angular.z;
+
+  // ---------- Residual (0-2) ----------
+  const mjtNum delta_time = data.time - initial_data->time;
+
+  mjtNum current_target_pos[3];
+  current_target_pos[0] = target_position[0] + target_position_velocity[0] * delta_time;
+  current_target_pos[1] = target_position[1] + target_position_velocity[1] * delta_time;
+  current_target_pos[2] = target_position[2] + target_position_velocity[2] * delta_time;
+
+  const mjtNum* tip_xpos_current = &data.site_xpos[3 * site_tip_ind_];
+
+  residual[0] = tip_xpos_current[0] - current_target_pos[0];
+  residual[1] = tip_xpos_current[1] - current_target_pos[1];
+  residual[2] = tip_xpos_current[2] - current_target_pos[2];
+
+  // ---------- Residual (3-5) ----------
+
+  // Set the current site quaternion
+  const mjtNum* tip_xmat_current = &data.site_xmat[9 * site_tip_ind_];
+  mjtNum tip_quat_current[4];
+  mju_mat2Quat(tip_quat_current, tip_xmat_current);
+
+  mjtNum current_target_quat[4];
+  mjtNum current_target_quat_diff[4];
+  mju_quatIntegrate(current_target_quat_diff, target_angular_velocity, delta_time);
+  mju_mulQuat(current_target_quat, current_target_quat_diff, target_quat);
+
+  mjtNum relative_quat_target[3];
+  mju_subQuat(relative_quat_target, current_target_quat, tip_quat_current);
+  residual[3] = relative_quat_target[0];
+  residual[4] = relative_quat_target[1];
+  residual[5] = relative_quat_target[2];
+}
+
+}  // namespace custom_mpc_behavior

src/custom_mpc_behavior/test/CMakeLists.txt

@@ -0,0 +1,5 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+
+ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp)
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})