[feat] add command 7 - grasp pose from camera

robot.py

@@ -93,12 +93,13 @@ class Franka:
         trajectory_msg.joint_names = self.config.reset.joint_names
         point_msg = JointTrajectoryPoint()
         point_msg.positions = self.config.reset.joint_positions
-        # Take 3 seconds to reach goal state (1 is not working)
-        point_msg.time_from_start = rospy.Duration.from_sec(3)
+        # Take K seconds to reach goal state (1 is not working)
+        K_sec = 10
+        point_msg.time_from_start = rospy.Duration.from_sec(K_sec)
 
         trajectory_msg.points.append(point_msg)
         self.pub_control_franka_joint.publish(trajectory_msg)
-        rospy.sleep(4)
+        rospy.sleep(K_sec+4)
 
         # Stop the reset controller and start the original controller
         self._switch_controller(current=self.config.reset.controller.name,
@@ -486,7 +487,7 @@ if __name__ == '__main__':
     franka = Franka(config)
     while True:
 
-        cmd = int(input("\n[0: Go to / 1: Open / 2: Close / 3: Reset / 4: Get info/ 5: EXIT/ 6: EFF Pose] CMD: "))
+        cmd = int(input("\n[0: Go to / 1: Open / 2: Close / 3: Reset / 4: Get info/ 5: EXIT / 6: EFF Pose / 7: Go to pose (camera)] CMD: "))
         if cmd == 0:
             # Delta
             delta_pose = list(map(float, input("Delta Pose (0.05 0 0.05 0 15 45): ").split()))
@@ -552,9 +553,105 @@ if __name__ == '__main__':
             exit()
 
         elif cmd == 6:
+            # QBSoftHand top-down grasping
             eef_pose = list(map(float, input("EEF Pose (0.415 -0.043  0.097): ").split()))
             desired_position = np.array(eef_pose)
             desired_euler_angles = np.array([-179.5, -1., 1.]) * np.pi / 180.
             print(f"desired_position: {desired_position}")
             input("GO?")
             franka.go_to(desired_position, desired_euler_angles, step_len=0.00005)
+
+        elif cmd == 7:
+            # QBSoftHand, grasp pose estimaton
+            grasp_pose = list(map(float, input("Grasp pose in camera frame (0.133 0.055 0.627 28 0 90): ").split()))
+            grasp_pose = np.array(grasp_pose)
+
+            # Target
+            cam_T_g = np.identity(4)
+            cam_T_g[:3, 3] = grasp_pose[:3]
+            cam_T_g[:3, :3] = R.from_euler(
+                'XYZ', grasp_pose[3:] * np.pi / 180).as_matrix()
+
+            # r_T_cam (calibration)
+            r_T_cam = np.identity(4)
+            r_T_cam[:3, 3] = np.array([0.45313028018862256, 0.6860418429753554, 0.4662189100115641])
+            ori = R.from_quat([-0.00955287142875244, -0.8567794707355275, 0.513465402320942, 0.04680771082854044]).as_matrix()
+            r_T_cam[:3, :3] = ori
+
+            # r_T_g
+            r_T_g = np.dot(r_T_cam, cam_T_g)
+            print("r_T_grasp =\n", r_T_g)
+
+            # g_T_hand
+            g_T_hand = np.identity(4)
+            g_T_hand[:3, 3] = np.array([0., 0., 0.])
+            ori = R.from_euler(
+                'XYZ', np.array([-90, 90, 0])*np.pi/180).as_matrix()
+            g_T_hand[:3, :3] = ori
+
+            # r_T_hand
+            r_T_hand = np.dot(r_T_g, g_T_hand)
+            # r_T_hand[:3, 3] = r_T_hand[:3, 3] + np.array([0.05, 0.05, -0.01])     # 2-finger gripper
+            # r_T_hand[:3, 3] = r_T_hand[:3, 3] + np.array([0.0, 0.0, 0.05])       # QB hand
+
+            # hand_T_point (use this to shift the grasp point --- adjust hand)
+            hand_T_point = np.identity(4)
+            hand_T_point[:3, 3] = np.array([0.09, -0.13, -0.01])
+            # hand_T_point[:3, 3] = np.array([0.0, -0.1, -0.0])
+            ori = R.from_euler(
+                'XYZ', np.array([0, 0, 45])*np.pi/180).as_matrix()
+            hand_T_point[:3, :3] = ori
+            r_T_point = np.dot(r_T_hand, hand_T_point)
+
+            desired_position = r_T_point[:3, 3]
+            desired_euler_angles = R.from_matrix(r_T_point[:3, :3]).as_euler('XYZ')
+            desired_euler_angles[2] += (np.pi/2)
+            # print("desired_position     =", desired_position)
+            # print("desired_euler_angles =", desired_euler_angles *180 / np.pi)
+
+            # Adjust the angles
+            for axis in range(3):
+                if desired_euler_angles[axis] > np.pi:
+                    desired_euler_angles[axis] -= (2 * np.pi)
+
+                elif desired_euler_angles[axis] < -np.pi:
+                    desired_euler_angles[axis] += (2 * np.pi)
+
+            # print("desired_position     =", desired_position)
+            # print("desired_euler_angles =", desired_euler_angles *180 / np.pi)
+
+            t, r = desired_position, desired_euler_angles * 180 / np.pi
+            # desired_position[2] = max(desired_position[2], 0.05)
+            desired_position[2] = max(desired_position[2], 0.09)
+            rospy.loginfo(f"\n\nDesired pose: \n    {t}, \n    {r}")
+            input("GO?")
+            franka.go_to(desired_position, desired_euler_angles, step_len=0.00001)
+            cmd = input("Grasp: ")
+            if cmd == '' or cmd == '1':
+                franka.close_fingers()
+
+            cmd = input("Go up: ")
+            if cmd == '' or cmd == '1':
+                pos = np.array([0.4, 0.1, 0.45])
+                euler_angles = np.array([-np.pi, 0, 0])
+                franka.go_to(pos, euler_angles, step_len=0.00001)
+
+                pos = np.array([0.4, -0.30, 0.45])
+                euler_angles = np.array([-np.pi, 0, 0])
+                franka.go_to(pos, euler_angles, step_len=0.00001)
+
+            cmd = input("Success Reset: ")
+            if cmd == '' or cmd == '1':
+                # pos = np.array([0.4, 0.1, 0.17])
+                pos = np.array([0.4, -0.30, 0.17])
+                euler_angles = np.array([-np.pi, 0, 0])
+                franka.go_to(pos, euler_angles, step_len=0.00001)
+                rospy.sleep(1)
+                franka.open_fingers()
+                rospy.sleep(1)
+                # pos = np.array([0.4, 0.1, 0.35])
+                pos = np.array([0.4, 0., 0.35])
+                euler_angles = np.array([-np.pi, 0, 0])
+                franka.go_to(pos, euler_angles, step_len=0.00001)
+
+            franka.open_fingers()