Distributed formation control for unicycle vehicles¶
In this page we show how to implement a distributed formation control algorithm for a team of unicycle vehicles. The resulting algorithm is simulated with Gazebo by using Turtlebot3 ground robots. A reference for this example can be found in [FCME10].
Prerequisites¶
We assume a working installation of ChoiRbot and Gazebo is available (see the installation page), and also that the Turtlebot3 ROS 2 files are installed. Moreover, we assume the reader to be familiar with the basic concepts of ROS 2, Python and ChoiRbot (see the quick start page).
Maths of the problem¶
We consider driving a team of \(N\) robots to a translationally independent formation in the \((x,y)\) plane that satisfies a given set of constraints.
System dynamics and communication graph¶
The considered algorithm models each robot as having single-integrator dynamics
where for all \(i \in \{1, \ldots, N\}\), \(x_i \in \mathbb{R}^2\) is the \(i\)-th system state (i.e., position) and \(u_i \in \mathbb{R}^2\) is the \(i\)-th system input (i.e., velocity).
We assume the robots communicate according to a given undirected graph \(\mathcal{G} = (\mathcal{V}, \mathcal{E})\), where \(\mathcal{V} = \{1, \ldots, N\}\) is the set of robots and \(\mathcal{E} \subset \mathcal{V} \times \mathcal{V}\) is the set of edges. If \((i,j) \in \mathcal{E}\), then also \((j,i) \in \mathcal{E}\) and robots \(i\) and \(j\) can send information to each other. The set of neighbors of each agent \(i\) is denoted by \(\mathcal{N}_i = \{j \in \mathcal{V} \mid (i,j) \in \mathcal{E}\}\).
Formation specification and distributed control law¶
The desired formation is assumed to be rigid (see also [FCME10]) and is specified by a set of desired distances \(\{d_{ij}\}_{(i,j) \in \mathcal{E}}\) between any two communicating robots \(i\) and \(j\).
The distributed control law applied by each robot \(i\) is
In the example considered here, there are \(N = 6\) ground robots that must form an hexagon with side \(L\). Adjacent robots can communicate and also opposite robots, therefore the adjacency matrix of the communication graph is
Adjacent robots must have distance \(L\), while opposite robots must have distance \(2L\), therefore the matrix of desired inter-robot distances is
Implementation in ChoiRbot¶
In order to implement the formation control example in ChoiRbot, we consider the following nodes for each robot:
a Team Guidance node that exchanges the current position with the neighbors and computes the input \(u_i(t)\) with a certain frequency
a Control node that converts the single-integrator input (vector velocity) into the corresponding suitable unicycle input (angular and linear velocity)
To run the simulation, we will also need to interface ChoiRbot with Turtlebot3 robots in Gazebo. Finally, we will also need a launch file and the executable scripts (as required by the ChoiRbot paradigm).
We analyze each of these components separately in the following subsections.
Team Guidance¶
The Team Guidance layer is responsible for the actual implementation of the
distributed control law outlined above. It is based on the abstract class
DistributedControlGuidance
,
which extends the basic Guidance
class as follows:
from geometry_msgs.msg import Vector3
from choirbot.guidance import Guidance
class DistributedControlGuidance(Guidance):
def __init__(self, update_frequency: float, pos_handler: str=None, pos_topic: str=None):
super().__init__(pos_handler, pos_topic)
self.publisher_ = self.create_publisher(Vector3, 'velocity', 1)
self.update_frequency = update_frequency
self.timer = self.create_timer(1.0/update_frequency, self.control)
When the class is instantiated, the __init__
method creates a publisher
for the velocity inputs and creates a timer that executes the method control
with a user-defined frequency. Note that, since this class extends
Guidance
, it inherits several useful attributes:
current_pose
, which always contains the most up-to-date robot pose and is periodically updated by the parent class;communicator
, which is an instance of the classCommunicator
and provides methods for graph-based communication;in_neighbors
andout_neighbors
, which are the lists of the robot’s in- and out- neighbors (in this example the graph is undirected so the two lists are identical and are equal to \(\mathcal{N}_i\)).
The main body of the class that is repeatedly executed is contained in the
control
method. The code is as follows:
def control(self):
# exchange current position with neighbors
data = self.communicator.neighbors_exchange(self.current_pose.position, self.in_neighbors, self.out_neighbors, False)
# compute input
u = self.evaluate_velocity(data)
# send input to planner/controller
self.send_input(u)
When the method is run, it first exchanges the current position with the neighbors
by calling neighbors_exchange()
,
which returns a dictionary with the positions received from the neighbors.
Then, it calls the method evaluate_velocity
, which computes the actual
value of \(u_i(t)\), and finally calls the method send_input
,
which sends the input to the controller node. The method evaluate_velocity
is left unimplemented in order to allow for arbitrary control laws,
while the body of send_input
is very simple and only publishes the
computed input on the velocity
topic:
def send_input(self, u):
msg = Vector3()
msg.x = u[0]
msg.y = u[1]
msg.z = u[2]
self.publisher_.publish(msg)
In order to implement the formation control law, the class
DistributedControlGuidance
must be extended to override the evaluate_velocity
method.
The child class FormationControlGuidance
is as follows:
import numpy as np
from numpy.linalg import norm
class FormationControlGuidance(DistributedControlGuidance):
def __init__(self, update_frequency: float, gain: float=0.1, pos_handler: str=None, pos_topic: str=None):
super().__init__(update_frequency, pos_handler, pos_topic)
self.formation_control_gain = gain
self.weights = self.get_parameter('weights').value
def evaluate_velocity(self, neigh_data):
u = np.zeros(3)
for ii, pos_ii in neigh_data.items():
error = pos_ii - self.current_pose.position
u += self.formation_control_gain*(norm(error)**2- self.weights[ii]**2) * error
return u
As it can be seen from the __init__
method, this class requires that
the ROS parameter weights
is set. This parameter represents the desired
inter-robot distances \(\{d_{ij}\}_{(i,j) \in \mathcal{E}}\) and must be
passed to each robot \(i\) as a dictionary with each element having key
\(j\) and value \(d_{ij}\).
The evaluate_velocity
method is simply the implementation of
the distributed formation control law and returns \(u_i(t)\).
Unicycle control¶
The goal of the control node is to translate the vector velocity input
\(u_i(t)\) into the corresponding unicycle inputs \(v_i(t)\)
(linear velocity) and \(\omega_i(t)\) (angular velocity).
This translation is performed according to the approach described in
[FCWGW+20] within the class UnicycleVelocityController
.
The initialization block of the class is as follows:
from choirbot.controller import Controller
from geometry_msgs.msg import Vector3, Twist
import numpy as np
class UnicycleVelocityController(Controller):
def __init__(self, pos_handler: str=None, pos_topic: str=None):
super().__init__(pos_handler, pos_topic)
self.subscription = self.create_subscription(Vector3, 'velocity', self.control_callback, 1)
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 1)
self.yaw = 0.0
self.yaw_old = 0.0
self.yaw_old_old = 0.0
Since the main job of this class is to translate the vector velocity input
into a unicycle input, in the __init__
method we simply create a publisher
and a subscription for the relative topics and we initialize the quantities
for the control translation scheme. The main job is performed by the subscription
callback method control_callback
, which implements the law described in
[FCWGW+20] and publishes the translated input in the cmd_vel
topic.
Interfacing with Gazebo¶
In order to run the algorithm within the Gazebo simulation environment,
we first need to create the robots. To this end, we use the
SpawnEntity
service provided by the Gazebo ros factory plugin, as suggested by
this thread
of the ROS community.
This service requires the Gazebo process to be executed with the following
command, which will be embedded later directly in the launch file:
gazebo -s libgazebo_ros_factory.so
To create the robots, we use a dedicated node interfacing with the SpawnEntity
service. It is implemented in the file turtlebot_spawner.py
in the
choirbot_examples
package. The node requires the following ROS parameters
position
: coordinates where the robot will be spawnednamespace
: ROS namespace for the robot nodes and topics
In the launch file, we will use differentiated namespaces for each robot and we also set the initial position for the formation control algorithm.
After each robot is created (suppose with the namespace ns
), Gazebo will publish
its updated pose in the /ns/odom
topic, which is retrieved by the Team guidance
class to compute the control input. Robots receive commands in the /ns/cmd_vel
topic as published by the unicycle control.
Launch file and executables¶
To launch the simulation, we first create the executable files in which all the relevant classes are instantiated. The following block shows the guidance executable:
import rclpy
from choirbot.guidance.distributed_control import FormationControlGuidance
import time
def main():
rclpy.init()
frequency = 100
gain = 0.1
guidance = FormationControlGuidance(frequency, gain, 'pubsub', 'odom')
rclpy.spin(guidance)
rclpy.shutdown()
The body of the main
function simply initializes the ROS python client (rclpy
),
creates the team guidance class
FormationControlGuidance
and spin
‘s over it. The code for the control layer is similar.
We need to add the two executables as entry points in the setup.py
file of the package,
together with the turtlebot spawner:
entry_points={
'console_scripts': [
'choirbot_formationcontrol_guidance = choirbot_examples.formationcontrol.guidance:main',
'choirbot_formationcontrol_control = choirbot_examples.formationcontrol.control:main',
'choirbot_turtlebot_spawner = choirbot_examples.turtlebot_spawner:main'
],
}
Finally, we can write the launch file, which will do the following tasks:
open an instance of Gazebo
spawn the Turtlebot robots with differentiated namespaces
execute the Team guidance layer and the control layer of each robot
First, we prepare a few needed variables as follows
import numpy as np
def generate_launch_description():
# reset seed of random number generator
np.random.seed(1)
L = 3.0 # length of hexagon sides
# generate communication matrix
Adj = np.array([ # alternated zeros and ones
[0, 1, 0, 1, 0, 1],
[1, 0, 1, 0, 1, 0],
[0, 1, 0, 1, 0, 1],
[1, 0, 1, 0, 1, 0],
[0, 1, 0, 1, 0, 1],
[1, 0, 1, 0, 1, 0]
])
# generate matrix of desired inter-robot distances
# adjacent robots have distance L
# opposite robots have distance 2L
W = np.array([
[0, L, 0, 2*L, 0, L],
[L, 0, L, 0, 2*L, 0],
[0, L, 0, L, 0, 2*L],
[2*L, 0, L, 0, L, 0],
[0, 2*L, 0, L, 0, L],
[L, 0, 2*L, 0, L, 0]
])
# generate coordinates of an hexagon with center in the origin
a = L/2
b = np.sqrt(3)*a
P = np.array([
[-b, a , 0],
[0, 2.0*a, 0],
[b, a, 0],
[b, -a, 0],
[0, -2.0*a, 0],
[-b, -a, 0]
])
# initial positions have a perturbation of at most L/3
P += np.random.uniform(-L/3, L/3, (6,3))
Now we create a launch description with the robot executables:
from launch_ros.actions import Node
from launch.actions import TimerAction
from launch import LaunchDescription
def generate_launch_description():
# ... previous code
robot_launch = []
# add executables for each robot
for i in range(6):
in_neighbors = np.nonzero(Adj[:, i])[0].tolist()
out_neighbors = np.nonzero(Adj[i, :])[0].tolist()
weights = W[i,:].tolist()
# guidance
robot_launch.append(Node(
package='choirbot_examples', node_executable='choirbot_formationcontrol_guidance', output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{'agent_id': i, 'N': N, 'in_neigh': in_neighbors, 'out_neigh': out_neighbors, 'weights': weights}]))
# controller
robot_launch.append(Node(
package='choirbot_examples', node_executable='choirbot_formationcontrol_controller', output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{'agent_id': i}]))
# the previous executables will be started with a 10-second delay to let Gazebo open
timer_action = TimerAction(period=10.0, actions=[LaunchDescription(robot_launch)])
Note that in the previous code we passed the relevant ROS parameters for the Team guidance and Control classes. For each robot, we must also run a robot spanwer node:
def generate_launch_description():
# ... previous code
launch_description = []
for i in range(6):
position = P[i, :].tolist()
# turtlebot spawner
launch_description.append(Node(
package='choirbot_examples', node_executable='choirbot_turtlebot_spawner', output='screen',
parameters=[{'namespace': 'agent_{}'.format(i), 'position': position}]))
Finally, we add the Gazebo launcher (gazebo.launch.py
) and the delayed nodes to the final
launch description:
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# ... previous code
# include launcher for gazebo
gazebo_launcher = os.path.join(get_package_share_directory('choirbot_examples'), 'gazebo.launch.py')
launch_description.append(IncludeLaunchDescription(PythonLaunchDescriptionSource(gazebo_launcher)))
launch_description.append(timer_action) # delayed nodes
return LaunchDescription(launch_description)
Running the simulation¶
To run the simulation, we simply need to execute the launch file. First we source the workspace:
source install/setup.bash
Now we are ready to run the example:
ros2 launch choirbot_examples formationcontrol.launch.py
A Gazebo window will open. After a few seconds, robots start to move until they reach the hexagonal formation:
References
- FCME10(1,2)
Mehran Mesbahi and Magnus Egerstedt. Graph theoretic methods in multiagent networks. Volume 33. Princeton University Press, 2010.
- FCWGW+20(1,2)
Sean Wilson, Paul Glotfelter, Li Wang, Siddharth Mayya, Gennaro Notomista, Mark Mote, and Magnus Egerstedt. The robotarium: globally impactful opportunities, challenges, and lessons learned in remote-access, distributed control of multirobot systems. IEEE Control Systems Magazine, 40(1):26–44, 2020.