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

Question

我正在尝试使用以下代码为 acrobot 创建一个简单的重力补偿 Controller :

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