search

dartsim / dart

DART: Dynamic Animation and Robotics Toolkit
http://dartsim.github.io/
BSD 2-Clause "Simplified" License
889 stars 286 forks source link

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

Open scpeters opened 1 year ago

scpeters commented 1 year ago

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 https://github.com/gazebosim/sdf_tutorials/pull/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 https://github.com/gazebosim/gz-physics/pull/431). However, I realized recently while testing https://github.com/gazebosim/gz-sim/pull/1838#issuecomment-1595953064 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 https://github.com/RobotLocomotion/drake/pull/17639 and are using that constraint type in their URDF parser to support the mimic element (see https://github.com/RobotLocomotion/drake/pull/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 https://github.com/gazebosim/gz-sim/pull/1838#issuecomment-1595953064

scpeters commented 1 month ago

I have tinkered with this a bit but haven't figured it out yet. For testing, I merged the branch from https://github.com/gazebosim/gz-physics/pull/431 with gz-physics7 and updated the dartsim plugin to use the current API for SetJointMimicConstraint, since that was changed by further iterations in https://github.com/gazebosim/gz-physics/pull/517 after https://github.com/gazebosim/gz-physics/pull/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 https://github.com/dartsim/dart/pull/1835

scpeters commented 1 month ago

my tinkering attempts are in https://github.com/dartsim/dart/compare/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