how to get a dynamic which we can be applied gradient in the next step (re-open)

Question

I want to ask a question related to a workflow like this:
1.obtain manipulator equation of multibody plant, 2.re-write it to a form of qdd = f(q,u), 3.calculate next state(x_next), 4.calculate the gradient of x_next w.r.t x and u.

I find the most difficult step is to get the inverse of Inertial matrix which support the gradient calculation in next following step. Drake provide many good tools support double, AutoDiffXd, Symbolic, could we use them to get thing done? Thanks!

Here is is a simple example of double pendulum.( I expect the work flow will be applied to more complicate system.)

def discrete_dynamics(x, u):
    
    dt = 0.05
    q = x[0:2]
    qd = x[2:4] 

    plant = MultibodyPlant(time_step=dt)
    parser = Parser(plant)
    parser.AddModelFromFile("double_pendulum.sdf")
    plant.Finalize()
    (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(plant.ToAutoDiffXd(), q, qd)
  
    M_inv = inv(M) <------ # ????, how to do it 
 
    qdd = np.dot(M_inv,(tauG + np.dot(B ,u) - np.dot(Cv,qd)))   
    
    x_d_list = qd.tolist()
    qdd_list = qdd.tolist()
    x_d_list.extend(qdd_list) 
    x_d = np.array(x_d_list)

    x_next = x+x_d*dt

    return x_next

f = discrete_dynamics(x, u)
f_x = ...  #calculate gradient of f w.r.t x 
f_u = ...  #calculate gradient of f w.r.t u

Answer 1

In order to get the gradient of x_next w.r.t x and u, you will need to use automatic differentiation. You could refer to pydrake.autodiffutils.initializeAutoDiff and pydrake.autodiffutils.autoDiffToGradientMatrix to extract the gradient. Here is an example

from pydrake.autodiffutils import initializeAutoDiff, autoDiffToGradientMatrix, AutoDiffXd
from pydrake.systems.framework import BasicVector_


def get_dynamics_gradient(plant_ad, context_ad, input_port, x_val, u_val):
    xu_val = np.hstack((x_val, u_val))
    nx = context_ad.num_continuous_states()
    # Set the gradient (default to identity), namely the gradient field of
    # xu_ad records the gradient w.r.t x and u. All of the  autodiff
    # scalars computed using xu_ad now carry the gradient w.r.t x and u.
    xu_ad = initializeAutoDiff(xu_val)
    x_ad = xu_ad[:nx]
    u_ad = xu_ad[nx:]
    context_ad.SetContinuousState(x_ad)
    plant_ad.get_input_port(input_port).FixValue(context_ad, BasicVector_[AutoDiffXd](u_ad))
    derivatives = plant_ad.AllocateTimeDerivatives()
    plant_ad.CalcTimeDerivatives(context_ad, derivatives)
    xdot_ad = derivatives.get_vector().CopToVector()    
    x_next_ad = xdot_ad * dt + x_ad
    AB = autoDiffToGradientMatrix(x_next_ad)
    A = AB[:, :nx]
    B = AB[:, nx:]
    return A, B

plant = MultibodyPlant(time_step=0.)
parser = Parser(plant)
parser.AddModelFromFile("double_pendulum.sdf")
plant.Finalize()
plant_autodiff = plant.ToAutoDiffXd()
context_ad = plant_ad.CreateDefaultContext()
x = np.array([1., 2., 3., 4.])
u = np.array([5., 6.])
# I don't know the name of the input port, please adjust this name
# based on your parsed plant.
input_port = 1
print(f"input port {input_port} name is {plant_autodiff.get_input_port(input_port).get_name()}")
f_x, f_u = get_dynamics_gradient(plant_autodiff, context_ad, input_port x, u)

BTW, I don't see ManipulatorDynamics function in drake. I assume you implemented this function by yourself. Typically to compute the dynamics, I use CalcTimeDerivatives for continuous time system, and CalcDiscreteVariableUpdates for discrete time system.You could refer to our tutorial for more details on setting Context for calling these two functions.