Unicycle to Differential Drive: Coursera’s Control of Mobile Robots with ROS and ROSbots — Part 2

Welcome back to Part 2 of our multi-part Coursera’s Control of Mobile Robots (CMR) series. This series implements concepts learned from CMR with ROS and a ROSbots robot.

ROSbots is a ROS + OpenCV robot kit for Makers based off a Raspberry Pi. The robot kit is designed to be an accessible and extremely hack-friendly platform for breadboarding, adding other sensors and actuators to implement any new robotics concepts you come across. Our code is open source on Github.

The kit takes about an hour to assemble. To avoid the pains of compiling ROS and OpenCV yourself, we have a Raspbian ROS + OpenCV SD card image you can download and use.

In Part 1 of our series, we wired up the ROSbots’ L9110 motor driver and implemented the ROS node to have the UNO board drive our robot’s wheels. The ROS firmware we compiled and uploaded onto the UNO subscribes to two ROS topics, /wheel_power_left and /wheel_power_right. The Float32 ROS messages published down these topics describe the normalized power (max reverse to max forward is between -1.0 and 1.0) to turn each respective wheel via the respective pulse width.

We ended Part 1 by manually publishing a Float32 ROS message to drive each wheel using the built-in rostopic command line tool.

In this Part 2, we will describe the kinematics and the dynamics of the ROSbots’ differential drive system. We’ll then apply this understanding and implement the ROS code to predictably drive the robot in a remote-control (RC) tele-operational manner. This will roughly follow along with the lessons and the programming assignments from CMR week 1 and week 2.

For those familiar with the CMR course and who perhaps implemented the course’s Matlab programming assignments using the Sim.I.am simulator, we will be implementing our ROS version using roughly the same file and architectural structure described in the CMR course.

Unicycle and Differential Drive Models

Since a wheeled robot cannot fly, we only care about these 3 states that define it’s pose:

x - position on the x-axis (ie in meters)
y - position on the y-axis (ie in meters)
φ - phi - angle of the unicycle counter clockwise from x-axis (ie in radians)

State and inputs for a unicycle model

For a unicycle model, there are 2 inputs that affect these 3 states of the robot:

v = forward velocity (ie meters per second)
w = angular velocity (ie radians per second)

While wheeled robot can have any number of wheels and more complicated factors that can affect its pose, we conveniently use a unicycle model to describe the dynamics of most wheeled robot since it is easy and intuitive to understand. Specifically, we intuitively use v and w to describe how the unicycle will move.

This is all fine and dandy, but our ROSbots robot is a differential drive robot with 2 inputs for each of its two wheels:

v_r = clockwise angular velocity of right wheel (ie radians per second)
v_l = counter-clockwise angular velocity of left wheel (ie radians per second)

Fortunately for us, we can convert unicycle inputs into differential drive inputs. Before we describe the equations to do the conversion, there are a couple of measurements we need to make with a straight edge.

Differential Drive Model

L = wheelbase (in meters per radian)
R = wheel radius (in meters per radian)

Because R is a measurement of the radius of a wheel, it makes sense to think of R as meters per radian.

For L, it is not as intuitive. In the differential drive kinematics model, you can think of L as the radius of the circle drawn by one wheel spinning while holding the other wheel still. So L is also in the units of meters per radian where the radius is of that circle’s.

L is in meters per radian

We won’t go into excruciating details (since this is not a post on kinematics), but in summary, we can use the kinematics for a unicycle model and kinematics for a differential drive model to come up with the following equations to convert unicycle v and w inputs into v_r and v_l differential drive inputs for our ROSbots robot.

v_r = ((2 * v) + (w * L)) / (2 * R)
v_l = ((2 * v) - (w * L)) / (2 * R)

The numerator for both equations are in meters per second. The denominator is in meters per radian. Both v_r and v_l result in radians per second, clock-wise and counter-clock-wise respectively— what we would expect.

Unicycle to Differential Drive Conversion in ROS

The two equations above are our sweetheart equations which we implement in a ROS node on our ROSbots robot.

To take our robot on a test spin, we will drive our robot in a remote-control (RC) tele-operational manner, with v and w inputs you specify through keyboard commands. The keyboard interface is through a teleop_twist_keyboard ROS node application created by the ROS community.

