Base Controller for real-world Stretch RE-2

Ask
1

Hi all!
I am planning to make a custom controller for Stretch RE-2. I know that based on base_controller_ROS1, the controller is platform specified. And in
Stretch-Gazebo it’s specified that diff_drive_controller is used on Gazebo (Simulation). My questions are, what is and could we get the access to the controller used in real-world Stretch RE-2? Is there anything similar to diff_drive_controller plugin to use? Any guidance to help me making a custom controller will be much appreciated. Thank you! :blush:

2

Hi @Mustofa, it’s great that you’re planning to design a base controller. It’s helpful to understand the differences between the real robot and the simulated robot’s controllers.

We’ll start with the real Stretch controllers. They are implemented as part of Stretch’s low level Python library, Stretch Body.

  1. There is a position controller that takes translation or rotational delta commands and follows a trapezoidal motion profile while executing it. Additionally, this controller is preemptible, which means new commands can replace old ones and the controller will smoothly switch to executing the new command. The controller is accessible through the translate_by() and rotate_by() API. For example:
    import stretch_body.robot
r = stretch_body.robot.Robot()
r.startup()
r.base.translate_by(0.1) # translate the base forward 10cm
r.push_command()
r.wait_command()
r.stop()
  2. There is a velocity controller that takes translational and rotational velocity commands and executes them. This controller is also preemptible. The controller is accessible through the set_velocity() API. For example:
    import stretch_body.robot
r= stretch_body.robot.Robot()
r.startup()
r.base.set_velocity(0.05, 0.1) # move forward at 0.05 m/s and counter-clockwise 0.1 rad/s
r.push_command()
import time; time.sleep(3)
r.stop()
  3. There is a Balkcom-Mason splined trajectory controller that takes in a sequence of SE2 waypoints and executes them. This controller is also preemptible. This controller is accessible through the follow_trajectory() API. For example:
    import stretch_body.robot
import math
r= stretch_body.robot.Robot()
r.startup()
r.base.trajectory.add(time=0,  x=0,   y=0, theta=0)
r.base.trajectory.add(time=6,  x=0.4, y=0, theta=0)
r.base.trajectory.add(time=12, x=0.4, y=0, theta=math.pi)
r.base.trajectory.add(time=18, x=0.0, y=0, theta=math.pi)
r.base.trajectory.add(time=24, x=0.0, y=0, theta=0)
r.follow_trajectory()
import time; time.sleep(26)
r.stop()

At this moment (Dec 19th, 2023), the most popular simulation of Stretch exists as a ROS1 package called Stretch Gazebo. The controller framework powering this simulation is called ROS Control. As you’ve noted, Stretch’s mobile base is powered by the diff_drive_controller in this simulation. The diff_drive_controller plugin takes translational and rotational velocity commands and executes them. Therefore, the diff_drive_controller is most similar to the velocity controller described in bullet #2 above. AFAIK, no standard Gazebo plugin exist to emulate the other two controllers we provide through Stretch Body, however, there is an experimental implementation created by a research group at UW that aims to emulate the position controller described in bullet #1 above.

I hope this information is helpful to you. Let me know if you have any other questions as you start writing your custom controller!

3

Hi! Thank you for your reply. From what I can see, that the 3 stretch’s low level controllers you mentioned are in the open-loop controller category. However, if you know work on Stretch that deploy closed-loop controller let me know. Thank you.

Yes, I understand the difference between the real robot and the simulated robot’s controllers. That’s why I asked if there’s something similar to ros_control mentioned in Making a Mobile Robot #13 - Using ros2_control on a real robot | Articulated Robotics. Usually, we added the plugin into the URDF, using the hardware interface that is suitable for its motor, in the example of the website I snapped, it is diffdrive_arduino/DiffDriveArduino. I think what Stretch team provided is somewhat a bit higher that hardware interface but still in the "low level’ category. I’m still thinking of the idea to first apply closed-loop control, then to add this ros_control. Any tips and answer will be appreciated :blush:

4

Hi @Mustofa, all 3 low level controllers described above are closed-loop with respect to wheel encoder odometry. Is this what you meant by “closed loop controller”?

In the article you’ve linked to, it seems like the author has written an hardware interface plugin for ros2_control called diffdrive_arduino/DiffDriveArduino because they want a /cmd_vel topic that accepts Twist commands to control their robot. When you launch Stretch’s ROS2 driver, it also provides the /stretch/cmd_vel topic and joint states as the odom frame. Stretch’s ROS2 driver achieves this by using the velocity controller from Stretch Body.

So instead of using ros2_control, you can use the Stretch Core package from the Stretch ROS2 repo. It will be already installed on your robot in the Ament workspace at ~/ament_ws. Technically, you could write a wrapper script around Stretch Body to create a ros2_control hardware interface plugin, however, the result would be the same as what Stretch ROS2 already provides. Does this make sense?