Motion Control

Motion control is a key aspect of robotic functionality, allowing precise movement based on predetermined references within the robot’s local coordinate system. This involves generating linear and angular velocity references, which are then translated into specific wheel velocities via inverse kinematics. The controller board receives these velocity references to control the wheel’s motion, with feedback on actual velocities provided by the wheel encoders. This feedback is crucial for odometry, which employs forward kinematics to convert wheel velocities back into robot frame velocities for accurate positioning.

The figure below illustrates how this is implemented in ROS 2.

Figure: Motion control communication diagram.

Motion Reference

UiAbot’s motion is guided by linear and angular velocity references, specified in the robot’s local coordinate frame. These references are generated by publishing geometry_msgs/Twist message to the /cmd_vel topic.

The figure above shows this by using the built-in teleop_twist_keyboard node, that converts keyboard input to linear and angular velocity. Another solution would be to create a node that generates a trajectory and publishes a geometry_msgs/Twist message to the /cmd_vel topic.

Note

Command to run the teleop_twist_keyboard node:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Inverse Kinematics

To translate the desired motion from the robot’s coordinate frame into wheel-specific velocity commands, inverse kinematics is used. The kinematic model considers the robot’s geometry, particularly how linear and angular velocities correlate with wheel rotations, as shown in the figure below.

Figure: Definition of the robot’s local coordinate system for kinematic analysis.

Each wheel has the linear velocities \(\dot{x_r}\) and \(\dot{x_l}\), corresponding to right and left wheels, respectively. As shown in the figure below, the wheel angular velocity is denoted by \(\dot{\phi}\), and wheel radius by \(r\). The linear velocities for the wheels can be written as.

(1)\[\begin{split}\dot{x}_{r} = \dot{\phi}_{r}r \\ \dot{x}_{l} = \dot{\phi}_{l}r\end{split}\]

Considering pure translational velocity

We first consider that the robot has pure translational velocity. Since we are looking at the robot in the local coordinate frame, the linear velocity is composed of a velocities along the x- and y-axis. However, assuming zero slip, which means \(\dot{y}=0\). The linear velocity is only \(\dot{x}\). For the robot to have pure translation velocity, both wheels will need to have the same angular velocities. This yields the following equation.

(2)\[\begin{split}\begin{align} \dot{x}_{r} = \dot{x}_{l} = \dot{x} \\ \Rightarrow \dot{\phi}_{r}r = \dot{\phi}_{l}r = \dot{x} \\ \Rightarrow \dot{\phi}_{r} = \dot{\phi}_{l} = \frac{\dot{x}}{r} \end{align}\end{split}\]

Considering pure angular velocity

We then consider that the robot has pure rotational velocity. The robot rotational velocity is denoted as \(\dot{\theta}\). For the robot to have pure rotational velocity, the wheels will need to have opposite angular velocites.

(3)\[\dot{\phi}_{r} = -\dot{\phi}_{l}\]

Assuming no lateral movement, the wheels must follow a circle, which yields the following equations for the right (4) and left wheel (5).

(4)\[\dot{\phi}_{r}r = \frac{L}{2}\dot{\theta} \Rightarrow \dot{\phi}_{r} = \frac{L}{2}\frac{\dot{\theta}}{r}\]

(5)\[\dot{\phi}_{l}r = -\frac{L}{2}\dot{\theta} \Rightarrow \dot{\phi}_{l} = -\frac{L}{2}\frac{\dot{\theta}}{r}\]

Combining the velocities

Since both the definintion of the linear (6) and angular velocites are valid in vector-space, these can be added together for the right and left wheel in equation (6) and (7). This yields expressions for the wheel angular velocities, \(\dot{\phi}_r\) and \(\dot{\phi}_l\), given the linear and angular robot velocites, \(\dot{x}\) and \(\dot{\theta}\).

(6)\[\dot{\phi}_{r} = \frac{\dot{x}}{r} + \frac{L}{2}\frac{\dot{\theta}}{r}\]

(7)\[\dot{\phi}_{l} = \frac{\dot{x}}{r} - \frac{L}{2}\frac{\dot{\theta}}{r}\]

The inverse kinematics can then be written in matrix form as.

(8)\[\begin{split}\begin{bmatrix}\dot{\phi}_r \\ \dot{\phi}_l \end{bmatrix} = \frac{1}{r} \begin{bmatrix}1 & \frac{L}{2} \\ 1 & -\frac{L}{2} \end{bmatrix}\begin{bmatrix}\dot{x} \\ \dot{\theta}\end{bmatrix}\end{split}\]

The inverse kinematics of the robot is implemented in the control node.

Figure: The UiAbot ROS 2 control node diagram.

Note

Command to run the control node:

ros2 run uiabot control

Forward Kinematics

The forward kinematics is used to transform wheel angular velocities, \(\dot{\phi}_r\) and \(\dot{\phi}_l\), to robot angular and linear velocities, \(\dot{x}\) and \(\dot{\theta}\), given in the local coordinate frame. Based on equation (8) from the inverse kinematics, we can solve for \(\dot{x}\) and \(\dot{\theta}\). Which gives the following equation.

(9)\[\begin{split}\begin{bmatrix}\dot{x} \\ \dot{\theta}\end{bmatrix} = \begin{bmatrix}\frac{r}{2} & \frac{r}{2} \\ \frac{L}{2} & -\frac{L}{2}\end{bmatrix} \begin{bmatrix}\dot{\phi}_r \\ \dot{\phi}_l \end{bmatrix}\end{split}\]

The forward kinematics of the robot is implemented in the mechanical_odometry node.

Figure: The UiAbot ROS 2 mechanical odometry node diagram.

Note

Command to run the mechanical_odometry node:

ros2 run uiabot mechanical_odometry

Motor Controller

On the uiabot, the motors are driven by a ODrive controller board, which is connected to the Jetson Nano through USB. To make the ODrive available on the ROS 2 network, an interface node is created: odrive-ros2. This node makes it possible to publish velocity references to each wheel.

Figure: The ROS 2 ODrive controller node diagram.

Note

Command to run the odrive_ros2 node:

ros2 run odrive_ros2 odrive_ros2

Driving remotely with keyboard

This guide launches all nodes required to drive the UiAbot remotely using the pc keyboard.

1. Run the following command on the jetson to launch the control and odrive_ros2 nodes:

   ros2 launch uiabot teleop.launch.py
   

2. Run the following command on the pc to launch the teleop_twist_keyboard node:

   export ROS_DOMAIN_ID=5
   ros2 run teleop_twist_keyboard teleop_twist_keyboard
   

Driving remotely with Game Controller

This guide launches all nodes required to drive the UiAbot remotely using the game controller

1. Run the following command on the jetson to launch the control and odrive_ros2 nodes:

   ros2 launch uiabot teleop.launch.py
   

2. Run the following command on the jetson to launch the teleop_twist_keyboard node:

   export CONFIG_PATH=/home/jetson/shanwan_gamepad_config.yaml # export CONFIG_PATH=/path/to/your/config.yml
   export ROS_DOMAIN_ID=5
   ros2 launch teleop_twist_joy teleop-launch.py joy_config_file:=$CONFIG_PATH