问题描述
我想问一个与工作流有关的问题，如下所示：
1.获得多体设备的机械手方程，
2.将其重写为qdd = f（q，u）的形式
3.计算下一个状态（x_next）
4.计算x_next w.r.t x和u的梯度。
我发现最困难的步骤是获取惯性矩阵的逆，该逆矩阵支持下一步的梯度计算。 Drake提供了很多好的工具，支持double，autodiffXd，Symbolic，我们可以使用它们来完成任务吗？谢谢！
这是一个双摆的简单例子。（我希望工作流程将应用于更复杂的系统。）
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
解决方法
为了获得x_next和x的{{1}}的梯度，您将需要使用自动微分。您可以参考pydrake.autodiffutils.initializeAutoDiff和pydrake.autodiffutils.autoDiffToGradientMatrix来提取渐变。这是一个例子
u顺便说一句，我没有看到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,input_port x,u)
功能在drake中。我假设您自己实现了此功能。通常，为了计算动力学，我将CalcTimeDerivatives用于连续时间系统，将CalcDiscreteVariableUpdates用于离散时间系统。有关设置ManipulatorDynamics的详细信息，请参考我们的tutorial。调用这两个函数。