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_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]).GetAsIsometry3()) 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 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_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() 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_state_access(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() self.assertEqual(plant.num_positions(), 2) self.assertEqual(plant.num_velocities(), 2) 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 = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, np.zeros(4))) # Write into a mutable reference to the state vector. x_reff = tree.get_mutable_multibody_state_vector(context) x_reff[:] = x0 # Verify we did modify the state stored in context. x = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, x0))
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_multibody_plant_parsing(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant) self.assertIsInstance(model_instance, ModelInstanceIndex) plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, model_name="acrobot", plant=plant)
def load_tables(time_step=0.0): mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() # TODO: meshes aren't supported during collision checking robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH, model_name='iiwa', scene_graph=scene_graph, plant=mbp) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table', scene_graph=scene_graph, plant=mbp) table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2', scene_graph=scene_graph, plant=mbp) sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink', scene_graph=scene_graph, plant=mbp) stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove', scene_graph=scene_graph, plant=mbp) broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli', scene_graph=scene_graph, plant=mbp) #wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall', # scene_graph=scene_graph, plant=mbp) wall = None table2_x = 0.75 table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z])) weld_to_world(mbp, table, create_transform()) weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0])) weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z])) weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z])) if wall is not None: weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z])) mbp.Finalize(scene_graph) movable = [broccoli] surfaces = [ VisualElement(sink, 'base_link', 0), # Could also just pass the link index VisualElement(stove, 'base_link', 0), ] initial_poses = { broccoli: create_transform(translation=[table2_x, 0, table_top_z]), } task = Task(mbp, scene_graph, robot, gripper, movable=movable, surfaces=surfaces, initial_poses=initial_poses, goal_cooked=[broccoli]) return mbp, scene_graph, task
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() H = tree.CalcMassMatrixViaInverseDynamics(context) Cv = tree.CalcBiasTerm(context) self.assertTrue(H.shape == (2, 2)) self.assertTrue(Cv.shape == (2, ))
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() self.assertEqual(plant.num_positions(), 2) self.assertEqual(plant.num_velocities(), 2) 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())
def test_multibody_dynamics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() H = tree.CalcMassMatrixViaInverseDynamics(context) Cv = tree.CalcBiasTerm(context) self.assertTrue(H.shape == (2, 2)) self.assertTrue(Cv.shape == (2, ))
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.model() world_frame = plant.world_frame() # TODO(eric.cousineau): Replace this with `GetFrameByName`. link1_frame = plant.GetBodyByName("Link1").body_frame() X_WL = tree.CalcRelativeTransform(context, frame_A=world_frame, frame_B=link1_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions(context=context, frame_B=link1_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 = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=link1_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
def test_set_free_body_pose(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() plant_model = AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() X_WB_desired = Isometry3.Identity() R_WB = np.array([[0., 1., 0.], [0., 0., 1.], [1., 0., 0.]]) X_WB_desired.set_rotation(R_WB) tree.SetFreeBodyPoseOrThrow( body=plant.GetBodyByName("base", plant_model), X_WB=X_WB_desired, context=context) world_frame = plant.world_frame() base_frame = plant.GetBodyByName("base").body_frame() X_WB = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
def test_multibody_plant_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 RunSimulation(self, real_time_rate=1.0): ''' The Princess Diaries was a good movie. ''' builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # object_file_path = FindResourceOrThrow( # "drake/examples/manipulation_station/models/061_foam_brick.sdf") # sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf") # urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf") sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" plant = builder.AddSystem(MultibodyPlant()) plant.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant, scene_graph).AddModelFromFile(sdf_file) plant.Finalize(scene_graph) assert plant.geometry_source_is_registered() builder.Connect( plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def test_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) parser = Parser(plant=kuka, scene_graph=scene_graph) parser.AddModelFromFile(file_name) kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) kuka.Finalize(scene_graph) assert kuka.geometry_source_is_registered() builder.Connect(kuka.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(kuka.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros(kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.StepTo(.1)
def test_parser(self): # Calls every combination of argments for the Parser methods 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 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)) AddModelFromSdfFile(file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) lcm = DrakeLcm() ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm) diagram = builder.Build() DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm) 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_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None) 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, Isometry3.Identity(), frame, parameters)
def testMultibodyPositionToGeometryPose(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant) 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 test_multibody_state_access(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() self.assertEqual(plant.num_positions(), 2) self.assertEqual(plant.num_velocities(), 2) 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 = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, np.zeros(4))) # Write into a mutable reference to the state vector. x_reff = tree.get_mutable_multibody_state_vector(context) x_reff[:] = x0 # Verify we did modify the state stored in context. x = tree.get_multibody_state_vector(context) self.assertTrue(np.allclose(x, x0))
def add_plant_and_scene_graph(builder): # TODO(eric.cousineau): Hoist to C++. plant = builder.AddSystem(MultibodyPlant()) 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())) return plant, scene_graph
def setUp(self): file_name = FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf") self.plant = MultibodyPlant(time_step=0.01) model_instance = Parser(self.plant).AddModelFromFile(file_name) self.plant.Finalize() self.body1_frame = self.plant.GetBodyByName("body1").body_frame() self.body2_frame = self.plant.GetBodyByName("body2").body_frame() self.ik_two_bodies = ik.InverseKinematics(self.plant) self.prog = self.ik_two_bodies.get_mutable_prog() self.q = self.ik_two_bodies.q() 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_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_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() AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None) plant.Finalize() slider = JointSliders(robot=plant, lower_limit=-5., upper_limit=5., resolution=0.001, update_period_sec=0.01, title='test') context = slider.CreateDefaultContext() output = slider.AllocateOutput() q = [.1, .2] slider.set(q) slider.CalcOutput(context, output) np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
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=Isometry3.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_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) plant = MultibodyPlant(time_step=0.01) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("default", DrakeDeprecationWarning) result = DeprecatedParser(plant).AddModelFromFile(sdf_file) self.assertIsInstance(result, ModelInstanceIndex) self.assertEqual(len(w), 1) plant = MultibodyPlant(time_step=0.01) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("default", DrakeDeprecationWarning) result = DeprecatedParser(plant).AddAllModelsFromFile(sdf_file) self.assertIsInstance(result, list) self.assertEqual(len(w), 1)
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()) 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 cartPoleTest(self): file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile(file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() 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.duration)
def test_multibody_plant_inverse_dynamics(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) AddModelFromSdfFile(file_name=sdf_path, plant=plant) 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 kukaTest(args): file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision" ".sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile( file_name=file_name, plant=kuka, scene_graph=scene_graph) kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) kuka.Finalize(scene_graph) assert kuka.geometry_source_is_registered() builder.Connect( kuka.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(kuka.get_source_id())) visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros( kuka.get_actuation_input_port().size())) 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.duration)
class TestInverseKinematics(unittest.TestCase): """ This test reflects inverse_kinematics_test.cc """ def setUp(self): file_name = FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf") self.plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile( file_name=file_name, plant=self.plant, scene_graph=None) self.plant.Finalize() self.body1_frame = self.plant.GetBodyByName("body1").body_frame() self.body2_frame = self.plant.GetBodyByName("body2").body_frame() self.ik_two_bodies = ik.InverseKinematics(self.plant) self.prog = self.ik_two_bodies.get_mutable_prog() self.q = self.ik_two_bodies.q() 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 _body1_quat(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[0:4] def _body1_xyz(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[4:7] def _body2_quat(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[7:11] def _body2_xyz(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[11:14] def test_AddPositionConstraint(self): p_BQ = np.array([0.2, 0.3, 0.5]) p_AQ_lower = np.array([-0.1, -0.2, -0.3]) p_AQ_upper = np.array([-0.05, -0.12, -0.28]) self.ik_two_bodies.AddPositionConstraint( frameB=self.body1_frame, p_BQ=p_BQ, frameA=self.body2_frame, p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_AQ = body2_rotmat.transpose().dot( body1_rotmat.dot(p_BQ) + body1_pos - body2_pos) self.assertTrue(np.greater(p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all()) self.assertTrue(np.less(p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all()) def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = pydrake.math.RotationMatrix( quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = pydrake.math.RotationMatrix( quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3)) self.ik_two_bodies.AddOrientationConstraint( frameAbar=self.body1_frame, R_AbarA=R_AbarA, frameBbar=self.body2_frame, R_BbarB=R_BbarB, theta_bound=theta_bound) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6)
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.lower_limits(), [-np.inf]) np.testing.assert_array_equal(shoulder.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_continuous_state_output_port(), OutputPort) 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_multibody_plant_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) AddModelFromSdfFile(file_name=sdf_path, plant=plant) 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.tree().get_mutable_multibody_state_vector( plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.tree().CalcInverseDynamics( plant_context, vd_d, MultibodyForces(plant.tree())) # Verify the result. controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
"default value.", default=[]) 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 = parser.parse_args() builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant and load the SDF into it. plant = MultibodyPlant() plant.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(args.filename, plant) 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):
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, Isometry3) 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, Isometry3) # Set pose for the base. X_WB_desired = Isometry3.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_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_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.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 = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack(( v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow( body=base, X_WB=X_WB_desired, context=context) 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]) tree.SetFreeBodySpatialVelocityOrThrow( body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational()))
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 = Isometry3.Identity() X_EeGripper.set_translation([0, 0, 0.081]) X_EeGripper.set_rotation( RollPitchYaw(np.pi / 2, 0, np.pi / 2). ToRotationMatrix().matrix()) 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.zeros(7) v_iiwa_desired = np.zeros(7) q_gripper_desired = np.zeros(2) v_gripper_desired = np.zeros(2) 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_plant_desired = np.zeros(nq + nv) x_plant_desired[0:7] = q_iiwa_desired x_plant_desired[7:9] = q_gripper_desired x_plant_desired[nq:nq+7] = v_iiwa_desired x_plant_desired[nq+7:nq+nv] = v_gripper_desired x_plant = plant.GetMutablePositionsAndVelocities(context) x_plant[:] = x_plant_desired q_plant = plant.GetPositions(context) v_plant = plant.GetVelocities(context) # Get state from context. x = plant.GetPositionsAndVelocities(context) x_plant_tmp = plant.GetMutablePositionsAndVelocities(context) self.assertTrue(np.allclose(x_plant_desired, x_plant_tmp)) # Get positions and velocities of specific model instances # from the postion/velocity vector of the plant. q_iiwa = plant.GetPositions(context, iiwa_model) q_iiwa_array = plant.GetPositionsFromArray(iiwa_model, q_plant) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions(context, gripper_model) v_iiwa = plant.GetVelocities(context, iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray(iiwa_model, v_plant) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities(context, 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(iiwa_model, np.zeros(nq_iiwa), q_plant) self.assertTrue(np.allclose( plant.GetPositionsFromArray(iiwa_model, q_plant), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray(iiwa_model, np.zeros(nv_iiwa), v_plant) self.assertTrue(np.allclose( plant.GetVelocitiesFromArray(iiwa_model, v_plant), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u_plant = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray(iiwa_model, u_iiwa, u_plant) self.assertTrue(np.allclose(u_plant[:7], u_iiwa))
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.StepTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.StepTo(np.inf)
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]).GetAsIsometry3()) 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 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 = Isometry3.Identity() X_EeGripper.set_translation([0, 0, 0.081]) X_EeGripper.set_rotation( RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix()) 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_plant_desired = np.zeros(nq + nv) x_plant_desired[0:7] = q_iiwa_desired x_plant_desired[7:9] = q_gripper_desired x_plant_desired[nq:nq+7] = v_iiwa_desired x_plant_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)))
import argparse parser = argparse.ArgumentParser() parser.add_argument("--test", action='store_true', help="Causes the script to run without blocking for " "user input.", default=False) 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()) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(file_name=file_name, plant=cart_pole) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) 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())) builder.ExportInput(cart_pole.get_actuation_input_port()) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build()
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") timestep = 0.0002 plant = MultibodyPlant(timestep) iiwa_model = AddModelFromSdfFile( file_name=iiwa_sdf_path, model_name='robot', scene_graph=None, plant=plant) gripper_model = AddModelFromSdfFile( file_name=wsg50_sdf_path, model_name='gripper', scene_graph=None, plant=plant) # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = Isometry3.Identity() X_EeGripper.set_translation([0, 0, 0.081]) X_EeGripper.set_rotation( RollPitchYaw(np.pi / 2, 0, np.pi / 2). ToRotationMatrix().matrix()) 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() tree = plant.tree() nq = plant.num_positions() nv = plant.num_velocities() q_iiwa_desired = np.zeros(7) v_iiwa_desired = np.zeros(7) q_gripper_desired = np.zeros(2) v_gripper_desired = np.zeros(2) 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_plant_desired = np.zeros(nq + nv) x_plant_desired[0:7] = q_iiwa_desired x_plant_desired[7:9] = q_gripper_desired x_plant_desired[nq:nq+7] = v_iiwa_desired x_plant_desired[nq+7:nq+nv] = v_gripper_desired x_plant = tree.get_mutable_multibody_state_vector(context) x_plant[:] = x_plant_desired # Get state from context. x = tree.get_multibody_state_vector(context) q = x[0:nq] v = x[nq:nq+nv] # Get positions and velocities of specific model instances # from the postion/velocity vector of the plant. q_iiwa = tree.get_positions_from_array(iiwa_model, q) q_gripper = tree.get_positions_from_array(gripper_model, q) v_iiwa = tree.get_velocities_from_array(iiwa_model, v) v_gripper = tree.get_velocities_from_array(gripper_model, v) # Assert that the get_positions_from_array 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))