如何在 pydrake 中正确使用 MultibodyPlant::CalcGravityGeneralizedForces？

Question

我正在尝试使用以下代码为杂技机器人创建一个简单的重力补偿控制器：

from pydrake.all import *

file_name = "acrobot.sdf"  # from drake/multibody/benchmarks/acrobot/acrobot.sdf
plant = MultibodyPlant()
parser = Parser(plant=plant)
robot = parser.AddModelFromFile(file_name)
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -9.81]))
plant.Finalize()

nq = plant.num_positions()
nv = plant.num_velocities()
nx = nq + nv
nu = plant.num_actuators()
assert (nx, nu) == (4, 1)

plant_ctx = plant.CreateDefaultContext()

x_plant = plant.GetMutablePositionsAndVelocities(plant_ctx)
x_plant[:] = [0.1, 0.2,
              0.3, 0.4]

tau_g = plant.CalcGravityGeneralizedForces(plant_ctx)

print tau_g

不幸的是，tau_g 始终为零。

似乎重力矢量没有应用到机器人上。如何解决？

更新：

工作 C++ 实现：

systems::DiagramBuilder<double> builder;
MultibodyPlant<double>& plant = *builder.AddSystem<MultibodyPlant>(0.001);
Parser parser(&plant);
drake::multibody::ModelInstanceIndex robot_instance_index = parser.AddModelFromFile(full_name);
plant.AddForceElement<UniformGravityFieldElement>();
plant.Finalize();

systems::Simulator<double> simulator(plant);

// Simulator Context
VectorXd state = plant.GetPositionsAndVelocities(simulator.get_mutable_context());
state(0) = 0.5;
state(1) = 0.5;
plant.SetPositionsAndVelocities(&simulator.get_mutable_context(),state);
// prints -15.3096 -8.25483
std::cout << plant.CalcGravityGeneralizedForces(simulator.get_mutable_context()) << std::endl;

// Default Context
std::unique_ptr<Context<double>> context_ptr = plant.CreateDefaultContext();
auto context = context_ptr.get();
VectorXd state_2 = plant.GetPositionsAndVelocities(*context, robot_instance_index);
state_2(0) = 0.5;
state_2(1) = 0.5;
plant.SetPositionsAndVelocities(context, state_2);
std::cout << plant.CalcGravityGeneralizedForces(*context) << std::endl;
// prints -15.3096 -8.25483

Answer 1

我确认我看到了相同的行为，并且在您的实施中没有发现任何明显的错误。
我打开了 https://github.com/RobotLocomotion/drake/issues/11051

如何在 pydrake 中正确使用 MultibodyPlant::CalcGravityGeneralizedForces？

How to properly use MultibodyPlant::CalcGravityGeneralizedForces in pydrake?

python

drake