Kinematics and Visualization

In this chapter, we will first discuss the theory that makes a robot without a torsion axis change its direction of travel. Then you will be able to drive the ROSbot using the keyboard or joystick and use tools such as plot_joggler and RViz2, which allows for eye-pleasing data visualization.

Prerequisites

This tutorial is dedicated to the ROSbot XL. However, most of the information and commands are also compatible with ROSbot 2R/2 PRO, and most changes are simple and come down to launching the appropriate container, depending on your hardware configuration. It is also possible to solve the tutorial using the Gazebo simulation.

info

If you don't already own a ROSbot, you can get one from the Husarion store.

• ROSbot XL
• ROSbot 2R
• ROSbot 2 PRO

The tutorials are designed to flow sequentially and are best followed in order. If you're looking to quickly assess the content of a specific tutorial, refer to the ROS 2 Tutorials repository, which contains the completed outcomes.

Robot control - theory

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 a robot kinematic model. ROSbot is a 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 W_L and W_R on the scheme) will have axes going through the robot geometric center. This way we can use a simpler kinematic model of differential wheeled robot. The name "differential" comes from the fact that a 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:

• R_c - robot geometric centre
• x_c - robot geometric centre x position
• y_c - robot geometric centre y position
• x_r - robot local x axis that determines front of the robot
• y_r - robot local y axis
• $α$ - robot angular position
• W_FL - front left wheel
• W_FR - front right wheel
• W_RL - rear left wheel
• W_RR - rear right wheel
• W_L - virtual left wheel
• W_R - virtual right wheel
• l₁ - distance between robot center and front/rear wheels
• l₂ - 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 the 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 (x_c, y_c, $α$). The forward kinematic task is to find new robot position (x_c, y_c, $α$)' after time $dt$ for given control parameters:

In our case the angular position $\phi$ and the angular speed $\omega$ of each virtual wheel will be an average of its real counterparts:

$$\begin{align*} \phi{W_L} &= \frac{\phi{W_{FL}}+\phi{W_{RL}}} 2 \qquad \phi{W_R} = \frac{\phi{W_{FR}}+\phi{W_{RR}}} 2 \\ \omega{W_L} &= \frac{\omega{W_{FL}}+\omega{W_{RL}}} 2 \qquad \omega{W_R} = \frac{\omega{W_{FR}}+\omega{W_{RR}}} 2 \end{align*}$$

The linear velocity of any virtual circle can be calculated by multiplying the angular velocity times the radius of the circle:

$$v_L = \omega{W_{L}}\cdot{r} \qquad v_R = \omega{W_{R}}\cdot{r}$$

where,

• v_L - linear speed of left virtual wheel
• v_R - linear speed of right virtual wheel
• r - the wheel radius.

We can determine robot angular position and speed with:

$$\alpha=(\phi{W_R}-\phi{W_L})\cdot\frac r l_2$$ $$\dot\alpha=\frac {d\alpha} {dt}$$

Then robot speed x and y component:

$$\dot{x}_c=(v_L+\dot\alpha\cdot\frac r l_2)\cos{\alpha}$$ $$\dot{y}_c=(v_L+\dot\alpha\cdot\frac r l_2)\sin{\alpha}$$

To get position:

$$x_c=\int^t_0\dot{x}_cdt \qquad y_c=\int^t_0\dot{y}_cdt$$

We assume starting position as (0,0).

Robot control - practice

Most common way to send movement commands to the robot is with use of geometry_msgs/Twist message type. Then the 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 sending appropriate values to /cmd_vel. Value x manipulating the component of linear speed vector and the z component of angular speed vector.

Robot speed control

Below is a simple example of posting a speed message using the keyboard. The geometry_msgs/Twist message is used for this purpose. The node that publishes it is called teleop_twist_keyboard from the package teleop_twist_keyboard. Alternatively you can use the joystick to control the robot, then you will need a joy_node from the joy package and a teleop_node from the teleop_twist_joy package. To run the node and control the robot using the keyboard, enter the following command.

ROSbot

On ROSbot

Running the ROSbot is very easy thanks to the created docker services.

