Allow specifying mimic joint properties per DoF (#1684)

[jslee02]

This PR supports specifying mimic joint properties per DOF, as requested in issue #1684.

Previously, DART allowed specifying mimic joints with the constraint that the dependent and reference joint must have the same DOF, and the whole DOF would be mimicked rather than individual DOFs.

This PR removes these constraints, allowing a mimic joint to mimic individual DOFs with different reference joints, DOF indices, offsets, and multipliers. The following functions have been added:

  /// Set the mimic joint with a single reference joint and the same multiplier
  /// and offset for all dependent joint's DOFs.
  void setMimicJoint(
      const Joint* referenceJoint,
      double mimicMultiplier = 1.0,
      double mimicOffset = 0.0);

  /// Sets the mimic joint properties for a specific DOF of the dependent joint.
  void setMimicJointDof(std::size_t index, const MimicDofProperties& mimicProp);

  /// Sets the mimic joint properties for all DOFs of the dependent joint using
  /// a vector of MimicDofProperties.
  void setMimicJointDofs(const std::vector<MimicDofProperties>& mimicProps);

  /// Sets the mimic joint properties for specific DOFs of the dependent joint
  /// using a map of DOF index and MimicDofProperties.
  void setMimicJointDofs(
      const std::map<std::size_t, MimicDofProperties>& mimicPropMap);

where

/// Properties of a mimicked Degree of Freedom (DOF) in a joint.
struct MimicDofProperties
{
  /// Pointer to the reference joint used for mimicking behavior.
  const Joint* referenceJoint;

  /// Index of the reference DOF in the reference joint.
  std::size_t referenceDofIndex = 0;

  /// Multiplier applied to the reference joint's DOF value.
  double multiplier = 1.0;

  /// Offset added to the reference joint's DOF value.
  double offset = 0.0;
};

Note that this change breaks API and ABI (by removing some public members) even though it's targeting 6.14. Please let us know if this is problematic for Gazebo to adopt this change.

Resolves #1684

CC: @adityapande-1995

---

Before creating a pull request

• Document new methods and classes
• Format new code files using ClangFormat by running make format
• Build with -DDART_TREAT_WARNINGS_AS_ERRORS=ON and resolve all the compile warnings

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change
• Add Python bindings for new methods and classes

[jslee02]

The member variables of MimicDofProperties will be renamed to follow the convention for DART 6.x to:

struct MimicDofProperties
{
  const Joint* mReferenceJoint;
  std::size_t mReferenceDofIndex = 0;
  double mMultiplier = 1.0;
  double mOffset = 0.0;
};

[jslee02]

@scpeters @adityapande-1995 I'm going to proceed with merging this for the time being. If you have any concerns or feedback regarding this change, please let me know!

[scpeters]

Sorry I didn't have a chance to review this yet. I'll try testing it and let you know how it goes

[scpeters]

Sorry I didn't have a chance to review this yet. I'll try testing it and let you know how it goes

I started writing a test with a multi-axis joint for which only one of its axes would use the mimic actuator type, but I noticed that Joint::setActuatorType does not appear to distinguish between the dofs of a joint, so I think there may still be something missing in the API.

I noticed it when I got to the following line in the current test:

• https://github.com/dartsim/dart/blob/release-6.14/unittests/integration/test_Joints.cpp#L1098

[jslee02]

Are you suggesting that there could be a scenario where a joint contains both mimic degrees of freedom (DOFs) and non-mimic DOFs? I did not consider this situation in this PR. Addressing such a case might require additional refactoring, enabling us to set the actuator type for individual DOFs rather than the entire joint.

[scpeters]

Are you suggesting that there could be a scenario where a joint contains both mimic degrees of freedom (DOFs) and non-mimic DOFs?

Yes, this would provide maximum modeling flexibility, and while I could write an integration test for this functionality, I don't have a real-world use case in mind that motivates this feature.

I did not consider this situation in this PR. Addressing such a case might require additional refactoring, enabling us to set the actuator type for individual DOFs rather than the entire joint.

Yes I think it would require more refactoring

[jslee02]

Understood. Indeed, fully supporting various actuator types for each degree of freedom (DOF) would necessitate extensive refactoring and might not be feasible for certain joint types, particularly those with non-Euclidean joint spaces such as BallJoint. As an alternative, we could consider a compromise by allowing different actuator types within the same category, such as kinematic (e.g., VELOCITY, LOCK) or dynamic (e.g., TORQUE, SERVO, MIMIC), given that the underlying forward dynamics algorithms might support this. However, I have concerns about adding a new feature in the absence of a clear use case.

For clarity, can the current version fulfill the requirements specified in issue #1684? If a real-world use case emerges, we could then reassess the possibility of supporting different actuator types per DOF.