Merge pull request #154 from hello-robot/feature/streaming_position

Streaming Position Controller

hello_helpers/src/hello_helpers/hello_misc.py

@@ -18,6 +18,7 @@
 import ros2_numpy
 
 import pyquaternion
+from statistics import mean
 
 from rclpy.action import ActionClient
 from control_msgs.action import FollowJointTrajectory
@@ -626,3 +627,35 @@ def preprocess_gripper_trajectory(trajectory):
     gripper_index = trajectory.joint_names.index(present_gripper_joints[0])
     trajectory.joint_names[gripper_index] = 'stretch_gripper'
     return trajectory
+
+
+class LoopTimer:
+    def __init__(self, name="Loop", print_debug = False, print_interval=.5):
+        self.name = name
+        self.print_interval = print_interval
+        self.start_time = time.time()
+        self.last_print_time = self.start_time
+        self.iterations = 0
+        self.print_debug = print_debug
+        self.last_update_time = self.start_time
+
+    def reset(self):
+        self.iterations = 0
+        self.start_time = time.time()
+
+    def update(self):
+        self.iterations += 1
+        current_time = time.time()
+        self.last_update_time = current_time
+        elapsed_time = current_time - self.last_print_time
+        total_elapsed_time = current_time - self.start_time
+        avg_rate = self.iterations / total_elapsed_time
+        if elapsed_time >= self.print_interval:
+            if self.print_debug:
+                print("-"*20)
+                print(f"{self.name} \n"
+                    f"Iteration: {self.iterations}\n"
+                    f"Elapsed Time: {total_elapsed_time:.2f}s\n"
+                    f"Average Rate: {avg_rate:.2f} Hz")
+                print("-"*20)
+            self.last_print_time = current_time

hello_helpers/src/hello_helpers/joint_qpos_conversion.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+
+class UnsupportedToolError(Exception):
+    pass
+
+def get_Idx(tool_name):
+    """
+    Returns the Idx class corresponding to the tool name.
+    """
+    if tool_name == 'eoa_wrist_dw3_tool_sg3':
+        return SE3_dw3_sg3_Idx
+    elif tool_name == 'eoa_wrist_dw3_tool_nil':
+        return eoa_wrist_dw3_tool_nil_Idx
+    elif tool_name == 'eoa_wrist_dw3_tool_tablet_12in':
+        return eoa_wrist_dw3_tool_tablet_12in_Idx
+    elif tool_name == 'tool_stretch_dex_wrist':
+        return tool_stretch_dex_wrist_Idx
+    elif tool_name == 'tool_stretch_gripper':
+        return tool_stretch_gripper_Idx
+    elif tool_name == 'tool_none':
+        return tool_none_Idx
+    else:
+        raise UnsupportedToolError('Undefined tool name in QposConversion.')
+
+class SE3_dw3_sg3_Idx:
+    LIFT = 1
+    ARM = 0
+    GRIPPER = 7
+    WRIST_ROLL = 4
+    WRIST_PITCH = 3
+    WRIST_YAW = 2
+    HEAD_PAN = 5
+    HEAD_TILT = 6
+    BASE_TRANSLATE = 8
+    BASE_ROTATE = 9
+
+    num_joints = 10
+
+class eoa_wrist_dw3_tool_nil_Idx:
+    LIFT = 1
+    ARM = 0
+    WRIST_ROLL = 4
+    WRIST_PITCH = 3
+    WRIST_YAW = 2
+    HEAD_PAN = 5
+    HEAD_TILT = 6
+    BASE_TRANSLATE = 7
+    BASE_ROTATE = 8
+
+    num_joints = 9
+
+class eoa_wrist_dw3_tool_tablet_12in_Idx:
+    LIFT = 1
+    ARM = 0
+    WRIST_ROLL = 4
+    WRIST_PITCH = 3
+    WRIST_YAW = 2
+    HEAD_PAN = 5
+    HEAD_TILT = 6
+    BASE_TRANSLATE = 7
+    BASE_ROTATE = 8
+
+    num_joints = 9
+
+class tool_stretch_dex_wrist_Idx:
+    LIFT = 1
+    ARM = 0
+    GRIPPER = 7
+    WRIST_ROLL = 4
+    WRIST_PITCH = 3
+    WRIST_YAW = 2
+    HEAD_PAN = 5
+    HEAD_TILT = 6
+    BASE_TRANSLATE = 8
+    BASE_ROTATE = 9
+
+    num_joints = 10
+
+class tool_stretch_gripper_Idx:
+    LIFT = 1
+    ARM = 0
+    GRIPPER = 3
+    WRIST_YAW = 2
+    HEAD_PAN = 4
+    HEAD_TILT = 5
+    BASE_TRANSLATE = 6
+    BASE_ROTATE = 7
+
+    num_joints = 8
+
+class tool_none_Idx:
+    LIFT = 1
+    ARM = 0
+    WRIST_YAW = 2
+    HEAD_PAN = 3
+    HEAD_TILT = 4
+    BASE_TRANSLATE = 5
+    BASE_ROTATE = 6
+
+    num_joints = 7
+
+class JointStateMapping:
+    """
+    Mapping of ROS joint names to the corresponding joint names in the robot.
+    """
+    ROS_ARM_JOINTS = ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3"]
+    ROS_LIFT_JOINT = "joint_lift"
+    ROS_GRIPPER_FINGER = "joint_gripper_finger_left"
+    ROS_HEAD_PAN = "joint_head_pan"
+    ROS_HEAD_TILT = "joint_head_tilt"
+    ROS_WRIST_YAW = "joint_wrist_yaw"
+    ROS_WRIST_PITCH = "joint_wrist_pitch"
+    ROS_WRIST_ROLL = "joint_wrist_roll"

