def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def test_multibody_gravity_default(self):
plant = MultibodyPlant()
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=1):
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
np.testing.assert_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(link2.GetForceInWorld(context, forces),
SpatialForce)
forces.SetZero()
F_expected = np.array([1, 2, 3, 4, 5, 6])
link2.AddInForceInWorld(context,
F_Bo_W=SpatialForce(F=F_expected),
forces=forces)
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
link2.AddInForce(context,
p_BP_E=[0, 0, 0],
F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(),
forces=forces)
# Also check accumulation.
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(),
2 * F_expected)
def test_multibody_gravity_vector(self):
plant = MultibodyPlant()
plant.AddForceElement(UniformGravityFieldElement([0.0, -9.81, 0.0]))
plant.Finalize()
def test_multibody_gravity_default(self):
plant = MultibodyPlant()
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
def test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = Isometry3.Identity()
X_TObject.set_translation([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(1.0)
contact_viz_context = (diagram.GetMutableSubsystemContext(
contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context, contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
