diff --git a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg index 153ee988640c1762fcc35044a13c429d70a2e9bb..dc6d89e0cd7d22c2feb5b9054fdea96cf45e65e7 100644 --- a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg +++ b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg @@ -11,11 +11,11 @@ demo.add("start_formation", bool_t, 0, demo.add("stop", bool_t, 0, "stop formation ", False) demo.add("mantain_position", bool_t, 0, "mantain goal centroid", False) demo.add("update_waypoint", bool_t, 0, "update waypoint", False) -demo.add("x", double_t, 0, "desired goal", 0.0, -3,-3) -demo.add("y", double_t, 0, "desired goal", 0.0, -3,-3) -demo.add("vx", double_t, 0, "desired goal", 0.0,-3,-3) -demo.add("vy", double_t, 0, "desired goal", 0.0, -3,-3) -demo.add("orientation", double_t, 0, "desired goal", 0, 0,360) +demo.add("x", double_t, 0, "desired goal x", 0.0, -3,3) +demo.add("y", double_t, 0, "desired goal y", 0.0, -3,3) +demo.add("vx", double_t, 0, "desired goal vx", 0.0,-3,3) +demo.add("vy", double_t, 0, "desired goal vy", 0.0, -3,3) +demo.add("orientation", double_t, 0, "desired goal orientation", 0, 0,360) exit(gen.generate(PACKAGE, "coordination_formation_control_pkg", "missionController")) diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml index d9a7a3d6904517a740a1dfdb4c814a6acb63a140..9acedfd4b3bd1f66c853e75fb11b89a32da5b82c 100644 --- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml +++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml @@ -1,5 +1,5 @@ n_drones: 4 -n_ugvs: 2 +n_ugvs: 1 hz_freq: 50 # controller frquency include_obstacle: false include_orientation: false diff --git a/real_robot/coordination_formation_control_pkg/include/mission_controller.h b/real_robot/coordination_formation_control_pkg/include/mission_controller.h index 1a5005a76af70e1228e4c85e9e659201c3e48d9e..8b3dedeac3d900c2f601cad3eb5d62fe22e873e1 100644 --- a/real_robot/coordination_formation_control_pkg/include/mission_controller.h +++ b/real_robot/coordination_formation_control_pkg/include/mission_controller.h @@ -34,7 +34,7 @@ class missionController /* execute main process*/ void executeMainProcess(); void updateCofig(coordination_formation_control_pkg::missionControllerConfig &config, uint32_t level); - Eigen::MatrixXf compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs); + Eigen::MatrixXf compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs,Mat &previous_labels, Eigen::MatrixXf & previous_centroids); private: fsm_mc_t state; @@ -44,6 +44,7 @@ class missionController bool stop; bool mantain_position; bool start_publishing_waypoint; + bool cluster_initialized; ros::NodeHandle nh; /*dynamic reconfigure*/ diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch index bc882f0afe3ec1673506e2bffbc756424ccd555a..04954831b9fcc33481daf2f328286a8f96cdd237 100644 --- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch +++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch @@ -5,7 +5,7 @@ <rosparam command="load" file="$(find coordination_formation_control_pkg)/config/formationConfig.yaml" /> <!-- Initialize all the drones first --> - <group ns="uav_1"> + <!-- <group ns="uav_1"> <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="1" launch-prefix="xterm -e"> </node> </group> @@ -24,7 +24,13 @@ <group ns="uav_4"> <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="4" launch-prefix="xterm -e"> </node> - </group> + </group> --> +--> + + <group ns="omniwheel_1"> + <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e"> + </node> + </group> diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp index d59a890f479dffe905e95d73a38fc4798d95f343..427b97082cce9c6a89b43a26cc19e1dd8db96d58 100644 --- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp +++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp @@ -29,6 +29,7 @@ nh(nh) /* execute main process*/ this->executeMainProcess(); + this->cluster_initialized = false; } /*destructor*/ @@ -39,11 +40,12 @@ missionController::~missionController() /* main process*/ void missionController::executeMainProcess() { - ros::Rate loop_rate(100); + ros::Rate loop_rate(50); /* create a list of drones*/ int n_drones, n_ugvs; this->nh.getParam("/n_drones", n_drones); this->nh.getParam("/n_ugvs", n_ugvs); + this->current_waypoint.cluster_pos.resize(n_ugvs*2); std::vector<robotState *> uavs; @@ -82,17 +84,44 @@ void missionController::executeMainProcess() } } + Eigen::MatrixXf uav_state_q(2,n_drones); + Eigen::MatrixXf uav_state_p(2,n_drones); + Eigen::MatrixXf ugv_state_q(2,n_ugvs); + Eigen::MatrixXf ugv_state_p(2,n_ugvs); Eigen::MatrixXf current_cluster(2,n_ugvs); row_vector_2d_t current_centroid_vel,goal_centroid,current_centroid, error; float dist; + int cluster_ind = 0; + // std::vector<Point2f> previous_center; + // for(int i = 0; i<n_ugvs; i++) + // { + // Point2f center; + // center.x = ugv_state_q(0,i); + // center.y = ugv_state_q(1,i); + // previous_center.push_back(center); + // } + Mat previous_labels; + previous_labels = cv::Mat(n_drones,1,CV_32S); + + for (int i = 0; i<n_drones; i++) + { + previous_labels.at<int>(i,0) = i; + } + Eigen::MatrixXf previous_centroids(2,n_ugvs); +for (int i = 0; i ++; i < n_ugvs) +{ + robot_state_t current_state; + current_state = ugvs[i-1]->get_state(); + previous_centroids.col(i) << current_state.q(0),current_state.q(1); +} while(ros::ok()) { - Eigen::MatrixXf uav_state_q(2,n_drones); - Eigen::MatrixXf uav_state_p(2,n_drones); - Eigen::MatrixXf ugv_state_q(2,n_ugvs); - Eigen::MatrixXf ugv_state_p(2,n_ugvs); + // std::cout <<"here" << std::endl; + + + for(int i = 1; i <=n_drones; i++) { robot_state_t current_state; @@ -115,6 +144,7 @@ void missionController::executeMainProcess() } + if(this->shut_down) { // stop all process here @@ -172,6 +202,36 @@ void missionController::executeMainProcess() break; case START_FORMATION: + /* testint*/ + uav_state_q(0, 1-1) =0; + uav_state_q(1, 1-1) =0; + uav_state_p(0, 1-1) =0; + uav_state_p(1, 1-1) = 0; + + uav_state_q(0, 2-1) =1; + uav_state_q(1, 2-1) =0; + uav_state_p(0, 2-1) = 0; + uav_state_p(1, 2-1) = 0; + + uav_state_q(0, 3-1) =0; + uav_state_q(1, 3-1) =1; + uav_state_p(0, 3-1) = 0; + uav_state_p(1, 3-1) = 0; + + uav_state_q(0, 4-1) =1; + uav_state_q(1, 4-1) =1; + uav_state_p(0, 4-1) = 0; + uav_state_p(1, 4-1) = 0; + + ugv_state_q(0, 1-1) =-1.5; + ugv_state_q(1, 1-1) =-0.5; + ugv_state_p(0, 1-1) = 0; + ugv_state_p(1, 1-1) =0; + + ugv_state_q(0, 2-1) =1.5; + ugv_state_q(1, 2-1) =-0.5; + ugv_state_p(0, 2-1) = 0; + ugv_state_p(1, 2-1) =0; /* compute waypoint formation*/ /* initial waypoint will be the centorid of the current configuration*/ @@ -186,39 +246,62 @@ void missionController::executeMainProcess() dist = error.norm(); + + /* compute cluster*/ - current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones); + // std::cout << "Here" << std::endl; + if (n_ugvs >0) + { + // std::cout << "outside before previous lable" << previous_labels << std::endl; + + // current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones); + current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones, previous_labels, previous_centroids); + previous_centroids = current_cluster; + // std::cout << "outside after previous lable" << previous_labels <<std::endl; + + + // std::cout << current_cluster << std::endl; + } + /* assign clusters*/ + cluster_ind = 0; + for(int i = 0; i < n_ugvs; i++) { - this->current_waypoint.cluster_pos.push_back(current_cluster(0,i)); - this->current_waypoint.cluster_pos.push_back(current_cluster(1,i)); + + this->current_waypoint.cluster_pos[cluster_ind] = current_cluster(0,i); + this->current_waypoint.cluster_pos[cluster_ind+1] = current_cluster(1,i); + // previous_center[i].x = current_cluster(0,i); + // previous_center[i].y = current_cluster(1,i); + cluster_ind +=2; } this->current_waypoint.cluster_vel[0] = current_centroid_vel(0); this->current_waypoint.cluster_vel[1] = current_centroid_vel(1); - /* send goals*/ - if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea - { - if(this->mantain_position == false) - { - this->current_waypoint.stop = true; - } - else - { - this->current_waypoint.stop = false; - /* if we want to mantain centroid, vel to 0*/ - this->current_waypoint.vel[0] = 0; - this->current_waypoint.vel[1] = 0; - } - } /* update waypoints*/ if (start_publishing_waypoint) { + // std::cout << dist << std::endl; + /* send goals*/ + if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea + { + if(this->mantain_position == false) + { + this->current_waypoint.stop = true; + } + else + { + this->current_waypoint.stop = false; + /* if we want to mantain centroid, vel to 0*/ + this->current_waypoint.vel[0] = 0; + this->current_waypoint.vel[1] = 0; + + } + } waypoint_publisher.publish(this->current_waypoint); } @@ -292,27 +375,31 @@ void missionController::updateCofig(coordination_formation_control_pkg::missionC } } -Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs) +Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs,Mat &previous_labels,Eigen::MatrixXf & previous_centroids) { + /* only implemented for two ugv*/ - if(num_ugvs > 2) + if(num_ugvs > 2 || num_ugvs <1) { ROS_INFO("n_ugv is only implemented for 2 ugv, should modify for more ugvs"); this->stop = true; + } else { - std::vector<Point2f> centers; - for (int i = 0; i <2; i++) - { - /* make intial ugv be the centroid */ - Point2f center; - center.x = ugv_q(0,i); - center.y = ugv_q(1,i); - centers.push_back(center); - } + + std::vector<Point2f> centers;; + // for (int i = 0; i <num_ugvs; i++) + // { + // /* make intial ugv be the centroid */ + // Point2f center; + // center.x = ugv_q(0,i); + // center.y = ugv_q(1,i); + // centers.push_back(center); + // } + /* points*/ std::vector<Point2f> points; @@ -323,12 +410,18 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen: point.y = uav_q(1,t); points.push_back(point); } - Mat labels; + // ROS_INFO("*****************************************Here"); + + Mat labels = previous_labels; + /* set up labels*/ int cluster_cout = num_ugvs; double compactness = kmeans(points, cluster_cout, labels, - TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 10, 1.0), + TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 0, 0.1), 10, KMEANS_PP_CENTERS, centers); + // std::cout << "previous lable" << previous_labels << "current labels" << labels << std::endl; + // std::cout << "Current center" << centers << std::endl; + previous_labels = labels; /* decide to change or not*/ /* we calculate the enegy to mantain the current confguration or change*/ Eigen::MatrixXf centroids(2,num_ugvs); @@ -338,25 +431,48 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen: centroids(1,p) = centers[p].y; } - float man1 = (centroids.col(0) - ugv_q.col(0)).norm(); - float man2 = (centroids.col(1) - ugv_q.col(0)).norm(); - float cha1 = (centroids.col(1) - ugv_q.col(1)).norm(); - float cha2 = (centroids.col(0) - ugv_q.col(1)).norm(); + // std::cout << "Current center set" << centroids << std::endl; + + - if( cha1 < man1) + + + + /* this is intended for two ugvs, it computed the energy to mantain leader cluster, + and change, if energy in both ugv to change is higher, the switch is perfomred*/ + if(num_ugvs == 2) { - if(cha2 <man2) + float man1 = (centroids.col(0) - previous_centroids.col(0)).norm(); + float man2 = (centroids.col(1) - previous_centroids.col(0)).norm(); + float cha1 = (centroids.col(1) - previous_centroids.col(1)).norm(); + float cha2 = (centroids.col(0) - previous_centroids.col(1)).norm(); + + if( cha1 < man1 && cha2 <man2) { - Eigen::MatrixXf dummy_centroid(2,num_ugvs); - dummy_centroid = centroids; - /* switch centroids*/ - centroids.col(0) = dummy_centroid.col(1); - centroids.col(1) = dummy_centroid.col(0); + + Eigen::MatrixXf dummy_centroid(2,num_ugvs); + dummy_centroid = centroids; + /* switch centroids*/ + centroids.col(0) = dummy_centroid.col(1); + centroids.col(1) = dummy_centroid.col(0); + } - } + else + { + // bad ondding + if(this->cluster_initialized) + { + centroids = previous_centroids; + + } + } + } + previous_centroids = centroids; + // std::cout << "final centroids" << centroids << std::endl; +this->cluster_initialized = true; return centroids; } 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 2b935494d60c52587bb2eabc883506965c9d824f..f8ac252c568ab663fb47cc4dd61e495f05700376 100644 --- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp @@ -167,96 +167,96 @@ int main(int argc, char **argv) { else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false) { /* start formation controll*/ - if (waypoint_received) - { - - /*get current state of all the ugv*/ - for(int i = 1; i <=n_ugvs; i++) - { - robot_state_t current_state; - current_state = ugvs[i-1]->get_state(); - ugv_state_q(0, i-1) =current_state.q(0); - ugv_state_q(1, i-1) =current_state.q(1); - ugv_state_p(0, i-1) = current_state.p(0); - ugv_state_p(1, i-1) = current_state.p(1); - - } - - /* create a state vector of the cluster and the current agv*/ - current_state_q(0,0) = next_waypoint->cluster_pos[robotid*2 -2]; - current_state_q(1,0) = next_waypoint->cluster_pos[robotid*2 -1]; - current_state_p(0,0) = next_waypoint->cluster_vel[0]; - current_state_p(1,0) = next_waypoint->cluster_vel[1]; - current_state_q.col(1) = ugv_state_q.col(robotid-1); - current_state_p.col(1) = ugv_state_p.col(robotid-1); - - /* update reference point*/ - current_waypoint.q(0) =next_waypoint->cluster_pos[robotid*2 -2]; - current_waypoint.q(1) = next_waypoint->cluster_pos[robotid*2 -1]; - current_waypoint.p(0) = next_waypoint->cluster_vel[0]; - current_waypoint.p(1) = next_waypoint->cluster_vel[1]; - - /* mantain a 180 degress*/ - current_waypoint.orientation = 180; - if (n_obs_ugv > 0) - { - /* add obstacle*/ - if(n_obs != 0) - { - int indx = 0; - for(int n = 0; n < n_obs; n++) - { - - current_obs[n].q(0) = next_waypoint->obstacle[indx]; - current_obs[n].q(1) = next_waypoint->obstacle[indx+1]; - current_obs[n].radius = next_waypoint->obstacle[indx+2]; - indx = indx+3; - } - } - /* add other ugv as obstacle*/ - if((n_ugvs-1) !=0) - { - int ugv_ind = 0; - for(int n = 0; n < (n_ugvs); n++) - { - if ((n+1) != robotid) // exclude current ugv as it own obstacle - { - current_obs[n_obs+ugv_ind].q(0) =ugv_state_q(0, n) ; - current_obs[n_obs+ugv_ind].q(1) =ugv_state_q(1, n) ; - current_obs[n_obs+ugv_ind].radius = robot_ind.safe_ditance + robot_ind.robot_size; - ugv_ind = ugv_ind+1; - } - - } - } - - } - else - { - current_obs = NULL; - } + // if (waypoint_received) + // { // - /* call controller*/ - input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs); - /* integrate acceleration*/ - if(next_waypoint->stop) - { - current_command.linear.x = 0; - current_command.linear.y = 0; - current_command.angular.z = 0; - } - else - { - current_command.linear.x = input(0,0); - current_command.linear.y = input(1,0); - current_command.angular.z = 0; - - } - - - - } - vel_cmd_publisher.publish(current_command); + // /*get current state of all the ugv*/ + // for(int i = 1; i <=n_ugvs; i++) + // { + // robot_state_t current_state; + // current_state = ugvs[i-1]->get_state(); + // ugv_state_q(0, i-1) =current_state.q(0); + // ugv_state_q(1, i-1) =current_state.q(1); + // ugv_state_p(0, i-1) = current_state.p(0); + // ugv_state_p(1, i-1) = current_state.p(1); + // + // } + // + // /* create a state vector of the cluster and the current agv*/ + // current_state_q(0,0) = next_waypoint->cluster_pos[robotid*2 -2]; + // current_state_q(1,0) = next_waypoint->cluster_pos[robotid*2 -1]; + // current_state_p(0,0) = next_waypoint->cluster_vel[0]; + // current_state_p(1,0) = next_waypoint->cluster_vel[1]; + // current_state_q.col(1) = ugv_state_q.col(robotid-1); + // current_state_p.col(1) = ugv_state_p.col(robotid-1); + // + // /* update reference point*/ + // current_waypoint.q(0) =next_waypoint->cluster_pos[robotid*2 -2]; + // current_waypoint.q(1) = next_waypoint->cluster_pos[robotid*2 -1]; + // current_waypoint.p(0) = next_waypoint->cluster_vel[0]; + // current_waypoint.p(1) = next_waypoint->cluster_vel[1]; + // + // /* mantain a 180 degress*/ + // current_waypoint.orientation = 180; + // if (n_obs_ugv > 0) + // { + // /* add obstacle*/ + // if(n_obs != 0) + // { + // int indx = 0; + // for(int n = 0; n < n_obs; n++) + // { + // + // current_obs[n].q(0) = next_waypoint->obstacle[indx]; + // current_obs[n].q(1) = next_waypoint->obstacle[indx+1]; + // current_obs[n].radius = next_waypoint->obstacle[indx+2]; + // indx = indx+3; + // } + // } + // /* add other ugv as obstacle*/ + // if((n_ugvs-1) !=0) + // { + // int ugv_ind = 0; + // for(int n = 0; n < (n_ugvs); n++) + // { + // if ((n+1) != robotid) // exclude current ugv as it own obstacle + // { + // current_obs[n_obs+ugv_ind].q(0) =ugv_state_q(0, n) ; + // current_obs[n_obs+ugv_ind].q(1) =ugv_state_q(1, n) ; + // current_obs[n_obs+ugv_ind].radius = robot_ind.safe_ditance + robot_ind.robot_size; + // ugv_ind = ugv_ind+1; + // } + // + // } + // } + // + // } + // else + // { + // current_obs = NULL; + // } + // // + // /* call controller*/ + // input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs); + // /* integrate acceleration*/ + // if(next_waypoint->stop) + // { + // current_command.linear.x = 0; + // current_command.linear.y = 0; + // current_command.angular.z = 0; + // } + // else + // { + // current_command.linear.x = input(0,0); + // current_command.linear.y = input(1,0); + // current_command.angular.z = 0; + // + // } + // + // + // + // } + // vel_cmd_publisher.publish(current_command); } if(coord_config->stop_demo) diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp index ce0145cbaf507b9811ed88cd68adc042c6679c66..6b9ffcff3bdc59f43f914bcc0275eaf34bee2f40 100644 --- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp +++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp @@ -24,7 +24,7 @@ t(robot_ind.t) this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUavVel, this); this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUavAcc, this); - ROS_INFO("Robot %d state subcribed", this->id ); + ROS_INFO("Crazyflie %d state subcribed", this->id ); ros::topic::waitForMessage< crazyswarm::GenericLogData >(ss_vel.str(), ros::Duration(5)); ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_pose.str(), ros::Duration(5)); ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_acc.str(), ros::Duration(5)); @@ -44,7 +44,7 @@ t(robot_ind.t) this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUgvVel, this); this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUgvAcc, this); - ROS_INFO("Robot %d state subcribed", this->id ); + ROS_INFO("Omniwheels %d state subcribed", this->id ); ros::topic::waitForMessage<geometry_msgs::Twist>(ss_vel.str(), ros::Duration(5)); ros::topic::waitForMessage<geometry_msgs::Pose>(ss_pose.str(), ros::Duration(5)); ros::topic::waitForMessage<geometry_msgs::Vector3>(ss_acc.str(), ros::Duration(5)); diff --git a/simulation/controller/SwamControllerAlg.asv b/simulation/controller/SwamControllerAlg.asv deleted file mode 100644 index 167694d7606d46da9ef52d918de705fee8e252c8..0000000000000000000000000000000000000000 --- a/simulation/controller/SwamControllerAlg.asv +++ /dev/null @@ -1,431 +0,0 @@ -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 - -