To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 49a92689 authored by Tobias's avatar Tobias
Browse files

200Hz controller, less weight on body rates

parent 402eedc9
......@@ -202,13 +202,14 @@ std::vector<float> load_setpoint = {0.0 , 0.0 , 0.0 , 0.0}; // The setpoint
// initialize measurement vector
Eigen::Matrix<float,24,1> y_to_controller;
Eigen::Matrix<float,24,1> y_only_agent_1;
// output matrix C
Eigen::Matrix<float,24,57> outputMatrix;
Eigen::Matrix<float,24,33> outputMatrix_centralized;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,33,1> estimator_state_prev;
Eigen::Matrix<float,33,1> estimator_state_prev_only_agent1;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,57,1> overlapping_estimator_state_prev;
......@@ -332,4 +333,5 @@ Eigen::MatrixXf readMatrix(std::string filename);
std::string getcwd_string( void );
void distributed_control_command( Controller::Request &request, Controller::Response &response );
void centralized_control_command( Controller::Request &request, Controller::Response &response);
void display_control_agent1( Controller::Request &request, Controller::Response &response) ;
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
\ No newline at end of file
......@@ -387,25 +387,33 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = request.otherCrazyflies[3].z;
load_setpoint[3] = request.otherCrazyflies[3].yaw;
gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
//load_setpoint[3] = request.otherCrazyflies[3].yaw;
//gravity_force_copterloadsystem = gravity_force + load_mass * 9.81/(1000*4*3);
}
// Distributed Controller runs at 100Hz --> downsampling by factor 2
if (true)//(outer_loop_counter % 2 == 0)
if (outer_loop_counter % 2 == 0)
{
// update measurement vector using vicon Data
//get_y_from_CFData( request );
get_y_from_CFData_withLoad( request );
/*
y_only_agent_1 = y_to_controller;
for(int i = 6 ; i < 24 ; ++i )
{
y_only_agent_1(i,0) = 0;
}
*/
}
// compute centralized controller commands
centralized_control_command( request, response );
float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
//display_control_agent1( request, response);
//float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
// gravity_force_copterloadsystem -= 0.001*delta_z_load_current;
// compute agent's lqr command for the setpoint
//agent_lqr_command( request , response );
......@@ -460,7 +468,7 @@ void distributed_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
if (outer_loop_counter % 50 == 0)
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
}
......@@ -483,6 +491,84 @@ void distributed_control_command( Controller::Request &request, Controller::Resp
}
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev_only_agent1;
// Estimator update
estimator_state_prev_only_agent1 = centralized_controller.Phi * estimator_state_prev_only_agent1 + centralized_controller.Gamma * y_only_agent_1;
// extract motor command from u:
float thrustSum = 0.0;
float outRoll = 0.0;
float outPitch = 0.0;
float outYaw = 0.0;
if (my_agentID == 1 )
{
// Controller for crazyflie 01
thrustSum = u(0,0);
outRoll = u(1,0);
outPitch = u(2,0);
outYaw = u(3,0);
}
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " << outYaw );
}
// Debugging
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_only_agent_1(3,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_only_agent_1(4,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_only_agent_1(5,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter2 = " << y_only_agent_1(6,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter2 = " << y_only_agent_1(7,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter2 = " << y_only_agent_1(8,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_only_agent_1(9,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_only_agent_1(10,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_only_agent_1(11,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter3 = " << y_only_agent_1(12,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter3 = " << y_only_agent_1(13,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter3 = " << y_only_agent_1(14,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_only_agent_1(15,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_only_agent_1(16,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_only_agent_1(17,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for load = " << y_only_agent_1(18,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for load = " << y_only_agent_1(19,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for load = " << y_only_agent_1(20,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_only_agent_1(21,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_only_agent_1(22,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_only_agent_1(23,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_only_agent_1^T = " << y_only_agent_1.transpose() );
}
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
// choosing the Crazyflie onBoard controller type.
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{
......@@ -525,9 +611,12 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
if (outer_loop_counter % 50 == 0)
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
}
// PREPARE AND RETURN THE VARIABLE "response"
......@@ -631,7 +720,7 @@ void get_y_from_CFData( Controller::Request &request )
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 50 == 0)
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
......@@ -733,24 +822,35 @@ void get_y_from_CFData_withLoad( Controller::Request &request )
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 50 == 0)
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_to_controller(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_to_controller(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_to_controller(2,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_to_controller(3,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_to_controller(4,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_to_controller(5,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter2 = " << y_to_controller(6,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter2 = " << y_to_controller(7,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter2 = " << y_to_controller(8,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_to_controller(9,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_to_controller(10,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(11,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(11,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter3 = " << y_to_controller(12,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter3 = " << y_to_controller(13,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter3 = " << y_to_controller(14,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_to_controller(15,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_to_controller(16,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_to_controller(17,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for load = " << y_to_controller(18,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for load = " << y_to_controller(19,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for load = " << y_to_controller(20,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_to_controller(21,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_to_controller(22,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_to_controller(23,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );
}
......@@ -1569,7 +1669,7 @@ int main(int argc, char* argv[]) {
// get current working directory for centralized controller
std::string folderpath_c = getcwd_string();
folderpath_c = folderpath_c.substr(0, folderpath_c.size()-5);
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements_200Hz/");
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements/");
// READ MATRICES FROM DATA FOLDER
std::string filename_K = folderpath;
......@@ -1596,6 +1696,7 @@ int main(int argc, char* argv[]) {
// Initialize some state vector of the output feedback controller
//augmented_state_eq.setZero();
overlapping_estimator_state_prev.setZero();
estimator_state_prev_only_agent1.setZero();
//state_eq.setZero();
estimator_state_prev.setZero();
// set measurement vector to zero
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment