diff --git a/real_robot/coordination_formation_control_pkg/README.md b/real_robot/coordination_formation_control_pkg/README.md
index 9f4aa13ef081db30e6eb3495228b18c58c90241c..ec46318bded23e6d7957ccc63703415cc9cb0adf 100644
--- a/real_robot/coordination_formation_control_pkg/README.md
+++ b/real_robot/coordination_formation_control_pkg/README.md
@@ -16,3 +16,6 @@ implement modules
- [X] send command should include conversion to velocity
. [ ] test omniwheels node
- [ ] Implement swam controller
+
+- Dependencies https://github.com/ethz-asl/libnabo.git
+It uses this ibrary to implement a k nearest negibour search
diff --git a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
index ab0bf2dc3c8e89c636052a0c0ba6e702e23bbfa0..be39e62b6d15e086aaf549c22e3b60d18312e6aa 100644
--- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
+++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
@@ -15,6 +15,15 @@ typedef enum robot_type
} robot_type_t;
+/* formation types*/
+typedef enum formation_matrix
+{
+ PAIR =2,
+ TRIANGLE = 3,
+ SQUARE = 4,
+ PENTAGON = 5
+}formation_matrix_t;
+
/* robot state-ugv and agv*/
typedef struct robot_state
{
@@ -27,6 +36,7 @@ typedef struct robot_state
typedef struct robot_ind
{
int id; /*d*/
+ int id_m; /* position in the distance vector*/
std::string robot_name; /* robot name*/
robot_type_t t; /*robot_type*/
float safe_ditance; /* important to prevent mistakenly putting a very small number*/
@@ -76,7 +86,7 @@ typedef struct form_gains
typedef struct form_param
{
float d; /* distance between agent*/
- float formation_t; /* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ int formation_t; /* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
float k;
float ratio; /* ratio between dist inter and obstacle*/
float eps;
@@ -85,10 +95,13 @@ typedef struct form_param
float h_alpha;
float h_beta;
float d_obs;
- float nav_type; /* convergence approach -1, parallel approach 2*/
- float integrator; /* single/ double integrator*/
+ int nav_type; /* convergence approach -1, parallel approach 2*/
+ int integrator; /* single/ double integrator*/
float dt; /* sample time*/
float int_max; /* maximum integral*/
+ float r; /* maximum allowd Neighbourhood radios, this is set based on the k ratio*/
+ float r_obs; /* maximum allowd Neighbourhood radios, this is set based on the k ratio*/
+
}form_param_t;
/* formation configuraiton*/
@@ -100,7 +113,7 @@ typedef struct formation_config
-typedef Eigen::Matrix<double, 2,6> input_vector_t;
+typedef Eigen::Matrix<float, 2,6> input_vector_t;
/* input vector conatains the following
* totall input x y
* ofr debugging I print the remaining input
@@ -110,7 +123,10 @@ typedef Eigen::Matrix<double, 2,6> input_vector_t;
* input orientation
*/
- typedef Eigen::Matrix <double, 2, 1>row_vector_2d_t;
+ typedef Eigen::Matrix <float, 2, 1>row_vector_2d_t;
+
+ typedef Eigen::Matrix <float,3,1> row_vector_3d_t;
+
diff --git a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
index c51b84511575b71650f25188030f55eb3ec0f81f..10656877b4c1ecbbecfcc06ac6d12064231838a2 100644
--- a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
+++ b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
@@ -1,15 +1,23 @@
#ifndef SWAM_CONTROLLER_ALG_UAV_H
#define SWAM_CONTROLLER_ALG_UAV_H
+#include "ros/ros.h"
+#include <eigen3/Eigen/Dense>
+#include "math.h"
#include "shared_definitions.h"
+#include <vector>
class swamControllerAlg
{
private:
formation_config_t form_param; /*parameters of the formation*/
row_vector_2d_t integral_error;
- row_vector_2d_t previous_error;
+ row_vector_2d_t previous_integral_error;
robot_ind_t robot_id;
+ formation_matrix_t form_type;
+ Eigen::MatrixXf iad; /* iter agent distances*/
+ int N; /* number of agents*/
+
public:
/* constructor*/
@@ -21,10 +29,28 @@ public:
protected:
/* configuration formation information like interagent distances*/
- void configure_formation();
-
-
-
+ void configureFormation();
+ std::vector<std::pair<int, float>> sortNeigbour(Eigen::MatrixXf M, row_vector_2d_t q);
+ /* compute fi function*/
+ float computeFi(row_vector_2d_t qj, row_vector_2d_t qi, float a, float b, float h, float eps, float r, float d);
+ /* compute sigma norm*/
+ float computeSigmaNorm(float z, float eps);
+ /*compute ph bumber function*/
+ float ph(float z, float h);
+ /* compute fi*/
+ float computefi(float a, float b, float z);
+ /* compute sigmaone*/
+ float sigmaOne(float z);
+/* sigma norm vector form*/
+ row_vector_2d_t sigmaOne(row_vector_2d_t z);
+ /* compute nij*/
+ row_vector_2d_t computeNij(row_vector_2d_t qj, row_vector_2d_t qi, float eps);
+ /*compute aij*/
+ float computeAij(row_vector_2d_t qj, row_vector_2d_t qi, float r,float eps, float h);
+ /* compute fi function for the obstacle*/
+ float computeFiB(row_vector_2d_t q_hat_i, row_vector_2d_t qi, float h_beta, float d_obs, float eps);
+ /* compute Bij*/
+ float computeBij(row_vector_2d_t q_hat_i, row_vector_2d_t qi, float d_obs, float eps, float h_beta);
};
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index e6b5f1c24c5ee389f3f8841c87a0a9ba2553b5ee..4adc2076d302d644d30d4e60505276d6d3d0fb87 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -79,7 +79,7 @@ int main(int argc, char **argv) {
}
else
{
- robot_ind.leader = false;
+ robot_ind.leader = true;
}
/* initialization of the formation configuration */
@@ -145,6 +145,7 @@ int main(int argc, char **argv) {
uav_def.robot_name = "cf";
uav_def.t = CRAZYFLIE;
uav_def.id = i;
+ uav_def.id_m = i-1;
sucess_initialization = false;
auto * uav = new robotState(uav_def, nh, sucess_initialization);
uavs.push_back(uav);
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index d3ee72a128bbad530bf3e15607c887afb884b59c..5f5d1f4ca59ce274617745cd56b94109d832eb04 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -112,6 +112,7 @@ int main(int argc, char **argv) {
ugv_def.robot_name = "omniwheel";
ugv_def.t = OMNIWHEELS;
ugv_def.id = i;
+ ugv_def.id_m = 1;
sucess_initialization = false;
auto * ugv = new robotState(ugv_def, nh, sucess_initialization);
ugvs.push_back(ugv);
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 3c82f9d4e28e9e079d8fe191b79c4cd04a001751..8f839e25181f996bb7cd71962964d08c8d06d618 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -5,6 +5,59 @@
swamControllerAlg::swamControllerAlg(robot_ind_t indentification, formation_config_t formation_config)
{
+ this->form_param = formation_config;
+ this->robot_id = indentification;
+ this->integral_error << 0,0,0;
+ this->previous_integral_error << 0,0,0;
+
+ /* we want to ensure the following condition is met
+ * c1_alpha < c1_gamma < c1_beta*/
+ if((this->form_param.gain.c1_alpha > this->form_param.gain.c1_gamma) || (this->form_param.gain.c1_gamma > this->form_param.gain.c1_beta))
+ {
+ ROS_INFO("Swam controller: Following condition not met!!!!!! c1_alpha < c1_gamma < c1_beta");
+ EXIT_FAILURE;
+ }
+ /* make sure that the following codition is met
+ * 0 < a <= b*/
+ if((0 > this->form_param.param.a) || (this->form_param.param.a > this->form_param.param.b))
+ {
+ ROS_INFO("Swam controller: Following condition not met!!!!!! 0 < a <= b");
+ EXIT_FAILURE;
+ }
+ if((this->form_param.param.nav_type != 1) || (this->form_param.param.nav_type != 2))
+ {
+ ROS_INFO("Swam Controller: Navigational_type should be 1-converge approach or 2 - parallel approach");
+ EXIT_FAILURE;
+ }
+
+ /* compute the c2 for each gains*/
+ this->form_param.gain.c2_alpha = 2*sqrt(this->form_param.gain.c1_alpha);
+ this->form_param.gain.c2_gamma = 2*sqrt(this->form_param.gain.c1_gamma);
+ this->form_param.gain.c2_beta = 2*sqrt(this->form_param.gain.c1_beta);
+
+ /* formation Neighbourhood distance*/
+ this->form_param.param.r = this->form_param.param.k*this->form_param.param.d;
+ /* set the obstcale avoidance parameters*/
+ this->form_param.param.d_obs = this->form_param.param.d*this->form_param.param.ratio;
+ this->form_param.param.r_obs = this->form_param.param.k*this->form_param.param.d_obs;
+ /* ensure there is no collision between robots and obstacles*/
+ float total_safety_distance = this->robot_id.robot_size/2 + this->robot_id.safe_ditance;
+ if((total_safety_distance < this->form_param.param.d) || (total_safety_distance < this->form_param.param.d_obs))
+ {
+ ROS_INFO("Swam controller: The distance between the robot and other robot or obstacle might result in collision: total_safety_distance: %f robot_size: %f safe_ditance: %f",
+ total_safety_distance, this->form_param.param.d, this->robot_id.safe_ditance);
+ EXIT_FAILURE;
+ }
+
+ this->N = this->form_param.param.formation_t;
+ this->form_type = formation_matrix_t(this->form_param.param.formation_t);
+
+ /* formation configuration steps*/
+ this->configureFormation();
+
+
+
+ ROS_INFO("Robot %s_%d swam controller initialized", this->robot_id.robot_name.c_str(), this->robot_id.id);
}
@@ -14,9 +67,369 @@ swamControllerAlg::~swamControllerAlg()
}
+void swamControllerAlg::configureFormation()
+{
+ /* this consists of predefined formation*/
+ float d = this->form_param.param.d;
+ float h = 0;
+
+ if(this->form_type == PAIR)
+ {
+ this->iad(2,2);
+ this->iad << 0, d,
+ d, 0;
+ }
+ else if(this->form_type == TRIANGLE)
+ {
+ this->iad(3,3);
+
+ this->iad << 0, d, d,
+ d, 0, d,
+ d, d, 0;
+
+ }
+ else if(this->form_type == SQUARE)
+ {
+ h = sqrt(2*d*d); // diagonal distance
+ this->iad(4,4);
+ this->iad << 0, d, h, d,
+ d,0,d,h,
+ h,d,0,d,
+ d,h,d,0;
+ }
+ else if(this->form_type == PENTAGON)
+ {
+ h = d + 2*d*cos(72*M_PI/180.0); // diagonal distance
+ this->iad(4,4);
+ this->iad << 0, d, h, h, d,
+ d,0,d,h,h,
+ h,d,0,d,h,
+ h,h,d,0,d,
+ d,h,h,d,0;
+ }
+ else
+ {
+ ROS_INFO("Swam Controller: Wrong formation type: 2 PAIR, 3 TRIANGLE, 4 SQUARE, 5 PENTAGON ");
+ EXIT_FAILURE;
+ }
+ ROS_INFO("Swam Controller: interagent matrix");
+ std::cout << this->iad << std::endl;
+
+
+}
+
/* main controller implementation*/
input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf p, waypoint_t ref, obstacle_t* obs)
{
+ /* implementation intended for control in the x y plane*/
+ /* make sure that the number of agent match the matrix*/
+ if((q.cols() != this->N ) || (p.cols() != this->N ))
+ {
+ ROS_INFO("Swam controller: p and q matrix is not correct, Number of agent: %d p/q column size: %d/%d", this->N, (int)q.cols(), (int)p.cols());
+ EXIT_FAILURE;
+ }
+ /* formation computation*/
+ row_vector_2d_t qi,pi,qj,pj;
+ qi(0) = q(0,this->robot_id.id_m);
+ qi(1) = q(1,this->robot_id.id_m);
+ pi(0) = p(0,this->robot_id.id_m);
+ pi(1) = p(1,this->robot_id.id_m);
+ std::vector<std::pair<int, float>> neighbour_sorted = this->sortNeigbour(q, qi);
+ row_vector_2d_t u_formation;//(0.0,0.0);
+ row_vector_2d_t grad_term;//(0.0,0.0);
+ row_vector_2d_t cons_term;//(0.0,0.0);
+ row_vector_2d_t deriv_term;//(0.0,0.0);
+ row_vector_2d_t error_grad_term;//(0.0,0.0);
+ u_formation << 0.0,0.0;
+ grad_term << 0.0,0.0;
+ cons_term << 0.0,0.0;
+ deriv_term << 0.0,0.0;
+ error_grad_term << 0.0,0.0;
+
+ float fi,aij,d;
+ row_vector_2d_t nij;
+
+
+ for(int i = 0; i < this->N; i++)
+ {
+ /* compute input for those in the neighbour*/
+ if(neighbour_sorted[i].second > this->form_param.param.r)
+ {
+ break;
+ }
+ /* only for neighbours*/
+ if(i != this->robot_id.id_m)
+ {
+ qj(0) =q(0,i);
+ qj(1) =q(1,i);
+ pj(0) =p(0,i);
+ pj(1) =p(1,i);
+ d = this->iad(this->robot_id.id_m,i); /* distance between neighbours*/
+ // compute the gradient term
+ fi = this->computeFi(qj,qi,this->form_param.param.a,this->form_param.param.b,this->form_param.param.h_alpha,
+ this->form_param.param.eps,this->form_param.param.r,d);
+ nij = this->computeNij(qj,qi,this->form_param.param.eps);
+ grad_term = grad_term + fi*nij;
+ // consesus term
+ if (this->form_param.param.integrator == 2)
+ {
+ aij = this->computeAij(qj,qi,this->form_param.param.r,this->form_param.param.eps,this->form_param.param.h_alpha);
+ cons_term = cons_term + aij*(pj-pi);
+ }
+
+ }
+ }
+ error_grad_term = grad_term;
+ u_formation = grad_term*this->form_param.gain.c1_alpha+cons_term*this->form_param.gain.c2_alpha;
+
+
+ /* obstacle formation*/
+ row_vector_2d_t u_obstacle;
+ u_obstacle << 0.0,0.0; //(0.0,0.0);
+
+ if (obs != NULL)
+ {
+ /* if we have obstacle*/
+ float obs_size = sizeof(obs)/sizeof(*obs);
+ row_vector_2d_t grad_term_obs, u_obstacle;//(0.0,0.0);
+ row_vector_2d_t cons_term_obs;//(0.0,0.0);
+ grad_term_obs << 0.0,0.0;
+ cons_term_obs << 0.0,0.0;
+ u_obstacle << 0.0,0.0;
+ row_vector_2d_t yk, ak, q_hat_i, p_hat_i;
+ float Rk, mu;
+ Eigen::MatrixXf a__;
+ for(int o = 0; o<obs_size; o++)
+ {
+ yk(0) = obs[o].q(0);
+ yk(1) = obs[o].q(1);
+ Rk = obs[o].radius;
+ mu = Rk/(qi-yk).norm();
+ ak = (qi-yk)/(qi-yk).norm();
+ a__ = ak*ak.transpose();
+ Eigen::Matrix2f P;
+ P << 1.0, 0.0, 1.0,0.0;
+ q_hat_i = mu*qi+(1-mu)*yk;
+ p_hat_i = mu*(P-a__)*pi;
+ float d2obs = (qi-q_hat_i).norm();
+ if (d2obs < this->form_param.param.r_obs) /* compute for neighbour obstacles*/
+ {
+ /* gradient term*/
+ float fi_beta = this->computeFiB(q_hat_i,qi,this->form_param.param.h_beta,this->form_param.param.d_obs,this->form_param.param.eps);
+ row_vector_2d_t nij = this->computeNij(q_hat_i, qi, this->form_param.param.eps);
+ grad_term_obs = grad_term_obs+fi_beta*nij;
+ /*consesus term*/
+ if (this->form_param.param.integrator == 2)
+ {
+ float bij = this->computeBij(q_hat_i,qi,this->form_param.param.d_obs,this->form_param.param.eps, this->form_param.param.h_beta);
+ cons_term_obs = cons_term_obs + bij*(p_hat_i-pi);
+ }
+ }
+ }
+
+ u_obstacle = this->form_param.gain.c1_beta*grad_term_obs + this->form_param.gain.c2_beta*cons_term_obs;
+ }
+ /* navigational term*/
+ row_vector_2d_t u_navigation;
+ u_navigation << 0.0,0.0;
+ row_vector_2d_t q_ref,p_ref;
+ q_ref << ref.q(0), ref.q(1);
+ p_ref << ref.p(0), ref.p(1);
+ row_vector_2d_t centroid_q, centroid_p;
+ centroid_q << q.col(0).mean(), q.col(1).mean();
+ centroid_p << p.col(0).mean(), p.col(1).mean();
+ if (this->robot_id.leader == true)
+ {
+
+
+ if(this->form_param.param.nav_type == 1)
+ {
+ u_navigation = -this->form_param.gain.c1_gamma*(qi-q_ref) -this->form_param.gain.c2_gamma*(pi-p_ref);
+ }
+ else
+ {
+
+ u_navigation = -this->form_param.gain.c1_gamma*(centroid_q-q_ref) -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
+
+ }
+ }
+
+ // orienation term
+ row_vector_2d_t u_orientation;
+ u_orientation << 0.0,0.0;
+ if (ref.orientation != NOORIENTATION)
+ {
+ float angle = ref.orientation * M_PI /180;
+ // convert to 3d
+ row_vector_3d_t desired_vector(cos(angle), sin(angle),0);
+ row_vector_3d_t q_centroid, q_leader;
+ if (this->robot_id.leader == true)
+ {
+ q_centroid << centroid_q(0), centroid_q(1),0;
+ q_leader << q(0,0),q(1,0),0;
+ }
+ else
+ {
+ q_centroid << centroid_q(0), centroid_q(1),0;
+ q_leader << q(0,0),q(1,0),0;
+ }
+ float dlc = (q_leader-q_centroid).norm(); // distance from centroid to leader drone
+ row_vector_3d_t lcv = q_leader-q_centroid; // vector from centroid to leader
+ row_vector_3d_t dcv = dlc*desired_vector; // vector from centroid to desired final pose of leader
+ float acl = atan2(lcv(1), lcv(0)); // angle of vector from centroid to leadership dron
+ float acc = atan2(dcv(1), dcv(0)); // angle of vector from fron centroid to final drone pose
+ if(this->robot_id.leader == false)
+ {
+ // not necessaty TODO improve to get angle from 0 to 360
+ acl = atan(lcv(1)/lcv(0)); // angle of vector from centroid to leadership dron
+ acc = atan(dcv(1)/dcv(0)); //
+ }
+ float alpha = acc-acl; // angle between vector lcv and dcv
+ // we want to compute the rotation acceleration
+ // direction
+ row_vector_3d_t q_current(qi(0), qi(1),0); // current drone pose
+ row_vector_3d_t cq_hat = q_current - q_centroid; // vector from cwentroid to current drone
+ row_vector_3d_t pot(0,0,-alpha);
+ row_vector_3d_t rot_d = cq_hat.cross(pot) ;// rotation direction
+
+ if(rot_d.norm() == 0)
+ {
+ rot_d << 0,0,0;
+ }
+ else
+ {
+ rot_d = rot_d/rot_d.norm(); // convert to unit vector
+ }
+
+ row_vector_2d_t final_rot_d(rot_d(0), rot_d(1));
+
+ u_orientation = this->form_param.gain.c1_theta*abs(alpha)*final_rot_d;
+ }
+
+ /* integral term*/
+ row_vector_2d_t u_integral;
+ u_integral << 0.0,0.0;
+ this->integral_error = this->integral_error + error_grad_term*this->form_param.param.dt;
+ u_integral = this->form_param.gain.c1_delta *this->integral_error;
+
+
+ row_vector_2d_t total_input = u_formation + u_navigation + u_integral + u_orientation+u_obstacle;
+ input_vector_t input2send;
+ input2send.col(0) = total_input;
+ input2send.col(1) = u_formation;
+ input2send.col(2) = u_obstacle;
+ input2send.col(3) = u_navigation;
+ input2send.col(4) = u_orientation;
+ input2send.col(5) = u_integral;
+
+ return (input2send);
+
+
+
+}
+// sort the neighbour distances
+typedef std::function<bool(std::pair<int, float>, std::pair<int, float>)> Comparator;
+Comparator compFunctor =
+ [](std::pair<int, float> elem1, std::pair<int, float> elem2) {
+ return elem1.second < elem2.second;
+ };
+
+
+std::vector<std::pair<int, float>> swamControllerAlg::sortNeigbour(Eigen::MatrixXf M, row_vector_2d_t q)
+{
+ std::vector<std::pair<int, float>> neighbour_dist;
+ float dist;
+ /* Compute the distance*/
+ row_vector_2d_t qj;
+ for (int i = 0; i <this->N;i++)
+ {
+ qj(0) = M(0,i);
+ qj(1) = M(1,i);
+ dist = (q-qj).norm();
+ neighbour_dist.emplace_back(std::pair<int,float>(i, dist));
+ }
+ /* sort the distance*/
+ std::sort(neighbour_dist.begin(), neighbour_dist.end(), compFunctor);
+
+ return neighbour_dist;
+
+}
+
+float swamControllerAlg::computeFi(row_vector_2d_t qj, row_vector_2d_t qi, float a, float b, float h, float eps, float r, float d)
+{
+ /* qj neighbour
+ * qi drone itself*/
+ float z_sn = this->computeSigmaNorm((qj-qi).norm(), eps);
+ float r_sn = this->computeSigmaNorm(r,eps);
+ float d_sn = this->computeSigmaNorm(d,eps);
+ return (this->ph(z_sn/r_sn,h) * this->computefi(a,b,(z_sn-d_sn)));
+}
+
+float swamControllerAlg::computeSigmaNorm(float z, float eps)
+{
+ return((1.0/eps)*(sqrt(1.0+eps*z*z)-1.0));
+}
+
+float swamControllerAlg::ph(float z, float h)
+{
+ if((z>= 0) && (z <h))
+ {
+ return 1;
+ }
+ else if((z>=h) && (z<=1))
+ {
+ return(0.5*(1.0+cos(M_PI*(z-h)/(1-h))));
+ }
+ else
+ {
+ return 0;
+ }
+}
+
+float swamControllerAlg::computefi(float a, float b, float z)
+{
+ float c = abs(a-b)/sqrt(4*a*b);
+ return (0.5*(((a+b)*this->sigmaOne(z+c))+(a-b)));
}
+
+float swamControllerAlg::sigmaOne(float z)
+{
+ return (z/sqrt(1.0+(z*z)));
+}
+row_vector_2d_t swamControllerAlg::sigmaOne(row_vector_2d_t z)
+{
+ float n = z.norm();
+ return (z/sqrt(1.0+(n*n)));
+}
+row_vector_2d_t swamControllerAlg::computeNij(row_vector_2d_t qj, row_vector_2d_t qi, float eps)
+{
+ row_vector_2d_t z = qj-qi;
+ float n = z.norm();
+ return(z/sqrt(1.0+(eps*n*n)));
+
+}
+
+float swamControllerAlg::computeAij(row_vector_2d_t qj, row_vector_2d_t qi, float r, float eps, float h)
+{
+ float z_sn = this->computeSigmaNorm((qj-qi).norm(),eps);
+ float r_sn = this->computeSigmaNorm(r,eps);
+ return (this->ph(z_sn/r_sn,h));
+
+}
+
+float swamControllerAlg::computeFiB(row_vector_2d_t q_hat_i,row_vector_2d_t qi, float h_beta, float d_obs, float eps)
+{
+ float z_sn = this->computeSigmaNorm((q_hat_i-qi).norm(),eps);
+ float d_obs_sn = this->computeSigmaNorm(d_obs,eps);
+ return (this->ph(z_sn/d_obs_sn, h_beta)* (this->sigmaOne(z_sn-d_obs_sn)-1));
+}
+
+
+float swamControllerAlg::computeBij(row_vector_2d_t q_hat_i, row_vector_2d_t qi, float d_obs, float eps, float h_beta)
+{
+ float z_sn = this->computeSigmaNorm((q_hat_i-qi).norm(),h_beta);
+ float d_obs_sn = this->computeSigmaNorm(d_obs,eps);
+ return (this->ph(z_sn/d_obs_sn,h_beta));
+}
diff --git a/simulation/controller/SwamControllerAlg.asv b/simulation/controller/SwamControllerAlg.asv
new file mode 100644
index 0000000000000000000000000000000000000000..167694d7606d46da9ef52d918de705fee8e252c8
--- /dev/null
+++ b/simulation/controller/SwamControllerAlg.asv
@@ -0,0 +1,431 @@
+classdef SwamControllerAlg
+ properties
+ formation_param % information on paramters
+ gain_param % struct gains c1 c2 for alpha(agent) c1 c2(obstacle) c1 c2(gamma)
+ indx %position of the robot in the distance matrix
+ integral_error % store the integrator
+ previous_error % store for integral control
+
+
+ end
+
+ properties (Access = private)
+ robot_size %impose robot safety circle diameter
+ id % to indxentify the robot
+ end
+
+ properties (Constant)
+ safe_distance = 0.15 % safety distance around the robot in
+ end
+
+
+ methods
+ % -- constructor
+ function thisSwamControllerAlg = SwamControllerAlg(robot_size, id,indx,form_param, gains)
+ % Important, this computation is valindx for 2d positions, 3d not
+ % included
+ thisSwamControllerAlg.robot_size = robot_size+thisSwamControllerAlg.safe_distance;
+ thisSwamControllerAlg.indx = indx;
+ thisSwamControllerAlg.id = id;
+ thisSwamControllerAlg.gain_param = gains;
+ thisSwamControllerAlg.integral_error = [0;0];
+ thisSwamControllerAlg.previous_error = [0;0];
+
+ % recompute c2 gain to ensure the following conditions
+ %c1_apha < c1_gamma< c1_beta
+ if false %gains.c1_alpha > gains.c1_gamma || gains.c1_gamma > gains.c1_beta
+ error("Gain conditions not meet: c1_alpha < c1_gamma < c1_beta");
+ end
+ % recompute c2 gain based on the following equation c2 =
+ % c1_gamma = s*sqrt(c) based on original paper
+ thisSwamControllerAlg.formation_param = form_param;
+ % ensure 0 < a < b is met
+ if (0 > form_param.a) && (form_param.a >form_param.b)
+ error('Error. 0 < a <= b not met');
+
+ end
+ % safety
+ if form_param.nav_type <= 0 || form_param.nav_type >2
+ error("Navigational_type should be 1-converge approach or 2 - parallel approach");
+ end
+ if form_param.nav_type ~= 2
+ thisSwamControllerAlg.gain_param.c2_alpha = 2*sqrt(thisSwamControllerAlg.gain_param.c1_alpha);
+ thisSwamControllerAlg.gain_param.c2_beta = 2*sqrt(thisSwamControllerAlg.gain_param.c1_beta);
+ thisSwamControllerAlg.gain_param.c2_gamma = 2*sqrt(thisSwamControllerAlg.gain_param.c1_gamma);
+ end
+ % formation
+ thisSwamControllerAlg.formation_param.r = form_param.k*form_param.dist; % Neighbourhood distance
+
+
+ % obstacle avoindxance parameters
+ thisSwamControllerAlg.formation_param.d_obs = form_param.ratio*form_param.dist;
+ thisSwamControllerAlg.formation_param.r_obs = form_param.k*thisSwamControllerAlg.formation_param.d_obs;
+ if thisSwamControllerAlg.formation_param.d_obs < thisSwamControllerAlg.robot_size/2
+ error("Minimum distance to obstacle is too low and can be dangerous, increase d/d_ob ratio");
+ end
+
+
+ % perform some formation configuration;
+ thisSwamControllerAlg = thisSwamControllerAlg.configureFormation();
+ disp("Robot: " + id + " Initialized");
+
+
+ end
+ % -- Function responsible forcreating the formation parameters
+ function thisSwamControllerAlg = configureFormation(thisSwamControllerAlg)
+ % ensure distance is more thatn robot safe area
+ dist = thisSwamControllerAlg.formation_param.dist;
+ form_t = thisSwamControllerAlg.formation_param.type;
+ if dist < (thisSwamControllerAlg.robot_size/2)
+ error("Distance between robot will result in collision minimum radius distance: " + thisSwamControllerAlg.robot_size/2);
+ end
+
+ % formation configuration
+ if form_t == 2
+ % 2 man formation following
+ thisSwamControllerAlg.formation_param.iad = [0,dist; %interagent distance
+ dist,0];
+ thisSwamControllerAlg.formation_param.N = 2; % number of agent
+ thisSwamControllerAlg.formation_param.node_names = {'d1','d2'};
+
+
+
+ elseif form_t == 3 % triangular formation
+ thisSwamControllerAlg.formation_param.iad = [0,dist, dist; %interagent distance
+ dist, 0, dist;
+ dist, dist, 0];
+ thisSwamControllerAlg.formation_param.N = 3; % number of agent
+ thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3'};
+
+ elseif form_t == 4 % square formation
+ h = sqrt(2*dist*dist); % diagonal distance
+ thisSwamControllerAlg.formation_param.iad = [0,dist, h, dist; %interagent distance
+ dist, 0,dist, h;
+ h,dist,0,dist;
+ dist,h, dist,0];
+ thisSwamControllerAlg.formation_param.N = 4; % number of agent
+ thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3','d4'};
+
+
+ elseif form_t == 5 % pentagon formation
+ h = dist + 2*dist*cos(deg2rad(72)); %diagonal distance
+ thisSwamControllerAlg.formation_param.iad = [0,dist,h,h,dist; %interagent distance
+ dist, 0, dist,h,h;
+ h,dist,0,dist, h;
+ h,h,dist,0,dist;
+ dist,h,h,dist,0];
+ thisSwamControllerAlg.formation_param.N = 5; % number of agent
+ thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3','d4','d5'};
+
+ else
+ error("Wrong formation type: triangle:1 square:2 pentagon: 3");
+ end
+ disp("Interagent distance matrix: ");
+ disp(thisSwamControllerAlg.formation_param.iad);
+ end
+
+ % -- display formation
+ function showFormation(thisSwamControllerAlg)
+ figure
+ if thisSwamControllerAlg.formation_param.type > 2
+ pgon1 = nsindxedpoly(thisSwamControllerAlg.formation_param.N,'Center',[0 0],'SindxeLength',thisSwamControllerAlg.formation_param.dist);
+ plot(pgon1)
+ for i =1:thisSwamControllerAlg.formation_param.N
+ text(pgon1.Vertices(i,1),pgon1.Vertices(i,2),"d" + string(i));
+
+ end
+ elseif thisSwamControllerAlg.formation_param.type == 2
+ plot([0; thisSwamControllerAlg.formation_param.dist], [0,0])
+ text(0,0,"d1")
+ text(thisSwamControllerAlg.formation_param.dist,0,"d2")
+
+ end
+% axis equal
+
+ end
+
+ % -- main controller
+ function [input, input_vec,q_obs_vector] = controller(thisSwamControllerAlg,q,p,ref,ob, ori, cooperation)
+ % q is the current position of each of the drone- column vector
+ % p is the current velocitu of each of the drones
+ % ob is the current position of the obstacles [x,y,r]
+ % orientatoin in degs
+ if size(q,1) > 2
+ error("Controller is intended for 2d plane, not 3d, send x y inputs");
+ end
+
+ % ----- formation computation
+ q_rf = q'; % convert to row format
+ [ind,dist] = knnsearch(q_rf,q_rf(thisSwamControllerAlg.indx,:),"K",thisSwamControllerAlg.formation_param.N);
+ I = find(dist <= thisSwamControllerAlg.formation_param.r);
+ if size(I,2) == 1 % only robot % dangerours because in real world the position can be different for the same robot, do not implement this in the real robot
+ warning("Robot does not seem to have any neighbour, make sure this was done intentionally"); % TODO implement better logging
+ end
+ N_agent = ind(1,1:size(I,2));% agent neigbours
+ s_Na = size(N_agent,2);
+ u_formation = [0;0];
+ grad_term = [0;0];
+ cons_term = [0;0];
+ deriv_term = [0;0];
+ a = thisSwamControllerAlg.formation_param.a;
+ b = thisSwamControllerAlg.formation_param.b;
+ h_alpha = thisSwamControllerAlg.formation_param.h_alpha;
+ eps = thisSwamControllerAlg.formation_param.eps;
+ r = thisSwamControllerAlg.formation_param.r;
+ qi = q(:,thisSwamControllerAlg.indx);
+ pi_ = p(:,thisSwamControllerAlg.indx);
+
+
+
+ for i = 1: s_Na
+ if N_agent(i) ~= thisSwamControllerAlg.indx % only for neigbours
+ qj = q(:,N_agent(i));
+ pj = p(:,N_agent(i));
+ if cooperation == 1
+ d = thisSwamControllerAlg.formation_param.iad(thisSwamControllerAlg.indx,N_agent(i));
+ else
+ d = thisSwamControllerAlg.formation_param.follow_dist;
+ end
+ % gradient term
+ fi = thisSwamControllerAlg.computeFi(qj,qi,a,b,h_alpha,eps,r,d);
+ nij = SwamControllerAlg.computeNij(qj,qi, eps);
+ grad_term = grad_term + fi*nij;
+ % consesus term
+ if thisSwamControllerAlg.formation_param.integrator == 2
+ aij = SwamControllerAlg.computeAij(qj,qi,r,eps,h_alpha);
+ cons_term = cons_term +aij*(pj-pi_);
+
+ deriv_term = deriv_term + aij*(qj-qi);
+
+ end
+
+
+ end
+
+ end
+ error_grad_term = grad_term;
+ u_formation = grad_term*thisSwamControllerAlg.gain_param.c1_alpha + cons_term*thisSwamControllerAlg.gain_param.c2_alpha;
+ % obstacle formation
+ % TODO current implementation only consindxers circular
+ % Todo plot gradient and potencial
+ % obstacles, include hyperplane
+
+ if isempty(ob)
+ u_obstacle = [0;0];
+q_obs_vector = [0;0];
+
+ else
+ % compute position and velocity of obstacles
+ % projection of agent onto obstacle
+ ob_s = size(ob,2);
+ r_obs = thisSwamControllerAlg.formation_param.r_obs;
+ d_obs = thisSwamControllerAlg.formation_param.d_obs;
+ h_beta = thisSwamControllerAlg.formation_param.h_beta;
+
+ % q_hat = zeros(2,ob_s); % positions of all the obstables
+ % p_hat = zeros(2,ob_s); % velocities of all the obstacles
+ % N_beta = [];
+ q_obs_vector = zeros(2,ob_s);
+ grad_term_obs = [0;0];
+ cons_term_obs = [0;0];
+ nav_term_obs = [0;0];
+ for o = 1:ob_s
+ yk = ob(1:2,o); % center of obstacle
+ Rk = ob(3,o); % radius of obstacle
+ mu = Rk/norm(qi-yk);
+ ak = (qi-yk)/norm(qi-yk);
+ a__ = ak*ak';
+ sa = size(a__,1);
+ P = eye(sa)- a__;
+ q_hat_i = mu*qi + (1-mu)*yk;
+ p_hat_i = mu *P*pi_;
+
+% qi = thisSwamControllerAlg.indx;
+% if thisSwamControllerAlg.indx == 1
+% disp("Here");
+% end
+ d2obs = norm(qi- q_hat_i);
+ q_obs_vector(:,o) = q_hat_i;
+ if d2obs < r_obs %Neighbourhood obstacles of agent
+ % gradient term
+ fi_beta = SwamControllerAlg.computeFiB(q_hat_i,qi,h_beta,d_obs,eps);
+ nij = SwamControllerAlg.computeNij(q_hat_i,qi, eps);
+ grad_term_obs = grad_term_obs + fi_beta*nij;
+ % consesus term
+ if thisSwamControllerAlg.formation_param.integrator == 2
+
+ bij = SwamControllerAlg.computeBij(q_hat_i,qi,d_obs,eps, h_beta);
+ cons_term_obs = cons_term_obs +bij*(p_hat_i-pi_);
+ end
+%
+% nav_term_obs = nav_term_obs -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne(q_hat_i-ref.q))- ...
+% thisSwamControllerAlg.gain_param.c2_gamma*(p_hat_i-ref.p);
+ end
+ end
+%
+ u_obstacle = thisSwamControllerAlg.gain_param.c1_beta* grad_term_obs ...
+ +thisSwamControllerAlg.gain_param.c2_beta*cons_term_obs;
+
+ end
+
+ % navigational term
+ % TODO: implement wait until formation potential has reach a
+ % threshold
+ u_navigation = [0;0];
+ if cooperation == 1
+ if thisSwamControllerAlg.formation_param.nav_type == 1
+ % convergence approach
+ u_navigation = -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne(qi-ref.q))- ...
+ thisSwamControllerAlg.gain_param.c2_gamma*(pi_-ref.p);
+ else
+ %parallel approach
+ % compute the centroindx info
+ mean_q = [mean(q(1,:)); mean(q(2,:))];
+ mean_p = [mean(p(1,:)); mean(p(2,:))];
+
+ u_navigation = -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne((mean_q-ref.q)))- ...
+ thisSwamControllerAlg.gain_param.c2_gamma*(mean_p-ref.p);
+% u_navigation = sat_func(u_navigation,0.05);
+
+
+ end
+ end
+
+
+ % orientation term
+ % compute the centroindx vector
+ u_orientation = [0;0];
+ if ~isempty(ori)
+ desired_angle = deg2rad(ori);
+ % convert to 3d
+ desired_vector = [cos(desired_angle); sin(desired_angle);0];
+ if cooperation == 1
+ q_centroid = [mean(q,2);0];
+ q_leader = [q(:,1);0];
+ else
+ q_centroid = [q(:,1);0];
+ q_leader = [q(:,2);0];
+
+ end
+ dlc = norm(q_leader-q_centroid); % distance from centroid to leader drone
+ lcv = q_leader-q_centroid; % vector from centroid to leader
+ dcv = dlc*desired_vector; % vector from centroid to desired final pose of leader
+ acl =atan2(lcv(2), lcv(1));%angle of vector from centroid to leadership dron
+ acc = atan2(dcv(2), dcv(1)); % angle of vector from fron centroid to final drone pose
+ if cooperation == 2
+ acl =atan(lcv(2)/ lcv(1));%angle of vector from centroid to leadership dron
+ acc = atan(dcv(2)/dcv(1)); % angle of vector from fron centroid to final drone pose
+ end
+ alpha = acc-acl;%angle between vector lcv and dcv
+ % ---- We want to compute the rotational acceleration
+ % direction
+ q_hat = [q(:,thisSwamControllerAlg.indx); 0];% current drone pose
+ cq_hat = q_hat - q_centroid;% vector from centroid to current drone
+ rot_d =cross(cq_hat, [0,0,-alpha]) ; %rotaiton direction
+
+ if norm(rot_d) == 0
+ rot_d = [0;0];
+ else
+ rot_d = rot_d/norm(rot_d); % convert to unit vecor;
+ end
+ u_orientation = thisSwamControllerAlg.gain_param.c1_theta*abs(alpha)*[rot_d(1);rot_d(2)];
+ end
+
+ % integral controld
+ % error obtained from the formation gradient term
+ thisSwamControllerAlg.integral_error = thisSwamControllerAlg.integral_error + error_grad_term*thisSwamControllerAlg.formation_param.dt;
+ % saturate error to prevent anti-winup
+% thisSwamControllerAlg.integral_error = sat_func( thisSwamControllerAlg.integral_error, thisSwamControllerAlg.formation_param.int_max);
+ u_integral = thisSwamControllerAlg.gain_param.c1_delta * thisSwamControllerAlg.integral_error; % + deriv_term*thisSwamControllerAlg.gain_param.c2_delta;
+% u_derivative = ((error_grad_term-previous_error)/ thisSwamControllerAlg.formation_param.dt)*thisSwamControllerAlg.gain_param.c3_delta;
+% thisSwamControllerAlg.previous_error = error_grad_term;
+% ce = error_grad_term;
+
+ input = u_formation + u_obstacle + u_navigation+u_orientation + u_integral+ 0;
+ input_vec = [u_formation,u_obstacle,u_navigation,u_orientation,u_integral]; % debug purposes
+% potential = [potential_formation,potential_obstacle, potential_navigation, potential_orietation];
+ end
+
+ end
+ methods (Static)
+
+ % - computes the sigma norm of a vector
+ function out = computeSigmaNorm(z,eps)
+ n = norm(z);
+ out = (1/eps)*((sqrt(1+eps*n*n))-1);
+ end
+ % compute the fi alpha function
+ function out = computeFi(qj,qi,a,b,h,eps,r,d)
+ % qj is the neighbour
+ % qi is the drone itself
+ % sigma norms of the error-cut off radius- desired distance
+ z_sn = SwamControllerAlg.computeSigmaNorm((qj-qi), eps); % this can be done outsindxe the function
+ r_sn = SwamControllerAlg.computeSigmaNorm(r, eps);
+ d_sn = SwamControllerAlg.computeSigmaNorm(d, eps);
+ out = SwamControllerAlg.ph(z_sn/r_sn,h) * SwamControllerAlg.fi(a,b,(z_sn-d_sn));
+ end
+
+
+ % - computes the fi function
+ function out = fi( a,b,z)
+ c = abs(a-b)/sqrt(4*a*b);
+ out = 0.5 * (((a+b)*SwamControllerAlg.sigmaOne(z+c)) + (a-b));
+ end
+
+ % - computes the ph bumper function
+ function out = ph(z, h)
+ % the bumper function maps R+ to [0,1]
+ if max(size(z)) > 1
+ error("Bumper function: value must be scalar")
+ end
+ if( z >= 0) && ( z<h)
+ out = 1;
+ elseif (z >= h) && (z<=1)
+ out = 0.5*(1+cos(pi*(z-h)/(1-h)));
+ else
+ out = 0;
+ end
+
+ end
+
+ % computes the sigma1
+ function out = sigmaOne(z)
+ n = norm(z);
+ out = z/sqrt(1+(n*n));
+ end
+
+ % --- compute nij function neighbourhood function
+ function out = computeNij(qj,qi, eps)
+ % qj is the neighbour
+ % qi is the drone itself
+ z = qj -qi;
+ n = norm(z);
+ out = z/sqrt(1+(eps*n*n));
+
+
+ end
+
+ % -- compute the adjency matrix element
+ function out = computeAij(qj,qi,r,eps,h)
+ z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi, eps);
+ r_sn = SwamControllerAlg.computeSigmaNorm(r,eps);
+ out = SwamControllerAlg.ph(z_sn/r_sn,h);
+ end
+
+ % -- helper function for obstacle
+ % damping function
+ function out = computeFiB(qj,qi,h_beta,d_obs,eps)
+ z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi,eps);
+ d_obs_sn = SwamControllerAlg.computeSigmaNorm(d_obs,eps);
+ out = SwamControllerAlg.ph(z_sn/d_obs_sn,h_beta) * (SwamControllerAlg.sigmaOne(z_sn-d_obs_sn)-1);
+ end
+ function out = computeBij(qj,qi,d_obs,eps, h)
+ z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi,eps);
+ d_obs_sn = SwamControllerAlg.computeSigmaNorm(d_obs,eps);
+ out = SwamControllerAlg.ph(z_sn/d_obs_sn,h);
+ end
+ end
+
+end
+
+