[multibody] Add introspection API for Joint to determine if actuatable

[IanTheEngineer]

Is your feature request related to a problem? Please describe.
When working with a MbP and a parallel jaw gripper for control purposes, it would be useful to know when a joint is able to be actuated versus when a joint is "mimicked" and therefore fully constrained. An extension to this request would be to determine if a joint is welded. This ticket evolved out of an in-person discussion with @ggould-tri and @rpoyner-tri.

Describe the solution you'd like
I would like to see an API level function in the Joint< T > class that gives access to knowledge of constraints underlying the joint's actuation. A solution could be something like:
bool can_actuate(const systems::Context< T > &context) which returns false if the joint is mimicked or welded, true otherwise.

Describe alternatives you've considered
A set of more granular API functions in the Joint< T > class would be acceptable as well:
bool is_mimic() and bool is_welded(const systems::Context< T > &context).

The mimic function wouldn't require a context as its value is determined by initial parsing of URDF / SDF.

[IanTheEngineer]

MultibodyPlant does have an API function named int num_actuated_dofs():

https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ae3fc8de8de8eaf2d37a24c79d6be8e97

With that being said, the ability to query if an individual joint is mimicked would still be a useful API addition.

[xuchenhan-tri]

Looks like this may be related to what you are working on @joemasterjohn? If not, please reassign to @amcastro-tri for triage.

[joemasterjohn]

First just some clarifications, what do you mean by "welded"? If the joint in question is a WeldJoint then it will never be actuated as it has 0 dofs (Joint::num_velocities() is 0 for weld joints) and you cannot create a JointActuator for that joint. If by welded you mean using the joint locking APIs, the the joint will likewise not be actuated. Locking the joint is equivalent to specifying v_i = 0 for all dofs of that joint. Even if there is a JointActuator associated with the given locked joint, actuation forced will simply be ignored when determining the velocities and accelerations of that joint's dofs.

Currently we only support "mimic" joint by adding a SAP specific coupler constraint between the dofs of the joint and its mimic. This constraint currently does not affect the ability to actuate the joint. So if there is an actuator associated with the joint and you apply actuation forces on it, those will compete with the constraint forces generated by the coupler. The effect being your actuation forces being cancelled out to the best of the constraint's ability (given stiffness and impulse limits). This is likewise true if the joint is "welded" using constraints. Whether or not we should be able to apply actuation forces to dofs associated with coupler constraints is probably up for debate. For instance we do not specify which joint is the "leader" and which is the "follower" of the coupler constraint. This allows you to actuate one of the joints and have the coupler effect on the unactuated joint and then switch which joint you are actuating and keep the coupling, without having to remodel your plant.

What we could do immediately is add introspection to give you back all constraints that could directly affect the dofs of a given joint. That would be a list of constraints that directly affect dofs associated with the joint. But welding two bodies in the sim does not necessarily affect the mobility of a joint that either body is a parent or child of. Further analysis of the entire multibody tree's kinematics would need to happen to determine the effect on certain dofs.

[jwnimmer-tri]

Well said.

Regarding writing controllers for mimic joints, see also #18917 (comment). Broadly, for controllers, we should able to obtain the constraints that affect the configuration, and control to those constraints (whether they be mimic, locking, or anything else). Possibly there is a fast-path code path for mimic in particular, but in general there can be all kinds of constraints on MbP kinematics that controllers are going to need to learn how to obey. It's up for debate (or exploration) whether a mimic-specific solution is worthwhile, or if we should just to constraint-based control from day one.

The overall premise of the feature request is correct -- that we lack a sufficient API to interrogate the plant. I would just argue that the missing API is not "is mimic" or "can actuate" but rather "tell me your constraints".

[joemasterjohn]

The overall premise of the feature request is correct -- that we lack a sufficient API to interrogate the plant. I would just argue that the missing API is not "is mimic" or "can actuate" but rather "tell me your constraints".

Ok that we can do more or less easily. I'll take a crack at it with some remaining time I have left in Q2.

[jwnimmer-tri]

See also related work https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_system_constraint.html. I am not sure if it is suitable for this case, but it's worth a look.

[IanTheEngineer]

Thanks for the clarifications and follow-up, Joe and Jeremy. Based on these, I think the proposed introspection API around "tell me your constraints" would be sufficient.

@joemasterjohn-TRI, you mentioned that WeldedJoints will have the property of Joint::num_velocities() == 0. Is it also the case that a mimicked joint (without a transmission) will have the property Joint::num_velocities() == 0?

If that holds true, I can use a similar check to mask out mimicked joints for control purposes, which could be a useful workaround. The num_actuated_dofs_ calculation in ModelInstance::add_joint_actuator() seems to back up this line of thinking:

drake/multibody/tree/model_instance.h

Lines 82 to 85 in 8d2fee2

	void add_joint_actuator(const JointActuator<T>* joint_actuator) {
	num_actuated_dofs_ += joint_actuator->joint().num_velocities();
	joint_actuators_.push_back(joint_actuator);
	}

[jwnimmer-tri]

... mimicked joint (without a transmission) ...

Oh. If you're already omitting the transmission in your model, then I think you already have what you need?

namespace {
bool IsJointActuated(const MultibodyPlant<T>& plant, const Joint<T>& joint) {
  for (JointActuatorIndex i{0}; i < plant.num_actuators(); ++i) {
    if (plant.get_joint_actuator(i).joint().index() == joint.index()) {
      return true;
    }
  }
  return false;
}  // namespace

[IanTheEngineer]

Thanks for that snippit, Jeremy. Using the JointActuatorIndex does seem like the most straightforward implementation pathway.

To confirm my understanding, does it hold that an unactuated joint without a transmission will always have Joint::num_velocities() == 0, but may still have Joint::num_positions() > 0?
(I can move this discussion to DrakeDeveloper's Slack if it's more appropriate there.)

[jwnimmer-tri]

To confirm my understanding, does it hold that an unactuated joint without a transmission will always have Joint::num_velocities() == 0, but may still have Joint::num_positions() > 0?

No, I'm pretty sure that joints (other than weld joints) always have nq > 0 and nv > 0. You could load up an acrobot model to check for sure. I assume it has nq == 2 and nv == 2.