stretch_core/API.md

@@ -32,9 +32,11 @@ default: stretch_core/config/controller_calibration_head.yaml
 `/magnetometer_mobile_base`: Publishes magnetometer data fromt he Pimu through a message of type sensor_msgs.msg.MagneticField
 `/imu_wrist`: Publishes IMU data from the Wacc through a message of type sensor_msgs.msg.Imu
 `/joint_states`: Publishes joint positions through a message of type sensor_msgs.msg.JointState
+`/is_streaming_position`: Publish whether streamig position mode is On
 
 #### Subscribed Topics
 `/cmd_vel`: Translational and rotational velocities through a message of type geometry_msgs.msg.Twist
+`/joint_pose_cmd`: Joint pose vector to execute with Streaming position controller turned On.
 
 #### Exposed Services
 `/switch_to_position_mode`: change mode to position mode
@@ -43,6 +45,8 @@ default: stretch_core/config/controller_calibration_head.yaml
 `/stop_the_robot`: stop the robot
 `/home_the_robot`: home the robot
 `/runstop`: switches the robot to standby mode where it will ignore new commands
+`/activate_streaming_position`: activates the streaming position mode
+`/deactivate_streaming_position`: deactivates the streaming position mode
 
 #### Exposed Action Servers
 `/stretch_controller/follow_joint_trajectory`: action server to control the robot through joint trajectories

stretch_core/README.md

@@ -46,6 +46,10 @@ If set to true, motion action servers will fail on out-of-range commands.
 
 Can be set to `position`, `navigation`, and `trajectory` modes.
 
