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)