From d130612bf70d5e18095dc3d20782f200a244b5c3 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 9 Aug 2024 14:58:13 +0200 Subject: [PATCH] Initial Working and cleaned up --- Tools/simulation/gz | 2 +- .../CollisionPrevention.cpp | 220 +++++++++++++++++- .../CollisionPrevention.hpp | 16 +- .../FlightTaskManualPosition.cpp | 5 - .../tasks/Utility/StickAccelerationXY.cpp | 7 + .../tasks/Utility/StickAccelerationXY.hpp | 3 + .../simulation/gz_bridge/CMakeLists.txt | 1 + 7 files changed, 235 insertions(+), 19 deletions(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 312cd002ff96..536305adee09 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 312cd002ff9602644efef58eef93e447c10dcbe8 +Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b6982..5e9f647aace7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -508,19 +508,221 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec } } +void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel, + const matrix::Vector2f &setpoint_vel) +{ + _updateObstacleMap(); + + const float col_prev_d = _param_cp_dist.get(); + const float col_prev_dly = _param_cp_delay.get(); + const bool move_no_data = _param_cp_go_nodata.get(); + const float max_jerk = _param_mpc_jerk_max.get(); + const float max_accel = _param_mpc_acc_hor.get(); + const float xy_p = _param_mpc_xy_p.get(); + const float vel_xy_p = _param_mpc_vel_p_acc.get(); // proportional gain for velocity + const float max_vel = _param_mpc_vel_manual.get(); + const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); + + const float setpoint_length = setpoint_accel.norm(); + + const hrt_abstime constrain_time = getTime(); + int num_fov_bins = 0; + + float acc_vel_constraint = INFINITY; + matrix::Vector2f acc_vel_constraint_dir = {0.f, 0.f}; + matrix::Vector2f acc_vel_constraint_setpoint = {0.f, 0.f}; + + if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + + const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d); + bool setpoint_possible = true; + matrix::Vector2f new_setpoint = {0.f, 0.f}; + + if (setpoint_length > 0.001f) { + Vector2f setpoint_dir = setpoint_accel / setpoint_length; + + const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - + _obstacle_map_body_frame.angle_offset); + int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + + // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + + float closest_distance = INFINITY; + matrix::Vector2f bin_closest_dist = {0.f, 0.f}; + + + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + // delete stale values + const hrt_abstime data_age = constrain_time - _data_timestamps[i]; + const float max_range = _data_maxranges[i] * 0.01f; + + if (data_age > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { + num_fov_bins ++; + } + + float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + + // get direction of current bin + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + + // only consider bins which are between min and max values + if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance + && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { + const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; + + // Assume current velocity is sufficiently close to the setpoint velocity + const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * col_prev_dly; + + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); + } + + const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); + const float vel_max_posctrl = xy_p * stop_distance; + const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); + + const float vel_max = math::min(vel_max_posctrl, vel_max_smooth); + const float acc_max_postrl = vel_xy_p * (vel_max - curr_vel_parallel) ; + + if (acc_max_postrl < acc_vel_constraint) { + acc_vel_constraint = acc_max_postrl; + acc_vel_constraint_dir = bin_direction; + } + + if (distance < closest_distance) { + closest_distance = distance; + bin_closest_dist = bin_direction; + } + + // calculate closest distance for acceleration constraint + + + } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { + if (!move_no_data || (move_no_data && _data_fov[i])) { + setpoint_possible = false; + } + } + } + + const matrix::Vector2f normal_component = setpoint_dir * (setpoint_dir.dot(bin_closest_dist)); + const matrix::Vector2f tangential_component = setpoint_dir - normal_component; + + + if (closest_distance < min_dist_to_keep && setpoint_possible) { + float scale = (closest_distance - min_dist_to_keep); // always negative meaning it will push us away from the obstacle + new_setpoint = tangential_component * setpoint_length + bin_closest_dist * xy_p * vel_xy_p * + scale; // scale is on the closest distance vector, as thats the critical direction + + } else if (closest_distance >= min_dist_to_keep && setpoint_possible) { + const float scale_distance = math::max(min_dist_to_keep, max_vel / xy_p); + float scale = (closest_distance - min_dist_to_keep) / scale_distance; + scale *= scale; // square the scale to lower commanded accelerations close to the obstacle + scale = math::min(scale, 1.0f); + + + // only scale accelerations towards the obstacle + if (bin_closest_dist.dot(setpoint_dir) > 0) { + new_setpoint = (tangential_component + normal_component * scale) * setpoint_length; + + } else { + new_setpoint = tangential_component * setpoint_length; + } + + } + + if (num_fov_bins == 0) { + new_setpoint.setZero(); + PX4_WARN("No fov bins"); + } + + acc_vel_constraint_setpoint = acc_vel_constraint_dir * acc_vel_constraint; + setpoint_accel = new_setpoint + acc_vel_constraint_setpoint; + + } else { + // If no setpoint is provided, still apply force when you are close to an obstacle + float closest_distance = INFINITY; + matrix::Vector2f bin_closest_dist = {0.f, 0.f}; + + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + if (constrain_time - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + //count number of bins in the field of valid_new + if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { + num_fov_bins ++; + } + + float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + + // get direction of current bin + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + + if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance + && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { + const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; + + if (distance < closest_distance) { + closest_distance = distance; + bin_closest_dist = bin_direction; + } + } + } + + if (closest_distance < min_dist_to_keep) { + float scale = (closest_distance - min_dist_to_keep); + new_setpoint = bin_closest_dist * xy_p * vel_xy_p * scale; + } + + if (num_fov_bins == 0) { + new_setpoint.setZero(); + PX4_WARN("No fov bins"); + } + setpoint_accel = new_setpoint; + } + + } else { + //allow no movement + PX4_WARN("No movement"); + setpoint_accel.setZero(); + + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + + if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US + && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } + + _last_timeout_warning = getTime(); + } + } + +} void -CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, - const Vector2f &curr_vel) +CollisionPrevention::modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel) { + const float max_accel = _param_mpc_acc_hor.get(); //calculate movement constraints based on range data - Vector2f new_setpoint = original_setpoint; - _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + matrix::Vector2f new_setpoint = setpoint_accel; + matrix::Vector2f original_setpoint = setpoint_accel; + _constrainAccelerationSetpoint(new_setpoint, setpoint_vel); //warn user if collision prevention starts to interfere - bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed - || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed - || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed - || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed); + bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_accel + || new_setpoint(0) > original_setpoint(0) + 0.05f * max_accel + || new_setpoint(1) < original_setpoint(1) - 0.05f * max_accel + || new_setpoint(1) > original_setpoint(1) + 0.05f * max_accel); _interfering = currently_interfering; @@ -531,7 +733,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max new_setpoint.copyTo(constraints.adapted_setpoint); _constraints_pub.publish(constraints); - original_setpoint = new_setpoint; + setpoint_accel = new_setpoint; } void CollisionPrevention::_publishVehicleCmdDoLoiter() diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cbdc..4d02bfa3dd46 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -79,9 +79,7 @@ class CollisionPrevention : public ModuleParams * @param curr_pos, current vehicle position * @param curr_vel, current vehicle velocity */ - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, - const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); - + void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: obstacle_distance_s _obstacle_map_body_frame {}; @@ -106,6 +104,13 @@ class CollisionPrevention : public ModuleParams */ void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + /** + * Computes an adaption to the setpoint direction to guide towards free space + * @param setpoint_accel current setpoint acceleration + * @param setpoint_vel current setpoint velocity + */ + void _constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + /** * Determines whether a new sensor measurement is used * @param map_index, index of the bin in the internal map the measurement belongs in @@ -148,7 +153,10 @@ class CollisionPrevention : public ModuleParams (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ - (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ + (ParamInt) _param_mpc_pos_mode, /**< position control mode*/ + (ParamFloat) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_vel_manual /**< p gain from velocity controller*/ ) /** diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 71dedaf0566a..352875764e30 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks() // Rotate setpoint into local frame Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); - // collision prevention - if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy()); - } - _velocity_setpoint.xy() = vel_sp_xy; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c0fe9b315b67..eed9d9d5aca7 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -96,6 +96,13 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); _acceleration_setpoint = stick_xy.emult(acceleration_scale); + if (_collision_prevention.is_active()) { + matrix::Vector2f accel_setpoint_xy = _acceleration_setpoint; + matrix::Vector2f vel_setpoint_xy = _velocity_setpoint; + _collision_prevention.modifySetpoint(accel_setpoint_xy, vel_setpoint_xy); + _acceleration_setpoint = accel_setpoint_xy; + + } // Add drag to limit speed and brake again Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 8bc2bb75313d..9fff1da1c426 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -65,6 +66,8 @@ class StickAccelerationXY : public ModuleParams void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; private: + CollisionPrevention _collision_prevention{this}; + void applyJerkLimit(const float dt); matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, const matrix::Vector2f &vel_sp); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 003a712282f9..0300397b1200 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -93,6 +93,7 @@ if(gz-transport_FOUND) windy baylands lawn + walls ) # find corresponding airframes