Add SetEqualityActive service for runtime equality (weld) toggle

[robotics-pathon-robotics]

Summary

Exposes a new ~/set_equality_active service so external nodes can enable or disable an MJCF <equality> 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. This is a common pattern in MuJoCo manipulation environments.

Service definition

string equality_name
bool active
bool relpose_from_current
---
bool success
string message

The relpose_from_current flag is critical for welds. Without it, activating a weld at runtime applies a large impulse because the constraint solver tries to enforce the XML-default relative pose (compiled into mj_model->eq_data at load time), not the current pose between the bodies. When this flag is true on activation and the equality type is mjEQ_WELD, the callback:

1. Reads current world poses of body1 and body2 from mj_data->xpos/xquat.
2. Computes relpose = body1^-1 * body2 via mju_negPose + mju_mulPose.
3. Writes the new pos+quat into mj_model->eq_data[id*mjNEQDATA + 3..9] (positions at 3..5, quat at 6..9 for mjEQ_WELD).
4. Then sets mj_data->eq_active[id] = 1.

The weld then locks the present configuration with zero residual and zero activation impulse.

The runtime enable flag lives on mj_data->eq_active, NOT mj_model->eq_active0 (which is only the initial value from XML).

References

• MuJoCo discussion #2323 — establishing weld via bodies and defining relative pose in eq_data.
• MuJoCo XML reference for <weld>.

Test plan

• ros2 service call /mujoco_node/set_equality_active mujoco_ros2_control_msgs/srv/SetEqualityActive "{equality_name: '<name>', active: false, relpose_from_current: false}" returns success=false with "not found" when the equality doesn't exist.
• Same call with an existing equality name returns success=true and the constraint deactivates (verified by absence of impulse).
• Activating a weld with relpose_from_current: true while body1 and body2 are at arbitrary current poses produces no impulse and locks the current relative pose (verified visually in simulate).
• The callback respects the existing sim_mutex_ so it doesn't race against the physics loop.

Downstream consumers

• pathon_so101 PR Reuse the logger with get_logger method ros-controls/mujoco_ros2_control#28 — adds <weld> blocks per spawned YCB object in scene_generator.py.
• pathon_vlgrasping PR [Jazzy] Added capability to provide a key frame as starting configuration ros-controls/mujoco_ros2_control#4 — control_gripper calls this service on close (activate with snapshot) and open (deactivate).

🤖 Generated with Claude Code