Designing and implementation of trajectory control for a wheeled robot with ROS
For this tutorial, I used the robot that I did in the last tutorial. If you want to see it, you can find it here.
This tutorial has two parts:
1. Creation of a node for the trajectory reference.
2. Creation of a node for the controller of the robot.
In my case, I used the same ROS package of the above tutorial, get it here, you can implement it in a new package if you wish.
First, in your ROS package create an src folder, in this folder, we should add the nodes that will be created in the next parts.
Creation of a node for the trajectory reference
For the example, I used a circular reference, for that I created a new python file named circle_referency.py on the src folder.
In all nodes that you create with python, the first line you should add is:
#!/usr/bin/env pythonWith this line, ROS knows what python interpreter has to use.
Next, add the following lines:
import rospy
import tf
import math
import geometry_msgs.msg if __name__ == '__main__':
# Init the node
rospy.init_node('circle_reference') # Publisher for the reference
ref_pub = rospy.Publisher(
'/mobile_base_controller/reference',
geometry_msgs.msg.Point,
queue_size=1) # Define a transform Broadcaster
br = tf.TransformBroadcaster() # Define the node execution frequency
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
# Doing the circle reference
t =rospy.Time.now().to_sec() * math.pi
x = 2.0 * math.cos(t/70)
y = 2.0 * math.sin(t/70) # Create a child frame of odom for see the reference in RVIZ
br.sendTransform(
[ x, y, 0.0],
[0.0, 0.0, 0.0, 1.0],
rospy.Time.now(),
"reference",
"odom") # Send the reference into topic
reference = geometry_msgs.msg.Point()
reference.x = x
reference.y = y
ref_pub.publish(reference)
rate.sleep()
For the node, I imported the necessary libraries, and on the main of code, begin the node with rospy.init_node, next create a topic publisher, next define a transform broadcaster if you want to see the reference in RVIZ, and finally define the time rate.
In the while loop, we define the code that we want to execute cyclically, in this example, every 0.1 seconds. First I defined the circular reference with ROS time and the x and y position with the help of the math library. After, I sent that position to a reference frame with the transform broadcaster, and finally, I published the positions of reference in the /mobile_base_controller/reference topic defined in ref_pub. The reference is sending with a Point message of ROS.
To test the node, save the file and open a terminal. In the terminal go to the src folder of your ROS package and add the following line:
chmod +x circle_reference.pyThat line makes your node executable.
For the example, we spawn the robot of the first tutorial with this line:
roslaunch ros_2wmr run_2wmr.launchAfter, open another terminal and run the node with this line:
rosrun <your_ros_pkg> circle_reference.pyIn RVIZ click on Add button and chooses the TF option and put the word reference in the name of the frame. If all is correct you can see how the circular reference is shown in the rviz as you see in the next gif:
And for seeing the reference is publishing in /mobile_base_controller/reference topic, open other terminal and put the next line:
rostopic echo /mobile_base_controller/reference And you can see in the terminal the Point msg as this gif:
Creation of a node for the controller of the robot
For the design of the controller with python, I used a NumPy library, if you don’t have the library only put this line in a terminal:
pip install numpyThe controller that I implemented is an inverse-kinematic control:
Where U_c is the control signal, J_a is the Jacobian of the robot, h_d is the derivate of the reference, k is a matrix 2x2 of constants for the calibration of the controller, and ~h is the error. The next image shows the block diagram of the controller with the robot that I implemented:
With the diagram, we need to subscribe to a Reference topic and an Odom topic of the robot and need to publish the Control signal. The derivate block I considered is the difference between the actual reference and the above position reference multiplied for frequency rate.
In the src folder create a python file called trajectory_control.py.
For the example, I defined three functions, the first is the controller:
#iNVERSE KINEMATIC PID CONTROL
def Control(angle,x_pos,y_pos,pos_before,pos_ref,K_sin):
# Jacobian of robot
Jr=np.array([
[math.cos(angle) ,-0.15 * math.sin(angle)],
[math.sin(angle) ,0.15 * math.cos(angle)]
]) # Calculate the inverse of Jacobian
Jr_inv=np.linalg.inv(Jr) # Define the k matrix
K=np.array([[K_sin[0], 0],[0, K_sin[1]]]) # Derivate of position reference
posp_d=pos_ref - pos_before # Define the position of robot
pos=np.array([[x_pos],[y_pos]]) # Position error
pos_error=pos_ref - pos
#Define a tanh as saturator of control
pos_error[0, 0]=math.tanh(pos_error[0, 0])
pos_error[1, 0]=math.tanh(pos_error[1, 0]) # Control signal
c=np.dot(Jr_inv, (10 * posp_d + np.dot(K,pos_error)))
c1=c[0, 0]
c2=c[1, 0]
return c1,c2
The entrances of function are the angle of the robot, the x and y position of the robot, the above reference, the actual reference, and the constants of the controller. The angle and the position in x and y of the robot are for subscribing a /mobile_base_controller/odom and the next function:
#ODOMETRY DATA
def Odom(msg):
global x
global y
global theta
x=msg.pose.pose.position.x
y=msg.pose.pose.position.y
rot_q=msg.pose.pose.orientation
angles=euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w]
)
theta=angles[2]Define global variables for changing these global variables for the odometry, only take the x and y value and take the quaternion angles and transform to Euler angles to take the angle of the robot. The reference of the controller are subscribing a /mobile_base_controller/reference and define the next function:
#CIRCLE REFERENCE
def reference(msg):
global p_ref
p_ref[0, 0] = msg.x
p_ref[1, 0] = msg.yDefine to global the vector that uses for reference and change with the data of the reference topic. With these declarations this is all code:
#!/usr/bin/env python
import rospy
import math
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
import geometry_msgs.msg
import numpy as np # DEFINE CONSTANTS FOR THE CODE
p_ref_ant=np.array([[0.0],[0.0]])
p_ref=np.array([[0.0],[0.0]])
x= 0.0
y= 0.0
theta=0.0 # iNVERSE KINEMATIC PID CONTROL
def Control(angle,x_pos,y_pos,pos_before,pos_ref,K_sin):
# Jacobian of robot
Jr=np.array([
[math.cos(angle) ,-0.15 * math.sin(angle)],
[math.sin(angle) ,0.15 * math.cos(angle)]
]) # Calculate the inverse of Jacobian
Jr_inv=np.linalg.inv(Jr) # Define the k matrix
K=np.array([[K_sin[0], 0],[0, K_sin[1]]]) # Derivate of position reference
posp_d=pos_ref - pos_before # Define the position of robot
pos=np.array([[x_pos],[y_pos]]) # Position error
pos_error=pos_ref - pos
# Define a tanh as saturator of control
pos_error[0, 0]=math.tanh(pos_error[0, 0])
pos_error[1, 0]=math.tanh(pos_error[1, 0]) # Control signal
c=np.dot(Jr_inv, (10 * posp_d + np.dot(K,pos_error)))
c1=c[0, 0]
c2=c[1, 0]
return c1,c2# ODOMETRY DATA
def Odom(msg):
global x
global y
global theta
x=msg.pose.pose.position.x
y=msg.pose.pose.position.y
rot_q=msg.pose.pose.orientation
angles=euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w]
)
theta=angles[2] # CIRCLE REFERENCE
def reference(msg):
global p_ref
p_ref[0, 0] = msg.x
p_ref[1, 0] = msg.yif __name__ == '__main__':
# NODE DEFINITION
rospy.init_node('trajectory_control')# SUBSCRIBING ODOM TOPIC
sub_odom = rospy.Subscriber(
'/mobile_base_controller/odom',Odometry,Odom) # SUBSCRIBING REFERENCE TOPIC
sub_reference = rospy.Subscriber(
'/mobile_base_controller/reference',
geometry_msgs.msg.Point,reference)# PUBLISH CONTROL SIGNAL
diff_vel = rospy.Publisher(
'/mobile_base_controller/cmd_vel',
geometry_msgs.msg.Twist,queue_size=1)
rate = rospy.Rate(10.0) while not rospy.is_shutdown():
# CONTROLLER
linear, angular = Control(
theta, x, y, p_ref_ant, p_ref, [0.6, 0.6]) # UPDATE THE ABOVE REFERENCE
p_ref_ant = p_ref # CONTROL SIGNAL SATURED
if linear > 1.0:
linear = 1
elif linear < -0.5:
linear = -0.5 if angular > 1.7 :
angular = 1.7
elif angular < -1.7:
angular = -1.7 # PRINT ON THE TERMINAL THE LINEAR AND ANGULAR SPEEDS
print("S. Control [u w]: [%s %s]"%(linear,angular)) # SEND VELOCITIES
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
diff_vel.publish(cmd)
rate.sleep()
You can see that the control signal is saturated with the max linear and angular velocity of the robot. With this code, you only save the file and put the line to convert to an executable file in the src folder on a terminal:
chmod +x trajectory_control.pyWith that, spawn the robot and run the reference node, finally put the next line to execute the trajectory_control node in a new terminal:
rosrun <your_ros_pkg> trajectory_control.pyIf all is OK you can see the robot is following the reference and you can see in the terminal the linear and angular velocity similar to this gif:
If you want to know more about robots you can go to my GitHub.
Thanks for your time reading this post 😃 !!
If you have questions or comments I am happy to answer. For contact with me, you can follow me on my Linkedin.
Can we help you?
We are ready to listen to you. If you need some help for creating the next big thing, you can contact our team on our website or at info@jrtec.io.
