def test_inverse_tf(set_seed): # Test against drake t = np.random.uniform(-1, 1, size=3) R = UniformlyRandomRotationMatrix(set_seed) drake_tf = RigidTransform(p=t, R=R) tf = drake_tf_to_torch_tf(drake_tf) tf_inv = invert_torch_tf(tf) assert torch.allclose(tf_inv, drake_tf_to_torch_tf(drake_tf.inverse()))
def make_gripper_frames(X_G, X_O): """ Takes a partial specification with X_G["initial"] and X_O["initial"] and X_0["goal"], and returns a X_G and times with all of the pick and place frames populated. """ # Define (again) the gripper pose relative to the object when in grasp. p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() # pregrasp is negative y in the gripper frame (see the figure!). X_GgraspGpregrasp = RigidTransform([0, -0.08, 0]) X_G["pick"] = X_O["initial"].multiply(X_OGgrasp) X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp) X_G["place"] = X_O["goal"].multiply(X_OGgrasp) X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp) # I'll interpolate to a halfway orientation by converting to axis angle and halving the angle X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"]) angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis() X_GprepickGclearance = RigidTransform( AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()), X_GprepickGpreplace.translation() / 2.0 + np.array([0, -0.3, 0])) X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance) # Not lets set timings times = {"initial": 0.0} X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"]) times["prepick"] = times["initial"] + 10.0 * \ np.linalg.norm(X_GinitialGprepick.translation()) # Allow some time for the gripper to close. times["pick_start"] = times["prepick"] + 2.0 times["pick_end"] = times["pick_start"] + 2.0 times["postpick"] = times["pick_end"] + 2.0 time_to_from_clearance = 10.0 * \ np.linalg.norm(X_GprepickGclearance.translation()) times["clearance"] = times["postpick"] + time_to_from_clearance times["preplace"] = times["clearance"] + time_to_from_clearance times["place_start"] = times["preplace"] + 2.0 times["place_end"] = times["place_start"] + 2.0 times["postplace"] = times["place_end"] + 2.0 return X_G, times
def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf" ), "grasp") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", grasp) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) meshcat.load() diagram.Publish(context)
def test_grasp_pose(self): """Testing grasp pose""" f = self.notebook_locals['design_grasp_pose'] X_WO = self.notebook_locals['X_WO'] test_X_OG, test_X_WG = f(X_WO) R_OG = RotationMatrix(np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]).T) p_OG = [-0.02, 0, 0] X_OG = RigidTransform(R_OG, p_OG) X_WG = X_WO.multiply(X_OG) test_X_CW = f(X_WO) test_result = test_X_OG.multiply(X_OG.inverse()) test_result = test_result.GetAsMatrix4() self.assertTrue(np.allclose(test_result, np.eye(4))) test_result = test_X_WG.multiply(X_WG.inverse()) test_result = test_result.GetAsMatrix4() self.assertTrue(np.allclose(test_result, np.eye(4)))
def test_X_CW(self): """Testing X_CW""" f = self.notebook_locals['compute_X_CW'] # construct a test case theta1, theta2, theta3 = np.pi / 3.0, np.pi / 6.0, np.pi / 4.0 R_WA = RotationMatrix.MakeXRotation(theta1) R_AB = RotationMatrix.MakeZRotation(theta2) R_CB = RotationMatrix.MakeYRotation(theta3) X_WA = RigidTransform(R_WA, [0.1, 0.2, 0.5]) X_AB = RigidTransform(R_AB, [0.3, 0.4, 0.1]) X_CB = RigidTransform(R_CB, [0.5, 0.9, 0.7]) true_X_WC = X_WA.multiply(X_AB).multiply(X_CB.inverse()) true_X_CW = true_X_WC.inverse() test_X_CW = f(X_WA, X_AB, X_CB) test_result = true_X_CW.multiply(test_X_CW.inverse()) test_result = test_result.GetAsMatrix4() self.assertTrue(np.allclose(test_result, np.eye(4)))