SSH into your Raspberry Pi (RPi) on your ROSbots robot. From your RPi run:

$ update_rosbots_git_repos

That should pick up the latest ROSbots repository source files from Github.

We’ll also assume you are picking up right where we left off from the Part 1 post. Specifically, you have the motor_driver firmware compiled and uploaded on the ROSBots’ UNO board via:

$ upload_firmware ~/gitspace/rosbots_driver/platformio/rosbots_firmware/examples/motor_driver/

As a sanity check, here are the running ROS nodes if you run $ rosnode list:

/rosout
/uno_serial_node

Here are the ROS topics if you run $ rostopic list:

/diagnostics
/rosout
/rosout_agg
/wheel_power_left
/wheel_power_right

Now run let’s run the unicycle to diff drive model robot code. Again, from your RPi SSH terminal run:

$ rosrun rosbots_driver part2_cmr.py

If you should see the following outputs, the ROS node is working:

[INFO] [1521228417.529282]: /part2_cmr RCTeleop initialized
[INFO] [1521228417.595446]: /part2_cmr wheelbase: 0.14 wheel radius: 0.035
[INFO] [1521228417.596918]: /part2_cmr v: 0 w: 0
[INFO] [1521228417.598303]: /part2_cmr vl: 0.0 vr: 0.0
[INFO] [1521228417.605237]: /part2_cmr right power: data: 0.0 left power: data: 0.0
[INFO] [1521228418.098403]: /part2_cmr v: 0 w: 0
[INFO] [1521228418.101346]: /part2_cmr vl: 0.0 vr: 0.0
[INFO] [1521228418.104728]: /part2_cmr right power: data: 0.0 left power: data: 0.0
...

Now open another SSH terminal for the RPi on your robot.

Let’s take a look at what additional ROS topics are published. From the 2nd SSH terminal on your RPi run $ rosnode list and you should see the new /part2_cmr ROS node listed:

/part2_cmr
/rosout
/uno_serial_node

Now let’s see what other topics are added to the ROS system from running the /part2_cmr ROS node.

From your 2nd SSH RPi terminal, type
$ rostopic list — you should see an additional /part2_cmr/cmd_vel topic.

Type $ rostopic info /part2_cmr/cmd_vel and you’ll get more info about the /part2_cmr/cmd_vel topic:

