Tested with UR

arm_teleop/avp_teleop_node.py

@@ -34,17 +34,17 @@ class AVPTeleopNode(Node):
         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.franka_cartesian_pose_subscriber = self.create_subscription(Float64MultiArray, '/franka/pose', self.cart_pos_callback, rclpy.qos.qos_profile_sensor_data)
+        self.avp_wrist_pose_subscriber = self.create_subscription(Float64MultiArray, '/wrist/avp_demand', self.avp_demand_callback, rclpy.qos.qos_profile_sensor_data)
         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)
+        self.franka_pose_publisher = self.create_publisher(Float64MultiArray, '/franka/pose_demand', rclpy.qos.qos_profile_sensor_data)
+        self.wrist_servo_demand_publisher = self.create_publisher(Float64MultiArray, '/wrist/servo_demand', rclpy.qos.qos_profile_sensor_data)
 
         # Gamepad
         self.gp = GamepadFunctions()
-        self.gamepad_subscriber = self.create_subscription(Joy, '/gamepad', self.gamepad_callback, 10)
+        self.gamepad_subscriber = self.create_subscription(Joy, '/gamepad', self.gamepad_callback, rclpy.qos.qos_profile_sensor_data)
 
         # Other
         self.hand_regulator = HandRegulator()
@@ -247,8 +247,8 @@ class AVPTeleopNode(Node):
         base = 0.3; reach = -2; max_damp = 0.5
         trans_damp = min(base + (1-base) * (1- np.exp(reach * trans_speed)), max_damp)
         
-        # base = 0.8; reach = -2; max_damp = 0.9
-        base = 0.4; reach = -2; max_damp = 0.6
+        base = 0.7; reach = -2; max_damp = 0.8
+        # base = 0.4; reach = -2; max_damp = 0.6
         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:]]))
@@ -260,7 +260,7 @@ class AVPTeleopNode(Node):
             if i < 3:
                 franka_demand.append(starting_franka_pose[i] * trans_damp + trans_demand[i] * (1-trans_damp))
             else:
-                franka_demand.appentarget_hand_orientationd(target_hand_orientation[i-3])
+                franka_demand.append(target_hand_orientation[i-3])
                 # franka_demand.append(self.static_franka_pose[i])
 
         # names = ["x", "y", "z", "qx", "qy", "qz", "qw"]
@@ -274,6 +274,7 @@ class AVPTeleopNode(Node):
                 msg = Float64MultiArray()
                 msg.data = franka_demand
                 self.franka_pose_publisher.publish(msg)
+                self.get_logger().info("publishing")
 
 def main():
     rclpy.init()