CODEWITHSUNDEEP
drake
zliu
zliu

Reputation: 153

Getting contact Jacobian through multibody plant

In using a multibody plant (MBP) for estabilishing dynamics constraints for multibody system with contact, we look at the dynamics equation

M(q) q_ddot + C(q, q_dot) = G(q) + B(q) u + J'(q) lambda

where q is the states of the system, *_dot and *_ddot denotes *'s time derivative and double derivative, respectively; M(q) is the inertial matrix, C(q, q_dot) consists of the terms for Coriolis, centripetal, and gyroscopic effect, G(q) is the gravitational force, B(q) is the actuator matrix, u is the input (torque/force), J'(q) is the contact Jacobian transposed, and lambda is the contact force in the contact frame.

is there a way to get the contact Jacobian terms J(q) from the plant or is it up for the users to implement it with the contact results?

Question Update:

I've been trying to verify the returned results from hand-derived Jacobian and there is some mis-matching that I don't understand.

First, I constructed a very simple system: a link fixed to world through a revolute joint and a sphere that can roll in the x-y plane through dummy prismatic joints. The link is named left proximal. The contact Jacobian is easy to derive by hand with contact point returned from contactResults using which, the contact dynamics is verified.

Now, here is how I find contact jacobian from drake functions: 

geometryA_id = left_proximal_geometry_id
geometryB_id = object_geometry_id
signed_distance_pair = query_object.ComputeSignedDistancePairClosestPoints(geometryA_id, geometryB_id)
print(signed_distance_pair.distance)

linkA = get_name_by_geometryId(plant, geometryA_id)
bodyA = plant.GetBodyByName(linkA)
linkB = get_name_by_geometryId(plant, geometryB_id)
bodyB = plant.GetBodyByName(linkB)

frame_A_id = inspector.GetFrameId(signed_distance_pair.id_A)
frame_B_id = inspector.GetFrameId(signed_distance_pair.id_B)
frameA = plant.GetBodyFromFrameId(frame_A_id).body_frame()
frameB = plant.GetBodyFromFrameId(frame_B_id).body_frame()

frame_W = plant.world_frame()
p_GbCb = signed_distance_pair.p_BCb
p_GaCa = signed_distance_pair.p_ACa

wrt = JacobianWrtVariable.kQDot

Jv_v_BCa_W = plant.CalcJacobianTranslationalVelocity(plant_context, wrt, frameB, p_GbCb, frameA, frame_W);


# alternative, compute Jacobian matrix by individually find Jv for A and B w.r.t. world
Jva = plant.CalcJacobianTranslationalVelocity(
    plant_context,
    JacobianWrtVariable(0),
    frameA,
    p_GaCa,
    frame_W,
    frame_W
)
Jvb = plant.CalcJacobianTranslationalVelocity(
    plant_context,
    JacobianWrtVariable(0),
    frameB,
    p_GbCb,
    frame_W,
    frame_W
)
Jv_alternative = Jvb - Jva

Judging by the numerical results, Jvb is correct (mostly because it's on the object with the prismatic joints) but Jva is not matching up with what's expected. I believe all the inputs are as described in the documentation and the printed values of p_GbCb looks reasonable (at least the the norm matches with the radius). However since the values don't check off there must be something I'm missing...

Upvotes: 1

Views: 1065

Answers (3)

Hongkai Dai
Hongkai Dai

Reputation: 2766

The contact Jacobian depends on where the contact point is. There are two ways to set the contact point

  1. As @RussTedrake mentioned, you can use ComputeSignedDistancePairClosestPoints to return all pairs of closest points. You can then check SignedDistancePair.distance, if the distance is no larger than 0, then you can use the witness points in the returned SignedDistancePair as the contact points (the caveat is that you need to transform the witness point in the geometry frame to the body frame using SceneGraphInspector::GetPoseInFrame(signed_distance_pair.id_A)), you could refer to this code. Notice that when q changes, the contact points in this approach would also change, and even the number of contact points could change (the point that was in contact becomes not in contact when q changes, or vice versa), hence the size of J and lambda would change. This size change could cause problem in trajectory optimization, as lambda is likely your decision variable, and you want your decision variables to have fixed size.
  2. If you are doing trajectory optimization, and you want to impose the constraint like "the robot heel is in contact with the ground", then I would suggest to designate the contact point to be the heel of the robot. Once you know the position of the robot heel in the foot frame (from URDF/SDF), you could compute the Jacobian using CalcJacobianTranslationalVelocity. And also impose a kinematic constraint that the heel is on the ground (you could refer to PositionConstraint on adding this kinematic constraint). By fixing the contact point in the body frame, you have a fixed size of J and lambda, which are important in optimization if lambda is your decision variable.

Upvotes: 2

Sherm
Sherm

Reputation: 678

If you know where the points are, it is easy to get the Jacobian from MBP using methods like CalcJacobianTranslationalVelocity().

Likely you would have to look at the contact results to find the points; I'm not sure about that.

Upvotes: 2

Russ Tedrake
Russ Tedrake

Reputation: 5533

I think the recommended workflow is to get a QueryObject from the SceneGraph output port. On this QueryObject you can call ComputeSignedDistancePairClosestPoints(), to find the closest points on each pair of bodies. The distance between these two points is what we call φ(q) in the notes. Then you can call the Jacobian methods on MultibodyPlant to take the gradients of φ with respect to q.

Upvotes: 2

Related Questions