leader_arm_controller

This Python code (`teleop_spacemouse_node.py`) is a ROS 2 node designed to control a robot arm using input from a 3D mouse (like a SpaceMouse). It involves coordinate transformations, robot control commands, and ROS 2 communication.

**Overall Functionality**

The node reads input from a SpaceMouse, converts it into robot arm movements, and publishes commands to control the arm and its gripper. It also handles coordinate frame transformations to ensure the robot moves as intended.

**Detailed Explanation**

**1. Imports**

The code starts by importing necessary Python libraries and ROS 2 modules:

* `rclpy`: The core ROS 2 Python client library.
* `rclpy.node`, `rclpy.publisher`, `rclpy.qos`: ROS 2 modules for creating nodes, publishers, and managing Quality of Service (QoS) settings.
* `sensor_msgs.msg`, `std_msgs.msg`, `geometry_msgs.msg`, `astra_controller_interfaces.msg`: ROS 2 message definitions for various data types (e.g., sensor data, poses, joint commands).
* `tf2_ros`: ROS 2 libraries for working with coordinate frame transformations.
* `numpy` (`np`):  A library for numerical operations, especially for handling matrices and arrays.
* `pytransform3d`: A Python library for 3D transformations and rotations.
* `modern_robotics` (`mr`): A library for robot kinematics.
* `mr_urdf_loader`:  A utility to load robot URDF files.
* `pathlib`, `ament_index_python`: Libraries for handling file paths and ROS 2 package resources.
* `astra_controller.experiments.spacemouse_agent`: A module specific to the robot setup, likely for interfacing with the SpaceMouse.
* `astra_controller.astra_controller`:  Another module related to the robot control.
* `astra_teleop_web.teleoprator`:  A module possibly related to web-based teleoperation.
* `math`, `time`: Standard Python libraries.

**2. QoS Profile**