+##### Streming Position Control
+When Streaming position control is activated, publish goal Joint pose as a `Float64MultiArray`. Streaming position control can be activated and deactivated by calling the services `/activate_streaming_position` and `/deactivate_streaming_position`. The current activation state is published in topic `/is_streaming_position`.
+This feature can be used in `position` and `navigation` modes. [More info on framing Goal pose array](https://github.com/hello-robot/stretch_ros2/pull/154#issue-2449787145).
+
 ##### calibrated_controller_yaml_file
 
 Path to the calibrated controller args file

stretch_core/stretch_core/joint_trajectory_server.py

@@ -122,7 +122,8 @@ def execute_cb(self, goal_handle):
         if self.node.robot_mode not in ['position', 'trajectory', 'navigation']:
             self.node.robot_mode_rwlock.release_read()
             return self.error_callback(goal_handle, FollowJointTrajectory.Result.INVALID_GOAL, "Cannot execute goals while in mode={0}".format(self.node.robot_mode))
-
+        if self.node.streaming_position_activated:
+            return self.error_callback(goal_handle, FollowJointTrajectory.Result.INVALID_GOAL, "Cannot execute goals while Streaming Position Controller is activated.".format(self.node.robot_mode))
         if self.node.robot_mode in ['position','navigation']:
             # For now, ignore goal time and configuration tolerances.
             commanded_joint_names = goal.trajectory.joint_names

stretch_core/stretch_core/stretch_driver.py

@@ -29,16 +29,18 @@
 from nav_msgs.msg import Odometry
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult
 from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField, Joy
-from std_msgs.msg import Bool, String
+from std_msgs.msg import Bool, String, Float64MultiArray
 
 from hello_helpers.gripper_conversion import GripperConversion
+from hello_helpers.joint_qpos_conversion import get_Idx, UnsupportedToolError
+from hello_helpers.hello_misc import LoopTimer
 from hello_helpers.gamepad_conversion import unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, get_default_joy_msg
 from .joint_trajectory_server import JointTrajectoryAction
 from .stretch_diagnostics import StretchDiagnostics
 
 GRIPPER_DEBUG = False
 BACKLASH_DEBUG = False
-
+STREAMING_POSITION_DEBUG = False
 
 class StretchDriver(Node):
 
@@ -79,6 +81,9 @@ def __init__(self):
 
         self.gamepad_teleop = None
         self.received_gamepad_joy_msg = get_default_joy_msg()
+        if STREAMING_POSITION_DEBUG:
+            self.streaming_controller_lt = LoopTimer(name="Streaming Position", print_debug=STREAMING_POSITION_DEBUG)
+        self.streaming_position_activated = False
         self.ros_setup()
 
     def set_gamepad_motion_callback(self, joy):
@@ -107,6 +112,62 @@ def set_mobile_base_velocity_callback(self, twist):
         self.angular_velocity_radps = twist.angular.z
         self.last_twist_time = self.get_clock().now()
         self.robot_mode_rwlock.release_read()
+
+    def set_robot_streaming_position_callback(self, msg):
+        self.robot_mode_rwlock.acquire_read()
+        if not self.streaming_position_activated:
+            self.get_logger().error('Streaming position is not activated.'
+                                    ' Please activate streaming position to receive command to joint_position_cmd.')
+            self.robot_mode_rwlock.release_read()
+            return
+
+        if not self.robot_mode in ['position', 'navigation']:
+            self.get_logger().error('{0} must be in position or navigation mode with streaming_position activated ' 
+                                    'enabled to receive command to joint_position_cmd. '
+                                    'Current mode = {1}.'.format(self.node_name, self.robot_mode))
+            self.robot_mode_rwlock.release_read()
+            return
+
+        if STREAMING_POSITION_DEBUG:
+            if (self.get_clock().now().nanoseconds * 1e-9) - self.streaming_controller_lt.last_update_time > 5.0:
+                print('Reset Streaming position looptimer after 5s no message received.')
+                self.streaming_controller_lt.reset()
+        qpos = msg.data
+        self.move_to_position(qpos)
+        self.robot_mode_rwlock.release_read()
+        if STREAMING_POSITION_DEBUG:
+            self.streaming_controller_lt.update()
+
+    def move_to_position(self, qpos):
+        try:
+            try:
+             Idx = get_Idx(self.robot.params['tool'])
+            except UnsupportedToolError:
+                self.get_logger().error('Unsupported tool for streaming position control.')
+            if len(qpos) != Idx.num_joints:
+                self.get_logger().error('Received qpos does not match the number of joints in the robot')
+                return
+            self.robot.arm.move_to(qpos[Idx.ARM])
+            self.robot.lift.move_to(qpos[Idx.LIFT])
+            self.robot.end_of_arm.move_to('wrist_yaw', qpos[Idx.WRIST_YAW])
+            if 'wrist_pitch' in self.robot.end_of_arm.joints:
+                self.robot.end_of_arm.move_to('wrist_pitch', qpos[Idx.WRIST_PITCH])
+            if 'wrist_roll' in self.robot.end_of_arm.joints:
+                self.robot.end_of_arm.move_to('wrist_roll', qpos[Idx.WRIST_ROLL])
+            self.robot.head.move_to('head_pan', qpos[Idx.HEAD_PAN])
+            self.robot.head.move_to('head_tilt', qpos[Idx.HEAD_TILT])
+            if abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode != 'position':
+                self.get_logger().error('Cannot move base in both translation and rotation at the same time in position mode')
+            elif abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and self.robot_mode == 'position':
+                self.robot.base.translate_by(qpos[Idx.BASE_TRANSLATE])
+            elif abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode == 'position':
+                self.robot.base.rotate_by(qpos[Idx.BASE_ROTATE])
+            if 'stretch_gripper' in self.robot.end_of_arm.joints:
+                pos = self.gripper_conversion.finger_to_robotis(qpos[Idx.GRIPPER])
+                self.robot.end_of_arm.move_to('stretch_gripper', pos)
+            self.get_logger().info(f"Moved to position qpos: {qpos}")
+        except Exception as e:
+            self.get_logger().error('Failed to move to position: {0}'.format(e))
 
     def command_mobile_base_velocity_and_publish_state(self):
         self.robot_mode_rwlock.acquire_read()
@@ -366,6 +427,11 @@ def command_mobile_base_velocity_and_publish_state(self):
         tool_msg.data = self.robot.end_of_arm.name
         self.tool_pub.publish(tool_msg)
 
+        # publish streaming position status
+        streaming_position_status = Bool()
+        streaming_position_status.data = self.streaming_position_activated
+        self.streaming_position_mode_pub.publish(streaming_position_status)
+
         # publish joint state for the arm
         joint_state = JointState()
         joint_state.header.stamp = current_time
@@ -590,7 +656,16 @@ def code_to_run():
         self.change_mode('gamepad', code_to_run)
         return True, 'Now in gamepad mode.'
 
-
+    def activate_streaming_position(self, request):
+        self.streaming_position_activated = True
+        self.get_logger().info('Activated streaming position.')
+        return True, 'Activated streaming position.'
+
+    def deactivate_streaming_position(self, request):
+        self.streaming_position_activated = False
+        self.get_logger().info('Deactivated streaming position.')
+        return True, 'Deactivated streaming position.'
+
     # SERVICE CALLBACKS ##############
 
     def stop_the_robot_callback(self, request, response):
@@ -659,6 +734,18 @@ def runstop_service_callback(self, request, response):
         response.success = True
         response.message = f'is_runstopped: {request.data}'
         return response
+
+    def activate_streaming_position_service_callback(self, request, response):
+        success, message = self.activate_streaming_position(request)
+        response.success = success
+        response.message = message
+        return response
+
+    def deactivate_streaming_position_service_callback(self, request, response):
+        success, message = self.deactivate_streaming_position(request)
+        response.success = success
+        response.message = message
+        return response
 
     def get_joint_states_callback(self, request, response):
         joint_limits = JointState()
@@ -887,6 +974,7 @@ def ros_setup(self):
         self.homed_pub = self.create_publisher(Bool, 'is_homed', 1)
         self.mode_pub = self.create_publisher(String, 'mode', 1)
         self.tool_pub = self.create_publisher(String, 'tool', 1)
+        self.streaming_position_mode_pub = self.create_publisher(Bool, 'is_streaming_position', 1)
 
         self.imu_mobile_base_pub = self.create_publisher(Imu, 'imu_mobile_base', 1)
         self.magnetometer_mobile_base_pub = self.create_publisher(MagneticField, 'magnetometer_mobile_base', 1)
@@ -901,6 +989,8 @@ def ros_setup(self):
 
         self.create_subscription(Joy, "gamepad_joy", self.set_gamepad_motion_callback, 1, callback_group=self.group)
 
+        self.create_subscription(Float64MultiArray, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group)
+
         self.declare_parameter('rate', 30.0)
         self.joint_state_rate = self.get_parameter('rate').value
         self.declare_parameter('timeout', 0.5, ParameterDescriptor(
@@ -956,6 +1046,14 @@ def ros_setup(self):
         self.switch_to_gamepad_mode_service = self.create_service(Trigger,
                                                                     '/switch_to_gamepad_mode',
                                                                     self.gamepad_mode_service_callback)
+
+        self.activate_streaming_position_service = self.create_service(Trigger,
+                                                                '/activate_streaming_position',
+                                                                self.activate_streaming_position_service_callback)
+
+        self.deactivate_streaming_position_service = self.create_service(Trigger,
+                                                                '/deactivate_streaming_position',
+                                                                self.deactivate_streaming_position_service_callback)
 
         self.stop_the_robot_service = self.create_service(Trigger,
                                                           '/stop_the_robot',