Simple kinematics for mobile robot
You can run this tutorial on:
A little bit of theory
Introduction
The purpose of forward kinematics in mobile robotics is to determine robot position and orientation based on wheels rotation measurements. To achieve that we'll create robot kinematic model. ROSbot is four wheeled mobile robot with separate drive for each wheel, but in order to simplify kinematic calculation we will treat it as two wheeled. Two virtual wheels (marked as WL and WR on the scheme) will have axis going through robot geometric center. This way we can use simpler kinematic model of differential wheeled robot. The name "differential" comes from the fact that robot can change its direction by varying the relative rate of rotation of its wheels and does not require additional steering motion. Robot scheme is presented below:

Description:
- Rc - robot geometric centre
- xc - robot geometric centre x position
- yc - robot geometric centre y position
- xr - robot local x axis that determines front of the robot
- yr - robot local y axis
- α - robot angular position
- WFL - front left wheel
- WFR - front right wheel
- WRL - rear left wheel
- WRR - rear right wheel
- WL - virtual left wheel
- WR - virtual right wheel
- l1 - distance between robot center and front/rear wheels
- l2 - distance between robot left and right wheels
Our mobile robot has constraints. It can only move in x-y
plane and it has 3 DOF (degrees of freedom). However not all of DOFs are controllable which means robot cannot move in every direction of its local axes (e.g. it cannot move sideways). Such drive system is called non-holonomic. When amount of controllable DOFs is equal to total DOFs then a robot can be called holonomic. To achieve that some mobile robots are built using Omni or Mecanum wheels and thanks to vectoring movement they can change position without changing their heading (orientation).
Forward Kinematics task
The robot position is determined by a tuple (xc, yc, α). The forward kinematic task is to find new robot position (xc, yc, α)' after time δt for given control parameters:
- vR - linear speed of right virtual wheel
- vL - linear speed of left virtual wheel
In our case the angular speed ω and the angular position Φ of each virtual wheel will be an average of its real counterparts:
Linear speed of each virtual wheel:
where r - the wheel radius.
We can determine robot angular position and speed with:
Then robot speed x and y component:
To get position:
We assume starting position as (0,0).
In order for code to work correctly wheels should be connected to ports in following manner:
- front left wheel (WFL) - hMot4
- front right wheel (WFR) - hMot1
- rear left wheel (WRL) - hMot3
- rear right wheel (WRR) - hMot2
The implementation of the equations above in hFramework can be found here.
Controlling the motor
Most common way to send movement commands to the robot is with use of
geometry_msgs/Twist
message type. Then motor driver node should use
data stored in them to control the motor.
The geometry_msgs/Twist
message express velocity in free space and consists of two fields:
Vector3 linear
- represents linear part of velocity [m/s]Vector3 angular
- represents angular part of velocity [rad/s]
You will control ROSbot in the x-y
plane by manipulating the x
component of linear speed vector and the z
component of angular speed vector.
Publishing the motion command for robot
You will use keyboard to control the movement of your robot. For getting the
key events and converting them to geometry_msgs/Twist
messages you can
use teleop_twist_keyboard.py
node from package
teleop_twist_keyboard
.
Alternatively you can use joystick to control your robot, then you will
need joy_node
node from joy
package and
teleop_node
node from teleop_twist_joy
package.
Converting motion command to motor drive signal
In this section you will see part of default firmware for interfacing motors. Your node will subscribe to topic with geometry_msgs/Twist
messages, drive the
motors, read encoders and publish their state to appropriate topic. Below you can see a fragment of the code that calculates the kinematics of the robot:
void updateRosbotOdometry(RosbotDrive *drive, RosbotOdometry_t *odom, float dtime)
{
double curr_wheel_R_ang_pos;
double curr_wheel_L_ang_pos;
odom->wheel_FR_ang_pos = drive->getAngularPos(MOTOR_FR);
odom->wheel_FL_ang_pos = drive->getAngularPos(MOTOR_FL);
odom->wheel_RR_ang_pos = drive->getAngularPos(MOTOR_RR);
odom->wheel_RL_ang_pos = drive->getAngularPos(MOTOR_RL);
if (drive->getRosbotDriveType() == 4)
{
curr_wheel_R_ang_pos = (odom->wheel_FR_ang_pos + odom->wheel_RR_ang_pos) / (2 * TYRE_DEFLATION);
curr_wheel_L_ang_pos = (odom->wheel_FL_ang_pos + odom->wheel_RL_ang_pos) / (2 * TYRE_DEFLATION);
}
else
{
curr_wheel_R_ang_pos = odom->wheel_FR_ang_pos;
curr_wheel_L_ang_pos = odom->wheel_FL_ang_pos;
}
odom->wheel_L_ang_vel = (curr_wheel_L_ang_pos - odom->wheel_L_ang_pos) / (dtime);
odom->wheel_R_ang_vel = (curr_wheel_R_ang_pos - odom->wheel_R_ang_pos) / (dtime);
odom->wheel_L_ang_pos = curr_wheel_L_ang_pos;
odom->wheel_R_ang_pos = curr_wheel_R_ang_pos;
odom->robot_angular_vel = (((odom->wheel_R_ang_pos - odom->wheel_L_ang_pos) * WHEEL_RADIUS / (ROBOT_WIDTH * DIAMETER_MODIFICATOR)) - odom->robot_angular_pos) / dtime;
odom->robot_angular_pos = (odom->wheel_R_ang_pos - odom->wheel_L_ang_pos) * WHEEL_RADIUS / (ROBOT_WIDTH * DIAMETER_MODIFICATOR);
odom->robot_x_vel = (odom->wheel_L_ang_vel * WHEEL_RADIUS + odom->robot_angular_vel * ROBOT_WIDTH_HALF) * cos(odom->robot_angular_pos);
odom->robot_y_vel = (odom->wheel_L_ang_vel * WHEEL_RADIUS + odom->robot_angular_vel * ROBOT_WIDTH_HALF) * sin(odom->robot_angular_pos);
odom->robot_x_pos = odom->robot_x_pos + odom->robot_x_vel * dtime;
odom->robot_y_pos = odom->robot_y_pos + odom->robot_y_vel * dtime;
}
As we can see function updateRosbotOdometry does exactly the same calculation that we described in the theoretical introduction.
Running motor controller step by step
In this section you will learn how to control your robot movement with
keyboard. You will need teleop_twist_keyboard
node from
teleop_twist_keyboard
package.
Log in to your CORE2 device through remote desktop and run terminal. In
first terminal window run roscore
, in second run:
For ROSbot 2.0:
roslaunch rosbot_ekf all.launch
For PRO version add parameter:
roslaunch rosbot_ekf all.launch rosbot_pro:=true
This program is responsible for bridging your CORE2 to ROS network. When you are working with simulator, then above bridge is not necessary. Gazebo will subscribe appropriate topics automatically. In third terminal window run:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Now you can control your robot with keyboard with following functions for buttons:
’i’
- move forward’,’
- move backward’j’
- turn left’l’
- turn right’k’
- stop’q’
- increase speed’z’
- decrease speed
You should get similar view in rqt_graph
:
Running motor controller with roslaunch
To enable control of ROSbot with single launch file we need to prepare it or use launch from tutorial_pkg.
Inside the ~/ros_workspace/src/tutorial_pkg/launch
create tutorial_3.launch
with below content:
<launch>
<arg name="rosbot_pro" default="false" />
<arg name="use_gazebo" default="false" />
<!-- Gazebo -->
<group if="$(arg use_gazebo)">
<include file="$(find rosbot_description)/launch/rosbot_gazebo.launch"/>
<param name="use_sim_time" value="true" />
</group>
<!-- ROSbot 2.0 -->
<group unless="$(arg use_gazebo)">
<include file="$(find rosbot_ekf)/launch/all.launch">
<arg name="rosbot_pro" value="$(arg rosbot_pro)" />
</include>
</group>
<node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen"/>
</launch>
We can use this launch files each time we want to start communication between SBC and CORE2.
Running motor controller with forward kinematics task
In this section you will control your robot with keyboard and observe as it publishes its own position.
If you are working with ROSbot: Log in to your CORE2 device through remote desktop, run terminal and start your robot as previously. In another terminal window run:
rostopic echo /pose
If you are working with Gazebo: Start Gazebo as previously. In another terminal window run:
rostopic echo /odom
Above difference comes from the fact, that Gazebo and ROSbot are publishing its position in different ways.
Remember, that you need to have active window with teleop_twist_keyboard
to control robot movement.
You should get something like this on your screen:
Data visualization with PlotJuggler
In this section you will learn how to visualize data from ros topics using PlotJuggler. It is a simple tool that allows you to plot logged data, in particular timeseries. You can learn more about the tool on its official webpage.
How to use
Start PlotJuggler:
rosrun plotjuggler plotjuggler
In case your image lacks this tool you can install it by typing:
sudo apt-get update
sudo apt-get install ros-melodic-plotjuggler-ros
From menu bar select Streaming > Start: ROS_Topic_Streamer. In pop-up menu that will appear choose /pose from available topic names and press ok.
Pressing CTRL and SHIFT select positions:
- /pose/pose/position/x
- /pose/pose/position/y
- /pose/pose/position/z
and then drag and drop them to the window area. This way you can comfortably observe changes in the odometry data during robot motion:
Robot visualization with Rviz
Rviz is tool which allows visualization of robot position, travelled path, planned trajectory, sensor state or obstacles surrounding robot.
To run it type in terminal:
rviz
You can also start all nodes with single .launch
file:
<launch>
<arg name="rosbot_pro" default="false" />
<arg name="use_gazebo" default="false" />
<!-- Gazebo -->
<group if="$(arg use_gazebo)">
<include file="$(find rosbot_description)/launch/rosbot_gazebo.launch"/>
<param name="use_sim_time" value="true" />
</group>
<!-- ROSbot 2.0 -->
<group unless="$(arg use_gazebo)">
<include file="$(find rosbot_ekf)/launch/all.launch">
<arg name="rosbot_pro" value="$(arg rosbot_pro)" />
</include>
</group>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find tutorial_pkg)/rviz/tutorial_3.rviz"/>
<node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen"/>
</launch>
Rviz can be launched with argument pointing to file .rviz
, it will contain window configuration. When you are satisfied with visualization parameters, choose File -> Save config. At next rviz
launch config will be restored.
New window will appear:
Application main view consists of:
Toolbar
Visualized items list
Visualization window
Object manipulation buttons
By default you will see only base frame, to add any other object push Add from object manipulation buttons. In new window, there are two tabs By display type and By topic. First one is for manual selection from all possible objects, second one contains only currently published topics.
After you choose object to display, click OK button and it will appear in visualization window.
Now we will visualize position published by your robot, run rviz
,
click Add and choose tab By topic.
If you are working with ROSbot:
Find topic /pose
and choose Pose
and click OK.
If you are working with Gazebo:
Find topic /odom
and choose Odometry
and click OK.
Again click Add, choose tab By display type, TF and click OK.
Then in visualized items list find position Fixed Frame
and from dropdown list choose odom
.
After this is done, you should see an arrow representing position and orientation of your robot. You will see also representation of coordinate frames bounded with robot starting position and robot base. Move your robot and observe as arrow changes its position.
Visualization of any other item is performed in the same way. In further lessons, as you will produce more objects to visualize, you will add them to the same view.
Summary
After completing this tutorial you should be able to control motor
attached to your CORE2 device, set desired velocity for robot with
geometry_msgs/Twist
message, determine position of your robot using
odometry, publish it into tf
frames or as a PoseStamped
message and visualize position of your robot using rviz
.
by Łukasz Mitka, Husarion
Do you need any support with completing this tutorial or have any difficulties with software or hardware? Feel free to describe your thoughts on our community forum: https://community.husarion.com/ or to contact with our support: support@husarion.com