PyDrake: Center of Mass in Free-Floating Simulation is moving wihtout any (External) Forces Applied

Question

As I understand, without any application of external force (such as gravity), the center of mass of a multibody system should be constant if only internal torques (at the joints) are applied.

In PyDrake, I've set up a free-floating simulation of a box spacecraft with a 6 DoF arm attached. At the start, I set an initial velocity to the first joint using the following command:

plant.GetJointByName("Joint_1").set_angular_rate(plant_context, -2.5)

As expected, the whole multibody system is tumbling based on the inertia properties. However, while looking at the meshcat simulation, the spacecraft systems seemed to move away from the initial position. I checked the System Center of mass location using the following command:

print(plant.CalcCenterOfMassPosition(plant_context))

And indeed, the command returns different position vectors at the beginning and end of the simulation. As I understand this should not happen as no forces are applied to the system except the initial angular rate which should only introduce angular momentum to the system and not linear momentum and hence no translation of the CoM but the simulation is different than this.

I wonder if this is an artifact of the integrator being used or some other phenomenon in drake that I am missing or setting up incorrectly.

The Pastebin of the urdf file can be found here: https://pastebin.com/rUVRkBBf

The pydrake script which I'm running is here:

builder = DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph, and wires them together.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant, scene_graph).AddModelFromFile(flairSCpath)
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("Base_SC"))
# Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
# plant.SetPositions(plant_context, [0, 0, 0, 0, 0, 0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(6))
# plant.GetJointByName("Joint_1").set_angular_rate(context, -2.5)
plant.GetJointByName("Joint_1").set_angular_rate(plant_context, -2.5)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassPosition(plant_context))

simulator = Simulator(diagram, context)
# simulator.set_target_realtime_rate(1.0)

meshcat.load()
# MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);

# meshcat.start_recording()
simulator.AdvanceTo(60.0 if running_as_notebook else 0.1)
# meshcat.stop_recording()
# meshcat.publish_recording()
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassPosition(plant_context))

The movement of CoM is present even when different timesteps are set for the integrator. My worry is that the system CoM shouldn't translate without any forces applied but it does and this means I'm probably setting up the simulation incorrectly.

Answer 1

There is another check that there are no external forces acting on your system. There is a C++ method (similarly in Python) whose signature is something like:

SpatialMomentum<T> MultibodyPlant::CalcSpatialMomentumInWorldAboutPoint(
      const systems::Context<T>& context,
      const Vector3<T>& p_WoP_W)

If the net external forces on your system is zero, the .translational() component of the returned spatial momentum should remain constant. This is true, regardless of what you pass in for p_WoP_W.

If there are no external forces on your system (hence the net external moment on your system is zero), the .rotational() component of the returned spatial momentum should remain constant as long as p_WoP_W is p_WoScm_W (position vector from World origin Wo to the system center of mass Scm, expressed in world W) or the zero vector.

Answer 2

So, finally, the answer is indeed from the comments to the question. The set_angular_rate() function does set the initial angular rate between two bodies but while enforcing this constraint, it also causes an initial velocity of the center of mass. A better way to move the joint to quickly test a free-floating system is by using get_actuation_input_port().FixValue() function instead. A quick example of this is given below (which has no CoM movement):

builder = DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph, and wires them together.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant, scene_graph).AddModelFromFile(flairSCpath)
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("Base_SC"))
# Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
# plant.SetPositions(plant_context, [0, 0, 0, 0, 0, 0])
jointAcutation =  np.zeros((6,1))
jointAcutation[0] = 1
plant.get_actuation_input_port().FixValue(plant_context, jointAcutation)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassPosition(plant_context))

simulator = Simulator(diagram, context)
# simulator.set_target_realtime_rate(1.0)

meshcat.load()
# MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);

# meshcat.start_recording()
simulator.AdvanceTo(60.0 if running_as_notebook else 0.1)
# meshcat.stop_recording()
# meshcat.publish_recording()
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassPosition(plant_context))

Thanks to Russ Tedrake and Sherm to help me (and future users hopefully) to understand this.

Answer 3

Here is my quick reproduction, in case it helps. I've confirmed that setting a very small target accuracy results in the same numbers, set the initial angular velocity down so it looks more reasonable, confirmed that the gravity is properly being zeroed, etc.

import numpy as np
import pydrake.all
import pydot

builder = pydrake.systems.framework.DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph, and wires them together.
plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0)
# Note that we parse into both the plant and the scene_graph here.
flair = pydrake.multibody.parsing.Parser(plant, scene_graph).AddModelFromFile("/home/russt/Downloads/flair.urdf")
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("Base_SC"))
# Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(builder, scene_graph, zmq_url="tcp://127.0.0.1:6000", delete_prefix_on_load=True)


diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
# plant.SetPositions(plant_context, [0, 0, 0, 0, 0, 0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(6))
plant.GetJointByName("Joint_1").set_angular_rate(plant_context, -.25)
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(6))

pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("flair_topology.svg")

simulator = pydrake.systems.analysis.Simulator(diagram, context)
simulator.get_mutable_integrator().set_target_accuracy(1e-10)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassPosition(plant_context, [flair]))

simulator.AdvanceTo(60.0)
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassPosition(plant_context, [flair]))