def test_joints_api(self): # Verify construction from both Isometry3d and 4x4 arrays, # and sanity-check that the accessors function. name = "z" prismatic_joint_np = PrismaticJoint(name, np.eye(4), np.array([0., 0., 1.])) prismatic_joint_isom = PrismaticJoint(name, Isometry3.Identity(), np.array([0., 0., 1.])) self.assertEqual(prismatic_joint_isom.get_num_positions(), 1) self.assertEqual(prismatic_joint_isom.get_name(), name) name = "theta" revolute_joint_np = RevoluteJoint(name, np.eye(4), np.array([0., 0., 1.])) revolute_joint_isom = RevoluteJoint(name, Isometry3.Identity(), np.array([0., 0., 1.])) self.assertEqual(revolute_joint_isom.get_num_positions(), 1) self.assertEqual(revolute_joint_isom.get_name(), name) name = "fixed" fixed_joint_np = FixedJoint(name, np.eye(4)) fixed_joint_isom = FixedJoint(name, Isometry3.Identity()) self.assertEqual(fixed_joint_isom.get_num_positions(), 0) self.assertEqual(fixed_joint_isom.get_name(), name) name = "rpy" rpy_floating_joint_np = RollPitchYawFloatingJoint( name, np.eye(4)) rpy_floating_joint_isom = RollPitchYawFloatingJoint( name, Isometry3.Identity()) self.assertEqual(rpy_floating_joint_isom.get_num_positions(), 6) self.assertEqual(rpy_floating_joint_isom.get_name(), name)
def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=Isometry3.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = X_WP self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer_' + name) self.DeclarePeriodicPublish(draw_period, 0.0) self.DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud()))
def __init__(self, config_file, viz=False, segment_scene_function=None, get_pose_function=None): """ A system that takes in 3 Drake PointClouds and ImageRgba8U from RGBDCameras and determines the pose of an object in them. The user must supply a segmentation function and pose alignment to determine the pose. If these functions aren't supplied, the returned pose will always be the 4x4 identity matrix. @param config_file str. A path to a .yml configuration file for the cameras. @param viz bool. If True, save the aligned and segmented point clouds as serialized numpy arrays. @param segment_scene_function A Python function that returns a subset of the scene point cloud. See self.SegmentScene for more details. @param get_pose_function A Python function that calculates a pose from a segmented point cloud. See self.GetPose for more details. @system{ @input_port{camera_left_rgb}, @input_port{camera_middle_rgb}, @input_port{camera_right_rgb}, @input_port{left_point_cloud}, @input_port{middle_point_cloud}, @input_port{right_point_cloud}, @output_port{X_WObject} } """ LeafSystem.__init__(self) # TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB # fields. self.left_rgb = self._DeclareAbstractInputPort( "camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.middle_rgb = self._DeclareAbstractInputPort( "camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.right_rgb = self._DeclareAbstractInputPort( "camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.left_depth = self._DeclareAbstractInputPort( "left_point_cloud", AbstractValue.Make(mut.PointCloud())) self.middle_depth = self._DeclareAbstractInputPort( "middle_point_cloud", AbstractValue.Make(mut.PointCloud())) self.right_depth = self._DeclareAbstractInputPort( "right_point_cloud", AbstractValue.Make(mut.PointCloud())) self._DeclareAbstractOutputPort( "X_WObject", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self.segment_scene_function = segment_scene_function self.get_pose_function = get_pose_function self._LoadConfigFile(config_file) self.viz = viz
def GetEndEffectorWorldAlignedFrame(): X_EEa = Isometry3.Identity() X_EEa.set_rotation(np.array([[ 0., 1., 0, ], [0, 0, 1], [1, 0, 0]])) return X_EEa
def test_collision_element_api(self): # Verify construction from both Isometry3d and 4x4 arrays. box_element = shapes.Box([1.0, 1.0, 1.0]) box_collision_element_np = CollisionElement(box_element, np.eye(4)) box_collision_element_isom = CollisionElement( box_element, Isometry3.Identity()) body = RigidBody() box_collision_element_isom.set_body(body) self.assertEqual(box_collision_element_isom.get_body(), body)
def test_multibody_add_frame(self): plant = MultibodyPlant() frame = plant.AddFrame( frame=FixedOffsetFrame(name="frame", P=plant.world_frame(), X_PF=Isometry3.Identity(), model_instance=None)) self.assertIsInstance(frame, Frame) np.testing.assert_equal(np.eye(4), frame.GetFixedPoseInBodyFrame().matrix())
def test_visual_element_api(self): material_in = [0.3, 0.4, 0.5, 0.6] material_in_2 = [0.6, 0.7, 0.8, 0.9] box = shapes.Box(size=[1., 1., 1.]) visual_element_np = shapes.VisualElement(box, np.eye(4), material_in) visual_element_isom = shapes.VisualElement(box, Isometry3.Identity(), material_in) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in)) visual_element_np.setMaterial(material_in_2) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in_2))
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self): station = ManipulationStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) plant = station.get_mutable_multibody_plant() # Add models for iiwa and wsg iiwa_model_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/" "iiwa7_no_collision.sdf") iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa") X_WI = Isometry3.Identity() plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) wsg_model_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg = parser.AddModelFromFile(wsg_model_file, "gripper") X_7G = Isometry3.Identity() plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Register models for the controller. station.RegisterIiwaControllerModel( iiwa_model_file, iiwa, plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) station.RegisterWsgControllerModel( wsg_model_file, wsg, plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Finalize station.Finalize() # This WSG gripper model has 2 independent dof, and the IIWA model # has 7. self.assertEqual(plant.num_positions(), 9) self.assertEqual(plant.num_velocities(), 9)
def test_frame_pose_vector_api(self): obj = mut.FramePoseVector() frame_id = mut.FrameId.get_new_id() obj.set_value(id=frame_id, value=Isometry3.Identity()) self.assertEqual(obj.size(), 1) self.assertIsInstance(obj.value(id=frame_id), Isometry3) self.assertTrue(obj.has_id(id=frame_id)) self.assertIsInstance(obj.frame_ids(), list) self.assertIsInstance(obj.frame_ids()[0], mut.FrameId) obj.clear() self.assertEqual(obj.size(), 0) with catch_drake_warnings(expected_count=1): mut.FramePoseVector(source_id=mut.SourceId.get_new_id(), ids=[mut.FrameId.get_new_id()])
def test_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") parameters = mut.DifferentialInverseKinematicsParameters(2, 2) mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame, parameters) mut.DoDifferentialInverseKinematics(plant, context, Isometry3.Identity(), frame, parameters)
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_rigid_transform(self): def check_equality(X_actual, X_expected_matrix): # TODO(eric.cousineau): Use `IsNearlyEqualTo`. self.assertIsInstance(X_actual, mut.RigidTransform) self.assertTrue( np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix)) # - Constructors. X_I = np.eye(4) check_equality(mut.RigidTransform(), X_I) check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I) check_equality(copy.copy(mut.RigidTransform()), X_I) R_I = mut.RotationMatrix() p_I = np.zeros(3) rpy_I = mut.RollPitchYaw(0, 0, 0) quaternion_I = Quaternion.Identity() angle = np.pi * 0 axis = [0, 0, 1] angle_axis = AngleAxis(angle=angle, axis=axis) check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I) check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I) check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I) check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I) check_equality(mut.RigidTransform(R=R_I), X_I) check_equality(mut.RigidTransform(p=p_I), X_I) # - Accessors, mutators, and general methods. X = mut.RigidTransform() X.set(R=R_I, p=p_I) X.SetFromIsometry3(pose=Isometry3.Identity()) check_equality(mut.RigidTransform.Identity(), X_I) self.assertIsInstance(X.rotation(), mut.RotationMatrix) X.set_rotation(R=R_I) self.assertIsInstance(X.translation(), np.ndarray) X.set_translation(p=np.zeros(3)) self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I)) self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3])) self.assertIsInstance(X.GetAsIsometry3(), Isometry3) check_equality(X.inverse(), X_I) self.assertIsInstance( X.multiply(other=mut.RigidTransform()), mut.RigidTransform) self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray) if six.PY3: self.assertIsInstance( eval("X @ mut.RigidTransform()"), mut.RigidTransform) self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
parser.add_argument( "-m", "--max_iter", type=int, default=200, help="Specify the maximum iterations the algorithm is allowed to run" ) parser.add_argument( "-n", "--num_runs", type=int, default=10, help="Number of runs for experiment" ) args = parser.parse_args() object_file_path = FindResourceOrThrow( 'drake/examples/manipulation_station/models/061_foam_brick.sdf' ) obstacle_file_path = "motion_planner/models/cracker_box.sdf" X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) X_WObstacle = Isometry3.Identity() X_WObstacle.set_translation([0.4, 0, 0]) manip_station_sim = ManipulationStationSimulator( time_step=2e-3, object_file_paths=[object_file_path, obstacle_file_path], object_base_link_names=['base_link', 'base_link_cracker'], X_WObject_list=[X_WObject, X_WObstacle] ) # initial q and goal p q0 = [0, 0, 0, -1.75, 0, 1.0, 0] p_WC_box = np.array([0.6, 0.05 / 2, 0.025 + 0.025])
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()) # prints the pose of the brick, of type Isometry3 print pc_to_pose.GetOutputPort("X_WObject").Eval(context)
def GetDoorPose(config_file, viz=False, left_door_angle=0.0, right_door_angle=0.0, num_trials=1): """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 door. """ builder = DiagramBuilder() output = add_camera_system(builder, config_file, viz, build_station_real_world, build_station_simulation) if output[0]: # use_hardware is True use_hardware, station, pc_to_pose_left, pc_to_pose_right = output else: use_hardware, station, pc_to_pose_left, pc_to_pose_right, brick = output 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)) left_hinge_joint = station.get_mutable_multibody_plant( ).GetJointByName("left_door_hinge") left_hinge_joint.set_angle( station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context), -left_door_angle) right_hinge_joint = station.get_mutable_multibody_plant( ).GetJointByName("right_door_hinge") right_hinge_joint.set_angle( station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context), right_door_angle) left_context = diagram.GetMutableSubsystemContext( pc_to_pose_left, simulator.get_mutable_context()) right_context = diagram.GetMutableSubsystemContext( pc_to_pose_right, simulator.get_mutable_context()) # returns the pose of the brick, of type Isometry3 isometries = {"left_door": [], "right_door": []} for _ in range(num_trials): isometries["left_door"].append( pc_to_pose_left.GetOutputPort("X_WObject").Eval(left_context)) isometries["right_door"].append( pc_to_pose_right.GetOutputPort("X_WObject").Eval(right_context)) return isometries
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_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.linspace(0, 0.3, 7) v_iiwa_desired = q_iiwa_desired + 0.4 q_gripper_desired = [0.4, 0.5] v_gripper_desired = [-1., -2.] x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) x_tmp = plant.GetMutablePositionsAndVelocities(context=context) self.assertTrue(np.allclose(x_desired, x_tmp)) # Get positions and velocities of specific model instances # from the position/velocity vector of the plant. q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model) q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model, q=q) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions(context=context, model_instance=gripper_model) v_iiwa = plant.GetVelocities(context=context, model_instance=iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities(context=context, model_instance=gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray(model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) self.assertTrue( np.allclose( plant.GetPositionsFromArray(model_instance=iiwa_model, q=q), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray(model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v) self.assertTrue( np.allclose( plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray(model_instance=iiwa_model, u_instance=u_iiwa, u=u) self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_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_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired # Check SetPositionsAndVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired)) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, gripper_model, x_gripper_desired) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetPositions() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, iiwa_model, q_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), q_iiwa_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), np.zeros(nv_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, gripper_model, q_gripper_desired) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), q_gripper_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), np.zeros(nq_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), v_iiwa_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), np.zeros(nq_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, gripper_model, v_gripper_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), v_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), np.zeros(nv_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def test_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())
from pydrake.examples.manipulation_station import ( ManipulationStation, ManipulationStationHardwareInterface) from pydrake.geometry import SceneGraph from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile from pydrake.systems.framework import DiagramBuilder from pydrake.systems.analysis import Simulator from pydrake.common.eigen_geometry import Isometry3 from pydrake.systems.primitives import Demultiplexer, LogOutput from underactuated.meshcat_visualizer import MeshcatVisualizer # from pydrake.systems.meshcat_visualizer import MeshcatVisualizer from plan_runner.manipulation_station_plan_runner import ManipStationPlanRunner from plan_runner.manipulation_station_plan_runner_diagram import CreateManipStationPlanRunnerDiagram from plan_runner.plan_utils import * X_WObject_default = Isometry3.Identity() X_WObject_default.set_translation([.6, 0, 0]) class ManipulationStationSimulator: def __init__( self, time_step, object_file_path=None, object_base_link_name=None, X_WObject=X_WObject_default, ): self.object_base_link_name = object_base_link_name self.time_step = time_step # Finalize manipulation station by adding manipuland.
def test_blank_system(self): pose = self.pc_to_pose.GetOutputPort("X_WObject").Eval(self.context) self.assertTrue( np.allclose(pose.matrix(), Isometry3.Identity().matrix()))