Motion Commands in ROS2

Quickstart

Sending motion commands is as easy as:

1. Launch the ROS2 driver in a terminal:

   ros2 launch stretch_core stretch_driver.launch.py
   

2. Open iPython and type the following code, one line at a time:

   import hello_helpers.hello_misc as hm
   node = hm.HelloNode.quick_create('temp')
   node.move_to_pose({'joint_lift': 0.4}, blocking=True)
   node.move_to_pose({'joint_wrist_yaw': 0.0, 'joint_wrist_roll': 0.0}, blocking=True)
   

Writing a node

You can also write a ROS2 node to send motion commands:

import hello_helpers.hello_misc as hm

class MyNode(hm.HelloNode):
    def __init__(self):
        hm.HelloNode.__init__(self)

    def main(self):
        hm.HelloNode.main(self, 'my_node', 'my_node', wait_for_first_pointcloud=False)

        # my_node's main logic goes here
        self.move_to_pose({'joint_lift': 0.6}, blocking=True)
        self.move_to_pose({'joint_wrist_yaw': -1.0, 'joint_wrist_pitch': -1.0}, blocking=True)

node = MyNode()
node.main()

Copy the above into a file called "example.py" and run it using:

python3 example.py

Retrieving joint limits

In a terminal, echo the /joint_limits topic:

ros2 topic echo /joint_limits

In a second terminal, request the driver publish the joint limits:

ros2 service call /get_joint_states std_srvs/srv/Trigger {}

In the first terminal, you'll see a single message get published. It'll look like this:

header:
  stamp:
    sec: 1725388967
    nanosec: 818893747
  frame_id: ''
name:
- joint_head_tilt
- joint_wrist_pitch
- joint_wrist_roll
- joint_wrist_yaw
- joint_head_pan
- joint_lift
- joint_arm
- gripper_aperture
- joint_gripper_finger_left
- joint_gripper_finger_right
position:
- -2.0171847360696185
- -1.5707963267948966
- -2.9114955354069467
- -1.3933658823294575
- -4.035903452927122
- 0.0
- 0.0
- -0.1285204486235414
- -0.3757907854489514
- -0.3757907854489514
velocity:
- 0.4908738521234052
- 0.45099035163837853
- 2.9176314585584895
- 4.416586351787409
- 1.7303303287350031
- 1.0966833704348709
- 0.5197662863936018
- 0.34289112948906764
- 1.0026056417808995
- 1.0026056417808995
effort: []

We're misusing the sensor_msgs/JointState message to publish the joint limits. The name array lists out each ranged joint. The position array lists the lower bound for each joint. The velocity array lists the upper bound. The length of these 3 arrays will be equal, because the index of the joint in the name array determines which index the corresponding limits will be in the other two arrays.

The revolute joints will have their limits published in radians, and the prismatic joints will have them published in meters. See the Hardware Overview to see the ranges represented visually.