```python
qos_profile_sensor_data_reliable = rclpy.qos.QoSProfile(**rclpy.impl.implementation_singleton.rclpy_implementation.rmw_qos_profile_t.predefined('qos_profile_sensor_data').to_dict())
qos_profile_sensor_data_reliable.reliability = 1

This section configures the Quality of Service (QoS) settings for ROS 2 communication. QoS determines how messages are delivered (e.g., reliability, history). Here, it sets up a qos_profile_sensor_data_reliable profile, making it reliable, which means the system will ensure messages are delivered.

3. main Function

The main function is the entry point of the ROS 2 node.

• Initialization

  rclpy.init(args=args)
  node = rclpy.node.Node("teleop_spacemouse_node")
  logger = node.get_logger()
  

  Initializes ROS 2, creates a node named "teleop_spacemouse_node", and gets a logger for outputting messages.

• pub_T Function

  def pub_T(pub: rclpy.publisher.Publisher, T, frame_id='base_link'):
      msg = geometry_msgs.msg.PoseStamped()
      msg.header.frame_id = frame_id
      msg.header.stamp = node.get_clock().now().to_msg()
      pq = pt.pq_from_transform(T)
      msg.pose.position.x = pq[0]
      msg.pose.position.y = pq[1]
      msg.pose.position.z = pq[2]
      msg.pose.orientation.w = pq[3]
      msg.pose.orientation.x = pq[4]
      msg.pose.orientation.y = pq[5]
      msg.pose.orientation.z = pq[6]
      pub.publish(msg)
  

  This function publishes a 4x4 transformation matrix (T) as a ROS 2 PoseStamped message. Here's a breakdown:

  • It takes a publisher (pub), a transformation matrix T, and a frame ID (frame_id) as input.
  • It creates a PoseStamped message, which includes a pose and a header with timestamp and frame ID.
  • pt.pq_from_transform(T) converts the 4x4 transformation matrix T into a position-quaternion representation (pq). A transformation matrix combines both rotation and translation. The position is a 3D vector, and the orientation is represented as a quaternion (w, x, y, z).
  • It populates the message with the position and quaternion.
  • It publishes the message.

• Publishers

  goal_pose_publisher = {}
  goal_pose_inactive_publisher = {}
  cam_pose_publisher = {}
  arm_gripper_joint_command_publisher = {}
  arm_joint_command_publisher = {}
  lift_joint_command_publisher = {}
  
  for side in ["left", "right"]:
      goal_pose_publisher[side] = node.create_publisher(geometry_msgs.msg.PoseStamped, f"{side}/goal_pose", 10)
      goal_pose_inactive_publisher[side] = node.create_publisher(geometry_msgs.msg.PoseStamped, f"{side}/goal_pose_inactive", 10)
      cam_pose_publisher[side] = node.create_publisher(geometry_msgs.msg.PoseStamped, f"{side}/cam_pose", 10)
  
      arm_gripper_joint_command_publisher[side] = node.create_publisher(astra_controller_interfaces.msg.JointCommand, f"{side}/arm/gripper_joint_command", 10)
      arm_joint_command_publisher[side] = node.create_publisher(astra_controller_interfaces.msg.JointCommand, f"{side}/arm/joint_command", 10)
      lift_joint_command_publisher[side] = node.create_publisher(astra_controller_interfaces.msg.JointCommand, f"{side}/lift/joint_command", 10)
  

  This code creates several ROS 2 publishers for sending data to different topics. It appears to handle control for both "left" and "right" sides (possibly for a dual-arm robot):

  • goal_pose_publisher: Publishes the desired end-effector pose.
  • goal_pose_inactive_publisher: Publishes a "goal pose inactive".
  • cam_pose_publisher: Publishes a camera pose.
  • arm_gripper_joint_command_publisher: Publishes commands to control the robot's gripper.
  • arm_joint_command_publisher: Publishes commands to control the robot's arm joints.
  • lift_joint_command_publisher: Publishes commands to control a lift joint.

• pub_goal_cb Function

  def pub_goal_cb(side, Tsgoal, Tscam=None, Tsgoal_inactive=None):
      if Tsgoal is not None:
          pub_T(goal_pose_publisher[side], Tsgoal)
      if Tsgoal_inactive is not None:
          pub_T(goal_pose_inactive_publisher[side], Tsgoal_inactive)
      if Tscam is not None:
          pub_T(cam_pose_publisher[side], Tscam)
  

  This is a callback function to publish goal poses and camera poses. It takes a side ("left" or "right") and transformation matrices as input and uses the pub_T function to publish them.

• pub_gripper_cb Function

  def pub_gripper_cb(side, gripper_open):
      arm_gripper_joint_command_publisher[side].publish(astra_controller_interfaces.msg.JointCommand(
          name=["joint_r7r" if side == "right" else "joint_l7r"],
          position_cmd=[gripper_open]
      ))
  

  This function publishes gripper control commands. It takes the side and a gripper opening value (gripper_open) and sends a JointCommand message.

• Transform Listener

  tf_buffer = Buffer()
  tf_listener = TransformListener(tf_buffer, node)
  
  # Wait until the transform from 'base_link' to 'link_ree_teleop' becomes available.
  while rclpy.ok():
      try:
          Tsgoal_msg: geometry_msgs.msg.TransformStamped = tf_buffer.lookup_transform('base_link', 'link_ree_teleop', rclpy.time.Time())
          break
      except Exception as e:
          logger.warn(str(e))
          # logger.warn("Transform lookup failed (base_link may not exist yet). Retrying...")
          rclpy.spin_once(node)
  
  Tsgoal = pt.transform_from_pq(np.array(pq_from_ros_transform(Tsgoal_msg.transform)))
  goal_gripper = GRIPPER_MAX
  

  This section handles coordinate frame transformations using tf2_ros.

  • It creates a TransformListener to listen for transformations.
  • It waits until the transformation from the 'base_link' frame to the 'link_ree_teleop' frame is available. This is crucial because it establishes the initial position and orientation of the robot's end-effector (where the SpaceMouse control starts from) relative to the robot's base. base_link is typically the robot's base frame, and link_ree_teleop is likely a frame associated with the end-effector and the teleoperation setup.
  • tf_buffer.lookup_transform retrieves this transformation.
  • It converts the ROS 2 transform message to a 4x4 transformation matrix (Tsgoal) using pt.transform_from_pq and pq_from_ros_transform.
  • It initializes goal_gripper to a maximum value.

• Spacemouse Agent and Control Loop

  agent = SpacemouseAgent()
  
  control_T = 0.01
  
  while True:
      action, buttons = agent.act(None)
  
      action[:3] *= 200.0 / 1000.0 * control_T # mm/s
      action[3:] *= 60 / 180 * math.pi * control_T # deg/s
  
      Tv = pt.transform_from(pr.matrix_from_euler(action[3:], 0, 1, 2, True), action[:3])
  
      # # Transform frame: cam
      # Tsgoal = Tsgoal @ Tv
  
      # Transform frame: base
      R = Tsgoal[:3, :3]
      p = Tsgoal[:3, 3]
      Tsgoal = pt.transform_from(np.eye(3), p) @ Tv @ pt.transform_from(R, np.zeros(3))
  
      if buttons[0]:
          goal_gripper += 100.0 / 1000.0 * control_T
          if goal_gripper > GRIPPER_MAX:
              goal_gripper = GRIPPER_MAX
      elif buttons[1]:
          goal_gripper -= 100.0 / 1000.0 * control_T
          if goal_gripper < 0:
              goal_gripper = 0
  
      pub_goal_cb("right", Tsgoal)
      pub_gripper_cb("right", goal_gripper)
  
      time.sleep(control_T)
  

  This is the main control loop of the node:

  • It creates an instance of SpacemouseAgent to handle SpaceMouse input.
  • control_T sets the control loop's time step (0.01 seconds).
  • The while True loop runs continuously:

    • agent.act(None) reads the SpaceMouse input, returning action (6D vector of translation and rotation) and buttons.

    • action[:3] *= 200.0 / 1000.0 * control_T scales the translational velocity from the SpaceMouse to mm/s.

    • action[3:] *= 60 / 180 * math.pi * control_T scales the rotational velocity from the SpaceMouse to rad/s.

    • Tv = pt.transform_from(pr.matrix_from_euler(action[3:], 0, 1, 2, True), action[:3]) creates a 4x4 transformation matrix Tv from the SpaceMouse input. action[:3] is the translation, and action[3:] (roll, pitch, yaw) is used to create a rotation matrix using Euler angles.

    • The core transformation logic is:

      R = Tsgoal[:3, :3]
      p = Tsgoal[:3, 3]
      Tsgoal_new = pt.transform_from(np.eye(3), p) @ Tv @ pt.transform_from(R, np.zeros(3))
      

      This updates the goal pose (Tsgoal) based on the SpaceMouse input (Tv). It decomposes the current goal pose into rotation (R) and position (p), calculates the new transformation, and updates Tsgoal. Let's break this down with a mathematical representation:

      • Tsgoal: Current transformation matrix representing the goal pose of the robot's end-effector.
      • R: 3x3 rotation matrix extracted from Tsgoal.
      • p: 3x1 position vector extracted from Tsgoal.
      • Tv: 4x4 transformation matrix representing the change in pose commanded by the SpaceMouse.
      • pt.transform_from(np.eye(3), p): Creates a transformation matrix representing only the translation p.
      • pt.transform_from(R, np.zeros(3)): Creates a transformation matrix representing only the rotation R.

      The update equation is:

      $$T_{goal_new} = T_{base_goal} \cdot T_v \cdot R_{goal_old}$$

      Where:

      • T_goal_new: The new goal transformation matrix.
      • T_base_goal: A transformation matrix representing the current position (p) of the goal, but with no rotation (identity rotation).
      • T_v: The transformation matrix from the SpaceMouse input.
      • R_goal_old: A transformation matrix representing the current rotation (R) of the goal, but with no translation (zero translation).

      This way, the translation from the SpaceMouse is applied in the base frame, while the rotation is applied relative to the current orientation.

    • The code then checks for button presses on the SpaceMouse to control the gripper:

      • Button 0 opens the gripper.
      • Button 1 closes the gripper.

    • pub_goal_cb and pub_gripper_cb publish the updated goal pose and gripper commands.

    • time.sleep(control_T) introduces a delay to control the loop rate.

• Shutdown

  rclpy.spin(node)
  
  # Destroy the node explicitly
  node.destroy_node()
  rclpy.shutdown()
  

  • rclpy.spin(node) keeps the node alive and listening for callbacks (though it's not reached in the current while True loop). It's more typical to use rclpy.spin to handle callbacks for subscriptions.
  • node.destroy_node() and rclpy.shutdown() properly clean up the ROS 2 node and resources.

• Entry Point

  if __name__ == '__main__':
      main()
  

  This ensures that the main function is executed when the script is run.

Summary

This code sets up a ROS 2 node that:

1. Receives 6D motion input from a SpaceMouse.
2. Transforms this motion into desired end-effector poses for a robot arm.
3. Publishes these pose commands and gripper control commands to the appropriate ROS 2 topics.
4. Uses tf2_ros to manage coordinate frame transformations, ensuring the robot's movement is correctly interpreted.

The core of the code lies in the transformation update logic, where the SpaceMouse input is used to incrementally adjust the robot's goal pose. The separation of translation and rotation in the transformation update is important for controlling how the robot moves and rotates in 3D space.

math

The core of the robot's pose update is performed by this formula:

$$T_{goal_new} = T_{base_goal} \cdot T_v \cdot R_{goal_old}$$

To fully understand this, we need to break down each component:

• T_goal_new: This is the new 4x4 homogeneous transformation matrix that represents the desired pose (position and orientation) of the robot's end-effector after applying the SpaceMouse's input. It's what the code calculates in each iteration of the control loop.

• T_base_goal: This 4x4 matrix represents the translational part of the current goal pose, but without any rotation. It's constructed in the code as:

  pt.transform_from(np.eye(3), p)
  

  • p is a 3x1 vector representing the position (x, y, z) extracted from the current goal pose (Tsgoal).
  • np.eye(3) creates a 3x3 identity matrix, representing no rotation.
  • pt.transform_from is a function from the pytransform3d library that constructs a 4x4 homogeneous transformation matrix from a rotation matrix and a translation vector. In this case, it creates a matrix that only translates (moves) the end-effector without rotating it.

  This matrix has the form:

  [ 1  0  0  x ]
  [ 0  1  0  y ]
  [ 0  0  1  z ]
  [ 0  0  0  1 ]
  

  Where x, y, and z are the coordinates of the current goal position.

• T_v: This 4x4 matrix represents the transformation (translation and rotation) commanded by the SpaceMouse. It's calculated in the code as:

  Tv = pt.transform_from(pr.matrix_from_euler(action[3:], 0, 1, 2, True), action[:3])
  

  • action is a 6x1 vector received from the SpaceMouse, where action[:3] represents the translational velocity (dx, dy, dz) and action[3:] represents the rotational velocity (roll, pitch, yaw).
  • pr.matrix_from_euler(action[3:], 0, 1, 2, True) converts the rotational velocity (Euler angles) into a 3x3 rotation matrix. The 0, 1, 2 specify the order of the Euler angles (x, y, z), and True indicates that the input is an extrinsic rotation (rotation about fixed axes).
  • pt.transform_from then combines this rotation matrix and the translational velocity (action[:3]) to create the 4x4 transformation matrix Tv.

  This matrix represents the change in pose that the SpaceMouse is commanding.

• R_goal_old: This 4x4 matrix represents the rotational part of the current goal pose, but without any translation. It's constructed in the code as:

  pt.transform_from(R, np.zeros(3))
  

  • R is the 3x3 rotation matrix extracted from the current goal pose (Tsgoal).
  • np.zeros(3) creates a 3x1 zero vector, representing no translation.
  • pt.transform_from creates a 4x4 homogeneous transformation matrix from the rotation matrix R and a zero translation vector.

  This matrix has the form:

  [ r11 r12 r13 0 ]
  [ r21 r22 r23 0 ]
  [ r31 r32 r33 0 ]
  [  0   0   0  1 ]
  

  Where r11 to r33 are the elements of the rotation matrix R.

Explanation of the Formula

The formula

$$T_{goal_new} = T_{base_goal} \cdot T_v \cdot R_{goal_old}$$

effectively updates the robot's goal pose in a way that:

1. Translates in the base frame: T_base_goal . T_v This part first applies the SpaceMouse's transformation (T_v) as if the robot were starting from its base frame's origin, but only considers the translation component of the current goal. This means the translational movement from the SpaceMouse is applied relative to the robot's base.

2. Rotates relative to the current orientation: (... ) . R_goal_old Then, it multiplies the result by R_goal_old. This applies the current rotation of the end-effector to the result from the previous step. The rotational part of the SpaceMouse input (T_v) is thus applied relative to the robot's current orientation, not the base frame.

Why this approach?

This approach provides more intuitive control. If you were to simply do Tsgoal = Tsgoal @ Tv, both the translation and rotation from the SpaceMouse would be applied relative to the current pose of the end-effector. This can lead to less predictable and harder-to-control movements, especially for the translational part.

By separating the translation and rotation and applying the translation in the base frame, the control becomes more aligned with how a user might intuitively expect a robot to respond to 3D mouse input. Moving the mouse "forward" translates the end-effector in the general direction it's currently facing, rather than moving it relative to its current rotated coordinate frame.

# teleop_leader_arm_node