Type: geometry_msgs/Twist
Publishers: None
Subscribers: 
 * /part2_cmr (http://192.168.0.22:33183/)

This topic transports a ROS Twist message type. The Twist message is part of the standard geometry_msgs ROS package which includes a number of common messages used to describe robot geometry and kinematics.

If you look at the definition of the ROS Twist message, you’ll notice it actually can encode motion in 6 degrees of freedom (DOF)— 3 as linear motion and 3 as angular. We actually only need 2 DOF’s — linear.x which is our v and angular.z which is our w.

Notice that our /part2_cmr ROS node is a subscriber to the topic, but currently there are no publishers! No one is telling the robot what to do.

Let’s add a publisher by running the teleop_twist_keyboard ROS node. From the 2nd SSH RPi terminal, type:

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

By default, the teleop_twist_keyboard ROS node publishes Twist messages to a topic named /cmd_vel. The /cmd_vel:=/part2_cmr/cmd_vel argument tells rosrun to remap the /cmd_vel name to /part2_cmr/cmd_vel so now the published topic name is the same for both the publishing teleop_twist_keyboard ROS node and the subscribing /part2_cmr ROS node.

Once you have the teleop_twist_keyboard ROS node running, let’s tune the linear and angular speed factors first. Hit the “x” button until you get to around speed 0.174 and hit the “e” key util you get turn 2.59. If you’re a 3 to 4 decimal places off, that’s fine:

...
currently: speed 0.17433922005 turn 2.5937424601

As described by the output from running teleop_twist_keyboard, use the lower case u i o j k l… keys to drive your ROSbots robot.

RC Teleoperating ROSbots

Aw yeah!

ROSbots Implementation Details

We roughly paralleled the software design and file structure from the CMR course’s Sim.I.am Matlab simulator.

From the top level of our source code via the ./part2/full folder, you’ll see the following file structure:

├── controller
│   ├── controller.py
│   ├── dynamics
│   │   ├── differential_drive.py
│   │   ├── __init__.py
│   ├── go_to_angle.py
│   ├── __init__.py
│   ├── rc_teleop.py
│   ├── robot.py
│   ├── supervisor.py
└── part2_cmr.py

The main() function for the /part2_cmr ROS node is in part2_cmr.py at the top level. Inside the main(), we create a Supervisor object and call its execute() function twice a second (2Hz) via:

When the Supervisor is created, it does 3 things.

1. Creates a Robot object
2. Creates a DifferentialDrive object.
3. Creates all the Controller objects it needs for the defined objectives — in our case, we only need one Controller object, the RCTeleop controller.

Controllers dictate what the robot does. For instance, you can create a Go-To-Goal controller to plan a path to go to a specific waypoint. You can create a Go-To-Angle controller to steer to and track at a specific angle orientation phi. Modern day cars have a Go-To-Velocity controller, aka cruise controller to accelerate/decelerate and track a specific velocity.

For the purposes of this Part 2 post, our supervisor will have a single RCTeleop controller that does whatever a teleoperational unicycle command dictates. At init time, the RCTeleop subscribes to the /part2_cmr/cmd_vel topic and listens for Twist unicycle commands.

rc_teleop.py

The self.twist_cb(.) callback function gets called when a Twist message comes in, and stores the v and w velocities away.

rc_teleop.py

When the supervisor creates the Robot object, the Robot object fetches the dimensions of its wheelbase and wheel radius as well as the min and max velocities of its motors (yes, real robots have real physical limits) from the ROS parameter server.

robot.py

When the supervisor creates a DifferentialDrive object, it passes the wheelbase and wheel radius measurements so the DifferentialDrive object can accurately convert unicycle forward and angular velocity inputs to differential drive wheel velocity inputs when asked to do so.

That’s pretty much for initialization.

Each time the supervisor.execute() gets called, the supervisor does the following:

1. Executes the current controller — the RCTeleop controller — to get the unicycle v and w velocities.
2. Calls the DifferentialDrive to convert the unicycle v, w velocities to the v_r and v_l wheel velocities.
3. Passes the v_r and v_l velocities to the Robot who publishes the Float32 ROS messages to the/wheel_power_left and /wheel_power_right ROS topics — directing our UNO board to turn ROSbots’ respective wheels as described in Part 1 of our series.

Each time the Supervisor gets executed: supervisor.py

The RCTeleop controller’s execute function simply returns back the current v and w it has stored away. Nothing exciting here.

rc_teleop.py

The DifferentialDrive’s uni_to_diff(.) function implements the unicycle to differential drive model conversion. This is where our 2 sweetheart functions get implemented.

The Robot’s set_wheel_speed(.) function has a couple of important responsibilities.

1. It clamps the wheel velocity to the motor’s physical limitations.
2. Converts wheel velocity to wheel power. If you recall from Part 1, our motor_driver firmware only supports wheel power normalized between -1.0 and 1.0 — full speed back to full speed forward. The set_wheel_speed(.) function converts velocity into power using a lookup table of no-load speeds we measured on our ROSbots robot prior.
3. Our motors also has a minimum speed velocity. There is a minimum pulse width threshold where the motors just won’t turn. If the controller wants a speed lower than this threshold, the motors won’t be able to support it so currently we just set the motor speed to zero to save power.

Don’t we love the messiness of real hardware?

This brings us to the conclusion of Part 2. We’ve touched upon unicycle models and how to convert unicycle model inputs to differential drive model inputs. We implemented this system using ROS on a ROSbots robot and got to test the system out in an RC tele-operational manner.

In the next Part 3 post, we’ll start to implement feedback which is the critical component to effective control systems.

As usual, follow @rosbots on Medium for updates. Follow us on Instagram and Facebook too!

Feel free to comment or ask questions here. But if you have technical questions, please post them on answers.ros.org (subject “ROSbots question: blahblah”, tag “rosbots”).

Don’t hesitate to reach out with general feedback, if you want to collaborate, or just to say hello.

And if you haven’t already done so, purchase your own ROSbots robot here to follow along.

Thanks!
Jack “the ROSbots Maker”