Rospy for tf and stretch_body

Is it possible to use tf (i.e. ROS) and stretch_body at the same time?
If not, is there any way to get transformations (or do forward kinematics) directly from stretch_body?

Here is the code I am currently trying:

import rospy, tf
import stretch_body.robot
robot = stretch_body.robot.Robot()
robot.startup()

rospy.init_node('transformations')
listener = tf.TransformListener()
from_frame_rel = 'base_link'
to_frame_rel = 'link_grasp_center'

rospy.sleep(1.0)
rate = rospy.Rate(1)

robot.arm.move_to(0.5)
robot.push_command()

while not rospy.is_shutdown():
    try:
        translation, rotation = listener.lookupTransform(to_frame_rel, from_frame_rel, rospy.Time(0))
        rospy.loginfo('The pose of target frame %s with respect to %s is: \n %s, %s’, to_frame_rel, from_frame_rel, translation, rotation)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        continue
    rate.sleep()

However, this throws the errors:

[ERROR] [pimu]: Port /dev/hello-pimu is busy. Check if another Stretch Body process is already running
[WARNING] [pimu]: Unable to open serial port for device /dev/hello-pimu

Hi @Zackory, you can use tf (or any ROS component accessible from Python) with stretch_body at the same time, but it’s easier to use tf with stretch_ros since it sets everything up nicely. In particular, TF expects a description of the robot’s kinematics at the /robot_description topic and the robot’s current joint states at the /joint_states topic. Running roslaunch stretch_core stretch_driver.launch makes both of these topics available, and your script (with the stretch_body parts removed) would return the transform as expected.

If you want to avoid ROS, I recommend checking out the urdfpy library. You can use it in conjunction with stretch_body to get the same transformation.

Hi Binit,

Any suggestions on how to overcome the error I am receiving then? Any example of using stretch_ros to access transformations?

I’d start with running: roslaunch stretch_core stretch_driver.launch

You should see no errors. Normal output will look something like:

Log of stretch_driver
hello-robot@stretch-re2-xxxx:~$ roslaunch stretch_core stretch_driver.launch
... logging to /home/hello-robot/.ros/log/0495089e-9531-11ed-87be-1bc954995ac2/roslaunch-stretch-re2-2002-3454.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://10.42.0.1:44199/

SUMMARY
========

PARAMETERS
[skipped...]

NODES
  /
    aggregator_node (diagnostic_aggregator/aggregator_node)
    joint_state_publisher (joint_state_publisher/joint_state_publisher)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    stretch_driver (stretch_core/stretch_driver)

auto-starting new master
process[master]: started with pid [3471]
ROS_MASTER_URI=http://10.42.0.1:11311/

setting /run_id to 0495089e-9531-11ed-87be-1bc954995ac2
process[rosout-1]: started with pid [3488]
started core service [/rosout]
process[joint_state_publisher-2]: started with pid [3491]
process[robot_state_publisher-3]: started with pid [3492]
process[stretch_driver-4]: started with pid [3493]
process[aggregator_node-5]: started with pid [3494]
[ WARN] [1673827328.956746046]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[INFO] [rospy.topics]: topicmanager initialized
[INFO] [1673827330.218683]: For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.
[INFO] [1673827330.219452]: /stretch_driver started
[INFO] [1673827330.725948]: mode = position
[INFO] [1673827330.727219]: /stretch_driver: Changed to mode = position
[INFO] [1673827330.728909]: broadcast_odom_tf = False
[INFO] [1673827330.730199]: Loading controller calibration parameters for the head from YAML file named /home/hello-robot/catkin_ws/src/stretch_ros/stretch_core/config/controller_calibration_head.yaml
[INFO] [1673827330.735480]: controller parameters loaded = {'arm_retracted_offset': 0.003591254818011023, 'pan_angle_offset': 0.14314813939215376, 'pan_looked_left_offset': -0.0041047044639369815, 'tilt_angle_backlash_transition': -0.4, 'tilt_angle_offset': 0.08094073411534239, 'tilt_looking_up_offset': 0.02777409244081353}
[INFO] [1673827330.736474]: head_tilt_calibrated_offset_rad in degrees = 4.637562455499678
[INFO] [1673827330.737495]: head_pan_calibrated_offset_rad in degrees = 8.201784232320817
[INFO] [1673827330.738459]: head_pan_calibrated_looked_left_offset_rad in degrees = -0.23518224193209805
[INFO] [1673827330.739457]: head_tilt_backlash_transition_angle_rad in degrees = -22.918311805232932
[INFO] [1673827330.741122]: head_tilt_calibrated_looking_up_offset_rad in degrees = 1.5913382766648185
[INFO] [1673827330.742017]: arm_calibrated_retracted_offset_m in meters = 0.003591254818011023
[INFO] [1673827330.749426]: /stretch_driver rate = 15.0 Hz
[INFO] [1673827330.750559]: /stretch_driver timeout = 0.5 s
[INFO] [1673827330.752581]: /stretch_driver use_fake_mechaduinos = False
[INFO] [1673827330.754968]: /stretch_driver base_frame_id = base_link
[INFO] [1673827330.756897]: /stretch_driver odom_frame_id = odom

Beyond that, your script will succeed in retrieving the transformation once the stretch_body parts are removed. I modified it like so:

import rospy, tf

rospy.init_node('transformations')
listener = tf.TransformListener()
from_frame_rel = 'base_link'
to_frame_rel = 'link_grasp_center'

rospy.sleep(1.0)
rate = rospy.Rate(1)

while not rospy.is_shutdown():
    try:
        translation, rotation = listener.lookupTransform(to_frame_rel, from_frame_rel, rospy.Time(0))
        rospy.loginfo('The pose of target frame %s with respect to %s is: \n %s, %s', to_frame_rel, from_frame_rel, translation, rotation)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        continue
    rate.sleep()

Saving that and running it using python3 <name-of-script> works and the output looks like:

Output
[INFO] [1673827525.587868]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45286983644850815, 0.011302539877841827, -0.647163005530671], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827526.587838]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45286732637956023, 0.011302488760229629, -0.647174625322772], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827527.587824]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.4528650166537622, 0.011302525176336925, -0.6471663566782381], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827528.587804]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.4528590629047038, 0.011302539985365928, -0.6471630054910974], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827529.587776]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45286265408597187, 0.011302539949524681, -0.6471630055042885], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827530.587882]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45286265408597187, 0.011302539949524681, -0.6471630055042885], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827531.587907]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45285673154700035, 0.011302569347528685, -0.6471563395777006], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827532.587250]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.4528649700101157, 0.011302503373033596, -0.6471713105745545], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827533.587853]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.4528590629047038, 0.011302539985365928, -0.6471630054910974], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827534.587921]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.4528590629047038, 0.011302539985365928, -0.6471630054910974], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]
[INFO] [1673827535.587893]: The pose of target frame link_grasp_center with respect to base_link is: 
 [-0.45286504820681733, 0.011302539925630295, -0.6471630055130826], [-0.007267520437737762, 0.01956926836731554, 0.7127514604158909, 0.7011059709896001]

