def test_joint_slider(self): # Simply test to make sure that the UI loads and produces the output. file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() slider = JointSliders(robot=plant, lower_limit=-5., upper_limit=5., resolution=0.001, update_period_sec=0.01, title='test', length=300) slider.window.withdraw() # Don't open a window during testing. context = slider.CreateDefaultContext() output = slider.AllocateOutput() q = [.1, .2] slider.set_position(q) slider.set_joint_position(q) slider.CalcOutput(context, output) np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() 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. self.assertEqual(plant.CalcPotentialEnergy(context), 0) plant.CalcConservativePower(context) tau_g = plant.CalcGravityGeneralizedForces(context) self.assertEqual(tau_g.shape, (nv,)) self.assert_sane(tau_g, nonzero=False) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces)
def test_model_instance_port_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant() parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') plant.Finalize() # Test that we can get an actuation input port and a continuous state # output port. self.assertIsInstance(plant.get_actuation_input_port(iiwa_model), InputPort) self.assertIsInstance( plant.get_continuous_state_output_port(gripper_model), OutputPort)
def test_model_directives(self): model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives(directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model") # Test that other bound symbols exist. ModelInstanceInfo.model_name ModelInstanceInfo.model_path ModelInstanceInfo.parent_frame_name ModelInstanceInfo.child_frame_name ModelInstanceInfo.X_PC ModelInstanceInfo.model_instance AddFrame.name AddFrame.X_PF frame = GetScopedFrameByName(plant, "world") self.assertIsNotNone(GetScopedFrameName(plant, frame))
def test_model_instance_port_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=2e-3) parser = Parser(plant) iiwa_model = parser.AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile( file_name=wsg50_sdf_path, model_name='gripper') plant.Finalize() # Test that we can get an actuation input port and a continuous state # output port. self.assertIsInstance( plant.get_actuation_input_port(iiwa_model), InputPort) self.assertIsInstance( plant.get_state_output_port(gripper_model), OutputPort) self.assertIsInstance( plant.get_generalized_contact_forces_output_port( model_instance=gripper_model), OutputPort)
def test_multibody_state_access(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() nq = 2 nv = 2 self.assertEqual(plant.num_positions(), nq) self.assertEqual(plant.num_velocities(), nv) q0 = np.array([3.14, 2.]) v0 = np.array([-0.5, 1.]) x0 = np.concatenate([q0, v0]) # The default state is all values set to zero. x = plant.GetPositionsAndVelocities(context) self.assertTrue(np.allclose(x, np.zeros(4))) # Write into a mutable reference to the state vector. x_ref = plant.GetMutablePositionsAndVelocities(context) x_ref[:] = x0 # Verify that positions and velocities were set correctly. self.assertTrue(np.allclose(plant.GetPositions(context), q0)) self.assertTrue(np.allclose(plant.GetVelocities(context), v0)) # Verify we did modify the state stored in context. x = plant.GetPositionsAndVelocities(context) self.assertTrue(np.allclose(x, x0)) # Now set positions and velocities independently and check them. zeros_2 = np.zeros([2, 1]) x_ref.fill(0) plant.SetPositions(context, q0) self.assertTrue(np.allclose(plant.GetPositions(context), q0)) self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2)) x_ref.fill(0) plant.SetVelocities(context, v0) self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2)) self.assertTrue(np.allclose(plant.GetVelocities(context), v0)) # Now test SetPositionsAndVelocities(). x_ref.fill(0) plant.SetPositionsAndVelocities(context, x0) self.assertTrue(np.allclose( plant.GetPositionsAndVelocities(context), x0)) # Test existence of context resetting methods. plant.SetDefaultState(context, state=context.get_mutable_state()) # Test existence of limits. self.assertEqual(plant.GetPositionLowerLimits().shape, (nq,)) self.assertEqual(plant.GetPositionUpperLimits().shape, (nq,)) self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv,)) self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv,)) self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv,)) self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv,))
def test_texture_override(self): """Draws a textured box to test the texture override pathway.""" object_file_path = FindResourceOrThrow( "drake/systems/sensors/test/models/box_with_mesh.sdf") # Find the texture path just to ensure it exists and # we're testing the code path we want to. FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png") builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) 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)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0)
def test_map_qdot_to_v_and_back(self): plant = MultibodyPlant() iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" "iiwa_description/sdf/iiwa14_no_collision.sdf") # Use floating base to effectively add a quatnerion in the generalized # quaternion. iiwa_model = Parser(plant=plant).AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') plant.Finalize() context = plant.CreateDefaultContext() # Try mapping velocity to qdot and back. nq = plant.num_positions() nv = plant.num_velocities() q_init = np.linspace(start=1.0, stop=nq, num=nq) plant.SetPositions(context, q_init) # Overwrite the (invalid) base coordinates, wherever in `q` they are. plant.SetFreeBodyPose( context, plant.GetBodyByName("iiwa_link_0"), RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]), p=[0.4, 0.5, 0.6])) v_expected = np.linspace(start=-1.0, stop=-nv, num=nv) qdot = plant.MapVelocityToQDot(context, v_expected) v_remap = plant.MapQDotToVelocity(context, qdot) self.assertTrue(np.allclose(v_expected, v_remap))
def make_diagram(): # Use a nested function to ensure that all locals get garbage # collected quickly. # Construct a trivial plant and ID controller. # N.B. We explicitly do *not* add this plant to the diagram. controller_plant = MultibodyPlant(time_step=0.002) controller_plant.Finalize() builder = DiagramBuilder() controller = builder.AddSystem( InverseDynamicsController( controller_plant, kp=[], ki=[], kd=[], has_reference_acceleration=False, ) ) # Forward ports for ease of testing. builder.ExportInput( controller.get_input_port_estimated_state(), "x_estimated") builder.ExportInput( controller.get_input_port_desired_state(), "x_desired") builder.ExportOutput(controller.get_output_port_control(), "u") diagram = builder.Build() return diagram
def test_textured_meshes(self): """Draws a solid green box (the texture map is just a green pixel) to test the texture override pathway. You should confirm that you see a green box in the visualizer.""" object_file_path = FindResourceOrThrow( "drake/systems/sensors/test/models/box_with_mesh.sdf") # Find the texture path just to ensure it exists and # we're testing the code path we want to. FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png") builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) plant.Finalize() # Add meshcat visualizer. visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0)
def test_multibody_add_frame(self): plant = MultibodyPlant() frame = plant.AddFrame(frame=FixedOffsetFrame( name="frame", P=plant.world_frame(), X_PF=RigidTransform.Identity(), model_instance=None)) self.assertIsInstance(frame, Frame) np.testing.assert_equal( np.eye(4), frame.GetFixedPoseInBodyFrame().GetAsMatrix4())
def copy_to_finalized_plant(plant_src): """Copies plant_src. This can be used to compute kinematics on a model by simply finalizing the copied version. Returns: plant_dest, src_to_dest """ plant_dest = MultibodyPlant(plant_src.time_step()) subgraph = MultibodyPlantSubgraph(get_elements_from_plant(plant_src)) src_to_dest = subgraph.add_to(plant_dest) plant_dest.Finalize() return plant_dest, src_to_dest
def test_deprecated_tree_api(self): plant = MultibodyPlant() plant.Finalize() with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) num_expected_warnings = [0] def expect_new_warning(msg_part): num_expected_warnings[0] += 1 self.assertEqual(len(w), num_expected_warnings[0]) self.assertIn(msg_part, str(w[-1].message)) tree = plant.tree() expect_new_warning("`tree()`") MobilizerIndex(0) expect_new_warning("`MobilizerIndex`") BodyNodeIndex(0) expect_new_warning("`BodyNodeIndex`") MultibodyForces(model=tree) expect_new_warning("`MultibodyForces(plant)`") element = plant.world_body() self.assertIsInstance(element.get_parent_tree(), MultibodyTree) expect_new_warning("`get_parent_tree()`") # Check old spellings (no deprecation warnings). self.check_old_spelling_exists(tree.CalcRelativeTransform) self.check_old_spelling_exists(tree.CalcPointsPositions) self.check_old_spelling_exists( tree.CalcFrameGeometricJacobianExpressedInWorld) self.check_old_spelling_exists(tree.EvalBodyPoseInWorld) self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow) self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow) self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld) self.check_old_spelling_exists(tree.GetPositionsFromArray) self.check_old_spelling_exists(tree.GetVelocitiesFromArray) self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics) self.check_old_spelling_exists(tree.CalcBiasTerm) self.check_old_spelling_exists(tree.CalcInverseDynamics) self.check_old_spelling_exists(tree.num_frames) self.check_old_spelling_exists(tree.get_body) self.check_old_spelling_exists(tree.get_joint) self.check_old_spelling_exists(tree.get_joint_actuator) self.check_old_spelling_exists(tree.get_frame) self.check_old_spelling_exists(tree.GetModelInstanceName) context = plant.CreateDefaultContext() # All body poses. X_WB, = tree.CalcAllBodyPosesInWorld(context) self.assertIsInstance(X_WB, Isometry3) v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertIsInstance(v_WB, SpatialVelocity)
def test_inverse_dynamics(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # Just test that the constructor doesn't throw. controller = InverseDynamics( plant=plant, mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
def test_model_directives(self): model_dir = os.path.dirname(FindResourceOrThrow( "drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives( directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model")
def setUp(self): builder_f = DiagramBuilder() self.plant_f, self.scene_graph_f = AddMultibodyPlantSceneGraph( builder_f, MultibodyPlant(time_step=0.01)) Parser(self.plant_f).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) self.plant_f.Finalize() diagram_f = builder_f.Build() diagram_ad = diagram_f.ToAutoDiffXd() plant_ad = diagram_ad.GetSubsystemByName(self.plant_f.get_name()) TypeVariables = namedtuple( "TypeVariables", ("plant", "plant_context", "body1_frame", "body2_frame")) def make_type_variables(plant_T, diagram_T): diagram_context_T = diagram_T.CreateDefaultContext() return TypeVariables( plant=plant_T, plant_context=diagram_T.GetMutableSubsystemContext( plant_T, diagram_context_T), body1_frame=plant_T.GetBodyByName("body1").body_frame(), body2_frame=plant_T.GetBodyByName("body2").body_frame()) self.variables_f = make_type_variables(self.plant_f, diagram_f) self.variables_ad = make_type_variables(plant_ad, diagram_ad)
def test_parser_file(self): """Calls every combination of arguments for the Parser methods which use a file_name (not contents) and inspects their return type. """ sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") urdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.urdf") for dut, file_name, model_name, result_dim in ( (Parser.AddModelFromFile, sdf_file, None, int), (Parser.AddModelFromFile, sdf_file, "", int), (Parser.AddModelFromFile, sdf_file, "a", int), (Parser.AddModelFromFile, urdf_file, None, int), (Parser.AddModelFromFile, urdf_file, "", int), (Parser.AddModelFromFile, urdf_file, "a", int), (Parser.AddAllModelsFromFile, sdf_file, None, list), (Parser.AddAllModelsFromFile, urdf_file, None, list), ): plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) if model_name is None: result = dut(parser, file_name=file_name) else: result = dut(parser, file_name=file_name, model_name=model_name) if result_dim is int: self.assertIsInstance(result, ModelInstanceIndex) else: assert result_dim is list self.assertIsInstance(result, list) self.assertIsInstance(result[0], ModelInstanceIndex)
def setUp(self): builder = DiagramBuilder() self.plant, self.scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.01)) Parser(self.plant).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) self.plant.Finalize() diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() plant_context = diagram.GetMutableSubsystemContext( self.plant, diagram_context) self.body1_frame = self.plant.GetBodyByName("body1").body_frame() self.body2_frame = self.plant.GetBodyByName("body2").body_frame() self.ik_two_bodies = ik.InverseKinematics( plant=self.plant, plant_context=plant_context) # Test non-SceneGraph constructor. ik.InverseKinematics(plant=self.plant) self.prog = self.ik_two_bodies.get_mutable_prog() self.q = self.ik_two_bodies.q() # Test constructor without joint limits ik.InverseKinematics(plant=self.plant, with_joint_limits=False) ik.InverseKinematics( plant=self.plant, plant_context=plant_context, with_joint_limits=False) def squaredNorm(x): return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2]) self.prog.AddConstraint( squaredNorm, [1], [1], self._body1_quat(self.q)) self.prog.AddConstraint( squaredNorm, [1], [1], self._body2_quat(self.q)) self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0]) self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def test_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") parameters = mut.DifferentialInverseKinematicsParameters(2, 2) mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame, parameters) mut.DoDifferentialInverseKinematics(plant, context, RigidTransform(), frame, parameters)
def test_contact(self): # PenetrationAsContactPair point_pair = PenetrationAsPointPair() self.assertTrue(isinstance(point_pair.id_A, GeometryId)) self.assertTrue(isinstance(point_pair.id_B, GeometryId)) self.assertTrue(point_pair.p_WCa.shape == (3, )) self.assertTrue(point_pair.p_WCb.shape == (3, )) self.assertTrue(isinstance(point_pair.depth, float)) # PointPairContactInfo id_A = BodyIndex(0) id_B = BodyIndex(1) contact_info = PointPairContactInfo(bodyA_index=id_A, bodyB_index=id_B, f_Bc_W=np.array([0, 0, 1]), p_WC=np.array([0, 0, 0]), separation_speed=0, slip_speed=0, point_pair=point_pair) self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex)) self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex)) self.assertTrue(contact_info.contact_force().shape == (3, )) self.assertTrue(contact_info.contact_point().shape == (3, )) self.assertTrue(isinstance(contact_info.slip_speed(), float)) self.assertIsInstance(contact_info.point_pair(), PenetrationAsPointPair) # ContactResults contact_results = ContactResults() contact_results.AddContactInfo(contact_info) self.assertTrue(contact_results.num_contacts() == 1) self.assertTrue( isinstance(contact_results.contact_info(0), PointPairContactInfo)) # ContactResultsToLcmSystem file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() contact_results_to_lcm = ContactResultsToLcmSystem(plant) context = contact_results_to_lcm.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(contact_results)) output = contact_results_to_lcm.AllocateOutput() contact_results_to_lcm.CalcOutput(context, output) result = output.get_data(0) self.assertIsInstance(result, AbstractValue)
def testMultibodyPositionToGeometryPose(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(file_name) scene_graph = SceneGraph() plant.RegisterAsSourceForSceneGraph(scene_graph) plant.Finalize() to_pose = MultibodyPositionToGeometryPose(plant) # Check the size of the input. self.assertEqual(to_pose.get_input_port().size(), 2) # Just check the spelling of the output port (size is not meaningful # for Abstract-valued ports). to_pose.get_output_port()
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step)) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.AddForceElement(UniformGravityFieldElement()) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() builder.Connect(scene_graph.get_query_output_port(), cart_pole.get_geometry_query_input_port()) builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.simulation_time)
def test_diff_ik_integrator(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") time_step = 0.1 parameters = mut.DifferentialInverseKinematicsParameters(2, 2) integrator = mut.DifferentialInverseKinematicsIntegrator( robot=plant, frame_E=frame, time_step=time_step, parameters=parameters, robot_context=context)
def test_deprecated_parsing(self): sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("default", DrakeDeprecationWarning) result = AddModelFromSdfFile(plant=plant, file_name=sdf_file) self.assertIsInstance(result, ModelInstanceIndex) self.assertEqual(len(w), 1)
def test_connect_contact_results(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.001)) Parser(plant).AddModelFromFile(file_name) plant.Finalize() publisher = ConnectContactResultsToDrakeVisualizer(builder, plant) self.assertIsInstance(publisher, LcmPublisherSystem)
def test_parser_string(self): """Checks parsing from a string (not file_name).""" sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") with open(sdf_file, "r") as f: sdf_contents = f.read() plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) result = parser.AddModelFromString( file_contents=sdf_contents, file_type="sdf") self.assertIsInstance(result, ModelInstanceIndex)
def _make_plant_parser_directives(self): """Returns a tuple (plant, parser, directives) for later testing.""" model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.dmd.yaml" directives = LoadModelDirectives(directives_file) return (plant, parser, directives)
def test_api(self): plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) plant.Finalize() context = plant.CreateDefaultContext() options = ik.GlobalInverseKinematics.Options() global_ik = ik.GlobalInverseKinematics(plant=plant, options=options) self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram) self.assertIsInstance(global_ik.get_mutable_prog(), mp.MathematicalProgram) body_index_A = plant.GetBodyIndices(model_instance)[0] body_index_B = plant.GetBodyIndices(model_instance)[1] self.assertEqual( global_ik.body_rotation_matrix(body_index=body_index_A).shape, (3, 3)) self.assertEqual( global_ik.body_position(body_index=body_index_A).shape, (3, )) global_ik.AddWorldPositionConstraint( body_index=body_index_A, p_BQ=[0, 0, 0], box_lb_F=[-np.inf, -np.inf, -np.inf], box_ub_F=[np.inf, np.inf, np.inf], X_WF=RigidTransform()) global_ik.AddWorldRelativePositionConstraint( body_index_B=body_index_B, p_BQ=[0, 0, 0], body_index_A=body_index_A, p_AP=[0, 0, 0], box_lb_F=[-np.inf, -np.inf, -np.inf], box_ub_F=[np.inf, np.inf, np.inf], X_WF=RigidTransform()) global_ik.AddWorldOrientationConstraint( body_index=body_index_A, desired_orientation=Quaternion(), angle_tol=np.inf) global_ik.AddPostureCost( q_desired=plant.GetPositions(context), body_position_cost=[1] * plant.num_bodies(), body_orientation_cost=[1] * plant.num_bodies()) gurobi_solver = GurobiSolver() if gurobi_solver.available(): global_ik.SetInitialGuess(q=plant.GetPositions(context)) result = gurobi_solver.Solve(global_ik.prog()) self.assertTrue(result.is_success()) global_ik.ReconstructGeneralizedPositionSolution(result=result)
def main(): builder = DiagramBuilder() sim_plant, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(mbp_time_step)) load_atlas(sim_plant, add_ground=True) sim_plant_context = sim_plant.CreateDefaultContext() controller_plant = MultibodyPlant(mbp_time_step) load_atlas(controller_plant) controller = builder.AddSystem( HumanoidController(controller_plant, Atlas.CONTACTS_PER_FRAME, is_wbc=False)) controller.set_name("HumanoidController") disturber = builder.AddSystem( ForceDisturber(sim_plant.GetBodyByName("utorso").index(), 4, 0.1, 2)) builder.Connect(disturber.get_output_port(0), sim_plant.get_applied_spatial_force_input_port()) builder.Connect(sim_plant.get_state_output_port(), controller.GetInputPort("q_v")) builder.Connect(controller.GetOutputPort("tau"), sim_plant.get_actuation_input_port()) ConnectContactResultsToDrakeVisualizer(builder=builder, plant=sim_plant) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() sim_plant_context = diagram.GetMutableSubsystemContext( sim_plant, diagram_context) set_atlas_initial_pose(sim_plant, sim_plant_context) controller_context = diagram.GetMutableSubsystemContext( controller, diagram_context) controller.GetInputPort("y_des").FixValue(controller_context, np.array([0.1, 0.0])) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(0.04) simulator.AdvanceTo(20.0)
def test_diff_ik_integrator(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") time_step = 0.1 parameters = mut.DifferentialInverseKinematicsParameters(2, 2) integrator = mut.DifferentialInverseKinematicsIntegrator( robot=plant, frame_E=frame, time_step=time_step, parameters=parameters, robot_context=context, log_only_when_result_state_changes=True) integrator.get_mutable_parameters().set_timestep(0.2) self.assertEqual(integrator.get_parameters().get_timestep(), 0.2)
def CreateIiwaControllerPlant(): # creates plant that includes only the robot, used for controllers. robot_sdf_path = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf" ) sim_timestep = 1e-3 plant_robot = MultibodyPlant(sim_timestep) parser = Parser(plant=plant_robot) parser.AddModelFromFile(robot_sdf_path) plant_robot.WeldFrames(A=plant_robot.world_frame(), B=plant_robot.GetFrameByName("iiwa_link_0")) plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0]) plant_robot.Finalize() link_frame_indices = [] for i in range(8): link_frame_indices.append( plant_robot.GetFrameByName("iiwa_link_" + str(i)).index()) return plant_robot, link_frame_indices
def test_multibody_add_joint(self): """ Tests joint constructors and `AddJoint`. """ instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 plant = MultibodyPlant() parser = Parser(plant) instances = [] for i in range(num_joints + 1): instance = parser.AddModelFromFile(instance_file, "instance_{}".format(i)) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints = [ RevoluteJoint( name="revolve_things", frame_on_parent=plant.GetBodyByName(distal_frame, instances[1]).body_frame(), frame_on_child=plant.GetBodyByName(proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint( name="weld_things", parent_frame_P=plant.GetBodyByName(distal_frame, instances[0]).body_frame(), child_frame_C=plant.GetBodyByName(proximal_frame, instances[1]).body_frame(), X_PC=RigidTransform.Identity()), ] for joint in joints: joint_out = plant.AddJoint(joint) self.assertIs(joint, joint_out) # The model is now complete. plant.Finalize() for joint in joints: self._test_joint_api(joint)
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 main(): parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument( "filename", type=str, help="Path to an SDF or URDF file.") MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() filename = args.filename if not os.path.isfile(filename): parser.error("File does not exist: {}".format(filename)) # Construct Plant + SceneGraph. builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect( scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) builder.Connect( plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Load the model file. Parser(plant).AddModelFromFile(filename) plant.Finalize() # Publish to Drake Visualizer. drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph) # Publish to Meshcat. if args.meshcat is not None: meshcat_viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=args.meshcat)) builder.Connect( scene_graph.get_pose_bundle_output_port(), meshcat_viz.get_input_port(0)) diagram = builder.Build() context = diagram.CreateDefaultContext() # Use Simulator to dispatch initialization events. # TODO(eric.cousineau): Simplify as part of #10015. Simulator(diagram).Initialize() # Publish draw messages with current state. diagram.Publish(context)
def __init__(self, scene_graph): LeafSystem.__init__(self) assert scene_graph mbp = MultibodyPlant(0.0) parser = Parser(mbp, scene_graph) model_id = parser.AddModelFromFile( FindResource("models/glider/glider.urdf")) mbp.Finalize() self.source_id = mbp.get_source_id() self.body_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[0]) self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[1]) self.DeclareVectorInputPort("state", BasicVector(7)) self.DeclareAbstractOutputPort( "geometry_pose", lambda: AbstractValue.Make(FramePoseVector()), self.OutputGeometryPose)
def test_strict(self): plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) model = """<robot name='robot' version='0.99'> <link name='a'/> </robot>""" parser.AddModelFromString(file_contents=model, file_type='urdf', model_name='lax') parser.SetStrictParsing() with self.assertRaises(RuntimeError) as e: result = parser.AddModelFromString(file_contents=model, file_type='urdf', model_name='strict') pattern = r'.*version.*ignored.*' message = str(e.exception) match = re.match(pattern, message) self.assertTrue(match, f'"{message}" does not match "{pattern}"')
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 :-). gravity_vector = np.array([0.0, 0.0, -3.71]) plant.mutable_gravity_field().set_gravity_vector(gravity_vector) np.testing.assert_equal(plant.gravity_field().gravity_vector(), gravity_vector) 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 main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) args_parser.add_argument( "filename", type=str, help="Path to an SDF or URDF file.") args_parser.add_argument( "--package_path", type=str, default=None, help="Full path to the root package for reading in SDF resources.") position_group = args_parser.add_mutually_exclusive_group() position_group.add_argument( "--position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions in the sdf model. Note that most models have a " "floating-base joint by default (unless the sdf explicitly welds " "the base to the world, and so have 7 positions corresponding to " "the quaternion representation of that floating-base position.") position_group.add_argument( "--joint_position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions ASSOCIATED WITH JOINTS in the sdf model. This " "does not include, e.g., floating-base coordinates, which will " "be assigned a default value.") args_parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") # TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. args = args_parser.parse_args() filename = args.filename if not os.path.isfile(filename): args_parser.error("File does not exist: {}".format(filename)) builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant. plant = MultibodyPlant() plant.RegisterAsSourceForSceneGraph(scene_graph) # Create the parser. parser = Parser(plant) # Get the package pathname. if args.package_path: # Verify that package.xml is found in the designated path. package_path = os.path.abspath(args.package_path) if not os.path.isfile(os.path.join(package_path, "package.xml")): parser.error("package.xml not found at: {}".format(package_path)) # Get the package map and populate it using the package path. package_map = parser.package_map() package_map.PopulateFromFolder(package_path) # Add the model from the file and finalize the plant. parser.AddModelFromFile(filename) plant.Finalize(scene_graph) # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect( to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Connect this to drake_visualizer. ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) if len(args.position): sliders.set_position(args.position) elif len(args.joint_position): sliders.set_joint_position(args.joint_position) # Make the diagram and run it. diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) if args.test: sliders.window.withdraw() simulator.AdvanceTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(np.inf)
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile( file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform( RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames( A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames( A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.linspace(0, 0.3, 7) v_iiwa_desired = q_iiwa_desired + 0.4 q_gripper_desired = [0.4, 0.5] v_gripper_desired = [-1., -2.] x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq+7] = v_iiwa_desired x_desired[nq+7:nq+nv] = v_gripper_desired x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) x_tmp = plant.GetMutablePositionsAndVelocities(context=context) self.assertTrue(np.allclose(x_desired, x_tmp)) # Get positions and velocities of specific model instances # from the position/velocity vector of the plant. q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model) q_iiwa_array = plant.GetPositionsFromArray( model_instance=iiwa_model, q=q) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions( context=context, model_instance=gripper_model) v_iiwa = plant.GetVelocities( context=context, model_instance=iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray( model_instance=iiwa_model, v=v) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities( context=context, model_instance=gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray( model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) self.assertTrue(np.allclose( plant.GetPositionsFromArray(model_instance=iiwa_model, q=q), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray( model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v) self.assertTrue(np.allclose( plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray( model_instance=iiwa_model, u_instance=u_iiwa, u=u) self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, RigidTransform) p_AQi = plant.CalcPointsPositions( context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity( context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, RigidTransform) # Set pose for the base. X_WB_desired = RigidTransform.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose( context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity( context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot( context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_plant_api_via_parsing(self): # TODO(eric.cousineau): Decouple this when construction can be done # without parsing. # This a subset of `multibody_plant_sdf_parser_test.cc`. file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(file_name) self.assertIsInstance(model_instance, ModelInstanceIndex) plant.Finalize() benchmark = MakeAcrobotPlant(AcrobotParameters(), True) self.assertEqual(plant.num_bodies(), benchmark.num_bodies()) self.assertEqual(plant.num_joints(), benchmark.num_joints()) self.assertEqual(plant.num_actuators(), benchmark.num_actuators()) self.assertEqual( plant.num_model_instances(), benchmark.num_model_instances() + 1) self.assertEqual(plant.num_positions(), benchmark.num_positions()) self.assertEqual( plant.num_positions(model_instance=model_instance), benchmark.num_positions()) self.assertEqual( plant.num_velocities(), benchmark.num_velocities()) self.assertEqual( plant.num_multibody_states(), benchmark.num_multibody_states()) self.assertEqual( plant.num_actuated_dofs(), benchmark.num_actuated_dofs()) self.assertTrue(plant.is_finalized()) self.assertTrue(plant.HasBodyNamed(name="Link1")) self.assertTrue(plant.HasBodyNamed( name="Link1", model_instance=model_instance)) self.assertTrue(plant.HasJointNamed(name="ShoulderJoint")) self.assertTrue(plant.HasJointNamed( name="ShoulderJoint", model_instance=model_instance)) shoulder = plant.GetJointByName(name="ShoulderJoint") self._test_joint_api(shoulder) np.testing.assert_array_equal( shoulder.position_lower_limits(), [-np.inf]) np.testing.assert_array_equal( shoulder.position_upper_limits(), [np.inf]) self.assertIs(shoulder, plant.GetJointByName( name="ShoulderJoint", model_instance=model_instance)) self._test_joint_actuator_api( plant.GetJointActuatorByName(name="ElbowJoint")) self._test_body_api(plant.GetBodyByName(name="Link1")) self.assertIs( plant.GetBodyByName(name="Link1"), plant.GetBodyByName(name="Link1", model_instance=model_instance)) self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2) self._test_frame_api(plant.GetFrameByName(name="Link1")) self.assertIs( plant.GetFrameByName(name="Link1"), plant.GetFrameByName(name="Link1", model_instance=model_instance)) self.assertEqual( model_instance, plant.GetModelInstanceByName(name="acrobot")) self.assertIsInstance( plant.get_actuation_input_port(), InputPort) self.assertIsInstance( plant.get_state_output_port(), OutputPort) # Smoke test of deprecated methods. with catch_drake_warnings(expected_count=2): plant.get_continuous_state_output_port() plant.get_continuous_state_output_port(model_instance) self.assertIsInstance( plant.get_contact_results_output_port(), OutputPort) self.assertIsInstance(plant.num_frames(), int) self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body) self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0))) self.assertIsInstance(plant.get_joint_actuator( actuator_index=JointActuatorIndex(0)), JointActuator) self.assertIsInstance( plant.get_frame(frame_index=FrameIndex(0)), Frame) self.assertEqual("acrobot", plant.GetModelInstanceName( model_instance=model_instance))
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 = RigidTransform([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.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, 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.AdvanceTo(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)
def test_model_instance_state_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant() parser = Parser(plant) iiwa_model = parser.AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile( file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform( RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames( A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nv = plant.num_velocities() nq_iiwa = 7 nv_iiwa = 7 nq_gripper = 2 nv_gripper = 2 q_iiwa_desired = np.zeros(nq_iiwa) v_iiwa_desired = np.zeros(nv_iiwa) q_gripper_desired = np.zeros(nq_gripper) v_gripper_desired = np.zeros(nv_gripper) q_iiwa_desired[2] = np.pi/3 q_gripper_desired[0] = 0.1 v_iiwa_desired[1] = 5.0 q_gripper_desired[0] = -0.3 x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa) x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired x_iiwa_desired[nq_iiwa:nq_iiwa+nv_iiwa] = v_iiwa_desired x_gripper_desired = np.zeros(nq_gripper + nv_gripper) x_gripper_desired[0:nq_gripper] = q_gripper_desired x_gripper_desired[nq_gripper:nq_gripper+nv_gripper] = v_gripper_desired x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq+7] = v_iiwa_desired x_desired[nq+7:nq+nv] = v_gripper_desired # Check SetPositionsAndVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) self.assertTrue(np.allclose( plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities( context, gripper_model, x_gripper_desired) self.assertTrue(np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetPositions() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, iiwa_model, q_iiwa_desired) self.assertTrue(np.allclose( plant.GetPositions(context, iiwa_model), q_iiwa_desired)) self.assertTrue(np.allclose(plant.GetVelocities( context, iiwa_model), np.zeros(nv_iiwa))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, gripper_model, q_gripper_desired) self.assertTrue(np.allclose( plant.GetPositions(context, gripper_model), q_gripper_desired)) self.assertTrue(np.allclose(plant.GetVelocities( context, gripper_model), np.zeros(nq_gripper))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) self.assertTrue(np.allclose( plant.GetVelocities(context, iiwa_model), v_iiwa_desired)) self.assertTrue(np.allclose(plant.GetPositions( context, iiwa_model), np.zeros(nq_iiwa))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, gripper_model, v_gripper_desired) self.assertTrue(np.allclose( plant.GetVelocities(context, gripper_model), v_gripper_desired)) self.assertTrue(np.allclose(plant.GetPositions( context, gripper_model), np.zeros(nv_gripper))) self.assertTrue(np.allclose(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def test_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # We verify the (known) size of the model. kNumPositions = 7 kNumVelocities = 7 kNumActuators = 7 kStateSize = kNumPositions + kNumVelocities self.assertEqual(plant.num_positions(), kNumPositions) self.assertEqual(plant.num_velocities(), kNumVelocities) self.assertEqual(plant.num_actuators(), kNumActuators) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), kNumVelocities) self.assertEqual( controller.get_input_port(estimated_state_port).size(), kStateSize) self.assertEqual( controller.get_input_port(desired_state_port).size(), kStateSize) self.assertEqual( controller.get_output_port(control_port).size(), kNumVelocities) # Current state. q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # Reference state and acceleration. q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # Set the plant's context. plant_context = plant.CreateDefaultContext() x_plant = plant.GetMutablePositionsAndVelocities(plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.CalcInverseDynamics( plant_context, vd_d, MultibodyForces(plant)) # Verify the result. controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))