Obtaining distance and orientation derivatives in pydrake

Question

We are designing a controller in pydrake which requires the following information:

1. Closest point pairs and minimum distance between the robot and objects, along with derivatives of these values with respect to the robot's state q (i.e. the Jacobian from the closest point pairs to q). We noticed the CalcDistanceDerivatives function but it is internal.
2. Derivative of the orientation of the end effector with respect to q. We know there is an OrientationConstraint but the needed information does not seem to be directly available without adding the constraint.
3. Other information we may want to obtain, but pydrake or C++ does not provide the corresponding exposed API.

What is the best workflow if we have to modify the Drake source code? Should we implement all of our algorithms in C++, or add Python bindings to the modified C++ code and still use Python?

Answer 1

Closest point pairs and minimum distance between the robot and objects, along with derivatives of these values with respect to the robot's state q (i.e. the Jacobian from the closest point pairs to q). We noticed the CalcDistanceDerivatives function but it is internal.

There are two ways to obtain this information. One is using CollisionChecker class. The second is to use ComputeSignedDistancePairwiseClosestDistance (you can find an example here)

Derivative of the orientation of the end effector with respect to q. We know there is an OrientationConstraint but the needed information does not seem to be directly available without adding the constraint.

To get this Jacobian matrix, you can call MultibodyPlant::CalcJacobianAngularVelocity function.

Other information we may want to obtain, but pydrake or C++ does not provide the corresponding exposed API.

Could you give the example? The function are probably available inside MultibodyPlant or SceneGraph classes.