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 781f9d94 authored by pragash1's avatar pragash1
Browse files

Linear Trajectory Generation added

parent c16f6e85
......@@ -230,6 +230,42 @@ float tol_land[3] = {0.03, 0.03, 0.03}; // *
//TODO we might want to use float64 instead of float
// trajectory generation variables
double trajectory_start_time = 0;
double total_time_since_start = 0;
bool first_trajectory_calculation = true;
// Trajectory: xcf0 -> xm1 -> xm2 -> xms
// xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
std::vector<float> xcf0(3);
std::vector<float> xm1(3);
std::vector<float> xm2(3);
std::vector<float> xms(3);
float trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
float trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
float trajectory_duration_3 = 0; // Duration of flight between xm2 and xms
float trajectory_t0 = 0; // Time when Button pressed to land on mothership
float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
float xm1_x_distance = 0.5;
float xm2_distance_to_ms = 0.3;
// Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF = 1.0; // Fly at 1 m/s
float trajectory_total_distance = 0;
float trajectory_duration = 0;
// Origin of the Flying zone
float trajectory_setpoint[4]; // x,y,z,yaw
......@@ -237,8 +273,6 @@ float trajectory_velocity[3]; // x,y,z
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
double trajectory_start_time = 0;
double total_time_since_start = 0;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 5; // 1 round in 5 sec
......@@ -492,10 +526,17 @@ void buttonPressed_follow_trajectory();
void buttonPressed_reset();
void calculateTrajectory();
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response);
// Trajectory methods
void calculateTrajectory(Controller::Request &request);
std::vector<float> trajectory_to_ms(Controller::Request &request, double t);
float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2);
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
......
......@@ -234,12 +234,18 @@ void buttonPressed_integrator_reset(){
void buttonPressed_follow_trajectory(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
// Initialize trajectory timers
flightSequence = SEQUENCE_NONE;
flying_state = DRONEX_FOLLOW_TRAJECTORY;
total_time_since_start = 0;
trajectory_x_radius = 0;
trajectory_y_radius = 0;
first_trajectory_calculation = true;
trajectory_start_time = ros::Time::now().toSec();
trajectory_t0 = ros::Time::now().toSec();
ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
}
......@@ -519,7 +525,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
// for testing hover over mothership
/*
dronexSetpoint.x = request.otherCrazyflies[0].x;
......@@ -822,7 +828,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float feed_forward_thrust_per_motor = F_in_newtons / 4.0;
// Only for motor wearing test
// Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4;
// }
......@@ -887,19 +893,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
calculateTrajectory();
calculateTrajectory(request);
m_setpoint_for_controller_2[0] = trajectory_setpoint[0];
m_setpoint_for_controller_2[1] = trajectory_setpoint[1];
m_setpoint_for_controller_2[2] = trajectory_setpoint[2];
//m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
//m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
m_velocity_for_controller_2[0] = trajectory_velocity[0];
m_velocity_for_controller_2[1] = trajectory_velocity[1];
m_velocity_for_controller_2[2] = trajectory_velocity[2];
/*
m_velocity_for_controller_2[0] = 0;
m_velocity_for_controller_2[1] = 0;
......@@ -1019,7 +1025,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float feed_forward_thrust_per_motor = F_in_newtons / 4.0;
// Only for motor wearing test
// Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4;
// }
......@@ -1098,21 +1104,21 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Calculate a trajectory
// Gives back computes trajectory_setpoint and trajectory_velocity
void calculateTrajectory(Controller::Response &response){
void calculateTrajectory(Controller::Request &request){
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
total_time_since_start = ros::Time::now().toSec() - trajectory_t0;
trajectory_setpoint[0] = trajectory(total_time_since_start)[0];
trajectory_setpoint[1] = trajectory(total_time_since_start)[1];
trajectory_setpoint[2] = trajectory(total_time_since_start)[2];
trajectory_setpoint[0] = trajectory_to_ms(request, total_time_since_start)[0];
trajectory_setpoint[1] = trajectory_to_ms(request, total_time_since_start)[1];
trajectory_setpoint[2] = trajectory_to_ms(request, total_time_since_start)[2];
trajectory_setpoint[3] = 0;
// just some deltaT to look ahead
float deltaT = 0.01;
trajectory_velocity[0] = (trajectory(total_time_since_start+deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory(total_time_since_start+deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[0] = (trajectory_to_ms(request,total_time_since_start+deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start+deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
/*
......@@ -1126,27 +1132,126 @@ void calculateTrajectory(Controller::Response &response){
trajectory_velocity[1] = 2*PI/trajectory_period * cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
trajectory_velocity[2] = 0;
*/
}
// returns (x,y,z) for a given time t
std::vector<float> trajectory(double t){
// Calculate the trajectory from the Crazyflie to the mothership
// Trajectory: xcf0 -> xm1 -> xm2 -> xms, All segments are straight lines. (For testing purposes) Later should be curves
// xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
// Please use trajectory_t0 as starting time
// Input: t in [trajectory_t0 and trajectory_t3]
// If t > trajectory_t3 then OUTPUT: (-1,-1,-1) -> exception handling
std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
std::vector<float> trajectory_return(3);
trajectory_x_radius = 1.0;
// If this method is called the first time after follow trajectory button
// or land on mothership button
// then save xcf0 and calculate xm1
if(first_trajectory_calculation){
xcf0[0] = request.ownCrazyflie.x;
xcf0[1] = request.ownCrazyflie.y;
xcf0[2] = request.ownCrazyflie.z;
// TODO possible change: choose xm1 based on relative position CF and mothership, CF velocity, mothership velocity
xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
xm1[1] = xcf0[1];
xm1[2] = request.otherCrazyflies[0].z;
first_trajectory_calculation = false;
}
float sinYaw = sin(request.ownCrazyflie.yaw);
float cosYaw = cos(request.ownCrazyflie.yaw);
// TODO cosYaw and sinYaw maybe otherway round
// xm2_distance_to_ms: hardcoded distance
// xm2_distance_to_ms should be chosen based on mothership velocity
xm2[0] = request.otherCrazyflies[0].x - cosYaw*xm2_distance_to_ms;
xm2[1] = request.otherCrazyflies[0].y - sinYaw*xm2_distance_to_ms;
xm2[2] = xm1[2];
xms[0] = request.otherCrazyflies[0].x;
xms[1] = request.otherCrazyflies[0].y;
xms[2] = request.otherCrazyflies[0].z; // TODO add a certain height to not collide with mothership
// Calculate the whole trajectory distance
trajectory_total_distance = calculate_distance_in_xyz(xcf0,xm1) +
calculate_distance_in_xyz(xm1,xm2) +
calculate_distance_in_xyz(xm2,xms);
trajectory_duration = trajectory_total_distance/trajectory_velocity_of_CF;
// Calculate each duration of the different segments of the trajectory
// Time between xcf0 and xm1
trajectory_duration_1 = calculate_distance_in_xyz(xcf0, xm1)/trajectory_velocity_of_CF;
// Time between xm1 and xm2
trajectory_duration_2 = calculate_distance_in_xyz(xm1,xm2)/trajectory_velocity_of_CF;
// Time between xm2 and xms
trajectory_duration_3 = calculate_distance_in_xyz(xm2,xms)/trajectory_velocity_of_CF;
// Time when at xm1
trajectory_t1 = trajectory_t0 + trajectory_duration_1;
// Time when at xm2
trajectory_t2 = trajectory_t1 + trajectory_duration_2;
// Time when at xms
trajectory_t3 = trajectory_t2 + trajectory_duration_3;
// Calculate the x,y,z position depending on where the CF should be at a certain time t
if(trajectory_t0 < t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*trajectory_velocity_of_CF*(xm1[i] - xcf0[i]);
}
}else if(trajectory_t1 < t && t <= trajectory_t2){ // Calculate position on line between xm1 and xm2
for(int i = 0; i < 3; i++){
trajectory_return[i] = xm1[i] + (t - trajectory_t1)*trajectory_velocity_of_CF*(xm2[i] - xm1[i]);
}
}else if(trajectory_t2 < t && t <= trajectory_t3){ // Calculate position on line between xm2 and xms
for(int i = 0; i < 3; i++){
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*trajectory_velocity_of_CF*(xms[i] - xm2[i]);
}
}else{
trajectory_return[0] = -1;
trajectory_return[1] = -1;
trajectory_return[2] = -1;
}
/*trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
//total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
trajectory_return[0] = originX + cos(2*PI/trajectory_period * t)*(trajectory_x_radius);
trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius);
trajectory_return[2] = 0.5;
trajectory_return[2] = 0.5;*/
return trajectory_return;
}
// Calculate distance between two points
float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2){
float distance = 0.0f;
distance = sqrt(pow(p2[0]-p1[0],2)+pow(p2[1]-p1[1],2)+pow(p2[2]-p1[2],2));
return distance;
}
......
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