Rospy for tf and stretch_body

Zackory · January 15, 2023, 11:35pm

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

bshah · January 15, 2023, 11:55pm

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.

Zackory · January 15, 2023, 11:57pm

Hi Binit,

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

bshah · January 16, 2023, 12:14am

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:

Zackory · January 16, 2023, 12:18am

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.

bshah · January 17, 2023, 9:13am

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.

Zackory · January 20, 2023, 11:00pm

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

lamsey · January 23, 2023, 11:02pm

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.

bshah · February 2, 2023, 1:07am

@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.

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.

Zackory · February 2, 2023, 2:54am

Thanks Binit and Matt, I appreciate it!

bshah · February 22, 2023, 8:08am

Hey @Zackory and @lamsey, just a heads up that link_grasp_center is now part of the Dex Wrist gripper xacro 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.

Topic		Replies	Views
Integrate velocity control into ROS Ask	5	371	August 4, 2023
Inverse Kinematics Tutorial & Workshop Recording Knowledge Base	17	1827	July 14, 2023
Basic ROS code for the Stretch Robot Discuss	4	1099	April 6, 2021
Stretch_body Python SDK and Python Multithreading Discuss	10	1339	November 10, 2021
Question about the base.translate_by() and rotate_by() function? Ask	4	77	July 15, 2024

Rospy for tf and stretch_body

Related topics