diff --git a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
index b633335cdcfc04647080f932dce112882984ae00..acc600214348ddea9b0d434680db9ba309afbfd2 100644
--- a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
+++ b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
@@ -13,6 +13,7 @@ private:
formation_config_t form_param; /*parameters of the formation*/
row_vector_2d_t integral_error;
row_vector_2d_t previous_integral_error;
+ row_vector_2d_t range_sat;
robot_ind_t robot_id;
formation_matrix_t form_type;
Eigen::MatrixXf iad; /* iter agent distances*/
@@ -51,6 +52,9 @@ protected:
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);
+ /* saturates values in range [min, max]*/
+ row_vector_2d_t saturate(row_vector_2d_t input, row_vector_2d_t range);
+
};
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
index aea77291a260268a21dd3d4e01658d30775fcf9e..35fe3e23a519a290214ef7f2c5a9a838545b46c2 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
index f41744a13b4775950038e4e976e6c824bf68bef8..4455b7ce4a8d7ef782371cff0b0412254ee99347 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
index 066de6cdf4fafcde646ef9a033fc8f61c329ccfb..fa364b222ae44a2c20ba6d4dcb771a26b539a400 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
index 5421490f93af12a7583ef3819b7f86566f18006b..34017b44f2d475666c6d71391ed504b5e55dd995 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -2,7 +2,7 @@ import rosbag
import sys
import numpy as np
from matplotlib import pyplot as plt
-
+import time
if(len(sys.argv) != 4):
print ("invalid number of arguments: " + str(len(sys.argv)))
@@ -38,16 +38,25 @@ acc_row_ = [0] * n_robots_logged*2;
# converting data to numpy for easy plotting
for topics, msg, t in bag.read_messages(topics =[topic_name]):
for ind,pose in enumerate(msg.current_pose):
+ # if(ind == 2):
+ # print(pose.position.y)
+ # # print(ind)
+ # print(str(pose.position.x) + " " +str(pose.position.y))
pose_row_[ind*2] = pose.position.x
pose_row_[ind*2+1] = pose.position.y
twist_row_[ind*2] = msg.current_twist[ind].linear.x
twist_row_[ind*2+1] = msg.current_twist[ind].linear.y
acc_row_[ind*2] = msg.current_acc[ind].linear.x
acc_row_[ind*2+1] = msg.current_acc[ind].linear.y
+ # print(pose_row_)
+ # print(pose_row_)
t_.append(msg.header.stamp.secs + msg.header.stamp.nsecs*1e-9);
- pose_data.append(pose_row_);
- vel_data.append(twist_row_);
- acc_data.append(acc_row_);
+ pose_data.append(pose_row_[:]);
+ # print(pose_data[0][:])
+ # print(pose_data[-1][:])
+ # time.sleep(1);
+ vel_data.append(twist_row_[:]);
+ acc_data.append(acc_row_[:]);
command_pose_data.append([msg.command_pose.x, msg.command_pose.y, msg.command_pose.theta]);
command_twist_data.append([msg.command_twist.linear.x, msg.command_twist.linear.y]);
input_data.append([msg.input.x, msg.input.y])
@@ -56,12 +65,15 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
input_navigation_data.append([msg.input_navigation.x, msg.input_navigation.y])
input_obstacle_data.append([msg.input_obstacle.x, msg.input_obstacle.y])
input_integration_data.append([msg.input_integration.x, msg.input_integration.y])
+# print(pose_data)
+
bag.close()
t_np = np.array(t_)
# remove the offset
t_np = t_np -t_np[0]
pose_data_np = np.array(pose_data);
+# print(pose_data_np)
vel_data_np = np.array(vel_data);
acc_data_np = np.array(acc_data);
x_list = range(0,n_robots_logged*2,2);
@@ -150,7 +162,7 @@ axs3[1, 0].legend(robot_names)
####################### Acceleration plot ###################################
fig4, axs4 = plt.subplots(nrows=2, ncols=2, figsize=(7, 7))
-st = fig3.suptitle("Accleration", fontsize="x-large")
+st = fig4.suptitle("Acceleration", fontsize="x-large")
robot_names = []
for i in range(n_robots_logged):
robot_names.append("robot_" + str(i+1))
@@ -173,7 +185,8 @@ axs4[1, 0].legend(robot_names)
# multple robot plot for comparision
####################### Inputs plot ###################################
-fig4, axs5 = plt.subplots(nrows=6, ncols=2, figsize=(7, 7))
+fig5, axs5 = plt.subplots(nrows=6, ncols=2, figsize=(10, 10))
+st = fig5.suptitle("Input values", fontsize="x-large")
axs5[0, 0].plot(t_np,input_data_np[:,0])
axs5[0, 1].plot(t_np,input_data_np[:,1])
@@ -188,40 +201,40 @@ axs5[4, 1].plot(t_np,input_obstacle_data_np[:,1])
axs5[5, 0].plot(t_np,input_integration_data_np[:,0])
axs5[5, 1].plot(t_np,input_integration_data_np[:,1])
-axs5[0, 0].set_ylabel("a(m2/s")
+axs5[0, 0].set_ylabel("a(m2/s)")
axs5[0, 0].set_xlabel("time(s)")
axs5[0, 0].set_title("input")
-axs5[0, 1].set_ylabel("a(m2/s")
+axs5[0, 1].set_ylabel("a(m2/s)")
axs5[0, 1].set_xlabel("time(s)")
axs5[0, 1].set_title("input")
-axs5[1, 0].set_ylabel("a(m2/s")
+axs5[1, 0].set_ylabel("a(m2/s)")
axs5[1, 0].set_xlabel("time(s)")
axs5[1, 0].set_title("input_formation")
-axs5[1, 1].set_ylabel("a(m2/s")
+axs5[1, 1].set_ylabel("a(m2/s)")
axs5[1, 1].set_xlabel("time(s)")
axs5[1, 1].set_title("input_formation")
-axs5[2, 0].set_ylabel("a(m2/s")
+axs5[2, 0].set_ylabel("a(m2/s)")
axs5[2, 0].set_xlabel("time(s)")
axs5[2, 0].set_title("input_orientation")
-axs5[2, 1].set_ylabel("a(m2/s")
+axs5[2, 1].set_ylabel("a(m2/s)")
axs5[2, 1].set_xlabel("time(s)")
axs5[2, 1].set_title("input_orientation")
-axs5[3, 0].set_ylabel("a(m2/s")
+axs5[3, 0].set_ylabel("a(m2/s)")
axs5[3, 0].set_xlabel("time(s)")
axs5[3, 0].set_title("input_navigation")
-axs5[3, 1].set_ylabel("a(m2/s")
+axs5[3, 1].set_ylabel("a(m2/s)")
axs5[3, 1].set_xlabel("time(s)")
axs5[3, 1].set_title("input_navigation")
-axs5[4, 0].set_ylabel("a(m2/s")
+axs5[4, 0].set_ylabel("a(m2/s)")
axs5[4, 0].set_xlabel("time(s)")
axs5[4, 0].set_title("input_obstacle")
-axs5[4, 1].set_ylabel("a(m2/s")
+axs5[4, 1].set_ylabel("a(m2/s)")
axs5[4, 1].set_xlabel("time(s)")
axs5[4, 1].set_title("input_obstacle")
-axs5[5, 0].set_ylabel("a(m2/s")
+axs5[5, 0].set_ylabel("a(m2/s)")
axs5[5, 0].set_xlabel("time(s)")
axs5[5, 0].set_title("input_integration")
-axs5[5, 1].set_ylabel("a(m2/s")
+axs5[5, 1].set_ylabel("a(m2/s)")
axs5[5, 1].set_xlabel("time(s)")
axs5[5, 1].set_title("input_integration")
axs5[0, 0].set_ylabel("ax(m)")
@@ -230,13 +243,16 @@ axs5[0, 0].set_xlabel("time(s)")
####################### position plot ###################################
fig6 = plt.figure()
axs6 = plt.axes()
-# st = fig6.suptitle("Distance plot", fontsize="x-large")
+st = fig6.suptitle("Position plot", fontsize="x-large")
# plot workspace
axs6.set_xlim(-3, 3)
axs6.set_ylim(-2, 2)
-#
+# print(pose_data_np[:,0])
+# print(pose_data_np[:,1])
+# axs6.plot(pose_data_np[:,0],pose_data_np[:,1],"*")
+
for i in range(n_robots_logged):
- axs6.plot(pose_data_np[:,i*2],pose_data_np[:,i*2+1])
+ axs6.plot(pose_data_np[:,i*2],pose_data_np[:,i*2+1],".")
axs6.set_ylabel("y(m)")
axs6.set_xlabel("x(m)")
axs6.legend(robot_names)
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 3789b62675a6101f1d5788e91b7e9630ecf48a8d..d8db631eefcaf27befb0664089ff3dc3aeafd8e3 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -56,6 +56,13 @@ int main(int argc, char **argv) {
coord_config->initialize_robot = false;
/* current waypoint*/
next_waypoint = new coordination_formation_control_pkg::waypoint();
+ next_waypoint->pos[0] = 0;
+ next_waypoint->pos[1] = 0;
+ next_waypoint->vel[0] = 0;
+ next_waypoint->vel[1] = 0;
+ next_waypoint->orientation = 0.0;
+
+
logdata = new coordination_formation_control_pkg::logData();
logdata->current_pose.resize(n_drones);
logdata->current_twist.resize(n_drones);
@@ -78,6 +85,11 @@ int main(int argc, char **argv) {
std::vector<obstacle_t> current_obs;
current_obs.resize(n_obs);
waypoint_t current_waypoint;
+ current_waypoint.q(0) = 0 ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.q(1) = 0; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(0) = 0 ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(1) = 0 ; /*position 2 position 1 orientation yaw*/
+
integration acc_int(0.0,0.0, dt);
@@ -213,6 +225,7 @@ int main(int argc, char **argv) {
uav_state_a(0, i-1) = current_state.a(0);
uav_state_a(1, i-1) = current_state.a(1);
}
+ // std::cout << uav_state_q << std::endl;
if(coord_config->initialize_robot && coord_config->init_formation ==false && coord_config->stop_demo == false)
{
// send the robot to hover at zero velocity in plane
@@ -221,13 +234,15 @@ int main(int argc, char **argv) {
current_command.vy = 0;
current_command.yawrate = 0;
current_command.zDistance = 0.5;
- vel_cmd_publisher.publish(current_command);
+ // vel_cmd_publisher.publish(current_command);
}
else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false)
{
/* start formation controll*/
+ // std::cout << waypoint_received <<std::endl;
+
if (waypoint_received)
{
@@ -268,10 +283,11 @@ int main(int argc, char **argv) {
/* call controller*/
input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+ // std::cout << input << std::endl;
/* integrate acceleration*/
Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0));
/* reached a certain waypoint and we want to stop sending command*/
- if(true)//next_waypoint->stop) change back TODO
+ if(next_waypoint->stop) // change back TODO
{
current_command.vx = 0;
current_command.vy = 0;
@@ -309,9 +325,11 @@ int main(int argc, char **argv) {
// std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl;
- vel_cmd_publisher.publish(current_command);
+ // vel_cmd_publisher.publish(current_command);
}
+
+ // std::cout << "here " << ((coord_config->stop_demo)?1:0) << std::endl;
if(coord_config->stop_demo)
{
// land_drone();
@@ -325,15 +343,15 @@ int main(int argc, char **argv) {
ros::Duration d(3);
land_drone_srv.request.duration = d;
- if(land_drone_client.call(land_drone_srv))
- {
- std::cout << "Shutting down uav: " << std::to_string(robotid) << "Successfull" << std::endl;
- }
- else
- {
- std::cout << "Shutting down uav: " << std::to_string(robotid) << "unsuccessfull" << std::endl;
-
- }
+ // if(land_drone_client.call(land_drone_srv))
+ // {
+ // std::cout << "Shutting down uav: " << std::to_string(robotid) << "Successfull" << std::endl;
+ // }
+ // else
+ // {
+ // std::cout << "Shutting down uav: " << std::to_string(robotid) << "unsuccessfull" << std::endl;
+ //
+ // }
sleep(3);
break;
@@ -348,6 +366,7 @@ int main(int argc, char **argv) {
{
logdata->current_pose[i].position.x = uav_state_q(0,i);
logdata->current_pose[i].position.y = uav_state_q(1,i);
+ // std::cout <<(uav_state_q(1,i)) << std::endl;
logdata->current_twist[i].linear.x = uav_state_p(0,i);
logdata->current_twist[i].linear.y = uav_state_p(1,i);
logdata->current_acc[i].linear.x = uav_state_a(0,i);
@@ -362,12 +381,12 @@ int main(int argc, char **argv) {
logdata->input.y = input(1,0);
logdata->input_formation.x = input(0,1);
logdata->input_formation.y = input(1,1);
- logdata->input_orientation.x = input(0,2);
- logdata->input_orientation.y = input(1,2);
+ logdata->input_obstacle.x = input(0,2);
+ logdata->input_obstacle.y = input(1,2);
logdata->input_navigation.x = input(0,3);
logdata->input_navigation.y = input(1,3);
- logdata->input_obstacle.x = input(0,4);
- logdata->input_obstacle.y = input(1,4);
+ logdata->input_orientation.x = input(0,4);
+ logdata->input_orientation.y = input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
@@ -385,7 +404,7 @@ int main(int argc, char **argv) {
for(int i = 0; i <= 1000; i++)
{
std_msgs::Empty stop_drone;
- stop_robot_publisher.publish(stop_drone);
+ // stop_robot_publisher.publish(stop_drone);
}
if(enable_log)
{
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 a0dd8393a1c11bb132c2333a1e7b484820b8e8a8..fad5e41e7b4798baa2a64f1d03ba38cb27c12fc5 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -49,10 +49,17 @@ int main(int argc, char **argv) {
/* current waypoint*/
next_waypoint = new coordination_formation_control_pkg::waypoint();
+ next_waypoint->pos[0] = 0;
+ next_waypoint->pos[1] = 0;
+ next_waypoint->vel[0] = 0;
+ next_waypoint->vel[1] = 0;
+ next_waypoint->orientation = 0.0;
+
logdata = new coordination_formation_control_pkg::logData();
logdata->current_pose.resize(n_ugvs);
logdata->current_twist.resize(n_ugvs);
logdata->current_acc.resize(n_ugvs);
+ // logdata->current_acc.resize(n_ugvs);
/*current command*/
geometry_msgs::Twist current_command;
@@ -75,6 +82,11 @@ int main(int argc, char **argv) {
std::vector<obstacle_t> current_obs;
current_obs.resize(n_obs_ugv);
waypoint_t current_waypoint;
+ current_waypoint.q(0) = 0 ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.q(1) = 0; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(0) = 0 ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(1) = 0 ; /*position 2 position 1 orientation yaw*/
+
/* robot indentification*/
@@ -331,14 +343,14 @@ int main(int argc, char **argv) {
logdata->command_twist.linear.y = current_waypoint.p(1);
logdata->input.x = input(0,0);
logdata->input.y = input(1,0);
- logdata->input.x = input(0,1);
+ logdata->input_formation.x = input(0,1);
logdata->input_formation.y = input(1,1);
- logdata->input_orientation.x = input(0,2);
- logdata->input_orientation.y = input(1,2);
+ logdata->input_obstacle.x = input(0,2);
+ logdata->input_obstacle.y = input(1,2);
logdata->input_navigation.x = input(0,3);
logdata->input_navigation.y = input(1,3);
- logdata->input_obstacle.x = input(0,4);
- logdata->input_obstacle.y = input(1,4);
+ logdata->input_orientation.x = input(0,4);
+ logdata->input_orientation.y = input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
// bag.write("robot_1",logdata->header.stamp, *logdata);
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 27e192d8cdb3186957a59725b2bc54eaaef7cd83..1c9a6813175978c1c0d2e39950813f8ac8a0c4bc 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -12,6 +12,7 @@ swamControllerAlg::swamControllerAlg(robot_ind_t indentification, formation_conf
this->robot_id = indentification;
this->integral_error << 0,0;
this->previous_integral_error << 0,0;
+ this->range_sat << -formation_config.param.int_max,formation_config.param.int_max;
/* we want to ensure the following condition is met
* c1_alpha < c1_gamma < c1_beta*/
@@ -307,8 +308,8 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
}
else
{
- std::cout << centroid_q << std::endl;
- std::cout << centroid_p << std::endl;
+ // std::cout << centroid_q << std::endl;
+ // std::cout << centroid_p << std::endl;
u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref) -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
}
@@ -377,6 +378,7 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
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;
+ u_integral = this->saturate(u_integral, range_sat);
// std::cout << u_integral << std::endl;
@@ -393,7 +395,7 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
input2send.col(4) = u_orientation;
input2send.col(5) = u_integral;
// ROS_INFO("Swam Controller: input completed");
-
+ // std::cout << total_input <<std::endl;
return (input2send);
@@ -513,3 +515,25 @@ float swamControllerAlg::computeBij(row_vector_2d_t q_hat_i, row_vector_2d_t qi,
float d_obs_sn = this->computeSigmaNorm(d_obs,eps);
return (this->ph(z_sn/d_obs_sn,h_beta));
}
+
+row_vector_2d_t swamControllerAlg::saturate(row_vector_2d_t input, row_vector_2d_t range)
+{
+ row_vector_2d_t return_value;
+ for(int i = 0; i <2; i++)
+ {
+ if(input(i) > range(1))
+ {
+ return_value(i) = range(i);
+ }
+ else if(input(i) < range(0))
+ {
+ return_value(i) = range(0);
+ }
+ else
+ {
+ return_value(i) = input(i);
+
+ }
+ }
+ return return_value;
+}