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 s

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.