# Rospy for tf and stretch\_body

**URL:** <https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565>\
**Category:** Ask\
**Created:** [January 15, 2023, 11:35pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565 "2023-01-15T23:35:31Z")\
**Posts on this page:** 11\
**Page:** 1

<div class="post-metadata">

**Author:** ![Zackory](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/zackory/32/114_2.png) [@Zackory](https://forum.hello-robot.com/u/Zackory)\
**Post date:** [January 15, 2023, 11:35pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/1 "2023-01-15T23:35:31Z")

</div>

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:

```auto
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

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [January 15, 2023, 11:55pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/2 "2023-01-15T23:55:46Z")

</div>

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](https://github.com/mmatl/urdfpy) library. You can use it in conjunction with stretch\_body to get the same transformation.

---

<div class="post-metadata">

**Author:** ![Zackory](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/zackory/32/114_2.png) [@Zackory](https://forum.hello-robot.com/u/Zackory)\
**Post date:** [January 15, 2023, 11:57pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/3 "2023-01-15T23:57:44Z")

</div>

Hi Binit,

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

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [January 16, 2023, 12:14am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/4 "2023-01-16T00:14:18Z")

</div>

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**
>
> ```auto
> 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:

```auto
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**
>
> ```auto
> [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](https://docs.hello-robot.com/0.2/stretch-tutorials/ros1/example_10/#tf2-static-listener)
2. [Follow Joint Trajectory commands](https://docs.hello-robot.com/0.2/stretch-tutorials/ros1/follow_joint_trajectory/)

---

<div class="post-metadata">

**Author:** ![Zackory](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/zackory/32/114_2.png) [@Zackory](https://forum.hello-robot.com/u/Zackory)\
**Post date:** [January 16, 2023, 12:18am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/5 "2023-01-16T00:18:56Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [January 17, 2023, 9:13am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/6 "2023-01-17T09:13:58Z")

</div>

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`

```python
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](https://github.com/hello-robot/stretch_ros/tree/dev/noetic/hello_helpers#move_to_posepose-return_before_donefalse-custom_contact_thresholdsfalse-custom_full_goalfalse).

---

<div class="post-metadata">

**Author:** ![Zackory](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/zackory/32/114_2.png) [@Zackory](https://forum.hello-robot.com/u/Zackory)\
**Post date:** [January 20, 2023, 11:00pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/8 "2023-01-20T23:00:38Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![lamsey](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/lamsey/32/177_2.png) [@lamsey](https://forum.hello-robot.com/u/lamsey)\
**Post date:** [January 23, 2023, 11:02pm UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/9 "2023-01-23T23:02:18Z")

</div>

The URDFs for our Stretches have `link_grasp_center` defined in `stretch_description/urdf/stretch_gripper.xacro` ([see here](https://github.com/hello-robot/stretch_ros/blob/master/stretch_description/urdf/stretch_gripper.xacro), 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.

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [February 2, 2023, 1:07am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/10 "2023-02-02T01:07:26Z")

</div>

@Zackory I suspect the reason why `link_grasp_center` is missing is your URDF was compiled with `stretch_dex_wrist.xacro` instead of [`stretch_gripper.xacro`](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/stretch_description/urdf/stretch_gripper.xacro).

`stretch_dex_wrist.xacro` shouldn’t be missing this link. I’ll get back to when it’s fixed, and you can recompile your URDF.

---

<div class="post-metadata">

**Author:** ![Zackory](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/zackory/32/114_2.png) [@Zackory](https://forum.hello-robot.com/u/Zackory)\
**Post date:** [February 2, 2023, 2:54am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/11 "2023-02-02T02:54:11Z")

</div>

Thanks Binit and Matt, I appreciate it!

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [February 22, 2023, 8:08am UTC](https://forum.hello-robot.com/t/rospy-for-tf-and-stretch-body/565/12 "2023-02-22T08:08:32Z")

</div>

Hey @Zackory and @lamsey, just a heads up that link\_grasp\_center is now part of the Dex Wrist gripper [xacro](https://github.com/hello-robot/stretch_tool_share/blob/master/tool_share/stretch_dex_wrist/stretch_description/urdf/stretch_dex_wrist.xacro#L435-L449) as well. If your URDF has the Dex Wrist, regenerate the URDF to get make this link available. Instructions for regenerating the Stretch URDF can be [found here](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/stretch_calibration/README.md#generate-a-new-urdf-after-changing-the-tool).

 ![Screenshot from 2023-02-22 00-42-14](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/8e2c931a5f8ab01596474dbea02c0ed8d36de337.png)