diff --git a/src/custom_mpc_behavior/CMakeLists.txt b/src/custom_mpc_behavior/CMakeLists.txt new file mode 100644 index 00000000..4253eb96 --- /dev/null +++ b/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 $ + $) +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 $ + $) +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() diff --git a/src/custom_mpc_behavior/behavior_plugin.yaml b/src/custom_mpc_behavior/behavior_plugin.yaml new file mode 100644 index 00000000..e46ad742 --- /dev/null +++ b/src/custom_mpc_behavior/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + custom_mpc_behavior: + - "custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader" diff --git a/src/custom_mpc_behavior/custom_mpc_behavior_plugin_description.xml b/src/custom_mpc_behavior/custom_mpc_behavior_plugin_description.xml new file mode 100644 index 00000000..18827121 --- /dev/null +++ b/src/custom_mpc_behavior/custom_mpc_behavior_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp b/src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp new file mode 100644 index 00000000..6933b9a0 --- /dev/null +++ b/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 +{ +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& 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 diff --git a/src/custom_mpc_behavior/include/custom_mpc_behavior/site_tracking.hpp b/src/custom_mpc_behavior/include/custom_mpc_behavior/site_tracking.hpp new file mode 100644 index 00000000..3ec43772 --- /dev/null +++ b/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 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 diff --git a/src/custom_mpc_behavior/package.xml b/src/custom_mpc_behavior/package.xml new file mode 100644 index 00000000..822f9ddb --- /dev/null +++ b/src/custom_mpc_behavior/package.xml @@ -0,0 +1,26 @@ + + + custom_mpc_behavior + 0.0.0 + Custom MPC behavior + + MoveIt Pro User + MoveIt Pro User + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/custom_mpc_behavior/src/custom_mpc_behavior.cpp b/src/custom_mpc_behavior/src/custom_mpc_behavior.cpp new file mode 100644 index 00000000..93a45680 --- /dev/null +++ b/src/custom_mpc_behavior/src/custom_mpc_behavior.cpp @@ -0,0 +1,25 @@ +#include "../include/custom_mpc_behavior/custom_mpc_behavior.hpp" + +#include + +#include "spdlog/spdlog.h" + +namespace custom_mpc_behavior +{ +CustomMpcBehavior::CustomMpcBehavior(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::MPCBehaviorBase(name, config, shared_resources) +{ +} + +BT::PortsList CustomMpcBehavior::providedPorts() +{ + return moveit_studio::behaviors::MPCBehaviorBase::providedPorts(); +} + +BT::KeyValueVector CustomMpcBehavior::metadata() +{ + return { { "description", "Custom MPC behavior" }, { "subcategory", "Motion - Execute" } }; +} + +} // namespace custom_mpc_behavior diff --git a/src/custom_mpc_behavior/src/register_behaviors.cpp b/src/custom_mpc_behavior/src/register_behaviors.cpp new file mode 100644 index 00000000..50c9942d --- /dev/null +++ b/src/custom_mpc_behavior/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace custom_mpc_behavior +{ +class CustomMpcBehaviorBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors( + BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "CustomMpcBehavior", shared_resources); + } +}; +} // namespace custom_mpc_behavior + +PLUGINLIB_EXPORT_CLASS(custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/custom_mpc_behavior/src/site_tracking.cpp b/src/custom_mpc_behavior/src/site_tracking.cpp new file mode 100644 index 00000000..41506ba9 --- /dev/null +++ b/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 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 diff --git a/src/custom_mpc_behavior/test/CMakeLists.txt b/src/custom_mpc_behavior/test/CMakeLists.txt new file mode 100644 index 00000000..ceb38dd7 --- /dev/null +++ b/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}) diff --git a/src/custom_mpc_behavior/test/test_behavior_plugins.cpp b/src/custom_mpc_behavior/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..a871cc0b --- /dev/null +++ b/src/custom_mpc_behavior/test/test_behavior_plugins.cpp @@ -0,0 +1,36 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("custom_mpc_behavior::CustomMpcBehaviorBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "CustomMpcBehavior", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/lab_sim/objectives/mpc_pose_tracking.xml b/src/lab_sim/objectives/mpc_pose_tracking.xml index 85102679..5581911b 100644 --- a/src/lab_sim/objectives/mpc_pose_tracking.xml +++ b/src/lab_sim/objectives/mpc_pose_tracking.xml @@ -109,38 +109,38 @@