Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Math/SCurve.cpp

972 lines
41 KiB
C++
Raw Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_InternalError/AP_InternalError.h>
#include "SCurve.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <stdio.h>
#endif
extern const AP_HAL::HAL &hal;
#define SEG_INIT 0
#define SEG_ACCEL_MAX 4
#define SEG_ACCEL_END 7
#define SEG_SPEED_CHANGE_END 14
#define SEG_CONST 15
#define SEG_DECEL_END 22
// constructor
SCurve::SCurve()
{
init();
}
// initialise and clear the path
void SCurve::init()
{
jerk_time = 0.0f;
jerk_max = 0.0f;
accel_max = 0.0f;
vel_max = 0.0f;
time = 0.0f;
num_segs = SEG_INIT;
add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
track.zero();
delta_unit.zero();
position_sq = 0.0f;
}
// generate a trigonometric track in 3D space that moves over a straight line
// between two points defined by the origin and destination
void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination,
float speed_xy, float speed_up, float speed_down,
float accel_xy, float accel_z,
float jerk_time_sec, float jerk_maximum)
{
init();
// leave track as zero length if origin and destination are the same
if (origin == destination) {
return;
}
// set jerk time and jerk max
jerk_time = jerk_time_sec;
jerk_max = jerk_maximum;
// update speed and acceleration limits along path
set_kinematic_limits(origin, destination,
speed_xy, speed_up, speed_down,
accel_xy, accel_z);
// avoid divide-by zeros. Path will be left as a zero length path
if (!is_positive(jerk_time) || !is_positive(jerk_max) || !is_positive(accel_max) || !is_positive(vel_max)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::calculate_track created zero length path\n");
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}
track = destination - origin;
const float track_length = track.length();
if (is_zero(track_length)) {
// avoid possible divide by zero
delta_unit.zero();
} else {
delta_unit = track.normalized();
add_segments(track_length);
}
// catch calculation errors
if (!valid()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::calculate_track invalid path\n");
debug();
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
init();
}
}
// set maximum velocity and re-calculate the path using these limits
void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)
{
// return immediately if zero length path
if (num_segs != segments_max) {
return;
}
// segment accelerations can not be changed after segment creation.
const float track_speed_max = kinematic_limit(delta_unit, speed_xy, speed_up, fabsf(speed_down));
if (is_equal(vel_max, track_speed_max)) {
// new speed is equal to current speed maximum so no need to change anything
return;
}
if (is_zero(track_speed_max)) {
// new speed is zero which is not supported
return;
}
vel_max = track_speed_max;
if (time >= segment[SEG_CONST].end_time) {
return;
}
// re-calculate the s-curve path based on update speeds
const float Pend = segment[SEG_DECEL_END].end_pos;
float Vend = MIN(vel_max, segment[SEG_DECEL_END].end_vel);
if (is_zero(time)) {
// path has not started so we can recompute the path
const float Vstart = MIN(vel_max, segment[SEG_INIT].end_vel);
num_segs = SEG_INIT;
add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
add_segments(Pend);
set_origin_speed_max(Vstart);
set_destination_speed_max(Vend);
return;
}
if ((time >= segment[SEG_ACCEL_END].end_time) && (time <= segment[SEG_SPEED_CHANGE_END].end_time)) {
// in the speed change phase
// move speed change phase to acceleration phase to provide room for further speed adjustments
// set initial segment to last acceleration segment
segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK;
segment[SEG_INIT].jerk_ref = 0.0f;
segment[SEG_INIT].end_time = segment[SEG_ACCEL_END].end_time;
segment[SEG_INIT].end_accel = segment[SEG_ACCEL_END].end_accel;
segment[SEG_INIT].end_vel = segment[SEG_ACCEL_END].end_vel;
segment[SEG_INIT].end_pos = segment[SEG_ACCEL_END].end_pos;
// move speed change segments to acceleration segments
for (uint8_t i = SEG_INIT+1; i <= SEG_ACCEL_END; i++) {
segment[i] = segment[i+7];
}
// set change segments to last acceleration speed
for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = segment[SEG_ACCEL_END].end_time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = segment[SEG_ACCEL_END].end_vel;
segment[i].end_pos = segment[SEG_ACCEL_END].end_pos;
}
} else if ((time > segment[SEG_SPEED_CHANGE_END].end_time) && (time <= segment[SEG_CONST].end_time)) {
// in the constant speed phase
// overwrite the acceleration and speed change phases with the current position and velocity
// set initial segment to last acceleration segment
segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK;
segment[SEG_INIT].jerk_ref = 0.0f;
segment[SEG_INIT].end_time = segment[SEG_SPEED_CHANGE_END].end_time;
segment[SEG_INIT].end_accel = 0.0f;
segment[SEG_INIT].end_vel = segment[SEG_SPEED_CHANGE_END].end_vel;
segment[SEG_INIT].end_pos = segment[SEG_SPEED_CHANGE_END].end_pos;
// set acceleration and change segments to current constant speed
float Jt_out, At_out, Vt_out, Pt_out;
get_jerk_accel_vel_pos_at_time(time, Jt_out, At_out, Vt_out, Pt_out);
for (uint8_t i = SEG_INIT+1; i <= SEG_SPEED_CHANGE_END; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = Vt_out;
segment[i].end_pos = Pt_out;
}
}
// adjust the INIT and ACCEL segments for new speed
if ((time <= segment[SEG_ACCEL_MAX].end_time) && is_positive(segment[SEG_ACCEL_MAX].end_time - segment[SEG_ACCEL_MAX-1].end_time) && (vel_max < segment[SEG_ACCEL_END].end_vel) && is_positive(segment[SEG_ACCEL_MAX].end_accel) ) {
// path has not finished constant positive acceleration segment
// reduce velocity as close to target velocity as possible
const float Vstart = segment[SEG_INIT].end_vel;
// minimum velocity that can be obtained by shortening SEG_ACCEL_MAX
const float Vmin = segment[SEG_ACCEL_END].end_vel - segment[SEG_ACCEL_MAX].end_accel * (segment[SEG_ACCEL_MAX].end_time - MAX(time, segment[SEG_ACCEL_MAX-1].end_time));
float Jm, t2, t4, t6;
calculate_path(jerk_time, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6);
uint8_t seg = SEG_INIT+1;
add_segments_jerk(seg, jerk_time, Jm, t2);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, -Jm, t6);
// remove numerical errors
segment[SEG_ACCEL_END].end_accel = 0.0f;
// add empty speed adjust segments
for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_CONST; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = segment[SEG_ACCEL_END].end_time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = segment[SEG_ACCEL_END].end_vel;
segment[i].end_pos = segment[SEG_ACCEL_END].end_pos;
}
calculate_path(jerk_time, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6);
seg = SEG_CONST + 1;
add_segments_jerk(seg, jerk_time, -Jm, t6);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, Jm, t2);
// remove numerical errors
segment[SEG_DECEL_END].end_accel = 0.0f;
segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, Pend - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
}
}
// adjust the speed change segments (8 to 14) for new speed
// start with empty speed adjust segments
for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = segment[SEG_ACCEL_END].end_time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = segment[SEG_ACCEL_END].end_vel;
segment[i].end_pos = segment[SEG_ACCEL_END].end_pos;
}
if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) {
// add velocity adjustment
// check there is enough time to make velocity change
// we use the approximation that the time will be distance/max_vel and 8 jerk segments
const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos;
float Jm = 0;
float t2 = 0;
float t4 = 0;
float t6 = 0;
if ((vel_max < segment[SEG_ACCEL_END].end_vel) && (jerk_time*12.0f < L/segment[SEG_ACCEL_END].end_vel)) {
// we have a problem here with small segments.
calculate_path(jerk_time, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, t6, t4, t2);
Jm = -Jm;
} else if ((vel_max > segment[SEG_ACCEL_END].end_vel) && (L/(jerk_time*12.0f) > segment[SEG_ACCEL_END].end_vel)) {
float Vm = MIN(vel_max, L/(jerk_time*12.0f));
calculate_path(jerk_time, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, t2, t4, t6);
}
uint8_t seg = SEG_ACCEL_END + 1;
if (!is_zero(Jm) && !is_negative(t2) && !is_negative(t4) && !is_negative(t6)) {
add_segments_jerk(seg, jerk_time, Jm, t2);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, -Jm, t6);
// remove numerical errors
segment[SEG_SPEED_CHANGE_END].end_accel = 0.0f;
}
}
// add deceleration segments
// earlier check should ensure that we should always have sufficient time to stop
uint8_t seg = SEG_CONST;
Vend = MIN(Vend, segment[SEG_SPEED_CHANGE_END].end_vel);
add_segment_const_jerk(seg, 0.0f, 0.0f);
if (Vend < segment[SEG_SPEED_CHANGE_END].end_vel) {
float Jm, t2, t4, t6;
calculate_path(jerk_time, jerk_max, Vend, accel_max, segment[SEG_CONST].end_vel, Pend - segment[SEG_CONST].end_pos, Jm, t2, t4, t6);
add_segments_jerk(seg, jerk_time, -Jm, t6);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, Jm, t2);
} else {
// No deceleration is required
for (uint8_t i = SEG_CONST+1; i <= SEG_DECEL_END; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = segment[SEG_CONST].end_time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = segment[SEG_CONST].end_vel;
segment[i].end_pos = segment[SEG_CONST].end_pos;
}
}
// remove numerical errors
segment[SEG_DECEL_END].end_accel = 0.0f;
segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, Pend - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
}
// catch calculation errors
if (!valid()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::set_speed_max invalid path\n");
debug();
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
init();
}
}
// set the maximum vehicle speed at the origin
// returns the expected speed at the origin which will always be equal or lower than speed
float SCurve::set_origin_speed_max(float speed)
{
// if path is zero length then start speed must be zero
if (num_segs != segments_max) {
return 0.0f;
}
// avoid re-calculating if unnecessary
if (is_equal(segment[SEG_INIT].end_vel, speed)) {
return speed;
}
const float Vm = segment[SEG_ACCEL_END].end_vel;
const float track_length = track.length();
speed = MIN(speed, Vm);
float Jm, t2, t4, t6;
calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6);
uint8_t seg = SEG_INIT;
add_segment(seg, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, speed, 0.0f);
add_segments_jerk(seg, jerk_time, Jm, t2);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, -Jm, t6);
// remove numerical errors
segment[SEG_ACCEL_END].end_accel = 0.0f;
// offset acceleration segment if we can't fit it all into half the original length
const float dPstart = MIN(0.0f, track_length * 0.5f - segment[SEG_ACCEL_END].end_pos);
const float dt = dPstart / segment[SEG_ACCEL_END].end_vel;
for (uint8_t i = SEG_INIT; i <= SEG_ACCEL_END; i++) {
segment[i].end_time += dt;
segment[i].end_pos += dPstart;
}
// add empty speed change segments and constant speed segment
for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) {
segment[i].seg_type = SegmentType::CONSTANT_JERK;
segment[i].jerk_ref = 0.0f;
segment[i].end_time = segment[SEG_ACCEL_END].end_time;
segment[i].end_accel = 0.0f;
segment[i].end_vel = segment[SEG_ACCEL_END].end_vel;
segment[i].end_pos = segment[SEG_ACCEL_END].end_pos;
}
seg = SEG_CONST;
add_segment_const_jerk(seg, 0.0f, 0.0f);
calculate_path(jerk_time, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, track_length * 0.5f, Jm, t2, t4, t6);
add_segments_jerk(seg, jerk_time, -Jm, t6);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, Jm, t2);
// remove numerical errors
segment[SEG_DECEL_END].end_accel = 0.0f;
segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, track_length - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
}
// catch calculation errors
if (!valid()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::set_origin_speed_max invalid path\n");
debug();
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
init();
return 0.0f;
}
return speed;
}
// set the maximum vehicle speed at the destination
void SCurve::set_destination_speed_max(float speed)
{
// if path is zero length then all speeds must be zero
if (num_segs != segments_max) {
return;
}
// avoid re-calculating if unnecessary
if (is_equal(segment[segments_max-1].end_vel, speed)) {
return;
}
const float Vm = segment[SEG_CONST].end_vel;
const float track_length = track.length();
speed = MIN(speed, Vm);
float Jm, t2, t4, t6;
calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6);
uint8_t seg = SEG_CONST;
add_segment_const_jerk(seg, 0.0f, 0.0f);
add_segments_jerk(seg, jerk_time, -Jm, t6);
add_segment_const_jerk(seg, t4, 0.0f);
add_segments_jerk(seg, jerk_time, Jm, t2);
// remove numerical errors
segment[SEG_DECEL_END].end_accel = 0.0f;
segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, track_length - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
}
// catch calculation errors
if (!valid()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::set_destination_speed_max invalid path\n");
debug();
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
init();
}
}
// move target location along path from origin to destination
// prev_leg and next_leg are the paths before and after this path
// wp_radius is max distance from the waypoint at the apex of the turn
// fast_waypoint should be true if vehicle will not stop at end of this leg
// dt is the time increment the vehicle will move along the path
// target_pos should be set to this segment's origin and it will be updated to the current position target
// target_vel and target_accel are updated with new targets
// returns true if vehicle has passed the apex of the corner
bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel)
{
prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel);
move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
bool s_finished = finished();
// check for change of leg on fast waypoint
const float time_to_destination = get_time_remaining();
if (fast_waypoint && braking() && is_zero(next_leg.get_time_elapsed()) && (time_to_destination <= next_leg.get_accel_finished_time())) {
Vector3f turn_pos = -get_track();
Vector3f turn_vel, turn_accel;
move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track());
const float accel_min = MIN(get_accel_along_track(), next_leg.get_accel_along_track());
if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) &&
(Vector2f(turn_vel.x, turn_vel.y).length() < speed_min) &&
(Vector2f(turn_accel.x, turn_accel.y).length() < 2.0f*accel_min)) {
next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
}
} else if (!is_zero(next_leg.get_time_elapsed())) {
next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
if (next_leg.get_time_elapsed() >= get_time_remaining()) {
s_finished = true;
}
}
return s_finished;
}
// time has reached the end of the sequence
bool SCurve::finished() const
{
return ((time >= time_end()) || (position_sq >= track.length_squared()));
}
// increment time pointer and return the position, velocity and acceleration vectors relative to the origin
void SCurve::move_from_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel)
{
advance_time(dt);
float scurve_P1 = 0.0f;
float scurve_V1, scurve_A1, scurve_J1;
get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
pos += delta_unit * scurve_P1;
vel += delta_unit * scurve_V1;
accel += delta_unit * scurve_A1;
position_sq = sq(scurve_P1);
}
// increment time pointer and return the position, velocity and acceleration vectors relative to the destination
void SCurve::move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel)
{
advance_time(dt);
float scurve_P1 = 0.0f;
float scurve_V1, scurve_A1, scurve_J1;
get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
pos += delta_unit * scurve_P1;
vel += delta_unit * scurve_V1;
accel += delta_unit * scurve_A1;
position_sq = sq(scurve_P1);
pos -= track;
}
// return the position, velocity and acceleration vectors relative to the origin at a specified time along the path
void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3f &vel, Vector3f &accel)
{
float scurve_P1 = 0.0f;
float scurve_V1, scurve_A1, scurve_J1;
get_jerk_accel_vel_pos_at_time(time_now, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
pos += delta_unit * scurve_P1;
vel += delta_unit * scurve_V1;
accel += delta_unit * scurve_A1;
}
// time at the end of the sequence
float SCurve::time_end() const
{
if (num_segs != segments_max) {
return 0.0f;
}
return segment[SEG_DECEL_END].end_time;
}
// time left before sequence will complete
float SCurve::get_time_remaining() const
{
if (num_segs != segments_max) {
return 0.0f;
}
return segment[SEG_DECEL_END].end_time - time;
}
// time when acceleration section of the sequence will complete
float SCurve::get_accel_finished_time() const
{
if (num_segs != segments_max) {
return 0.0f;
}
return segment[SEG_ACCEL_END].end_time;
}
// return true if the sequence is braking to a stop
bool SCurve::braking() const
{
if (num_segs != segments_max) {
return true;
}
return time >= segment[SEG_CONST].end_time;
}
// increment the internal time
void SCurve::advance_time(float dt)
{
time = MIN(time+dt, time_end());
}
// calculate the jerk, acceleration, velocity and position at the provided time
void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const
{
if ((num_segs != segments_max) || !is_positive(jerk_time)) {
Jt_out = 0;
At_out = 0;
Vt_out = 0;
Pt_out = 0;
return;
}
SegmentType Jtype;
uint8_t pnt = num_segs;
float Jm, T0, A0, V0, P0;
// find active segment at time_now
for (uint8_t i = 0; i < num_segs; i++) {
if (time_now < segment[num_segs - 1 - i].end_time) {
pnt = num_segs - 1 - i;
}
}
if (pnt == 0) {
Jtype = SegmentType::CONSTANT_JERK;
Jm = 0.0f;
T0 = segment[pnt].end_time;
A0 = segment[pnt].end_accel;
V0 = segment[pnt].end_vel;
P0 = segment[pnt].end_pos;
} else if (pnt == num_segs) {
Jtype = SegmentType::CONSTANT_JERK;
Jm = 0.0f;
T0 = segment[pnt - 1].end_time;
A0 = segment[pnt - 1].end_accel;
V0 = segment[pnt - 1].end_vel;
P0 = segment[pnt - 1].end_pos;
} else {
Jtype = segment[pnt].seg_type;
Jm = segment[pnt].jerk_ref;
T0 = segment[pnt - 1].end_time;
A0 = segment[pnt - 1].end_accel;
V0 = segment[pnt - 1].end_vel;
P0 = segment[pnt - 1].end_pos;
}
switch (Jtype) {
case SegmentType::CONSTANT_JERK:
calc_javp_for_segment_const_jerk(time_now - T0, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
break;
case SegmentType::POSITIVE_JERK:
calc_javp_for_segment_incr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
break;
case SegmentType::NEGATIVE_JERK:
calc_javp_for_segment_decr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
break;
}
Pt_out = MAX(0.0f, Pt_out);
}
// calculate the jerk, acceleration, velocity and position at time time_now when running the constant jerk time segment
void SCurve::calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const
{
Jt = J0;
At = A0 + J0 * time_now;
Vt = V0 + A0 * time_now + 0.5f * J0 * (time_now * time_now);
Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (1.0f / 6.0f) * J0 * (time_now * time_now * time_now);
}
// Calculate the jerk, acceleration, velocity and position at time time_now when running the increasing jerk magnitude time segment based on a raised cosine profile
void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const
{
const float Alpha = Jm * 0.5f;
const float Beta = M_PI / tj;
Jt = Alpha * (1.0f - cosf(Beta * time_now));
At = A0 + Alpha * time_now - (Alpha / Beta) * sinf(Beta * time_now);
Vt = V0 + A0 * time_now + (Alpha * 0.5f) * (time_now * time_now) + (Alpha / (Beta * Beta)) * cosf(Beta * time_now) - Alpha / (Beta * Beta);
Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (-Alpha / (Beta * Beta)) * time_now + Alpha * (time_now * time_now * time_now) / 6.0f + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * time_now);
}
// Calculate the jerk, acceleration, velocity and position at time time_now when running the decreasing jerk magnitude time segment based on a raised cosine profile
void SCurve::calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const
{
const float Alpha = Jm * 0.5f;
const float Beta = M_PI / tj;
const float AT = Alpha * tj;
const float VT = Alpha * ((tj * tj) * 0.5f - 2.0f / (Beta * Beta));
const float PT = Alpha * ((-1.0f / (Beta * Beta)) * tj + (1.0f / 6.0f) * (tj * tj * tj));
Jt = Alpha * (1.0f - cosf(Beta * (time_now + tj)));
At = (A0 - AT) + Alpha * (time_now + tj) - (Alpha / Beta) * sinf(Beta * (time_now + tj));
Vt = (V0 - VT) + (A0 - AT) * time_now + 0.5f * Alpha * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta)) * cosf(Beta * (time_now + tj)) - Alpha / (Beta * Beta);
Pt = (P0 - PT) + (V0 - VT) * time_now + 0.5f * (A0 - AT) * (time_now * time_now) + (-Alpha / (Beta * Beta)) * (time_now + tj) + (Alpha / 6.0f) * (time_now + tj) * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * (time_now + tj));
}
// generate the segments for a path of length L
// the path consists of 23 segments
// 1 initial segment
// 7 segments forming the acceleration S-Curve
// 7 segments forming the velocity change S-Curve
// 1 constant velocity S-Curve
// 7 segments forming the deceleration S-Curve
void SCurve::add_segments(float L)
{
if (is_zero(L)) {
return;
}
float Jm, t2, t4, t6;
calculate_path(jerk_time, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, t2, t4, t6);
add_segments_jerk(num_segs, jerk_time, Jm, t2);
add_segment_const_jerk(num_segs, t4, 0.0f);
add_segments_jerk(num_segs, jerk_time, -Jm, t6);
// remove numerical errors
segment[SEG_ACCEL_END].end_accel = 0.0f;
// add empty speed adjust segments
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
add_segment_const_jerk(num_segs, 0.0f, 0.0f);
const float t15 = MAX(0.0f, (L - 2.0f * segment[SEG_SPEED_CHANGE_END].end_pos) / segment[SEG_SPEED_CHANGE_END].end_vel);
add_segment_const_jerk(num_segs, t15, 0.0f);
add_segments_jerk(num_segs, jerk_time, -Jm, t6);
add_segment_const_jerk(num_segs, t4, 0.0f);
add_segments_jerk(num_segs, jerk_time, Jm, t2);
// remove numerical errors
segment[SEG_DECEL_END].end_accel = 0.0f;
segment[SEG_DECEL_END].end_vel = 0.0f;
}
// calculate the segment times for the trigonometric S-Curve path defined by:
// tj - duration of the raised cosine jerk profile
// Jm - maximum value of the raised cosine jerk profile
// V0 - initial velocity magnitude
// Am - maximum constant acceleration
// Vm - maximum constant velocity
// L - Length of the path
// t2_out, t4_out, t6_out are the segment durations needed to achieve the kinimatic path specified by the input variables
void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const
{
// init outputs
Jm_out = 0.0f;
t2_out = 0.0f;
t4_out = 0.0f;
t6_out = 0.0f;
// check for invalid arguments
if (!is_positive(tj) || !is_positive(Jm) || !is_positive(Am) || !is_positive(Vm) || !is_positive(L)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::calculate_path invalid inputs\n");
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return;
}
if (V0 >= Vm) {
// no velocity change so all segments as zero length
return;
}
Am = MIN(MIN(Am, (Vm - V0) / (2.0f * tj)), (L + 4.0f * V0 * tj) / (4.0f * sq(tj)));
if (fabsf(Am) < Jm * tj) {
Jm = Am / tj;
if ((Vm <= V0 + 2.0f * Am * tj) || (L <= 4.0f * V0 * tj + 4.0f * Am * sq(tj))) {
// solution = 0 - t6 t4 t2 = 0 0 0
t2_out = 0.0f;
t4_out = 0.0f;
t6_out = 0.0f;
} else {
// solution = 2 - t6 t4 t2 = 0 1 0
t2_out = 0.0f;
t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
t6_out = 0.0f;
}
} else {
if ((Vm < V0 + Am * tj + (Am * Am) / Jm) || (L < 1.0f / (Jm * Jm) * (Am * Am * Am + Am * Jm * (V0 * 2.0f + Am * tj * 2.0f)) + V0 * tj * 2.0f + Am * (tj * tj))) {
// solution = 5 - t6 t4 t2 = 1 0 1
Am = MIN(MIN(Am, MAX(Jm * (tj + safe_sqrt((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f), Jm * (tj - safe_sqrt((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f))), Jm * tj * (-2.0f / 3.0f) + ((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f)) * 1.0f / powf(safe_sqrt(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f) + powf(safe_sqrt(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f));
t2_out = Am / Jm - tj;
t4_out = 0.0f;
t6_out = t2_out;
} else {
// solution = 7 - t6 t4 t2 = 1 1 1
t2_out = Am / Jm - tj;
t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
t6_out = t2_out;
}
}
Jm_out = Jm;
// check outputs and reset back to zero if necessary
if (!isfinite(Jm_out) || is_negative(Jm_out) ||
!isfinite(t2_out) || is_negative(t2_out) ||
!isfinite(t4_out) || is_negative(t4_out) ||
!isfinite(t6_out) || is_negative(t6_out)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
::printf("SCurve::calculate_path invalid outputs\n");
#endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
Jm_out = 0.0f;
t2_out = 0.0f;
t4_out = 0.0f;
t6_out = 0.0f;
}
}
// generate three consecutive segments forming a jerk profile
// the index variable is the position within the path array that this jerk profile should be added
// the index is incremented to reference the next segment in the array after the jerk profile
void SCurve::add_segments_jerk(uint8_t &index, float tj, float Jm, float Tcj)
{
add_segment_incr_jerk(index, tj, Jm);
add_segment_const_jerk(index, Tcj, Jm);
add_segment_decr_jerk(index, tj, Jm);
}
// generate constant jerk time segment
// calculate the information needed to populate the constant jerk segment from the segment duration tj and jerk J0
// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0)
{
// if no time increase copy previous segment
if (is_zero(tj)) {
add_segment(index, segment[index - 1].end_time,
SegmentType::CONSTANT_JERK,
J0,
segment[index - 1].end_accel,
segment[index - 1].end_vel,
segment[index - 1].end_pos);
return;
}
const float J = J0;
const float T = segment[index - 1].end_time + tj;
const float A = segment[index - 1].end_accel + J0 * tj;
const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + 0.5f * J0 * sq(tj);
const float P = segment[index - 1].end_pos + segment[index - 1].end_vel * tj + 0.5f * segment[index - 1].end_accel * sq(tj) + (1.0f / 6.0f) * J0 * powf(tj, 3.0f);
add_segment(index, T, SegmentType::CONSTANT_JERK, J, A, V, P);
}
// generate increasing jerk magnitude time segment based on a raised cosine profile
// calculate the information needed to populate the increasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm)
{
const float Beta = M_PI / tj;
const float Alpha = Jm * 0.5f;
const float AT = Alpha * tj;
const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));
const float J = Jm;
const float T = segment[index - 1].end_time + tj;
const float A = segment[index - 1].end_accel + AT;
const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + VT;
const float P = segment[index - 1].end_pos + segment[index - 1].end_vel * tj + 0.5f * segment[index - 1].end_accel * sq(tj) + PT;
add_segment(index, T, SegmentType::POSITIVE_JERK, J, A, V, P);
}
// generate decreasing jerk magnitude time segment based on a raised cosine profile
// calculate the information needed to populate the decreasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
// the index variable is the position of this segment in the path and is incremented to reference the next segment in the array
void SCurve::add_segment_decr_jerk(uint8_t &index, float tj, float Jm)
{
const float Beta = M_PI / tj;
const float Alpha = Jm * 0.5f;
const float AT = Alpha * tj;
const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));
const float A2T = Jm * tj;
const float V2T = Jm * sq(tj);
const float P2T = Alpha * ((-1.0f / sq(Beta)) * 2.0f * tj + (4.0f / 3.0f) * powf(tj, 3.0f));
const float J = Jm;
const float T = segment[index - 1].end_time + tj;
const float A = (segment[index - 1].end_accel - AT) + A2T;
const float V = (segment[index - 1].end_vel - VT) + (segment[index - 1].end_accel - AT) * tj + V2T;
const float P = (segment[index - 1].end_pos - PT) + (segment[index - 1].end_vel - VT) * tj + 0.5f * (segment[index - 1].end_accel - AT) * sq(tj) + P2T;
add_segment(index, T, SegmentType::NEGATIVE_JERK, J, A, V, P);
}
// add single S-Curve segment
// populate the information for the segment specified in the path by the index variable.
// the index variable is incremented to reference the next segment in the array
void SCurve::add_segment(uint8_t &index, float end_time, SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos)
{
segment[index].end_time = end_time;
segment[index].seg_type = seg_type;
segment[index].jerk_ref = jerk_ref;
segment[index].end_accel = end_accel;
segment[index].end_vel = end_vel;
segment[index].end_pos = end_pos;
index++;
}
// set speed and acceleration limits for the path
// origin and destination are offsets from EKF origin
// speed and acceleration parameters are given in horizontal, up and down.
void SCurve::set_kinematic_limits(const Vector3f &origin, const Vector3f &destination,
float speed_xy, float speed_up, float speed_down,
float accel_xy, float accel_z)
{
// ensure arguments are positive
speed_xy = fabsf(speed_xy);
speed_up = fabsf(speed_up);
speed_down = fabsf(speed_down);
accel_xy = fabsf(accel_xy);
accel_z = fabsf(accel_z);
Vector3f direction = destination - origin;
const float track_speed_max = kinematic_limit(direction, speed_xy, speed_up, speed_down);
const float track_accel_max = kinematic_limit(direction, accel_xy, accel_z, accel_z);
vel_max = track_speed_max;
accel_max = track_accel_max;
}
// return true if the curve is valid. Used to identify and protect against code errors
bool SCurve::valid() const
{
// check number of segments
if (num_segs != segments_max) {
return false;
}
for (uint8_t i = 0; i < num_segs; i++) {
// jerk_ref should be finite (i.e. not NaN or infinity)
// time, accel, vel and pos should finite and not negative
if (!isfinite(segment[i].jerk_ref) ||
!isfinite(segment[i].end_time) ||
!isfinite(segment[i].end_accel) ||
!isfinite(segment[i].end_vel) || is_negative(segment[i].end_vel) ||
!isfinite(segment[i].end_pos)) {
return false;
}
// time and pos should be increasing
if (i >= 1) {
if (is_negative(segment[i].end_time - segment[i-1].end_time) ||
is_negative(segment[i].end_pos - segment[i-1].end_pos)) {
return false;
}
}
}
// last segment should have zero acceleration
if (!is_zero(segment[num_segs-1].end_accel)) {
return false;
}
// if we get this far then the curve must be valid
return true;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// debugging messages
void SCurve::debug() const
{
::printf("num_segs:%u, time:%4.2f, jerk_time:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n",
(unsigned)num_segs, (double)time, (double)jerk_time, (double)jerk_max, (double)accel_max, (double)vel_max);
::printf("T, Jt, J, A, V, P \n");
for (uint8_t i = 0; i < num_segs; i++) {
::printf("i:%u, T:%4.2f, Jtype:%4.2f, J:%4.2f, A:%4.2f, V: %4.2f, P: %4.2f\n",
(unsigned)i, (double)segment[i].end_time, (double)segment[i].seg_type, (double)segment[i].jerk_ref,
(double)segment[i].end_accel, (double)segment[i].end_vel, (double)segment[i].end_pos);
}
::printf("track x:%4.2f, y:%4.2f, z:%4.2f\n", (double)track.x, (double)track.y, (double)track.z);
::printf("delta_unit x:%4.2f, y:%4.2f, z:%4.2f\n", (double)delta_unit.x, (double)delta_unit.y, (double)delta_unit.z);
}
#endif
Reference in New Issue View Git Blame Copy Permalink