def add_arms_from_config(self, config): X_iiwa_wsg = RigidTransform(RollPitchYaw(np.pi/2., 0, np.pi/2.), [0, 0, 0.114]) X_franka = RigidTransform(RollPitchYaw(0, 0, -np.pi/4.), [0, 0, 0]) for arm_name, arm_config in iteritems(config['env']['arms']): X_arm = transform_from_dict(arm_config) if arm_config["arm_type"] == "iiwa_7": if arm_config["collision_model"] == "none": iiwa7_path = iiwa_7_no_collision_path elif arm_config["collision_model"] == "box": iiwa7_path = iiwa_7_box_collision_path self.add_arm_gripper(arm_name, iiwa7_path, "iiwa_link_0", X_arm, schunk_path, "iiwa_link_7", "body", X_iiwa_wsg) elif arm_config["arm_type"] == "iiwa_14": if arm_config["collision_model"] == "none": iiwa14_path = iiwa14_no_collision_path elif arm_config["collision_model"] == "sphere": iiwa14_path = iiwa14_sphere_collision_path elif arm_config["collision_model"] == "cylinder": iiwa14_path = iiwa14_primitive_collision_path elif arm_config["collision_model"] == "ee_mesh": iiwa14_path = iiwa14_poly_collision_path self.add_arm_gripper(arm_name, iiwa14_path, "base", X_arm, schunk_path, "iiwa_link_7", "body", X_iiwa_wsg) elif arm_config["arm_type"] == "franka": self.add_arm_gripper(arm_name, franka_arm_path, "panda_link0", X_arm, franka_hand_path, "panda_link8", "panda_hand", X_franka) elif arm_config["arm_type"] == "floating": self.add_floating_gripper(arm_name, schunk_path, None, "body", X_arm)
def get_q_object_final(self, q_object_init, ee_init, ee_final): """ Input: q_object_init: [quaternion, xyz] ee_init: [x y z r p y] ee_final: [x y z r p y] xyz_iiwa: [x, y, z] Return: q_object_final: [quaternion, xyz] """ xyz_obj = np.array(q_object_init[4:]).transpose() # xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame quaternion_obj = Quaternion(np.array(q_object_init[:4])) rot_obj = RotationMatrix(quaternion_obj) X_WO = RigidTransform(rot_obj, xyz_obj) xyz_ee_init = np.array(ee_init[:3]).transpose() rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix() X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init) xyz_ee_final = np.array(ee_final[:3]).transpose() rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix() X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final) X_EO = X_WE1.inverse().multiply(X_WO) X_WO2 = X_WE2.multiply(X_EO) wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz() xyz_obj_final = X_WO2.translation() # xyz_ee_obj_init = xyz_obj - xyz_ee_init # rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose()) # xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init) # # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame # xyz_obj_final = xyz_ee_final + xyz_ee_obj_final # rot_obj_final = rot_ee_init_final.multiply(rot_obj) # quaternion_obj_final = rot_obj_final.ToQuaternion() # wxyz_obj_final = quaternion_obj_final.wxyz() q_object_final = [] for i in range(4): q_object_final.append(wxyz_obj_final[i]) for i in range(3): q_object_final.append(xyz_obj_final[i]) return q_object_final
def _slider_callback(change, index, body=None): if body: # Then it's a floating base pose = plant.GetFreeBodyPose(plant_context, body) vector_pose = np.concatenate( (RollPitchYaw(pose.rotation()).vector(), pose.translation())) vector_pose[index] = change.new plant.SetFreeBodyPose( plant_context, body, RigidTransform(RollPitchYaw(vector_pose[:3]), vector_pose[3:])) else: positions[index] = change.new plant.SetPositions(plant_context, positions) publishing_system.Publish(publishing_context) if my_callback: my_callback(plant_context)
def InterpolatePitchAngle(i, num_knot_points): assert i <= num_knot_points pitch_start = np.pi / 180 * 135 pitch_end = np.pi / 180 * 90 pitch_angle = pitch_start + (pitch_end - pitch_start) / num_knot_points * i return RollPitchYaw(0, pitch_angle, 0).ToRotationMatrix()
def DoCalcOutput(self, context, output): """ Constructs the output values from the widget elements. """ output.set_value(RigidTransform( RollPitchYaw(self._roll.value, self._pitch.value, self._yaw.value), [self._x.value, self._y.value, self._z.value]))
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa, context_iiwa, handle_angle_ref): # calculate Geometric jacobian (6 by 7 matrix) of point Q in EE frame. p_L7Q = X_L7E.multiply(p_EQ) Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld( context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q) # Translation # Hr: handle reference frame X_WHr = Isometry3() X_WHr.set_rotation( RollPitchYaw(0, 0, -handle_angle_ref).ToRotationMatrix().matrix()) X_WHr.set_translation(p_WC_left_hinge + [ -r_handle * np.sin(handle_angle_ref), -r_handle * np.cos(handle_angle_ref), 0 ]) X_HrW = X_WHr.inverse() X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa, frame_A=world_frame, frame_B=l7_frame) p_WQ = X_WL7.multiply(p_L7Q) p_HrQ = X_HrW.multiply(p_WQ) return Jv_WL7q, p_HrQ
def xyz_rpy_deg(xyz, rpy_deg): """Shorthand to create a rigid transform from XYZ and RPY (degrees).""" rpy = np.deg2rad(rpy_deg) return RigidTransform( R=RotationMatrix(rpy=RollPitchYaw(rpy), ), p=xyz, )
def transform_from_dict(data): pos, _ = get_data_from_dict(data, ['pos', 'position', 'translation']) orientation, key = get_data_from_dict(data, ['quat', 'quaternion', 'rpy']) if key == "rpy": return RigidTransform(RollPitchYaw(orientation), pos) else: return RigidTransform(Quaternion(orientation), pos)
def _DoCalcOutput(self, context, output): output.get_mutable_value().set_rotation( RollPitchYaw(self.roll.get(), self.pitch.get(), self.yaw.get()).ToRotationMatrix().matrix()) output.get_mutable_value().set_translation( [self.x.get(), self.y.get(), self.z.get()])
def SetPose(self, pose): """ @param pose is an Isometry3. """ tf = RigidTransform(pose) self.SetRPY(RollPitchYaw(tf.rotation())) self.SetXYZ(pose.translation())
def test_map_qdot_to_v_and_back(self): plant = MultibodyPlant() iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" "iiwa_description/sdf/iiwa14_no_collision.sdf") # Use floating base to effectively add a quatnerion in the generalized # quaternion. iiwa_model = Parser(plant=plant).AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') plant.Finalize() context = plant.CreateDefaultContext() # Try mapping velocity to qdot and back. nq = plant.num_positions() nv = plant.num_velocities() q_init = np.linspace(start=1.0, stop=nq, num=nq) plant.SetPositions(context, q_init) # Overwrite the (invalid) base coordinates, wherever in `q` they are. plant.SetFreeBodyPose( context, plant.GetBodyByName("iiwa_link_0"), RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]), p=[0.4, 0.5, 0.6]).GetAsIsometry3()) v_expected = np.linspace(start=-1.0, stop=-nv, num=nv) qdot = plant.MapVelocityToQDot(context, v_expected) v_remap = plant.MapQDotToVelocity(context, qdot) self.assertTrue(np.allclose(v_expected, v_remap))
def DepthCameraDemoSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Add a single object into it. Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf")) # Add a rendering engine renderer = "my_renderer" scene_graph.AddRenderer(renderer, MakeRenderEngineVtk(RenderEngineVtkParams())) plant.Finalize() # Add a visualizer just to help us see the object. use_meshcat = False if use_meshcat: meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # Add a camera to the environment. pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5]) properties = DepthCameraProperties(width=640, height=480, fov_y=np.pi / 4.0, renderer_name=renderer, z_near=0.1, z_far=10.0) camera = builder.AddSystem( RgbdSensor(parent_id=scene_graph.world_frame_id(), X_PB=pose, properties=properties, show_window=False)) camera.set_name("rgbd_sensor") builder.Connect(scene_graph.get_query_output_port(), camera.query_object_input_port()) # Export the camera outputs builder.ExportOutput(camera.color_image_output_port(), "color_image") builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image") # Add a system to convert the camera output into a point cloud to_point_cloud = builder.AddSystem( DepthImageToPointCloud(camera_info=camera.depth_camera_info(), fields=BaseField.kXYZs | BaseField.kRGBs)) builder.Connect(camera.depth_image_32F_output_port(), to_point_cloud.depth_image_input_port()) builder.Connect(camera.color_image_output_port(), to_point_cloud.color_image_input_port()) # Export the point cloud output. builder.ExportOutput(to_point_cloud.point_cloud_output_port(), "point_cloud") diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def SetPose(self, pose): """ @param pose is a RigidTransform or else any type accepted by RigidTransform's constructor """ tf = RigidTransform(pose) self.SetRPY(RollPitchYaw(tf.rotation())) self.SetXYZ(pose.translation())
def add_thrusters(builder, mbp): robot_body_index = mbp.GetBodyByName("robot").index() robot_tail_forward = RigidTransform(p=[-0.5, 0, 0], rpy=RollPitchYaw([math.pi / 2, 0, 0])) robot_tail_clockwise = RigidTransform(p=[-0.5, 0, 0], rpy=RollPitchYaw( [math.pi / 2, 0, math.pi / 2])) thrusters = builder.AddSystem( Propeller([ PropellerInfo(robot_body_index, X_BP=robot_tail_forward), PropellerInfo(robot_body_index, X_BP=robot_tail_clockwise) ])) builder.Connect(thrusters.get_spatial_forces_output_port(), mbp.get_applied_spatial_force_input_port()) builder.Connect(mbp.get_body_poses_output_port(), thrusters.get_body_poses_input_port()) return thrusters
def get_q_object_init(self, object_name, t, xyz_iiwa): """ Returns vector of length 7 [quaternion, xyz] """ q = self.get_object_state(object_name, t) quaternion = RollPitchYaw(q[3], q[4], q[5]).ToQuaternion() wxyz = quaternion.wxyz() xyz_obj = np.array(q[:3]) - np.array(xyz_iiwa) q_object_init = [] for i in range(4): q_object_init.append(wxyz[i]) for i in range(3): q_object_init.append(xyz_obj[i]) return q_object_init
def SetPose(self, pose): """ Sets the current value of the sliders. Args: pose: Any viable argument for the RigidTransform constructor. """ tf = RigidTransform(pose) self.SetRpy(RollPitchYaw(tf.rotation())) self.SetXyz(tf.translation())
def reset_simulator_from_config(config, simulator, diagram, systems): for rope_name, _ in iteritems(config['env']['ropes']): initialize_rope_zero(diagram, simulator, systems["station"], rope_name) sim_context = simulator.get_mutable_context() station_context = diagram.GetMutableSubsystemContext( systems["station"], sim_context) if 'arms' in config['env']: values = {} for arm_name, arm_config in iteritems(config['env']['arms']): rpy = RollPitchYaw(config["env"]["arms"][arm_name]["rpy"]) xyz = config["env"]["arms"][arm_name]["pos"] init_state = np.append(rpy.vector(), xyz) grip = config["env"]["arms"][arm_name]["grip"] quat = rpy.ToQuaternion() systems["station"].set_model_state( station_context, arm_name, np.array([ quat.w(), quat.x(), quat.y(), quat.z(), xyz[0], xyz[1], xyz[2], -grip / 2, grip / 2 ]), np.zeros(8)) values[arm_name] = init_state values[f"{arm_name}_width"] = grip set_targets(simulator, diagram, systems, values) sp_context = diagram.GetMutableSubsystemContext(systems["sp_control"], sim_context) if 'arms' in config['env']: for arm_name, arm_config in iteritems(config['env']['arms']): systems["sp_control"].SetPositions( sp_context, arm_name, np.append(config["env"]["arms"][arm_name]["rpy"], config["env"]["arms"][arm_name]["pos"]), [config["env"]["arms"][arm_name]["grip"]]) pid_context = diagram.GetMutableSubsystemContext(systems["pid"], sim_context) systems["pid"].reset(pid_context) simulator.set_target_realtime_rate(config['env']['target_realtime_rate']) sim_context.SetTime(0.) simulator.Initialize()
def transform_to_vector(X, rpy=True): """ If rpy True, returns RollPitchYaw vector followed by translation vector. Otherwise, returns Quaternion vector followed by translation vector. """ if rpy: vector = np.empty(6) vector[:3] = RollPitchYaw(X.rotation()).vector() else: vector = np.empty(7) vector[:4] = X.rotation().ToQuaternion().wxyz() vector[-3:] = X.translation() return vector
def DoCalcAbstractOutput(self, context, y_data): """ Sets external forces """ external_forces = [] body_positions = self.body_positions_input_port.Eval(context) for arm in self.arm_info: desired = self.desired_input_ports[arm].Eval(context) pose = body_positions[int(self.arm_info[arm])] rotation = RollPitchYaw(pose.rotation()) estimated = [ rotation.roll_angle(), rotation.pitch_angle(), rotation.yaw_angle() ] estimated.extend(pose.translation()) error = np.subtract(desired, estimated) error[0:3] = self.clamp_angles(error[0:3]) prev_error = context.get_mutable_discrete_state( self.previous_error[arm]) integral = context.get_mutable_discrete_state(self.integral[arm]) integral.SetFromVector(integral.get_value() + error * self.timestep) derivative = np.subtract(error, prev_error.get_value()) / self.timestep forces = np.multiply(self.kp, error) forces += np.multiply(self.kd, derivative) forces += np.multiply(self.ki, integral.get_value()) clipped_forces = np.clip( forces, [-100, -100, -100, -10000, -10000, -10000], [100, 100, 100, 10000, 10000, 10000]) prev_error.SetFromVector(error) # print(f"arm {arm} forces {clipped_forces}") external_forces.append( self.get_external_force(self.arm_info[arm], clipped_forces)) y_data.set_value(external_forces)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): rpy_xyz_desired = self.EvalVectorInput(context, 0).get_value() X_WE_desired = RigidTransform(RollPitchYaw(rpy_xyz_desired[:3]), rpy_xyz_desired[-3:]).GetAsIsometry3() q_last = context.get_discrete_state_vector().get_value() x = self.robot.GetMutablePositionsAndVelocities(self.robot_context) x[:robot.num_positions()] = q_last result = DoDifferentialInverseKinematics(self.robot, self.robot_context, X_WE_desired, self.frame_E, self.parameters) if (result.status != result.status.kSolutionFound): print("Differential IK could not find a solution.") discrete_state.get_mutable_vector().SetFromVector(q_last) else: discrete_state.get_mutable_vector().\ SetFromVector(q_last + self.time_step*result.joint_velocities)
def make_ball_paddle(contact_model, contact_surface_representation, time_step): multibody_plant_config = \ MultibodyPlantConfig( time_step=time_step, contact_model=contact_model, contact_surface_representation=contact_surface_representation) # We pose the paddle, so that its top surface is on World's X-Y plane. # Intuitively we push it down 1 cm because the box is 2 cm thick. p_WPaddle_fixed = RigidTransform(RollPitchYaw(0, 0, 0), np.array([0, 0, -0.01])) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder) parser = Parser(plant) paddle_sdf_file_name = \ FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle" "/paddle.sdf") paddle = parser.AddModelFromFile(paddle_sdf_file_name, model_name="paddle") plant.WeldFrames(frame_on_parent_F=plant.world_frame(), frame_on_child_M=plant.GetFrameByName("paddle", paddle), X_FM=p_WPaddle_fixed) ball_sdf_file_name = \ FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle" "/ball.sdf") parser.AddModelFromFile(ball_sdf_file_name) # TODO(DamrongGuoy): Let users override hydroelastic modulus, dissipation, # and resolution hint from the two SDF files above. plant.Finalize() DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph) ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant, scene_graph=scene_graph) nx = plant.num_positions() + plant.num_velocities() state_logger = builder.AddSystem(VectorLogSink(nx)) builder.Connect(plant.get_state_output_port(), state_logger.get_input_port()) diagram = builder.Build() return diagram, plant, state_logger
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False): parser = pydrake.multibody.parsing.Parser(plant) if welded: parser.package_map().Add( "wsg_50_description", os.path.dirname( pydrake.common.FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/package.xml") )) gripper = parser.AddModelFromFile( FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper") else: gripper = parser.AddModelFromFile( pydrake.common.FindResourceOrThrow( "drake/manipulation/models/" "wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")) X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114]) plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance), plant.GetFrameByName("body", gripper), X_7G) return gripper
def setUp(self): self.points = np.array([[1, 1, 0], [2, 1, 0]]).T self.translation = RigidTransform(p=[1, 2, 3]) self.rotation = RigidTransform( RotationMatrix(RollPitchYaw(0, 0, np.pi / 2)))
def _xyz_rpy_deg(xyz, rpy_deg): return RigidTransform(RollPitchYaw(np.asarray(rpy_deg) * np.pi / 180), xyz)
def _xyz_rpy(p, rpy): return RigidTransform(rpy=RollPitchYaw(rpy), p=p)
def test_composition_gripper_workflow(self): """Tests subgraphs (pre-finalize) for composition, with a scene graph, welding bodies together across different subgraphs.""" # N.B. The frame-welding is done so that we can easily set the # positions of the IIWA / WSG without having to worry about / work # around the floating body coordinates. # Create IIWA. iiwa_builder = DiagramBuilder() iiwa_plant, iiwa_scene_graph = AddMultibodyPlantSceneGraph( iiwa_builder, time_step=0.) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") iiwa_model = Parser(iiwa_plant).AddModelFromFile(iiwa_file, "iiwa") iiwa_plant.WeldFrames(iiwa_plant.world_frame(), iiwa_plant.GetFrameByName("base")) iiwa_plant.Finalize() if VISUALIZE: print("test_composition") DrakeVisualizer.AddToBuilder(iiwa_builder, iiwa_scene_graph) iiwa_diagram = iiwa_builder.Build() iiwa_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph)) self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph) iiwa_subgraph.remove_body(iiwa_plant.world_body()) # Create WSG. wsg_builder = DiagramBuilder() wsg_plant, wsg_scene_graph = AddMultibodyPlantSceneGraph(wsg_builder, time_step=0.) wsg_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg_model = Parser(wsg_plant).AddModelFromFile(wsg_file, "gripper_model") wsg_plant.WeldFrames(wsg_plant.world_frame(), wsg_plant.GetFrameByName("__model__")) wsg_plant.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder(wsg_builder, wsg_scene_graph) wsg_diagram = wsg_builder.Build() wsg_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(wsg_plant, wsg_scene_graph)) self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph) wsg_subgraph.remove_body(wsg_plant.world_body()) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_to_plant = iiwa_subgraph.add_to(plant, scene_graph) self.assertIsInstance(iiwa_to_plant, mut.MultibodyPlantElementsMap) wsg_to_plant = wsg_subgraph.add_to(plant, scene_graph) self.assertIsInstance(wsg_to_plant, mut.MultibodyPlantElementsMap) if VISUALIZE: DrakeVisualizer.AddToBuilder(builder, scene_graph) rpy_deg = np.array([90., 0., 90]) X_7G = RigidTransform(p=[0, 0, 0.114], rpy=RollPitchYaw(rpy_deg * np.pi / 180)) frame_7 = plant.GetFrameByName("iiwa_link_7") frame_G = plant.GetFrameByName("body") plant.WeldFrames(frame_7, frame_G, X_7G) plant.Finalize() q_iiwa = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7] q_wsg = [-0.03, 0.03] iiwa_d_context = iiwa_diagram.CreateDefaultContext() iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context) iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa) wsg_d_context = wsg_diagram.CreateDefaultContext() wsg_context = wsg_plant.GetMyContextFromRoot(wsg_d_context) wsg_plant.SetPositions(wsg_context, wsg_model, q_wsg) # Transfer state and briefly compare. diagram = builder.Build() d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) iiwa_to_plant.copy_state(iiwa_context, context) wsg_to_plant.copy_state(wsg_context, context) # Compare frames from sub-plants. util.compare_frame_poses(plant, context, iiwa_plant, iiwa_context, "base", "iiwa_link_7") util.compare_frame_poses(plant, context, wsg_plant, wsg_context, "body", "left_finger") # Visualize. if VISUALIZE: print(" Visualize IIWA") Simulator(iiwa_diagram, iiwa_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize WSG") Simulator(wsg_diagram, wsg_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize Composite") Simulator(diagram, d_context.Clone()).Initialize() input(" Press enter...")
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.linspace(0, 0.3, 7) v_iiwa_desired = q_iiwa_desired + 0.4 q_gripper_desired = [0.4, 0.5] v_gripper_desired = [-1., -2.] x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) x_tmp = plant.GetMutablePositionsAndVelocities(context=context) self.assertTrue(np.allclose(x_desired, x_tmp)) # Get positions and velocities of specific model instances # from the position/velocity vector of the plant. q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model) q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model, q=q) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions(context=context, model_instance=gripper_model) v_iiwa = plant.GetVelocities(context=context, model_instance=iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities(context=context, model_instance=gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray(model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) self.assertTrue( np.allclose( plant.GetPositionsFromArray(model_instance=iiwa_model, q=q), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray(model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v) self.assertTrue( np.allclose( plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray(model_instance=iiwa_model, u_instance=u_iiwa, u=u) self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_model_instance_state_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant() parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nv = plant.num_velocities() nq_iiwa = 7 nv_iiwa = 7 nq_gripper = 2 nv_gripper = 2 q_iiwa_desired = np.zeros(nq_iiwa) v_iiwa_desired = np.zeros(nv_iiwa) q_gripper_desired = np.zeros(nq_gripper) v_gripper_desired = np.zeros(nv_gripper) q_iiwa_desired[2] = np.pi / 3 q_gripper_desired[0] = 0.1 v_iiwa_desired[1] = 5.0 q_gripper_desired[0] = -0.3 x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa) x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired x_iiwa_desired[nq_iiwa:nq_iiwa + nv_iiwa] = v_iiwa_desired x_gripper_desired = np.zeros(nq_gripper + nv_gripper) x_gripper_desired[0:nq_gripper] = q_gripper_desired x_gripper_desired[nq_gripper:nq_gripper + nv_gripper] = v_gripper_desired x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired # Check SetPositionsAndVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired)) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, gripper_model, x_gripper_desired) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetPositions() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, iiwa_model, q_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), q_iiwa_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), np.zeros(nv_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, gripper_model, q_gripper_desired) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), q_gripper_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), np.zeros(nq_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), v_iiwa_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), np.zeros(nq_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, gripper_model, v_gripper_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), v_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), np.zeros(nv_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def xyz_rpy_deg(xyz, rpy_deg): """Shorthand for defining a pose.""" rpy_deg = np.asarray(rpy_deg) return RigidTransform(RollPitchYaw(rpy_deg * np.pi / 180), xyz)
def _get_transform(self): return RigidTransform( RollPitchYaw(self._value[0], self._value[1], self._value[2]), self._value[3:])