If you want to then simultaneously move the joints, you can run the keyboard teleop node using: rosrun stretch_core keyboard_teleop.

I’d recommend checking out these two tutorials for a more depth look at how to write TF listeners and sending action goals:

  1. TF2 Listener
  2. Follow Joint Trajectory commands

Hi Binit,

It seems maybe my original question was not clear enough.

I would like to move the robot using robot.arm.move_to(0.5) and I would like to use tf at the same time to output the end effector position as the robot is moving.
Your code example removed the robot movement.

Got it, below is an example that moves the arm to 0.5m while using tf at the same time to output the end effector position. However, I don’t use robot.arm.move_to() in my example because it’s a Stretch Body method which will conflict with Stretch ROS. Instead, I use HelloNode.move_to_pose() which is part of Stretch ROS. If interested, I can explain why the difference matters.

Before running the following, perform a git update on Stretch ROS using: roscd stretch_core && git pull, and launch Stretch’s ROS driver using: roslaunch stretch_core stretch_driver.launch

import rospy, tf
import hello_helpers.hello_misc as hm

node = hm.HelloNode.quick_create('transformations')
listener = tf.TransformListener()
from_frame_rel = 'base_link'
to_frame_rel = 'link_grasp_center'

rospy.sleep(1.0)
rate = rospy.Rate(1)

node.move_to_pose({'joint_arm': 0.5}, return_before_done=True)

while not rospy.is_shutdown():
    try:
        translation, rotation = listener.lookupTransform(to_frame_rel, from_frame_rel, rospy.Time(0))
        rospy.loginfo('The pose of target frame %s with respect to %s is: \n %s, %s', to_frame_rel, from_frame_rel, translation, rotation)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        continue
    rate.sleep()

For more info on move_to_pose(), its API is available here.

I realized our robot does not have link_grasp_center. Is this normal? Is our URDF incorrect?

The URDFs for our Stretches have link_grasp_center defined in stretch_description/urdf/stretch_gripper.xacro (see here, and they appear in our /tf trees). I’m not sure how this is affected by calibration procedures, dex wrist installs, or changing the config yaml files, etc. You might be able to regenerate the URDF, or manually append the info from this file into your URDF.