From fcb5768b0c8feeecd2b418ade790655cabc4ea81 Mon Sep 17 00:00:00 2001 From: robotics-pathon-ai Date: Sun, 10 May 2026 19:51:00 -0700 Subject: [PATCH] Add SetEqualityActive service for runtime weld toggle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Exposes ~/set_equality_active service so external nodes can enable or disable an MJCF at runtime by name. Primary use case: weld constraints between a gripper jaw and a graspable object, toggled on close / off on open so the object follows the gripper rigidly during pick-and-place tasks without depending on contact friction. The runtime enable flag lives on mj_data->eq_active, NOT mj_model->eq_active0 (which is only the initial value from XML). Crucially: for mjEQ_WELD, activation with a non-matching XML-default relpose applies a large impulse to satisfy the compiled relpose. The service takes an optional relpose_from_current flag — when set with active=true on a weld, the callback reads body1/body2 current world poses, computes relpose = body1^-1 * body2 via mju_negPose + mju_mulPose, and writes it into mj_model->eq_data[id*mjNEQDATA + 3..9] before flipping eq_active. The weld then locks the present configuration with zero residual and zero activation impulse. References: https://github.com/google-deepmind/mujoco/discussions/2323 Co-Authored-By: Claude Opus 4.7 (1M context) --- .../mujoco_system_interface.hpp | 18 +++++ .../src/mujoco_system_interface.cpp | 80 +++++++++++++++++++ mujoco_ros2_control_msgs/CMakeLists.txt | 1 + .../srv/SetEqualityActive.srv | 11 +++ 4 files changed, 110 insertions(+) create mode 100644 mujoco_ros2_control_msgs/srv/SetEqualityActive.srv diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp index 5809c9d2..521f17c2 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -282,6 +283,19 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface void reset_world_callback(const std::shared_ptr request, std::shared_ptr response); + /** + * @brief Service callback to enable/disable an MJCF by name. + * + * Looks up the equality with mj_name2id(mjOBJ_EQUALITY, name) and toggles + * mj_data->eq_active[id]. Primary use case: weld constraints between a + * gripper jaw and a graspable object, toggled on close / off on open so + * the object follows the gripper rigidly during pick-and-place tasks + * without depending on contact friction. + */ + void set_equality_active_callback( + const std::shared_ptr request, + std::shared_ptr response); + /** * @brief Spins the physics simulation for the Simulate Application */ @@ -390,6 +404,10 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface // Reset world service rclcpp::Service::SharedPtr reset_world_service_; + // Set equality active service (toggles MJCF by name at runtime) + rclcpp::Service::SharedPtr + set_equality_active_service_; + // Storage for initial state (used for reset_world) std::vector initial_qpos_; std::vector initial_qvel_; diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 82ae7bcc..e7dfe275 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -986,6 +986,15 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf std::bind(&MujocoSystemInterface::reset_world_callback, this, std::placeholders::_1, std::placeholders::_2)); RCLCPP_INFO(get_logger(), "Created reset_world service at: %s/reset_world", get_node()->get_fully_qualified_name()); + // Create set_equality_active service (toggle MJCF by name) + set_equality_active_service_ = + get_node()->create_service( + "~/set_equality_active", + std::bind(&MujocoSystemInterface::set_equality_active_callback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(get_logger(), "Created set_equality_active service at: %s/set_equality_active", + get_node()->get_fully_qualified_name()); + // Ready cameras RCLCPP_INFO(get_logger(), "Initializing cameras..."); cameras_ = std::make_unique(get_node(), sim_mutex_, mj_data_, mj_model_, camera_publish_rate); @@ -2635,6 +2644,77 @@ void MujocoSystemInterface::reset_world_callback( RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); } +void MujocoSystemInterface::set_equality_active_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + const std::unique_lock lock(*sim_mutex_); + int eq_id = mj_name2id(mj_model_, mjOBJ_EQUALITY, request->equality_name.c_str()); + if (eq_id == -1) + { + response->success = false; + response->message = "Equality '" + request->equality_name + "' not found in MJCF model."; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + return; + } + + // For mjEQ_WELD activation: if requested, snapshot the current relative pose + // between body1 and body2 into eq_data so the weld locks the CURRENT + // configuration rather than the XML-default pose. Without this, activating + // a weld between a gripper jaw and an object that spawned far away applies + // a massive impulse to satisfy the original (now-incorrect) relpose. + if (request->active && request->relpose_from_current && + mj_model_->eq_type[eq_id] == mjEQ_WELD) + { + int b1 = mj_model_->eq_obj1id[eq_id]; + int b2 = mj_model_->eq_obj2id[eq_id]; + if (b1 < 0 || b2 < 0) + { + response->success = false; + response->message = "Weld equality has invalid body ids."; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + return; + } + // Body world poses live in mj_data->xpos (3) and mj_data->xquat (4). + mjtNum p1[3], q1[4], p2[3], q2[4]; + mju_copy3(p1, mj_data_->xpos + 3 * b1); + mju_copy4(q1, mj_data_->xquat + 4 * b1); + mju_copy3(p2, mj_data_->xpos + 3 * b2); + mju_copy4(q2, mj_data_->xquat + 4 * b2); + // relpose = body1^-1 * body2 (pose of body2 expressed in body1's frame) + mjtNum inv_p1[3], inv_q1[4]; + mju_negPose(inv_p1, inv_q1, p1, q1); + mjtNum rel_p[3], rel_q[4]; + mju_mulPose(rel_p, rel_q, inv_p1, inv_q1, p2, q2); + // eq_data layout for mjEQ_WELD (mjNEQDATA = 11): + // [0..2] anchor (point on body2, in body2 local frame; leave as-is) + // [3..5] relpose position + // [6..9] relpose quaternion + // [10] torquescale (leave as-is) + mjtNum* d = mj_model_->eq_data + eq_id * mjNEQDATA; + d[3] = rel_p[0]; + d[4] = rel_p[1]; + d[5] = rel_p[2]; + d[6] = rel_q[0]; + d[7] = rel_q[1]; + d[8] = rel_q[2]; + d[9] = rel_q[3]; + RCLCPP_INFO(get_logger(), + "Snapshotted current relpose for weld '%s': pos=[%.4f %.4f %.4f] quat=[%.4f %.4f %.4f %.4f]", + request->equality_name.c_str(), rel_p[0], rel_p[1], rel_p[2], + rel_q[0], rel_q[1], rel_q[2], rel_q[3]); + } + + // Runtime equality enable lives on mj_data (per-equality, 0=off, 1=on). + // mj_model->eq_active0 is only the initial value from XML; toggling there + // has no effect once the sim is running. + mj_data_->eq_active[eq_id] = request->active ? 1 : 0; + response->success = true; + response->message = std::string("Equality '") + request->equality_name + "' set to " + + (request->active ? "active" : "inactive"); + RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); +} + // simulate in background thread (while rendering in main thread) void MujocoSystemInterface::PhysicsLoop() { diff --git a/mujoco_ros2_control_msgs/CMakeLists.txt b/mujoco_ros2_control_msgs/CMakeLists.txt index 7325488c..b6f509a1 100644 --- a/mujoco_ros2_control_msgs/CMakeLists.txt +++ b/mujoco_ros2_control_msgs/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(rosidl_default_generators REQUIRED) set(srv_files srv/ResetWorld.srv + srv/SetEqualityActive.srv ) diff --git a/mujoco_ros2_control_msgs/srv/SetEqualityActive.srv b/mujoco_ros2_control_msgs/srv/SetEqualityActive.srv new file mode 100644 index 00000000..31a91261 --- /dev/null +++ b/mujoco_ros2_control_msgs/srv/SetEqualityActive.srv @@ -0,0 +1,11 @@ +string equality_name +bool active +# When true AND active=true, snapshot the current relative pose between the +# weld's body1 and body2 into eq_data before activating. This prevents the +# weld from applying an impulse when the bodies aren't at the XML-default +# relative pose (e.g., gripper closed on an object that spawned far away). +# Only meaningful for type=mjEQ_WELD. +bool relpose_from_current +--- +bool success +string message