Add Coupler Constraint (similar to a gearbox constaint from Gazebo)

[scpeters]

Problem

We have been working to port the Gearbox joint functionality from Gazebo classic to gz-sim (new Gazebo) and have been planning to change the SDFormat specification for this feature to use //joint/axis/mimic (similar to URDF) instead of the Gearbox joint type (see proposal in gazebosim/sdf_tutorials#62). This feature creates a linear relationship between the output position of two joints, and we have been working to implement it in the gz-physics dartsim plugin using the MimicMotorConstraint (see gazebosim/gz-physics#431). However, I realized recently while testing gazebosim/gz-sim#1838 (comment) that the MimicMotorConstraint does not create a bilateral constraint between the two joints. In order to support the Gearbox joint functionality, we need a constraint that applies equal and opposite impulses to each joint in the constraint, and in my testing the MimicMotorConstraint does not do this.

Proposed Solution

I'd like to be able to create a constraint between two joints that creates a linear relationship between the positions of two joints by applying equal and opposite impulses to each joint. Since the MimicMotorConstraint was already present in dartsim, I think it would be least disruptive to add a new constraint type. In Drake, for example, they added Coupler constraints in RobotLocomotion/drake#17639 and are using that constraint type in their URDF parser to support the mimic element (see RobotLocomotion/drake#18728). "Coupler constraint" sounds like a reasonable name to me.

Describe alternatives you've considered

Change the behavior of the MimicMotorConstraint to be a bilateral constraint. This would be disruptive, however, for an already released feature.

Additional context

See gazebosim/gz-sim#1838 (comment)

[scpeters]

I have tinkered with this a bit but haven't figured it out yet. For testing, I merged the branch from gazebosim/gz-physics#431 with gz-physics7 and updated the dartsim plugin to use the current API for SetJointMimicConstraint, since that was changed by further iterations in gazebosim/gz-physics#517 after gazebosim/gz-physics#431 was closed. I also enabled the joint mimic tests for dartsim in branch scpeters/dart_mimic_7, so that can be used for testing potential fixes like #1835

[scpeters]

my tinkering attempts are in release-6.13...scpeters/mimic_hacking_6.13; it's not working yet. I did notice that an API change may be needed in order to use a non-const pointer to the leader / follower joints

[roncapat]

Sorry to ask, but is there any progress on this? I believe this was blocking also mimick support in Gazebo with DART IIRC.

[scpeters]

I haven't seen any progress since the draft PR #1835 was opened, and yes it is blocking mimic support in gz-physics and gz-sim

[hbaqueiro]

I'm also interested in this feature. Please let me know if I can help somehow.