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

Trajectory xm1, xm2 dynamically calculated, integral_max changed

parent 0629e9e1
......@@ -263,10 +263,19 @@ float trajectory_t1 = 0; // Time at xm1
float trajectory_t2 = 0; // Time at xm2
float trajectory_t3 = 0; // Time at xms
// fetched from .yaml
float xm1_x_distance;
float xm2_distance_to_ms;
float xm1_normalizing_factor = 3; // normalize the difference between xcf0 and xms
float xm1_scaling_factor = 0.6; // Scale in the direction of the the difference between xcf0 and xms
float xm2_distance_to_ms_at_zero_velocity = 0.6;
float xm2_distance_to_ms_scaling_factor = 1;
// Calculate from approximate distance and velocity the total duration of the trajectory
float trajectory_velocity_of_CF; // fetched from yaml
float trajectory_total_distance = 0;
......
......@@ -4,11 +4,11 @@ mass_CF : 28 #32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# max absolute value for integrator
integrator_max : [1.0,1.0,1.0]
integrator_max : [0.4, 0.4, 0.4] #[1.0, 1.0, 1.0]
# Frequency of the controller, in hertz
vicon_frequency : 200
......@@ -30,8 +30,12 @@ xm1_x_distance: 0.5
# distance from MS to second point on trajectory
xm2_distance_to_ms: 0.6
# velocity of trajctory
trajectory_velocity_of_CF: 2.0
trajectory_velocity_of_CF : 2.0
xm1_normalizing_factor : 3 # normalize the difference between xcf0 and xms
xm1_scaling_factor : 0.6 # Scale in the direction of the the difference between xcf0 and xms
xm2_distance_to_ms_at_zero_velocity : 0.6 # xm2 distance behind the mothership when mothership velocity is 0
xm2_distance_to_ms_scaling_factor : 1 # Scale in direction of the mothership velocity
# tolerances
tol_takeoff: [0.07, 0.07, 0.07]
......@@ -132,4 +136,4 @@ gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
# [10°, 10°]=[-0.174532925, 0.174532925]
gainFeedforwardAnglefromVelocity: [-0.174532925, 0.174532925]
\ No newline at end of file
gainFeedforwardAnglefromVelocity: [-0.174532925, 0.174532925]
......@@ -457,7 +457,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
......@@ -544,11 +544,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
break;
case DRONEX_STATE_FOLLOWING_TRAJECTORY:
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
/*if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] )*/
if(abs(request.ownCrazyflie.x-xm2[0]) < tol_approach[0] &&
abs(request.ownCrazyflie.y-xm2[1]) < tol_approach[1] &&
abs(request.ownCrazyflie.z-xm2[2]) < tol_approach[2] ){
//flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
}
break;
......@@ -848,18 +851,18 @@ void calculateTrajectory(Controller::Request &request){
}
// TO BE TESTED: (different concept for clipping velocity)
// TO BE TESTED: (different concept for clipping velocity)
float trajectory_real_velocity[3];
trajectory_real_velocity[0] = (trajectory_temp_setpoint_future[0]-trajectory_temp_setpoint[0])/deltaT;
trajectory_real_velocity[1] = (trajectory_temp_setpoint_future[1]-trajectory_temp_setpoint[1])/deltaT;
trajectory_real_velocity[2] = (trajectory_temp_setpoint_future[2]-trajectory_temp_setpoint[2])/deltaT;
float tot_real_velocity_xy = sqrt(pow(trajectory_real_velocity[0],2)+pow(trajectory_real_velocity[1],2));
trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy;
trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy;
*/
}
......@@ -890,6 +893,17 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
xcf0[2] = request.ownCrazyflie.z;
// TODO possible change: choose xm1 based on relative position CF and mothership, CF velocity, mothership velocity
float xm1_normalized_direction = 1.0/(xm1_normalizing_factor) * (request.otherCrazyflies[0].x - xcf0[0]);
//normalized direction has to be between -1 and 1 such that together with the scaling factor we get reasonable values
if(xm1_normalized_direction > 1){
xm1_normalized_direction = 1;
}else if(xm1_normalized_direction < -1){
xm1_normalized_direction = -1;
}
xm1_x_distance = xm1_scaling_factor*xm1_normalized_direction;
xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
xm1[1] = xcf0[1];
xm1[2] = request.otherCrazyflies[0].z+0.6;
......@@ -903,6 +917,18 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// TODO cosYaw and sinYaw maybe otherway round
// xm2_distance_to_ms: hardcoded distance
// xm2_distance_to_ms should be chosen based on mothership velocity
float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]);
float normalized_ms_velocity = ms_abs_velocity/xm2_distance_to_ms_at_zero_velocity;
if(normalized_ms_velocity > xm2_distance_to_ms_at_zero_velocity){
normalized_ms_velocity = xm2_distance_to_ms_at_zero_velocity;
}
xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_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];
......@@ -962,10 +988,10 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_return[3] = mothership_vel[0];
trajectory_return[4] = mothership_vel[1];
trajectory_return[5] = mothership_vel[2];
}
// test trajectory: Oval
/*
......@@ -979,7 +1005,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius);
trajectory_return[2] = 0.5;
*/
return trajectory_return;
}
......@@ -1492,7 +1518,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugPublisher.publish(debugMsg);
if (ros::ok()){
//ROS_INFO("Init RVIZ STUFF");
......@@ -1523,11 +1549,11 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
if(drone_position_list.points.size()<5000)
//drone_position_list.points.erase(drone_position_list.points.begin());
drone_position_list.points.push_back(p);
rviz_points_trajectory_publisher.publish(drone_position_list);
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
......@@ -1552,13 +1578,13 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
setpoint_position_list.points.push_back(p);
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
}
}
......@@ -2033,6 +2059,12 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
xm2_distance_to_ms = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms");
trajectory_velocity_of_CF = getParameterFloat(nodeHandle_for_dronexController, "trajectory_velocity_of_CF");
xm1_normalizing_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_normalizing_factor");
xm1_scaling_factor = getParameterFloat(nodeHandle_for_dronexController, "xm1_scaling_factor");
xm2_distance_to_ms_at_zero_velocity = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_at_zero_velocity");
xm2_distance_to_ms_scaling_factor = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_scaling_factor");
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);
......@@ -2339,7 +2371,7 @@ int main(int argc, char* argv[]) {
ROS_INFO("[DroneX CONTROLLER] Service ready :-)");
rviz_points_trajectory_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory", 1);
rviz_points_trajectory_generated_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory_generated", 1);
......
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