drake

How to correctly update the mass of a body in the context

I want to change the mass of a link in each trial to tabulate the max joint torque. Upon loading the robot model,

m1 = plant.GetBodyByName('linear_slide').get_mass(plant_context)

Shows the expected default value of mass m1 = 1.66 Then,

diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)

plant.GetBodyByName('linear_slide').SetMass(plant_context, 10)

Now when I check the mass of the body it is infact updated to the value set.

My controller class accepts the system directly and extracts all other components from it directly. However, I do not know how to extract the above modified mass from the system context. For example,

pp = diagram.GetSubsystemByName('plant')
cc = diagram.CreateDefaultContext()

pp.GetBodyByName('linear_slide').get_mass(cc)

In this case, I do not get the updated value. This is expected since I am invoking the default context and not the mutable one. How do I extract the mutable context in this case?

I tried diagram.GetMyMutableContextFromRoot(), however, this needs a root context too, which again, would be a default context.

Is there no way to modify the mass to of a body in the plant and have it reflected in the system context? Another way to overcome this would be to make the mass modification after the system is sent to the controller as a sub-step, which soloves the problem but is not the job of the controller class.

Solution

  • I'm not 100% sure what you're asking, but I think this is it:

    plant_context = plant.GetMyMutableContextFromRoot(
    root_context=diagram_context)
plant.GetBodyByName('linear_slide').SetMass(plant_context, 10)