husarion@husarion:~$

docker compose up -d microros <rosbot_service>

Depending on your ROSbot model, you should use an appropriate name for your <rosbot_service>. To check available docker service use the following command docker compose config --services, or find suitable Docker images prepared by Husarion.

Then in the new terminal run teleop_twist_keyboard.

husarion@husarion:~$

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Gazebo simulator

On Gazebo simulator

Running the simulator is very easy thanks to the created aliases. If you haven't done so already check configure your environment.

user@mylaptop:~$

ROSBOT_SIM

Then in the new terminal run teleop_twist_keyboard.

user@mylaptop:~$

ros2 run teleop_twist_keyboard teleop_twist_keyboard

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

Checking state of robot

Below is an example that allows you to read the current position in the terminal. To do this, open a new term and do:

husarion@husarion:~$

ros2 topic echo /odometry/filtered

After executing the above command, information about the current location should appear in the terminal, it looks like this.

The presented format is related to the type of message used to determine the state of the robot, a message of the nav_msgs/Odometry type containing information about speeds (twist) and positions (pose) is used. If you would like to get data only for a specific field, you can add the --field flag and names of these fields. Example:

husarion@husarion:~$

ros2 topic echo /odometry/filtered --field pose.pose

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.

Install additional package

To install additional package you can type:

husarion@husarion:~$

sudo apt-get install ros-$ROS_DISTRO-NAME-OF-PACKAGE

For PlotJuggler use:

husarion@husarion:~$

sudo apt-get install ros-$ROS_DISTRO-plotjuggler-ros

And run it:

husarion@husarion:~$

ros2 run plotjuggler plotjuggler

From menu bar select Streaming > Start: ROS Topic Subscriber. In pop-up menu that will appear choose /odometry/filtered from available topic names and press ok. To visualize data drag entrusted field from Timeseries List and drop it on the figure area. This way you can comfortably observe multiple values from odometry data during robot motion.

Data visualization with RViz2

RViz2 is tool which allows visualization of robot position, traveled path, planned trajectory, sensor state or obstacles surrounding the robot.

To run it type in terminal:

husarion@husarion:~$

rviz2

New window will appear:

By default you will see only empty space, to add any other object push Add. 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. Find RobotModel in By display type tab and press OK.

Then in new left panel in RobotModel find Description Topic and select /robot_description. You should see white object on the middle of space. This is because there is no defined transformation between the robot's position and the defined map reference system. To fix this, select the odom frame in the Fixed Frame field.

Now ROSbot should be visible. Let's add a global frame of reference. To do this, press again Add and select Axes.

The last thing will be added odometry msg. Click Add->By topic->/odometry/filtered. Information about covariance isn't necessary so we disable it Odometry->Covariance.

After this is done, you should see an arrow representing position and orientation of your robot. You will also see a representation of coordinate frames bounded with robot starting position and robot base. Move your robot and observe as the arrow changes its position.

Now lest save this configuration. Go to File->Save Config As and save it as rosbot.rviz in the new folder ~/ros2_ws/src/tutorial_pkg/rviz.

Now RViz2 can be launched with an argument pointing to file .rviz. To do this use:

husarion@husarion:~$

rviz2 -d ~/ros2_ws/src/tutorial_pkg/rviz/rosbot.rviz

Task 1

Try adding the visualize camera image in your current RViz2 configuration.

Summary

After completing this tutorial you should understand the math that enables the robot to move. You be able to control your ROSbot, change desired velocity and have better knowledge of new types of message like geometry_msgs/Twist, geometry_msgs/Pose and nav_msgs/Odometry, witch determine the speed and position of your robot. You know how to plot all your data and build your own RViz2 configuration for your robot. In the next chapter, we will look at the issue of vision in detail.

---

by Rafał Górecki, Husarion

Need help with this article or experiencing issues with software or hardware? 🤔

• Feel free to share your thoughts and questions on our Community Forum. 💬
• To contact service support, please use our dedicated Issue Form. 📝
• Alternatively, you can also contact our support team directly at: support@husarion.com. 📧