CODEWITHSUNDEEP
drake
Shubham Vyas
Shubham Vyas

Reputation: 165

Partial Differentiation of the Mass Matrix for Free-Floating system with 7 DoF Arm

I'm trying to partially differentiate the mass matrix with respect to the positions (including the free-floating base quaternions). However, I try to first substitute the unit quaternion constraint in the symbolic mass matrix i.e. unit quaternion constraint and then differentiate with respect to all the positions (except q_0).

When I try to use plant.ToSymbolic() and then extract the symbolic mass matrix using plant.CalcMassMatrixViaInverseDynamics() (ManipulatorDynamics function taken from this example) it works for a single free-floating rigid body (3-DoF floating) and also for a free-floating box with a 3-DoF arm (6-DoF system) but the same process does not work when I change to a 7-DoF arm attached to the floating base (13-DoF system). The mass matrix is computed but the function to substitute the unit quaternion constraint takes too long (never finishes and returns). Portion of the applicable code:

q0 = Variable("q0")
q1 = Variable("q1")
q2 = Variable("q2")
q3 = Variable("q3")

unit_cons = Expression.sqrt(1 - q1**2 - q2**2 - q3**2)

mixed_q = np.array([q0, q1, q2, q3,
                    0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0])
v = np.zeros(plant.num_velocities())

(M, Cv, tauG, B, tauExt) = ManipulatorDynamics(plant.ToSymbolic(), mixed_q, v)
M[0, 0].Substitute({q0: unit_cons})

This runs well for a single floating rigid body, floating body + 3-DoF arm. But the last line takes a long time and never returns for a floating base + 7-DoF arm.

Is there a better way to approach this with Drake's symbolic engine? Or would AutoDiff be a better choice for this? I only need the 1st partial derivative in this case.

As for some background, I am trying to create a custom linearization of the MultiBodyPlant which embeds the unit quaternion constraint. This requires the partial derivatives of the terms after substituting the unit quaternion constraint to only have the vector part of the quaternion in the equations of motion.

Upvotes: 1

Views: 78

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

Or would AutoDiff be a better choice for this? I only need the 1st partial derivative in this case.

If you want to evaluate the partial differentiation, then I would suggest using autodiff. To handle your unit quaternion constraint, you can compute your q0 from the vector part of the quaternion.

Here is the pseudo code.

quaternion_vec_val = np.array([0.1, 0.2, 0.3])
q_non_quaternion_val = np.array([0.4, 0.5, 0.6])
q_variable = np.concantenate((quaternion_vec_val, q_non_quaternion_val))
# We want to take derivative w.r.t the vector part of the quaternion, and all non-quaternion variables. Hence we call InitializeAutoDiff on the variables we want to take derivative.
q_variable_ad = pydrake.math.Initialize(q_variable)
quaternion_scalar_ad = np.array([np.sqrt(1 - np.sum(q_variable_ad[:3]**2))])
q_ad = np.concantenate((quaternion_scalar_ad, q_variable_ad))
plant_ad = plant.ToAutoDiffXd()
# Initialize v_ad
v_ad = np.zeros(6, dtype=np.object)
for i in range(6):
  v_ad[i] = pydrake.AutoDiffXd(0)
(M, C, g, B, tau_ext) = ManipulatorDynamics(plant_ad, q_ad, v_ad)
print(M(0, 0).derivatives())

(I haven't run the code, especially the part that initializes v_ad is fuzzy, but hopefully you get the idea).

Upvotes: 3

Related Questions