def test_model_instance_state_access(self, T): # N.B. Please check warning above in `check_multibody_state_access`. # 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. RigidTransform = RigidTransform_[T] RollPitchYaw = RollPitchYaw_[T] 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") # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f = MultibodyPlant_[float]() parser = Parser(plant_f) 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_[float]( RollPitchYaw_[float](np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant_f.WeldFrames( A=plant_f.world_frame(), B=plant_f.GetFrameByName("iiwa_link_0", iiwa_model)) plant_f.WeldFrames( A=plant_f.GetFrameByName("iiwa_link_7", iiwa_model), B=plant_f.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant_f.Finalize() plant = to_type(plant_f, T) # 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)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired) numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper)) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetPositionsAndVelocities( context, gripper_model, x_gripper_desired) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired) numpy_compare.assert_float_equal( 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)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetPositions(context, iiwa_model, q_iiwa_desired) numpy_compare.assert_float_equal( plant.GetPositions(context, iiwa_model), q_iiwa_desired) numpy_compare.assert_float_equal(plant.GetVelocities( context, iiwa_model), np.zeros(nv_iiwa)) numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper)) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetPositions(context, gripper_model, q_gripper_desired) numpy_compare.assert_float_equal( plant.GetPositions(context, gripper_model), q_gripper_desired) numpy_compare.assert_float_equal(plant.GetVelocities( context, gripper_model), np.zeros(nq_gripper)) numpy_compare.assert_float_equal(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)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) numpy_compare.assert_float_equal( plant.GetVelocities(context, iiwa_model), v_iiwa_desired) numpy_compare.assert_float_equal(plant.GetPositions( context, iiwa_model), np.zeros(nq_iiwa)) numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities( context, gripper_model), np.zeros(nq_gripper + nv_gripper)) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) numpy_compare.assert_float_equal( plant.GetPositionsAndVelocities(context), np.zeros(nq + nv)) plant.SetVelocities(context, gripper_model, v_gripper_desired) numpy_compare.assert_float_equal( plant.GetVelocities(context, gripper_model), v_gripper_desired) numpy_compare.assert_float_equal(plant.GetPositions( context, gripper_model), np.zeros(nv_gripper)) numpy_compare.assert_float_equal(plant.GetPositionsAndVelocities( context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))
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() tree = plant.tree() 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)))
def test_contact_force(self): """A block sitting on a table.""" object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") table_file_path = FindResourceOrThrow( "drake/examples/kuka_iiwa_arm/models/table/" "extra_heavy_duty_table_surface_only_collision.sdf") # T: tabletop frame. X_TObject = Isometry3.Identity() X_TObject.set_translation([0, 0, 0.2]) builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) table_model = Parser(plant=plant).AddModelFromFile(table_file_path) # Weld table to world. plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("link", table_model)) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) # Add contact visualizer. contact_viz = builder.AddSystem( MeshcatContactVisualizer(meshcat_viz=viz, force_threshold=0, contact_force_scale=10, plant=plant)) contact_input_port = contact_viz.GetInputPort("contact_results") builder.Connect(plant.GetOutputPort("contact_results"), contact_input_port) builder.Connect(scene_graph.get_pose_bundle_output_port(), contact_viz.GetInputPort("pose_bundle")) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( plant, diagram_context) X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(), plant.GetFrameByName("top_center")) plant.SetFreeBodyPose(mbp_context, plant.GetBodyByName("base_link", object_model), X_WT.multiply(X_TObject)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.StepTo(1.0) contact_viz_context = (diagram.GetMutableSubsystemContext( contact_viz, diagram_context)) contact_results = contact_viz.EvalAbstractInput( contact_viz_context, contact_input_port.get_index()).get_value() self.assertGreater(contact_results.num_contacts(), 0) self.assertEqual(contact_viz._contact_key_counter, 4)
def GetBrickPose(config_file, viz=False): """Estimates the pose of the foam brick in a ManipulationStation setup. @param config_file str. The path to a camera configuration file. @param viz bool. If True, save point clouds to numpy arrays. @return An Isometry3 representing the pose of the brick. """ builder = DiagramBuilder() # create the PointCloudToPoseSystem pc_to_pose = builder.AddSystem( PointCloudToPoseSystem(config_file, viz, SegmentFoamBrick, GetFoamBrickPose)) # realsense serial numbers are >> 100 use_hardware = int(pc_to_pose.camera_configs["left_camera_serial"]) > 100 if use_hardware: camera_ids = [ pc_to_pose.camera_configs["left_camera_serial"], pc_to_pose.camera_configs["middle_camera_serial"], pc_to_pose.camera_configs["right_camera_serial"] ] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect() else: station = builder.AddSystem(ManipulationStation()) station.AddCupboard() object_file_path = \ "drake/examples/manipulation_station/models/061_foam_brick.sdf" brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path), station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) station.Finalize() # add systems to convert the depth images from ManipulationStation to # PointClouds left_camera_info = pc_to_pose.camera_configs["left_camera_info"] middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"] right_camera_info = pc_to_pose.camera_configs["right_camera_info"] left_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=left_camera_info)) middle_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=middle_camera_info)) right_dut = builder.AddSystem( mut.DepthImageToPointCloud(camera_info=right_camera_info)) left_name_prefix = \ "camera_" + pc_to_pose.camera_configs["left_camera_serial"] middle_name_prefix = \ "camera_" + pc_to_pose.camera_configs["middle_camera_serial"] right_name_prefix = \ "camera_" + pc_to_pose.camera_configs["right_camera_serial"] # connect the depth images to the DepthImageToPointCloud converters builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"), left_dut.depth_image_input_port()) builder.Connect(station.GetOutputPort(middle_name_prefix + "_depth_image"), middle_dut.depth_image_input_port()) builder.Connect(station.GetOutputPort(right_name_prefix + "_depth_image"), right_dut.depth_image_input_port()) # connect the rgb images to the PointCloudToPoseSystem builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"), pc_to_pose.GetInputPort("camera_left_rgb")) builder.Connect(station.GetOutputPort(middle_name_prefix + "_rgb_image"), pc_to_pose.GetInputPort("camera_middle_rgb")) builder.Connect(station.GetOutputPort(right_name_prefix + "_rgb_image"), pc_to_pose.GetInputPort("camera_right_rgb")) # connect the XYZ point clouds to PointCloudToPoseSystem builder.Connect(left_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("left_point_cloud")) builder.Connect(middle_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("middle_point_cloud")) builder.Connect(right_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("right_point_cloud")) diagram = builder.Build() simulator = Simulator(diagram) if not use_hardware: X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow( station.get_mutable_multibody_plant().GetBodyByName( "base_link", brick), X_WObject, station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context)) context = diagram.GetMutableSubsystemContext( pc_to_pose, simulator.get_mutable_context()) # returns the pose of the brick, of type Isometry3 return pc_to_pose.GetOutputPort("X_WObject").Eval(context)
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) tree = plant.tree() self.check_old_spelling_exists(tree.num_frames) self.assertIsInstance(plant.num_frames(), int) self.check_old_spelling_exists(tree.get_body) self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body) self.check_old_spelling_exists(tree.get_joint) self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0))) self.check_old_spelling_exists(tree.get_joint_actuator) self.assertIsInstance( plant.get_joint_actuator(actuator_index=JointActuatorIndex(0)), JointActuator) self.check_old_spelling_exists(tree.get_frame) self.assertIsInstance(plant.get_frame(frame_index=FrameIndex(0)), Frame) self.check_old_spelling_exists(tree.GetModelInstanceName) self.assertEqual( "acrobot", plant.GetModelInstanceName(model_instance=model_instance))
def load_station(time_step=0.0, plan=None): # TODO: map names to SDF paths # https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc #object_file_path = FindResourceOrThrow( # "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") # RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14) station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision) plant = station.get_mutable_multibody_plant() scene_graph = station.get_mutable_scene_graph() robot = plant.GetModelInstanceByName('iiwa') gripper = plant.GetModelInstanceByName('gripper') station.AddCupboard() brick = AddModelFromSdfFile(file_name=object_file_path, model_name="brick", plant=plant, scene_graph=scene_graph) station.Finalize() door_names = ['left_door', 'right_door'] doors = [plant.GetBodyByName(name).index() for name in door_names] initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions = { plant.GetJointByName('left_door_hinge'): -DOOR_CLOSED, #plant.GetJointByName('left_door_hinge'): -np.pi / 2, plant.GetJointByName('right_door_hinge'): np.pi / 2, #plant.GetJointByName('right_door_hinge'): np.pi, } initial_positions.update( zip(get_movable_joints(plant, robot), initial_conf)) initial_poses = { brick: create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]), } goal_poses = { brick: create_transform(translation=[0.8, 0.2, 0.2927], rotation=[0, 0, 5 * np.pi / 4]), } diagram, state_machine = build_manipulation_station(station, plan) task = Task(diagram, plant, scene_graph, robot, gripper, movable=[brick], doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_poses=goal_poses, reset_robot=True, reset_doors=False) task.set_initial() return task, diagram, state_machine
from pydrake.common import FindResourceOrThrow from pydrake.multibody.rigid_body_plant import RigidBodyPlant from pydrake.multibody.rigid_body_tree import RigidBodyTree from pydrake.systems.analysis import Simulator tree = RigidBodyTree( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) simulator = Simulator(RigidBodyPlant(tree))
# The following code demonstrates how to access the components of the manipulator dynamics equations from Drake's MultibodyPlant class, using the Acrobot as an example system. Note that not all functionality from the C++ Drake API is available in the Python API; however, the Python API is sufficiently powerful to give access to the all componetents of the manipulator dynamics equations from math import pi # Import utilities from pydrake from pydrake.common import FindResourceOrThrow from pydrake.all import MultibodyPlant from pydrake.multibody.parsing import Parser #from pydrake.autodiffutils import AutoDiffXd from pydrake.multibody.tree import MultibodyForces #%% [markdown] # # Loading a the acrobot model from a URDF # # The Acrobot model is supplied with the Drake docker image. We can load it and directly create a Drake MultibodyPlant. To fully utilitze the MultibodyPlant, we will also need to create a Context, which stores values associated with the MultibodyPlant such as the configuration and velocity variables. # Find and load the Acrobot URDF acro_file = FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf") # Create a Multibody plant model from the acrobot plant = MultibodyPlant(0.0) acrobot = Parser(plant).AddModelFromFile(acro_file) plant.Finalize() # The CONTEXT of the system stores information, such as the state, about the multibody plant. The context is necessary to calculate values such as the inertia matrix context = plant.CreateDefaultContext() ##% [markdown] # # The default context is not very interesting, so let's set some nonzero values for the configuration and velocity variables q = [pi / 4, pi / 4] v = [0.1, -0.1] plant.SetPositions(context, q) plant.SetVelocities(context, v) #%% [markdown] # # DYNAMICS
builder = DiagramBuilder() # A multibody plant is a *system* (in the drake sense) holding all the info for multi-body rigid bodies, providing ports for forces and continuous state, geometry etc. # The "builder" as an argument is a kind of a "side-effectey" thing, we want to add our created multibodyplant *to* this builder # The 0.0 is the "discrete time step". A value of 0.0 means that we have made this a continuous system # Note also, this constructor is overloaded (but this is not a thing you can do in python naturally. It is an artefact of the C++ port) time_step = 0.002 plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step) # Load in the panda # The parser takes a multibody plant mutably, so that anything parsed with it gets automatically added to this multibody system parser = Parser(plant) # Files are getting grabbed from "/opt/drake/share/drake/... panda_arm_hand_file = FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/panda_arm_hand.urdf") brick_file = FindResourceOrThrow("drake/examples/manipulation_station/models/061_foam_brick.sdf") bin_file = FindResourceOrThrow("drake/examples/manipulation_station/models/bin.sdf") # Actually parse in the model panda = parser.AddModelFromFile(panda_arm_hand_file, model_name="panda") # Don't want panda to drop through the sky or fall over so... # It would be nice if there was an option to create a floor... # The X_AB part is "relative pose" (monogram notation: The pose (X) of B relative to A plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("panda_link0", panda), X_AB=RigidTransform(np.array([0.0, 0.5, 0.0])) ) # Add a little example brick brick = parser.AddModelFromFile(brick_file, model_name="brick")
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(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))
def test_kinematics_api(self): # TODO(eric.cousineau): Reduce these tests to only test API, and do # simple sanity checks on the numbers. tree = RigidBodyTree( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) num_q = 7 num_v = 7 self.assertEqual(tree.number_of_positions(), num_q) self.assertEqual(tree.number_of_velocities(), num_v) q = np.zeros(num_q) v = np.zeros(num_v) # Trivial kinematics. kinsol = tree.doKinematics(q, v) p = tree.transformPoints(kinsol, np.zeros(3), 0, 1) self.assertTrue(np.allclose(p, np.zeros(3))) # AutoDiff jacobians. def do_transform(q): kinsol = tree.doKinematics(q) point = np.ones(3) return tree.transformPoints(kinsol, point, 2, 0) # - Position. value = do_transform(q) self.assertTrue(np.allclose(value, np.ones(3))) # - Gradient. g = jacobian(do_transform, q) g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]], [[0, 1, 0, -1, 0, 1, 0]], [[0, 0, 1, 1, -1, 0, -1]]]) self.assertTrue(np.allclose(g, g_expected)) # Relative transform. q[:] = 0 q[6] = np.pi / 2 kinsol = tree.doKinematics(q) T = tree.relativeTransform(kinsol, 1, 2) T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.assertTrue(np.allclose(T, T_expected)) # Do FK and compare pose of 'arm' with expected pose. q[:] = 0 q[6] = np.pi / 2 kinsol = tree.doKinematics(q) T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm")) T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.assertTrue(np.allclose(T, T_expected)) # Geometric Jacobian. # Construct a new tree with a quaternion floating base. tree = RigidBodyTree( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"), floating_base_type=FloatingBaseType.kQuaternion) num_q = 8 num_v = 7 self.assertEqual(tree.number_of_positions(), num_q) self.assertEqual(tree.number_of_velocities(), num_v) q = tree.getZeroConfiguration() kinsol = tree.doKinematics(q) # - Sanity check sizes. J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0) self.assertEqual(J_default.shape[0], 6) self.assertEqual(J_default.shape[1], num_v) self.assertEqual(len(v_indices_default), num_v) # - Check that default value for in_terms_of_qdot is false. J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \ tree.geometricJacobian(kinsol, 0, 2, 0, False) self.assertTrue((J_default == J_not_in_terms_of_q_dot).all()) self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot) # - Check with in_terms_of_qdot set to True. J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \ tree.geometricJacobian(kinsol, 0, 2, 0, True) self.assertEqual(J_in_terms_of_q_dot.shape[0], 6) self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q) self.assertEqual(len(v_indices_in_terms_of_qdot), num_q) # - Check that drawKinematicTree runs tree.drawKinematicTree( join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
def test_multibody_dynamics(self, T): MultibodyPlant = MultibodyPlant_[T] MultibodyForces = MultibodyForces_[T] SpatialForce = SpatialForce_[T] file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f = MultibodyPlant_[float]() Parser(plant_f).AddModelFromFile(file_name) # Getting ready for when we set foot on Mars :-). gravity_vector = np.array([0.0, 0.0, -3.71]) plant_f.mutable_gravity_field().set_gravity_vector(gravity_vector) plant_f.Finalize() plant = to_type(plant_f, T) context = plant.CreateDefaultContext() numpy_compare.assert_float_equal( plant.gravity_field().gravity_vector(), gravity_vector) # 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. potential_energy = plant.CalcPotentialEnergy(context) numpy_compare.assert_float_not_equal(potential_energy, 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() numpy_compare.assert_float_equal(B, np.array([[0.], [1.]])) forces = MultibodyForces(plant=plant) plant.CalcForceElementsContribution(context=context, forces=forces) # Test generalized forces. # N.B. Cannot use `ndarray[object]` to reference existing C arrays # (#8116). if T == float: 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) coeff = numpy_compare.to_float( link2.GetForceInWorld(context, forces).get_coeffs()) numpy_compare.assert_float_equal(coeff, 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(numpy_compare.to_float( link2.GetForceInWorld(context, forces).get_coeffs()), 2 * F_expected)
def test_model_instance_state_access_by_array(self, T): # N.B. Please check warning above in `check_multibody_state_access`. MultibodyPlant = MultibodyPlant_[T] # 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 # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f = MultibodyPlant_[float](timestep) parser = Parser(plant_f) 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_[float]( RollPitchYaw_[float](np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant_f.WeldFrames( A=plant_f.world_frame(), B=plant_f.GetFrameByName("iiwa_link_0", iiwa_model)) plant_f.WeldFrames( A=plant_f.GetFrameByName("iiwa_link_7", iiwa_model), B=plant_f.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant_f.Finalize() plant = to_type(plant_f, T) # 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 if T == float: x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired else: plant.SetPositionsAndVelocities(context, x_desired) q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) if T == float: 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) numpy_compare.assert_equal(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) numpy_compare.assert_equal(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. numpy_compare.assert_float_equal(q_iiwa, q_iiwa_desired) numpy_compare.assert_float_equal(v_iiwa, v_iiwa_desired) numpy_compare.assert_float_equal(q_gripper, q_gripper_desired) numpy_compare.assert_float_equal(v_gripper, v_gripper_desired) if T == float: # Verify that SetPositionsInArray() and SetVelocitiesInArray() # works. plant.SetPositionsInArray( model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) numpy_compare.assert_float_equal( 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) numpy_compare.assert_float_equal( 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) numpy_compare.assert_float_equal(u_iiwa, u[:7])
def load_manipulation(time_step=0.0, use_meshcat=True, use_controllers=True, use_external=False): if not use_external: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' #goal_shelf = 'shelf_upper' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf" ) IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf" ) goal_shelf = 'bottom' #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") plant = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa', scene_graph=scene_graph, plant=plant) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=plant) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='table', scene_graph=scene_graph, plant=plant) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=plant) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=plant) weld_gripper(plant, robot, gripper) weld_to_world(plant, robot, create_transform()) weld_to_world( plant, table, create_transform(translation=[ dx_table_center_to_robot_base, 0, -dz_table_top_robot_base ])) weld_to_world( plant, cupboard, create_transform(translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) plant.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper', 'top', ] goal_surface = Surface(plant, cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #Surface(plant, table, 'amazon_table', 0), #goal_surface, ] door_names = ['left_door', 'right_door'] #door_names = [] doors = [plant.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # ~np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/2 #door_position = np.pi/8 initial_positions = { plant.GetJointByName('left_door_hinge'): -door_position, #plant.GetJointByName('right_door_hinge'): door_position, plant.GetJointByName('right_door_hinge'): np.pi / 2, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update( zip(get_movable_joints(plant, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]), #brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]), } goal_poses = { brick: create_transform(translation=[0.8, 0.2, 0.2927], rotation=[0, 0, np.pi / 8]), } goal_holding = [ #brick, ] goal_on = [ #(brick, goal_surface), ] diagram, state_machine = build_diagram(plant, scene_graph, robot, gripper, meshcat=use_meshcat, controllers=use_controllers) task = Task(diagram, plant, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_poses=goal_poses, goal_holding=goal_holding, goal_on=goal_on, reset_robot=True, reset_doors=False) return task, diagram, state_machine
def setUp(self): self.dut = FindResourceOrThrow( f"drake/examples/acrobot/dev/spong_sim_main_{_backend}") # 4 states x 30 seconds of samples at 20 Hz per sample_scenario. self.nominal_x_tape_shape = (4, 601)
def load_manipulation(time_step=0.0): source = True if source: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") #IIWA7_PATH = FindResourceOrThrow( # "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf") #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") goal_shelf = 'bottom' mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_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 amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table', scene_graph=scene_graph, plant=mbp) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=mbp) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=mbp) # left_door, left_door_hinge, cylinder weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform()) weld_to_world(mbp, amazon_table, create_transform( translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base])) weld_to_world(mbp, cupboard, create_transform( translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) mbp.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper' 'top', ] goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #VisualElement(amazon_table, 'amazon_table', 0), goal_surface, ] door_names = [ 'left_door', 'right_door', ] doors = [mbp.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/8 initial_positions = { mbp.GetJointByName('left_door_hinge'): -door_position, #mbp.GetJointByName('right_door_hinge'): door_position, mbp.GetJointByName('right_door_hinge'): np.pi, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.4, 0, 0]), } goal_on = [ (brick, goal_surface), ] task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on) return mbp, scene_graph, task
def RunSimulation(robobee_plantBS_torque, control_law, x0=np.random.random((15, 1)), duration=30): robobee_controller = RobobeeController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( RobobeePlantBSTorque(m=robobee_plantBS_torque.m, Ixx=robobee_plantBS_torque.Ixx, Iyy=robobee_plantBS_torque.Iyy, Izz=robobee_plantBS_torque.Izz, g=robobee_plantBS_torque.g, rw_l=robobee_plantBS_torque.rw_l, bw=robobee_plantBS_torque.bw, input_max=robobee_plantBS_torque.input_max)) Rigidbody_selector = builder.AddSystem(RigidBodySelection()) print("1. Connecting plant and controller\n") controller = builder.AddSystem(robobee_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant set_time_interval = 0.001 time_interval_multiple = 1000 publish_period = set_time_interval * time_interval_multiple print("2. Connecting plant to the logger\n") input_log = builder.AddSystem(SignalLogger(4)) # input_log._DeclarePeriodicPublish(publish_period, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(18)) # state_log._DeclarePeriodicPublish(publish_period, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Drake visualization print("3. Connecting plant output to DrakeVisualizer\n") rtree = RigidBodyTree( FindResourceOrThrow("drake/examples/robobee/robobee_twobar.urdf"), FloatingBaseType.kQuaternion) lcm = DrakeLcm() visualizer = builder.AddSystem( DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True)) builder.Connect(plant.get_output_port(0), Rigidbody_selector.get_input_port(0)) builder.Connect(Rigidbody_selector.get_output_port(0), visualizer.get_input_port(0)) print("4. Building diagram\n") diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. print("5. Create simulation\n") simulator = Simulator(diagram, context) simulator.Initialize() # simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(set_time_interval) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
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) table_model = Parser(plant=plant).AddModelFromFile(table_file_path) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) # Weld table to world. plant.WeldFrames( frame_on_parent_P=plant.world_frame(), frame_on_child_C=plant.GetFrameByName("link", table_model), X_PC=RigidTransform(RotationMatrix.MakeXRotation(0.2))) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_query_output_port(), viz.get_geometry_query_input_port()) # Add contact visualizer. contact_viz = builder.AddSystem( MeshcatContactVisualizer(meshcat_viz=viz, force_threshold=0, contact_force_scale=1, plant=plant, contact_force_radius=0.005)) contact_input_port = contact_viz.GetInputPort("contact_results") builder.Connect(plant.GetOutputPort("contact_results"), contact_input_port) 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_point_pair_contacts(), 0) self.assertEqual(len(contact_viz._published_contacts), 4)
def _make_tree(self): urdf_path = FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) return tree
from pydrake.common import FindResourceOrThrow from pydrake.multibody.parsing import Parser from pydrake.multibody.plant import AddMultibodyPlantSceneGraph from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder from pydrake.systems.primitives import ConstantVectorSource from pydrake.systems.planar_scenegraph_visualizer import ( PlanarSceneGraphVisualizer) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf" )) # noqa plant.Finalize() visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) torque_command = builder.AddSystem(ConstantVectorSource(np.zeros(3))) builder.Connect(torque_command.get_output_port(0), plant.get_actuation_input_port()) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0)
def RunSimulation(robobee_plant, control_law, x0=np.random.random((13, 1)), duration=30): robobee_controller = RobobeeController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( RobobeePlant(m=robobee_plant.m, Ixx=robobee_plant.Ixx, Iyy=robobee_plant.Iyy, Izz=robobee_plant.Izz, g=robobee_plant.g, input_max=robobee_plant.input_max)) controller = builder.AddSystem(robobee_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant input_log = builder.AddSystem(SignalLogger(4)) input_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(13)) state_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Drake visualization rtree = RigidBodyTree( FindResourceOrThrow("drake/examples/robobee/robobee.urdf"), FloatingBaseType.kQuaternion) lcm = DrakeLcm() visualizer = builder.AddSystem( DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True)) builder.Connect(plant.get_output_port(0), visualizer.get_input_port(0)) diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() # simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1) simulator.get_integrator().set_fixed_step_mode(False) simulator.get_integrator().set_maximum_step_size(0.005) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
def test_flat_terrain(self): tree = RigidBodyTree( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) # Test that AddFlatTerrainToWorld is spelled correctly. AddFlatTerrainToWorld(tree)
from pydrake.multibody.rigid_body_tree import ( AddModelInstanceFromUrdfFile, FloatingBaseType, RigidBodyTree, RigidBodyFrame, ) from pydrake.systems.framework import ( BasicVector, ) from pydrake.systems.sensors import ( CameraInfo, RgbdCamera, ) # Create tree describing scene. urdf_path = FindResourceOrThrow( "drake/multibody/models/box.urdf") tree = RigidBodyTree() AddModelInstanceFromUrdfFile( urdf_path, FloatingBaseType.kFixed, None, tree) # - Add frame for camera fixture. frame = RigidBodyFrame( name="rgbd camera frame", body=tree.world(), xyz=[-2, 0, 2], # Ensure that the box is within range. rpy=[0, np.pi / 4, 0]) tree.addFrame(frame) # Create camera. camera = RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0,
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() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") self.check_old_spelling_exists(tree.CalcRelativeTransform) X_WL = plant.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) self.check_old_spelling_exists(tree.CalcPointsPositions) 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)) self.check_old_spelling_exists( tree.CalcFrameGeometricJacobianExpressedInWorld) 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. self.check_old_spelling_exists(tree.EvalBodyPoseInWorld) X_WBase = plant.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 = plant.CalcRelativeTransform(context, world_frame, base_frame) self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow) 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]) self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow) plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB) self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld) 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())
from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile from pydrake.systems.drawing import plot_system_graphviz from pydrake.systems.framework import DiagramBuilder 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()) 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(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())
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. tree = plant.tree() self.check_old_spelling_exists(tree.GetPositionsFromArray) 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) self.check_old_spelling_exists(tree.GetVelocitiesFromArray) 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))
gripper_setpoint_list = [0.03, 0.0, 0.0] return plan_list, gripper_setpoint_list if __name__ == '__main__': # define command line arguments parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") args = parser.parse_args() is_hardware = args.hardware object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") manip_station_sim = ManipulationStationSimulator( time_step=2e-3, object_file_path=object_file_path, object_base_link_name="base_link", is_hardware=is_hardware) # Generate plans. q0 = [0, 0, 0, -1.75, 0, 1.0, 0] plan_list, gripper_setpoint_list = \ GenerateIiwaPlansAndGripperSetPoints(manip_station_sim, q0) # Add the position/impedance plan that opens the left door. plan_list.append( OpenLeftDoorPositionPlan(angle_start=theta0_hinge,
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.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.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))
def test_multibody_state_access(self, T): MultibodyPlant = MultibodyPlant_[T] plant_f = MultibodyPlant_[float]() file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") # N.B. `Parser` only supports `MultibodyPlant_[float]`. Parser(plant_f).AddModelFromFile(file_name) plant_f.Finalize() plant = to_type(plant_f, T) 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) numpy_compare.assert_float_equal(x, np.zeros(4)) # WARNING: The following oddities occur from the fact that # `ndarray[object]` cannot be referenced (#8116). Be careful when # writing scalar-generic code. if T == float: # Can reference matrices. Use `x_ref`. # Write into a mutable reference to the state vector. x_ref = plant.GetMutablePositionsAndVelocities(context) x_ref[:] = x0 def set_zero(): x_ref.fill(0) else: # Cannot reference matrices. Use setters. plant.SetPositionsAndVelocities(context, x0) def set_zero(): plant.SetPositionsAndVelocities( context, np.zeros(nq + nv)) # Verify that positions and velocities were set correctly. numpy_compare.assert_float_equal(plant.GetPositions(context), q0) numpy_compare.assert_float_equal(plant.GetVelocities(context), v0) # Verify we did modify the state stored in context. x = plant.GetPositionsAndVelocities(context) numpy_compare.assert_float_equal(x, x0) # Now set positions and velocities independently and check them. zeros_2 = np.zeros([2, ]) set_zero() plant.SetPositions(context, q0) numpy_compare.assert_float_equal(plant.GetPositions(context), q0) numpy_compare.assert_float_equal(plant.GetVelocities(context), zeros_2) set_zero() plant.SetVelocities(context, v0) numpy_compare.assert_float_allclose( plant.GetPositions(context), zeros_2) numpy_compare.assert_float_allclose(plant.GetVelocities(context), v0) # Now test SetPositionsAndVelocities(). set_zero() plant.SetPositionsAndVelocities(context, x0) numpy_compare.assert_float_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,))