Some working version of AVP teleop for arm.

arm_teleop/avp_teleop_node.py

+from geometry_msgs.msg import TransformStamped, PoseStamped
+
+import numpy as np
+from sensor_msgs.msg import Joy
+import time
+
+from std_msgs.msg import Float32MultiArray, Int32MultiArray, Float64MultiArray
+from .teleop_helper_functions import broadcast_tf, get_relative_quaternion
+from helper_functions.gamepad_functions import GamepadFunctions
+
+from scipy.spatial.transform import Rotation as R
+from scipy.spatial.transform import Slerp
+
+import rclpy
+from rclpy.node import Node
+
+from copy import deepcopy as cp
+
+from helper_functions.hand_regulator import HandRegulator
+
+from tf2_ros import TransformBroadcaster
+
+
+class AVPTeleopNode(Node):
+
+    def __init__(self):
+        super().__init__('avp_teleop_node')
+
+        # tf related
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        # Main loop
+        self.mainloop = self.create_timer(0.05, self.mainloop_callback) 
+
+        # Subscriber
+        self.franka_cartesian_pose_subscriber = self.create_subscription(Float64MultiArray, '/franka/pose', self.cart_pos_callback, 10)
+        self.avp_wrist_pose_subscriber = self.create_subscription(Float64MultiArray, '/wrist/avp_demand', self.avp_demand_callback, 10)
+        self.hand_wrist_servo_position_subscriber = self.create_subscription(Float64MultiArray, '/hand_wrist/servo_positions',self.hand_wrist_servo_callback, 10)
+
+        # Publisher
+        self.franka_pose_publisher = self.create_publisher(Float64MultiArray, '/franka/pose_demand', 10)
+        self.wrist_servo_demand_publisher = self.create_publisher(Float64MultiArray, '/wrist/servo_demand', 10)
+
+        # Gamepad
+        self.gp = GamepadFunctions()
+        self.gamepad_subscriber = self.create_subscription(Joy, '/gamepad', self.gamepad_callback, 10)
+
+        # Other
+        self.hand_regulator = HandRegulator()
+
+        # Variables
+        self.avp_wrist_q = None
+        self.avp_wrist_t = None
+        self.static_avp_t = None
+        self.raw_q = None
+        self.raw_t = None
+        self.translation_origin = None
+        self.current_franka_pose = None
+        self.static_franka_pose = None
+        self.translation_demand = None
+        self.simulated_robot_pose = None
+        self.left = None
+        self.right = None
+
+        # Parameters
+        self.is_sim = False
+        self.is_pressed = False
+
+    def gamepad_callback(self, gamepad_raw_msg:Joy):
+        self.gp.convert_joy_msg_to_dictionary(gamepad_raw_msg)
+
+        if self.is_pressed == False and self.gp.button_data["L1"] == 1:
+            self.reset_pose()
+            self.is_pressed = True
+        if self.gp.button_data["L1"] == 0:
+            self.is_pressed = False
+
+    def hand_wrist_servo_callback(self, msg:Float64MultiArray):
+        self.left = msg.data[13]
+        self.right = msg.data[14]
+
+    def cart_pos_callback(self, msg:Float64MultiArray):
+        self.current_franka_pose = msg
+
+    def reset_pose(self):
+        self.static_avp_t = cp(self.avp_wrist_t)
+        self.static_franka_pose = cp(self.current_franka_pose.data)
+        self.simulated_robot_pose = cp(self.current_franka_pose.data)
+
+    def avp_demand_callback(self, msg:Float64MultiArray):
+        if self.current_franka_pose is None:
+            return
+        
+        raw_t = np.array([msg.data[0], msg.data[1], msg.data[2]])
+        raw_q = np.array([msg.data[3], msg.data[4], msg.data[5], msg.data[6]])
+
+        base_rot = R.from_euler("z", -90, degrees=True)
+        base_rot_m = base_rot.as_matrix()
+
+        self.avp_wrist_t = np.matmul(base_rot_m, raw_t)
+        avp_wrist_m = np.matmul(base_rot_m, R.from_quat(raw_q).as_matrix())
+        self.avp_wrist_q = R.from_matrix(avp_wrist_m).as_quat()
+
+        if self.static_avp_t is None:
+            self.reset_pose()
+
+        self.translation_demand = []
+        for i in range(3):
+            self.translation_demand.append(self.avp_wrist_t[i] - self.static_avp_t[i] + self.static_franka_pose[i])
+
+    def arm_to_hand_base(self, arm_coord):
+        output = cp(arm_coord)
+
+        ee_m = R.from_quat(output[3:]).as_matrix()
+        trans_vec = np.matmul(ee_m, [0, 0, 0.1])
+
+        adjust_rot_m1 = R.from_euler("z", 135, degrees=True).as_matrix()
+        adjust_rot_m2 = R.from_euler("y", 90, degrees=True).as_matrix()
+        hand_m = np.matmul(np.matmul(ee_m, adjust_rot_m1), adjust_rot_m2)
+        hand_q = R.from_matrix(hand_m).as_quat()
+
+        for i in range(3):
+            output[i] += trans_vec[i]
+
+        for i in range(4):
+            output[3+i] = hand_q[i]
+
+        return output
+    def hand_base_to_wirst(self, hand_base):
+        ee_m = R.from_quat(hand_base[3:]).as_matrix()
+        pitch, yaw = self.hand_regulator.wrist_motor_pos_to_pitch_yaw(self.left, self.right)
+        
+        pitch_m = R.from_euler("z", pitch, degrees=True).as_matrix()
+        raw_m = R.from_euler("y", yaw, degrees=True).as_matrix()
+        wrist_m = np.matmul(np.matmul(ee_m, pitch_m), raw_m)
+        wrist_q = R.from_matrix(wrist_m).as_quat()
+
+        return wrist_q
+
+    def hand_to_arm(self, hand_coord):
+        output = cp(hand_coord)
+        ee_m = R.from_quat(output[3:]).as_matrix()
+
+        adjust_rot_m1 = R.from_euler("y", -90, degrees=True).as_matrix()
+        adjust_rot_m2 = R.from_euler("z", -135, degrees=True).as_matrix()
+        hand_rotated_m = np.matmul(np.matmul(ee_m, adjust_rot_m1), adjust_rot_m2)
+
+        trans_vec = np.matmul(hand_rotated_m, [0, 0, -0.1])
+        hand_q = R.from_matrix(hand_rotated_m).as_quat()
+
+        for i in range(3):
+            output[i] += trans_vec[i]
+
+        for i in range(4):
+            output[3+i] = hand_q[i]
+
+        return output
+
+    def get_wrist_arm_coord(self, ee_m, pitch, yaw):
+        pitch_m = R.from_euler("z", pitch, degrees=True).as_matrix()
+        raw_m = R.from_euler("y", yaw, degrees=True).as_matrix()
+        wrist_m = np.matmul(np.matmul(ee_m, pitch_m), raw_m)
+        wrist_q = R.from_matrix(wrist_m).as_quat()
+
+        residual_m = get_relative_quaternion(wrist_q, self.avp_wrist_q).as_matrix()
+
+        residual_wrt_ee_m = np.matmul(ee_m, residual_m)
+        residual_wrt_ee_q = R.from_matrix(residual_wrt_ee_m).as_quat()
+
+        return wrist_q, residual_wrt_ee_q
+    
+    def command_wrist(self, pitch_demand ,yaw_demand):
+        d_left, d_right = self.hand_regulator.pitch_yaw_to_wrist_motor_pos(pitch_demand, yaw_demand)
+        demand = {"Wrist_Left":d_left + self.left, "Wrist_Right":d_right + self.right}
+        limited = self.hand_regulator.check_wrist_pitch_yaw_limit(demand)
+
+        demand_left = limited["Wrist_Left"]
+        demand_right = limited["Wrist_Right"]
+
+        msg = Float64MultiArray()
+        msg.data = [demand_left, demand_right]
+        self.wrist_servo_demand_publisher.publish(msg)
+
+        return self.hand_regulator.wrist_motor_pos_to_pitch_yaw(demand_left, demand_right)
+
+    def mainloop_callback(self):
+        timer = time.time()
+        dt = 0.05
+
+        if (self.static_avp_t is None) or (self.left is None):
+            return
+
+        if self.is_sim:
+            starting_franka_pose = cp(self.simulated_robot_pose)
+            hand_base = self.arm_to_hand_base(self.simulated_robot_pose)
+        else:
+            starting_franka_pose = cp(self.current_franka_pose.data)
+            hand_base = self.arm_to_hand_base(self.current_franka_pose.data)
+        
+        current_hand_quat = self.hand_base_to_wirst(hand_base)
+
+        current_hand_trans = hand_base[:3]
+    
+        diff = get_relative_quaternion(current_hand_quat, self.avp_wrist_q).as_euler("zyx", degrees = True)
+
+        # Get current pitch yaw and see viability
+        applied_pitch, applied_yaw = self.command_wrist(diff[0], diff[1])
+
+        wrist_q, residual_q = self.get_wrist_arm_coord(R.from_quat(current_hand_quat).as_matrix(), applied_pitch, applied_yaw)
+
+        panda_demand = self.hand_to_arm([current_hand_trans[0], current_hand_trans[1], current_hand_trans[2], 
+                                         residual_q[0], residual_q[1], residual_q[2], residual_q[3]])
+
+        broadcast_tf(self, "world", "robo_coord_wrist", self.translation_demand, self.avp_wrist_q)
+        broadcast_tf(self, "world", "hand", current_hand_trans, current_hand_quat)
+        broadcast_tf(self, "world", "wrist", current_hand_trans, wrist_q)
+        broadcast_tf(self, "world", "residual", current_hand_trans, residual_q)
+        broadcast_tf(self, "world", "panda_demand", panda_demand[:3], panda_demand[3:])
+
+        if self.is_sim:
+            broadcast_tf(self, "world", "robot sim", self.simulated_robot_pose[:3], self.simulated_robot_pose[3:])
+
+        trans_demand = np.asarray(self.translation_demand) + np.asarray(panda_demand[:3]) - np.asarray(starting_franka_pose[:3])
+        trans_delta = trans_demand - np.asarray(starting_franka_pose[:3])
+        rot_delta = get_relative_quaternion(panda_demand[3:], starting_franka_pose[3:])
+
+        trans_speed = np.linalg.norm(trans_delta)/dt
+        rot_speed = np.degrees(rot_delta.magnitude())/dt
+
+        # base = 0.7; reach = -3; max_damp = 0.95
+        base = 0.2; reach = -2; max_damp = 0.6
+        trans_damp = min(base + (1-base) * (1- np.exp(reach * trans_speed)), max_damp)
+        
+        # base = 0.8; reach = -3; max_damp = 0.95
+        base = 0.3; reach = -2; max_damp = 0.7
+        rot_damp = min(base + (1-base) * (1- np.exp(reach * rot_speed)), max_damp)
+        
+        slerp = Slerp([0, 1], R.from_quat([panda_demand[3:], starting_franka_pose[3:]]))
+        target_R = slerp([rot_damp])
+        target_hand_orientation = target_R.as_quat()[0]
+
+        franka_demand = []
+        for i in range(7):
+            if i < 3:
+                franka_demand.append(starting_franka_pose[i] * trans_damp + trans_demand[i] * (1-trans_damp))
+            else:
+                franka_demand.append(target_hand_orientation[i-3])
+
+        # names = ["x", "y", "z", "qx", "qy", "qz", "qw"]
+        # for name, value in zip(names, franka_demand):
+        #     print(name + ": ", round(value, 4))
+            
+        if self.gp.button_data["L1"] == 1:
+            if self.is_sim:
+                self.simulated_robot_pose = cp(franka_demand)
+            else:
+                msg = Float64MultiArray()
+                msg.data = franka_demand
+                self.franka_pose_publisher.publish(msg)
+        print(time.time() - timer)
+
+def main():
+    rclpy.init()
+    node = AVPTeleopNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
+    rclpy.shutdown()
+
+

setup.py

@@ -23,7 +23,8 @@ setup(
             'arm_teleop_node = arm_teleop.arm_teleop_node:main',
             'orientation_node = arm_teleop.orientation_node:main',
             'translation_node = arm_teleop.translation_node:main',
-            'imu_suite_node = arm_teleop.imu_suite_node:main'
+            'imu_suite_node = arm_teleop.imu_suite_node:main',
+            'avp_teleop_node = arm_teleop.avp_teleop_node:main'
         ],
     },
 )