Integrate velocity control into ROS

Ask
1

I’m trying to do velocity control of the base, arm and wrist according to this post. I also need the stretch_driver in ROS to get the odometry information. However, they seem to conflict with each other. How can I get stretch_ros to run with the stretch body sdk?

2

I wrote a simple node to listen to the speed command in ros, and then operate the real robot. The node works perfectly when stretch_driver is not running.

#!/usr/bin/env python3

import rospy
import stretch_body.end_of_arm
import stretch_body.robot
import stretch_body.stepper
from std_msgs.msg import Float64, Float64MultiArray


class StretchController:
    def __init__(self) -> None:
        self.r = stretch_body.robot.Robot()
        self.r.startup()
        assert self.r.is_calibrated()  # the robot must be homed
        rospy.Subscriber("/stretch_controller/joint_cmd", Float64MultiArray, self.joint_cb)
        rospy.Subscriber("/stretch_controller/gripper_cmd", Float64, self.gripper_cb)

    def joint_cb(self, msg: Float64MultiArray) -> None:
        q_dot = msg.data
        self.r.base.set_velocity(q_dot[0], q_dot[1])
        self.r.lift.set_velocity(q_dot[2])
        self.r.arm.set_velocity(sum(q_dot[3:7]))
        self.r.end_of_arm.get_joint("wrist_yaw").set_velocity(q_dot[7])
        self.r.push_command()

    def gripper_cb(self, msg: Float64):
        self.r.end_of_arm.get_joint("stretch_gripper").set_velocity(msg.data)
        self.r.push_command()


if __name__ == "__main__":
    rospy.init_node("stretch_controller")
    ctrler = StretchController()
    rospy.spin()

However when running this node with stretch_driver, I got this error:

Traceback (most recent call last):
  File "/usr/lib/python3/dist-packages/serial/serialposix.py", line 500, in read
    raise SerialException(
serial.serialutil.SerialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/home/hat/catkin_ws/src/stretch_ros/stretch_core/nodes/stretch_driver", line 541, in <module>
    node.main()
  File "/home/hat/catkin_ws/src/stretch_ros/stretch_core/nodes/stretch_driver", line 413, in main
    self.robot.startup()
  File "/home/hat/stretch_body/body/stretch_body/robot.py", line 226, in startup
    if not self.devices[k].startup(threaded=False):
  File "/home/hat/stretch_body/body/stretch_body/head.py", line 24, in startup
    return DynamixelXChain.startup(self, threaded=threaded)
  File "/home/hat/stretch_body/body/stretch_body/dynamixel_X_chain.py", line 80, in startup
    if not self.motors[mk].startup(threaded=False):
  File "/home/hat/stretch_body/body/stretch_body/dynamixel_hello_XL430.py", line 183, in startup
    if self.motor.do_ping(verbose=False):
  File "/home/hat/stretch_body/body/stretch_body/dynamixel_XL430.py", line 292, in do_ping
    dxl_model_number, dxl_comm_result, dxl_error = self.packet_handler.ping(self.port_handler, self.dxl_id)
  File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 369, in ping
    rxpacket, result, error = self.txRxPacket(port, txpacket)
  File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 346, in txRxPacket
    rxpacket, result = self.rxPacket(port)
  File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/protocol2_packet_handler.py", line 257, in rxPacket
    rxpacket.extend(port.readPort(wait_length - rx_length))
  File "/home/hat/.local/lib/python3.8/site-packages/dynamixel_sdk/port_handler.py", line 78, in readPort
    return self.ser.read(length)
  File "/usr/lib/python3/dist-packages/serial/serialposix.py", line 509, in read
    raise SerialException('read failed: {}'.format(e))
serial.serialutil.SerialException: read failed: device reports readiness to read but returned no data (device disconnected or multiple access on port?)
3

Hi @CalaW, stretch_ros and stretch_body cannot run at the same time. The reason is only one process can talk to the hardware at a time. This is why your node works when the stretch_driver node is not running.

There’s two options available:

  1. Some of what stretch_driver does can also be done with stretch_body. Depending on what functionality you need, you may be able to use stretch_body solely. For example, if you are reading wheel odometry that doesn’t fuse lidar from stretch_driver (i.e. the /odom topic), stretch_body provides the same odometry as a dictionary of base called status. This is what it would look like:
    import stretch_body.robot
r = stretch_body.robot.Robot()
r.startup()
print(r.base.status['x'], r.base.status['y'], r.base.status['theta'])
  2. I can add support for velocity control of all joints to stretch_driver. This might be preferred since stretch_ros can accomplish more than stretch_body can alone. For example, you can use the stretch_navigation ROS pkg with the driver node to fuse measurements from the laser range finder, yielding a better odometry estimate than wheel encoders alone.
4

Thanks Binit!
Yes, I prefer the second approach since I’m already using lidar fusion to get better odometry.

5

Okay! I will begin adding this. I estimate it will take ~1 week, and I’ll post updates here about delays and/or when it becomes available.

6

Hi @CalaW, I’ve opened a pull request called “Velocity Control” that adds a new control mode called ‘velocity’ to the stretch_driver node. When the driver is in this mode, the cmd_vel topic still controls the mobile base, but now the FollowJointTrajectory action server will look at the velocities in the JointTrajectoryPoint message and command the robot’s joints accordingly. The positions array in the JointTrajectoryPoint message must be empty. I’ve also added a method that makes working with this new mode easier, called the move_to_speed() method. You can use it to send velocity commands to any joint like this:

# ensure driver is launched in a different terminal

$ ipython3
Python 3.8.10 (default, May 26 2023, 14:05:08) 
Type 'copyright', 'credits' or 'license' for more information
IPython 8.7.0 -- An enhanced Interactive Python. Type '?' for help.

In [1]: import hello_helpers.hello_misc as hm
   ...: temp = hm.HelloNode.quick_create('temp')
[INFO] [1691144362.585790]: /temp started
[INFO] [1691144362.633690]: Node /temp connected to robot services.

In [2]: temp.move_at_speed({'joint_lift': 0.0})

In [3]: temp.move_at_speed({'joint_lift': 0.05})

In [4]: temp.move_at_speed({'joint_lift': -0.05})

In [5]: temp.move_at_speed({'joint_lift': 0.0})

In [6]: temp.move_at_speed({'joint_lift': 0.01, 'joint_arm': 0.01})

More details can be found in the PR. Note that there’s a few known bugs that will be fixed before the PR is merged. Also note that the gripper is still controlled with position commands (i.e. the move_to_pose() method).

2 Likes