Styjun

I am trying to create a simple gravity compensation controller for acrobot with the following code:

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

Unfortunately, tau_g is always zero.

It seems that the gravity vector is not applied to the robot.
How can this be fixed?

UPDATE:

Working C++ implementation:

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

I confirm that I see the same behavior, and don't see any obvious errors in your implementation.

I've opened https://github.com/RobotLocomotion/drake/issues/11051