def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle.set_pose(kIndex, Isometry3(q, p)) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) self.assertTrue( (bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry(road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_rotation(), Quaternion)) self.assertTrue(isinstance(value.get_translation(), np.ndarray)) # - Value. self.assertTrue( np.allclose(value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue( np.allclose(value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue( np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def test_set_free_body_pose(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() plant_model = AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() X_WB_desired = Isometry3.Identity() R_WB = np.array([[0., 1., 0.], [0., 0., 1.], [1., 0., 0.]]) X_WB_desired.set_rotation(R_WB) tree.SetFreeBodyPoseOrThrow( body=plant.GetBodyByName("base", plant_model), X_WB=X_WB_desired, context=context) world_frame = plant.world_frame() base_frame = plant.GetBodyByName("base").body_frame() X_WB = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
def 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 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) R_I = mut.RotationMatrix() p_I = np.zeros(3) check_equality(mut.RigidTransform(R=R_I, 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)
def create_transform(translation=None, rotation=None): pose = Isometry3.Identity() if translation is not None: pose.set_translation(translation) if rotation is not None: pose.set_rotation(matrix_from_euler(rotation)) return pose
def __init__(self): LeafSystem.__init__(self) self._DeclareAbstractOutputPort( "X_WE", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self._DeclarePeriodicPublish(0.1, 0.0) self.window = tk.Tk() self.window.title("End-Effector TeleOp") self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="roll", length=800, orient=tk.HORIZONTAL) self.roll.pack() self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="pitch", length=800, orient=tk.HORIZONTAL) self.pitch.pack() self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="yaw", length=800, orient=tk.HORIZONTAL) self.yaw.pack() self.x = tk.Scale(self.window, from_=0.1, to=0.8, resolution=-1, label="x", length=800, orient=tk.HORIZONTAL) self.x.pack() self.y = tk.Scale(self.window, from_=-0.3, to=0.3, resolution=-1, label="y", length=800, orient=tk.HORIZONTAL) self.y.pack() self.z = tk.Scale(self.window, from_=0, to=0.8, resolution=-1, label="z", length=800, orient=tk.HORIZONTAL) self.z.pack()
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_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)
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_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
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_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") parameters = mut.DifferentialInverseKinematicsParameters(2, 2) mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame, parameters) mut.DoDifferentialInverseKinematics(plant, context, Isometry3.Identity(), frame, parameters)
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa, context_iiwa, t_plan): ''' :param X_L7E: transformation from frame E (end effector) to frame L7. :param l7_frame: A BodyFrame object of frame L7. :param world_frame: A BodyFrame object of the world frame. :param tree_iiwa: A MultibodyTree object of the robot. :param context_iiwa: A Context object that describes the current state of the robot. :param t_plan: time passed since the beginning of this Plan, expressed in seconds. :return: Jv_WL7q: geometric jacbian of point Q in frame L7. p_HrQ: position of point Q relative to frame Hr. ''' # calculate Geometric jacobian (6 by 7 matrix) of point Q in frame L7. 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 # p_HrQ: position of point Q relative to frame Hr. # X_WHr: transformation from Hr to world frame W. X_WHr = Isometry3() # Your code here #------------------------------------------------------------------------# angle = self.traj.value(t_plan) X_WHr.set_rotation( np.array([[np.cos(angle), np.sin(angle), 0.], [-np.sin(angle), np.cos(angle), 0.], [0., 0., 1.]])) X_WHr.set_translation( p_WC_handle + [-r_handle * np.sin(angle), -r_handle * np.cos(angle), 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 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) 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)
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 frame L7. 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 # p_HrQ position of point Q relative to frame Hr. # X_WHr: transformation from Hr to world frame W. X_WHr = Isometry3() 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 read_poses_from_file(filename): pose_dict = {} row_num = 0 cur_matrix = np.eye(4) with open(filename, "r") as f: for line in f: line = line.rstrip() if not line.lstrip(" ").startswith("["): object_name = line else: row = np.matrix(line) cur_matrix[row_num, :] = row row_num += 1 if row_num == 4: pose_dict[object_name] = Isometry3(cur_matrix) translation = pose_dict[object_name].translation() translation[2] = -dz_table_top_robot_base pose_dict[object_name].set_translation(translation) pose_dict[object_name].set_rotation(np.eye(3)) cur_matrix = np.eye(4) row_num %= 4 return pose_dict
def __init__(self, time_step, object_file_path, object_base_link_name, is_hardware=False): self.object_base_link_name = object_base_link_name self.time_step = time_step self.is_hardware = is_hardware # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() # Initial pose of the object X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) self.X_WObject = X_WObject
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))
station_context.FixInputPort( station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7)) if not args.hardware: # Set the initial positions of the IIWA to a comfortable configuration # inside the workspace of the station. q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0] station.SetIiwaPosition(q0, station_context) station.SetIiwaVelocity(np.zeros(7), station_context) # Set the initial configuration of the gripper to open. station.SetWsgPosition(0.1, station_context) station.SetWsgVelocity(0, station_context) # Place the object in the middle of the workspace. X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) station.get_multibody_plant().tree().SetFreeBodyPoseOrThrow( station.get_multibody_plant().GetBodyByName("base_link", object), X_WObject, station.GetMutableSubsystemContext(station.get_multibody_plant(), station_context)) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval( station_context).get_value() teleop.set_position(q0) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False)
def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7)): ''' Constructs a Diagram that sends commands to ManipulationStation. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner plan_runner = KukaPlanRunner(self.plant) builder.AddSystem(plan_runner) builder.Connect(plan_runner.get_output_port(0), self.station.GetInputPort("iiwa_position")) # Add state machine. state_machine = ManipStateMachine( plant=self.plant, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(state_machine) builder.Connect(state_machine.kuka_plan_output_port, plan_runner.plan_input_port) builder.Connect(state_machine.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(state_machine.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), state_machine.iiwa_position_input_port) # Add meshcat visualizer from underactuated.meshcat_visualizer import MeshcatVisualizer scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph) builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.get_input_port(0)) # Add logger iiwa_position_command_log = builder.AddSystem( SignalLogger(self.station.GetInputPort("iiwa_position").size())) iiwa_position_command_log._DeclarePeriodicPublish(0.005) builder.Connect(plan_runner.get_output_port(0), iiwa_position_command_log.get_input_port(0)) # build diagram diagram = builder.Build() viz.load() time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgState(0.05, 0, context) # set initial hinge angles of the cupboard. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-np.pi / 2) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=np.pi / 2) # set initial pose of the object X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) # fix feedforward torque input to 0. context.FixInputPort( self.station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_pose(0, Isometry3(q3, p3)) bundle_value = AbstractValue.Make(bundle) context.FixInputPort(0, pose1) context.FixInputPort(1, pose2) context.FixInputPort(2, velocity) context.FixInputPort(3, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) isom1_actual = Isometry3() isom1_actual.set_translation(p1) self.assertTrue( (value.get_pose(0).matrix() == isom1_actual.matrix()).all()) isom2_actual = Isometry3() isom2_actual.set_translation(p2) self.assertTrue( (value.get_pose(1).matrix() == isom2_actual.matrix()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack( (v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow(body=base, X_WB=X_WB_desired, context=context) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) tree.SetFreeBodySpatialVelocityOrThrow(body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational()))
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = Isometry3.Identity() X_EeGripper.set_translation([0, 0, 0.081]) X_EeGripper.set_rotation( RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix()) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.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())