diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 85c274ab5d..8a38f93471 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -84,59 +84,42 @@ bool PositionSmoothing::_isTurning(const Vector3f &target) const float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const { - Vector3f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition(), - _trajectory[2].getCurrentPosition()); + const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); - math::trajectory::VehicleDynamicLimits config; - config.z_accept_rad = _vertical_acceptance_radius; - config.xy_accept_rad = _target_acceptance_radius; - config.max_acc_xy = _trajectory[0].getMaxAccel(); - config.max_jerk = _trajectory[0].getMaxJerk(); - config.max_speed_xy = _cruise_speed; - config.max_acc_xy_radius_scale = _horizontal_trajectory_gain; + math::trajectory::VehicleDynamicLimits config_xy; + config_xy.accept_rad = _target_acceptance_radius; + config_xy.max_acc = _trajectory[0].getMaxAccel(); + config_xy.max_jerk = _trajectory[0].getMaxJerk(); + config_xy.max_speed = _cruise_speed; + config_xy.max_acc_radius_scale = _horizontal_trajectory_gain; // constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source // (eg. Obstacle Avoidance) Vector3f pos_to_waypoints[3] = {pos_traj, waypoints[1], waypoints[2]}; - return math::trajectory::computeXYSpeedFromWaypoints<3>(pos_to_waypoints, config); + return math::trajectory::computeXYSpeedFromWaypoints<3>(pos_to_waypoints, config_xy); } float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { - const Vector3f &start_position = {_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition(), - _trajectory[2].getCurrentPosition() - }; - const Vector3f &target = waypoints[1]; - const Vector3f &next_target = waypoints[2]; + const Vector3f pos_traj = {_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition() + }; - const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; - const Vector2f target_xy_z = {target.xy().norm(), target(2)}; - const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; + math::trajectory::VehicleDynamicLimits config_z; + config_z.accept_rad = _vertical_acceptance_radius; + config_z.max_acc = _trajectory[2].getMaxAccel(); + config_z.max_jerk = _trajectory[2].getMaxJerk(); + config_z.max_speed = _trajectory[2].getMaxVel(); + config_z.max_acc_radius_scale = 1.f; // no scaling for Z - float arrival_z_speed = 0.0f; - const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; + Vector3f pos_to_waypoints[3] = {pos_traj, waypoints[1], waypoints[2]}; - if (target_next_different) { - const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( - Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); - - const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); - float accel_tmp = _trajectory[2].getMaxAccel(); - float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, - _vertical_acceptance_radius); - arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); - } - - const float distance_start_target = fabs(target(2) - start_position(2)); - float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( - _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), - distance_start_target, arrival_z_speed)); - - return max_speed; + return math::trajectory::computeZSpeedFromWaypoints<3>(pos_to_waypoints, config_z); } const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index cc52a984de..120714f174 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -46,16 +46,15 @@ using matrix::Vector3f; using matrix::Vector2f; struct VehicleDynamicLimits { - float z_accept_rad; - float xy_accept_rad; + float accept_rad; - float max_acc_xy; + float max_acc; float max_jerk; - float max_speed_xy; + float max_speed; // TODO: remove this - float max_acc_xy_radius_scale; + float max_acc_radius_scale; }; /* @@ -68,12 +67,12 @@ struct VehicleDynamicLimits { * */ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target, - const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config) + const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config_xy) { const float distance_target_next = (target - next_target).xy().norm(); const bool target_next_different = distance_target_next > 0.001f; - const bool waypoint_overlap = distance_target_next < config.xy_accept_rad; + const bool waypoint_overlap = distance_target_next < config_xy.accept_rad; float speed_at_target = 0.0f; @@ -83,15 +82,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( Vector2f((target - next_target).xy()).unit_or_zero())); const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); - float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy; - float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad); - speed_at_target = min(max_speed_in_turn, exit_speed, config.max_speed_xy); + float accel_tmp = config_xy.max_acc_radius_scale * config_xy.max_acc; + float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config_xy.accept_rad); + speed_at_target = min(max_speed_in_turn, exit_speed, config_xy.max_speed); } float start_to_target = (start_position - target).xy().norm(); - float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target); + float max_speed = computeMaxSpeedFromDistance(config_xy.max_jerk, config_xy.max_acc, start_to_target, speed_at_target); - return min(config.max_speed_xy, max_speed); + return min(config_xy.max_speed, max_speed); } /* @@ -105,7 +104,7 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits */ template -float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) +float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config_xy) { static_assert(N >= 2, "Need at least 2 points to compute speed"); @@ -116,7 +115,34 @@ float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDyna max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], waypoints[i + 1], waypoints[min(i + 2, N - 1)], - max_speed, config); + max_speed, config_xy); + } + + return max_speed; +} + +inline float computeStartZSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target, + float exit_speed, const VehicleDynamicLimits &config_z) +{ + const float start_to_target = fabsf(target(2) - start_position(2)); + const float accel_tmp = config_z.max_acc_radius_scale * config_z.max_acc; + const float max_speed = computeMaxSpeedFromDistance(config_z.max_jerk, accel_tmp, start_to_target, exit_speed); + + return min(config_z.max_speed, max_speed); +} + +template +float computeZSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config_z) +{ + static_assert(N >= 2, "Need at least 2 points to compute speed"); + + float max_speed = 0.f; + + // go backwards through the waypoints + for (int i = (N - 2); i >= 0; i--) { + max_speed = computeStartZSpeedFromWaypoints(waypoints[i], + waypoints[i + 1], + max_speed, config_z); } return max_speed; diff --git a/src/lib/motion_planning/TrajectoryConstraintsTest.cpp b/src/lib/motion_planning/TrajectoryConstraintsTest.cpp index f59302ddf2..d4a8e96bd6 100644 --- a/src/lib/motion_planning/TrajectoryConstraintsTest.cpp +++ b/src/lib/motion_planning/TrajectoryConstraintsTest.cpp @@ -8,7 +8,7 @@ using namespace math::trajectory; class TrajectoryConstraintsTest : public ::testing::Test { public: - VehicleDynamicLimits config; + VehicleDynamicLimits config_xy; Vector3f vehicle_location; Vector3f target; @@ -18,15 +18,14 @@ public: void SetUp() override { - config.z_accept_rad = 1.f; - config.xy_accept_rad = 0.99f; + config_xy.accept_rad = 0.99f; - config.max_acc_xy = 3.f; - config.max_jerk = 10.f; + config_xy.max_acc = 3.f; + config_xy.max_jerk = 10.f; - config.max_speed_xy = 10.f; + config_xy.max_speed = 10.f; - config.max_acc_xy_radius_scale = 0.8f; + config_xy.max_acc_radius_scale = 0.8f; /* * (20,20) @@ -53,11 +52,11 @@ TEST_F(TrajectoryConstraintsTest, testStraight) // WHEN: we get the speed for straight line travel Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be the same as speed directly to the end point Vector3f direct_points[2] = {vehicle_location, next_target}; - float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy); EXPECT_FLOAT_EQ(through_speed, direct_speed); } @@ -72,11 +71,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightNaN) // WHEN: we get the speed for points which are NaN afterwards Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be the same as speed to the closer point Vector3f direct_points[2] = {vehicle_location, target}; - float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy); EXPECT_FLOAT_EQ(through_speed, direct_speed); } @@ -86,15 +85,15 @@ TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose) // GIVEN: 3 waypoints in straight line next_target = target + 2.f * (target - vehicle_location); target = vehicle_location + 0.05f * (next_target - vehicle_location); - config.max_jerk = 8.f; + config_xy.max_jerk = 8.f; // WHEN: we get the speed for straight line travel Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be the same as speed directly to the end point Vector3f direct_points[2] = {vehicle_location, next_target}; - float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy); EXPECT_FLOAT_EQ(through_speed, direct_speed); } @@ -107,11 +106,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightMidClose) // WHEN: we get the speed for straight line travel Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be the same as speed directly to the end point Vector3f direct_points[2] = {vehicle_location, next_target}; - float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy); EXPECT_FLOAT_EQ(through_speed, direct_speed); } @@ -124,11 +123,11 @@ TEST_F(TrajectoryConstraintsTest, testStraightMidFar) // WHEN: we get the speed for straight line travel Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be the same as speed directly to the end point Vector3f direct_points[2] = {vehicle_location, next_target}; - float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config_xy); EXPECT_FLOAT_EQ(through_speed, direct_speed); } @@ -141,11 +140,11 @@ TEST_F(TrajectoryConstraintsTest, test90Angle) // WHEN: we get the speed for travel around the path Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be slightly faster than stopping at the intermediate point Vector3f stop_points[2] = {vehicle_location, target}; - float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy); EXPECT_GT(through_speed, stop_speed); //faster EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster @@ -158,11 +157,11 @@ TEST_F(TrajectoryConstraintsTest, test45Angle) // WHEN: we get the speed for travel around the path Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be slightly faster than stopping at the intermediate point Vector3f stop_points[2] = {vehicle_location, target}; - float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy); EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster @@ -175,11 +174,11 @@ TEST_F(TrajectoryConstraintsTest, test10Angle) // WHEN: we get the speed for travel around the path Vector3f waypoints[3] = {vehicle_location, target, next_target}; - float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config_xy); // THEN: it should be slightly faster than stopping at the intermediate point Vector3f stop_points[2] = {vehicle_location, target}; - float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config_xy); EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster @@ -192,12 +191,12 @@ TEST_F(TrajectoryConstraintsTest, test10AngleFarNext) // WHEN: we get the speed for travel around the path Vector3f far_waypoints[3] = {vehicle_location, target, next_target}; - float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config); + float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config_xy); // THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn next_target = Vector3f(30, 11.7, 5); Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; - float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config_xy); EXPECT_FLOAT_EQ(far_speed, close_speed); } @@ -209,12 +208,12 @@ TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext) // WHEN: we get the speed for travel around the path Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; - float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config_xy); // THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance next_target = Vector3f(30, 11.7, 5); Vector3f normal_waypoints[3] = {vehicle_location, target, next_target}; - float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config); + float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config_xy); EXPECT_LT(close_speed, normal_speed); }