Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Control: Add XY S-Curve shapers and shaping limits

Browse Source
This commit is contained in:
Leonard Hall 2021-04-17 01:20:15 +09:30 committed by Andrew Tridgell
parent 6bed10c434
commit c4bb8baf0c
2 changed files with 459 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

436
libraries/AP_Math/control.cpp
View File

@ -22,19 +22,190 @@
#include "AP_Math.h"
#include "vector2.h"
#include "vector3.h"
#include <AP_InternalError/AP_InternalError.h>
// control default definitions
#define CONTROL_TIME_CONSTANT_RATIO 4.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
#define CONTROL_TIME_CONSTANT_RATIO 4.0f // time constant to ensure stable kinimatic path generation
// update_pos_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_pos_vel_accel(float& pos, float& vel, float accel, float dt)
// update_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_vel_accel(float& vel, float accel, float dt, float limit)
{
// move position and velocity forward by dt.
pos = pos + vel * dt + accel * 0.5f * sq(dt);
vel = vel + accel * dt;
const float delta_vel = accel * dt;
if (!is_positive(delta_vel * limit)){
vel += delta_vel;
}
}
/* shape_vel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
// update_vel_accel_z - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_vel_accel_z(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit)
{
update_vel_accel(vel.z, accel.z, dt, limit.z);
}
// update_pos_vel_accel - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_pos_vel_accel(float& pos, float& vel, float accel, float dt, float limit)
{
// move position and velocity forward by dt if it does not increase error when limited.
float delta_pos = vel * dt + accel * 0.5f * sq(dt);
if (!is_positive(delta_pos * limit)){
pos += delta_pos;
}
update_vel_accel(vel, accel, dt, limit);
}
// update_pos_vel_accel_z - single axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_pos_vel_accel_z(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit)
{
update_pos_vel_accel(pos.z, vel.z, accel.z, dt, limit.z);
}
// update_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_vel_accel(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit)
{
// increase velocity by acceleration * dt if it does not increase error when limited.
Vector2f delta_vel = accel * dt;
if (!is_zero(limit.length_squared())) {
// zero delta_vel if it will increase the velocity error
limit.normalize();
if (is_positive(delta_vel * limit)) {
delta_vel.zero();
}
}
vel += delta_vel;
}
// update_vel_accel_xy - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// This function only updates the x and y axis leaving the z axis unchanged.
void update_vel_accel_xy(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit)
{
Vector2f vel_2f = Vector2f{vel.x, vel.y};
Vector2f accel_2f = Vector2f{accel.x, accel.y};
Vector2f limit_2f = Vector2f{limit.x, limit.y};
update_vel_accel(vel_2f, accel_2f, dt, limit_2f);
vel.x = vel_2f.x;
vel.y = vel_2f.y;
}
// update_pos_vel_accel - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
void update_pos_vel_accel(Vector2f& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit)
{
// move position and velocity forward by dt.
Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt);
if (!is_zero(limit.length_squared())) {
// zero delta_vel if it will increase the velocity error
limit.normalize();
if (is_positive(delta_pos * limit)) {
delta_pos.zero();
}
}
pos += delta_pos;
update_vel_accel(vel, accel, dt, limit);
}
// update_pos_vel_accel_xy - dual axis projection of position and velocity, pos and vel, forwards in time based on a time step of dt and acceleration of accel.
// This function only updates the x and y axis leaving the z axis unchanged.
void update_pos_vel_accel_xy(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit)
{
Vector2f pos_2f = Vector2f{pos.x, pos.y};
Vector2f vel_2f = Vector2f{vel.x, vel.y};
Vector2f accel_2f = Vector2f{accel.x, accel.y};
Vector2f limit_2f = Vector2f{limit.x, limit.y};
update_pos_vel_accel(pos_2f, vel_2f, accel_2f, dt, limit_2f);
pos.x = pos_2f.x;
pos.y = pos_2f.y;
vel.x = vel_2f.x;
vel.y = vel_2f.y;
}
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
acceleration limits - accel_min, accel_max,
time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
*/
void shape_accel(float accel_input, float& accel,
float accel_min, float accel_max,
float tc, float dt)
{
// sanity check tc
if (!is_positive(tc)) {
return;
}
// Calculate time constants and limits to ensure stable operation
const float KPa = 1.0 / tc;
float jerk_max = 0.0;
if (is_negative(accel_min) && is_positive(accel_max)){
jerk_max = MAX(-accel_min, accel_max) * KPa;
}
// jerk limit acceleration change
float accel_delta = accel_input - accel;
if (is_positive(jerk_max)) {
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
}
accel += accel_delta;
// limit acceleration to accel_max
if (is_negative(accel_min) && is_positive(accel_max)){
accel = constrain_float(accel, accel_min, accel_max);
}
}
/* shape_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
float accel_max, float tc, float dt)
{
// sanity check tc
if (!is_positive(tc)) {
return;
}
// Calculate time constants and limits to ensure stable operation
float KPa = 1.0 / tc;
float jerk_max = accel_max * KPa;
// jerk limit acceleration change
Vector2f accel_delta = accel_input - accel;
if (is_positive(jerk_max)) {
accel_delta.limit_length(jerk_max * dt);
}
accel = accel + accel_delta;
// limit acceleration to accel_max
if (is_negative(accel_max)) {
// we may want to allow this for some applications but call error for now.
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
} else if (is_positive(accel_max)) {
accel.limit_length(accel_max);
}
}
/* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
maximum velocity - vel_max,
@ -46,7 +217,11 @@ void update_pos_vel_accel(float& pos, float& vel, float accel, float dt)
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
The accel_max limit can be removed by setting it to zero.
*/
void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float tc, float dt)
void shape_vel_accel(float vel_input, float accel_input,
float vel, float& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float tc, float dt)
{
// sanity check tc
if (!is_positive(tc)) {
@ -55,33 +230,92 @@ void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float
// Calculate time constants and limits to ensure stable operation
const float KPa = 1.0 / tc;
const float jerk_max = accel_max * KPa;
// limit velocity to vel_max
if (is_negative(vel_min) && is_positive(vel_max)){
vel_input = constrain_float(vel_input, vel_min, vel_max);
}
// velocity error to be corrected
float vel_error = vel_input - vel;
// acceleration to correct velocity
const float accel_target = vel_error * KPa;
float accel_target = vel_error * KPa;
// jerk limit acceleration change
float accel_delta = accel_target - accel;
if (is_positive(jerk_max)) {
accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
}
accel += accel_delta;
// velocity correction with input velocity
accel_target += accel_input;
// limit acceleration to accel_max
if (is_positive(accel_max)) {
accel = constrain_float(accel, -accel_max, accel_max);
}
// Calculate maximum pos_input and vel_input based on limited system
vel_error = accel / KPa;
vel_input = vel_error + vel;
shape_accel(accel_target, accel, accel_min, accel_max, tc, dt);
}
void shape_vel_accel_z(const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& vel, Vector3f& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float tc, float dt)
{
shape_vel_accel(vel_input.z, accel_input.z,
vel.z, accel.z,
vel_min, vel_max,
accel_min, accel_max,
tc, dt);
}
/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
/* shape_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_vel_accel_xy(Vector2f vel_input, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float tc, float dt)
{
// sanity check tc
if (!is_positive(tc)) {
return;
}
// limit velocity to vel_max
if (is_negative(vel_max)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
} else if (is_positive(vel_max)) {
vel_input.limit_length(vel_max);
}
// Calculate time constants and limits to ensure stable operation
float KPa = 1.0 / tc;
// velocity error to be corrected
Vector2f vel_error = vel_input - vel;
// acceleration to correct velocity
Vector2f accel_target = vel_error * KPa;
accel_target += accel_input;
shape_accel_xy(accel_target, accel, accel_max, tc, dt);
}
void shape_vel_accel_xy(const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& vel, Vector3f& accel, float vel_max, float accel_max, float tc, float dt)
{
Vector2f vel_input_2f = Vector2f{vel_input.x, vel_input.y};
Vector2f accel_input_2f = Vector2f{accel_input.x, accel_input.y};
Vector2f vel_2f = Vector2f{vel.x, vel.y};
Vector2f accel_2f = Vector2f{accel.x, accel.y};
shape_vel_accel_xy(vel_input_2f, accel_input_2f, vel_2f, accel_2f, vel_max, accel_max, tc, dt);
accel.x = accel_2f.x;
accel.y = accel_2f.y;
}
/* shape_pos_vel_accel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
maximum velocity - vel_max,
@ -93,7 +327,10 @@ void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/
void shape_pos_vel(float& pos_input, float vel_input, float pos, float vel, float& accel, float vel_max, float vel_correction_max, float accel_max, float tc, float dt)
void shape_pos_vel_accel(float pos_input, float vel_input, float accel_input,
float pos, float vel, float& accel,
float vel_correction_max, float vel_min, float vel_max,
float accel_min, float accel_max, float tc, float dt)
{
// sanity check tc
if (!is_positive(tc)) {
@ -118,16 +355,91 @@ void shape_pos_vel(float& pos_input, float vel_input, float pos, float vel, floa
// velocity correction with input velocity
vel_target += vel_input;
// limit velocity to vel_max
if (is_positive(vel_max)) {
vel_target = constrain_float(vel_target, -vel_max, vel_max);
shape_vel_accel(vel_target, accel_input, vel, accel, vel_min, vel_max, accel_min, accel_max, tc, dt);
}
void shape_pos_vel_accel_z(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
float vel_correction_max, float vel_min, float vel_max,
float accel_min, float accel_max, float tc, float dt)
{
shape_pos_vel_accel(pos_input.z, vel_input.z, accel_input.z,
pos.z, vel.z, accel.z,
vel_correction_max, vel_min, vel_max,
accel_min, accel_max,
tc, dt);
}
/* shape_pos_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/
void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
float vel_correction_max, float vel_max, float accel_max, float tc, float dt)
{
if (!is_positive(tc)) {
return;
}
shape_vel(vel_target, vel, accel, accel_max, tc, dt);
// Calculate time constants and limits to ensure stable operation
const float KPv = 1.0f / (CONTROL_TIME_CONSTANT_RATIO*tc);
const float accel_tc_max = accel_max*(1.0f - 1.0f/CONTROL_TIME_CONSTANT_RATIO);
vel_target -= vel_input;
pos_error = stopping_distance(vel_target, KPv, accel_tc_max);
pos_input = pos_error + pos;
// position error to be corrected
Vector2f pos_error = pos_input - pos;
// velocity to correct position
Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);
// limit velocity correction to vel_correction_max
if (is_positive(vel_correction_max)) {
vel_target.limit_length(vel_correction_max);
}
// velocity correction with input velocity
vel_target = vel_target + vel_input;
shape_vel_accel_xy(vel_target, accel_input, vel, accel, vel_max, accel_max, tc, dt);
}
/* shape_pos_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
This function operates only on the x and y axis of the Vector2f or Vector3f inputs.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/
void shape_pos_vel_accel_xy(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
float vel_max, float vel_correction_max, float accel_max, float tc, float dt)
{
Vector2f pos_input_2f = Vector2f{pos_input.x, pos_input.y};
Vector2f vel_input_2f = Vector2f{vel_input.x, vel_input.y};
Vector2f accel_input_2f = Vector2f{accel_input.x, accel_input.y};
Vector2f pos_2f = Vector2f{pos.x, pos.y};
Vector2f vel_2f = Vector2f{vel.x, vel.y};
Vector2f accel_2f = Vector2f{accel.x, accel.y};
shape_pos_vel_accel_xy(pos_input_2f, vel_input_2f, accel_input_2f,
pos_2f, vel_2f, accel_2f, vel_max, vel_correction_max, accel_max, tc, dt);
accel.x = accel_2f.x;
accel.y = accel_2f.y;
}
// proportional controller with piecewise sqrt sections to constrain second derivative
@ -180,59 +492,33 @@ Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, f
// inverse of the sqrt controller. calculates the input (aka error) to the sqrt_controller required to achieve a given output
float inv_sqrt_controller(float output, float p, float D_max)
{
if (!is_positive(D_max) && is_zero(p)) {
if (is_positive(D_max) && is_zero(p)) {
return (output * output) / (2.0f * D_max);
}
if ((is_negative(D_max) || is_zero(D_max)) && !is_zero(p)) {
return output / p;
}
if ((is_negative(D_max) || is_zero(D_max)) && is_zero(p)) {
return 0.0f;
}
if (!is_positive(D_max)) {
// second order limit is zero or negative.
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
const float linear_velocity = D_max / p;
if (fabsf(output) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
return output / p;
}
if (is_zero(p)) {
// P term is zero but we have a second order limit.
if (is_positive(D_max)) {
return sq(output)/(2*D_max);
}
return -sq(output)/(2*D_max);
}
// both the P and second order limit have been defined.
float linear_out = D_max / p;
if (output > linear_out) {
if (is_positive(D_max)) {
return sq(output)/(2*D_max) + D_max/(4*sq(p));
}
return -sq(output)/(2*D_max) + D_max/(4*sq(p));
}
return output / p;
const float linear_dist = D_max / sq(p);
const float stopping_dist = (linear_dist * 0.5f) + sq(output) / (2.0f * D_max);
return is_positive(output) ? stopping_dist : -stopping_dist;
}
// calculate the stopping distance for the square root controller based deceleration path
float stopping_distance(float velocity, float p, float accel_max)
{
if (is_positive(accel_max) && is_zero(p)) {
return (velocity * velocity) / (2.0f * accel_max);
}
if ((is_negative(accel_max) || is_zero(accel_max)) && !is_zero(p)) {
return velocity / p;
}
if ((is_negative(accel_max) || is_zero(accel_max)) && is_zero(p)) {
return 0.0f;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
const float linear_velocity = accel_max / p;
if (fabsf(velocity) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
return velocity / p;
}
const float linear_dist = accel_max / sq(p);
const float stopping_dist = (linear_dist * 0.5f) + sq(velocity) / (2.0f * accel_max);
return is_positive(velocity) ? stopping_dist : -stopping_dist;
return inv_sqrt_controller(velocity, p, accel_max);
}
// calculate the maximum acceleration or velocity in a given direction

107
libraries/AP_Math/control.h
View File

@ -4,22 +4,82 @@
common controller helper functions
*/
// update_pos_vel_accel projects the position and velocity, pos and vel, forward in time based on a time step of dt and acceleration of accel.
// update_pos_vel_accel - single axis projection.
void update_pos_vel_accel(float& pos, float& vel, float accel, float dt);
// update_vel_accel projects the velocity, vel, forward in time based on a time step of dt and acceleration of accel.
// update_vel_accel - single axis projection.
void update_vel_accel(float& vel, float accel, float dt, float limit);
void update_vel_accel_z(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
/* shape_vel calculates a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
// update_vel_accel projects the velocity, vel, forward in time based on a time step of dt and acceleration of accel.
// update_vel_accel - single axis projection.
void update_pos_vel_accel(float& pos, float& vel, float accel, float dt, float limit);
void update_pos_vel_accel_z(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
void update_vel_accel(Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
void update_vel_accel_xy(Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
// update_pos_vel_accel_xy - dual axis projection operating on the x, y axis of Vector2f or Vector3f inputs.
void update_pos_vel_accel(Vector2f& pos, Vector2f& vel, const Vector2f& accel, float dt, Vector2f limit);
void update_pos_vel_accel_xy(Vector3f& pos, Vector3f& vel, const Vector3f& accel, float dt, Vector3f limit);
/* shape_accel calculates a jerk limited path from the current acceleration to an input acceleration.
The function takes the current acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
maximum velocity - vel_max,
maximum acceleration - accel_max,
acceleration limits - accel_min, accel_max,
time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
*/
void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float tc, float dt);
void shape_accel(float accel_input, float& accel,
float accel_min, float accel_max,
float tc, float dt);
void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel,
float accel_max, float tc, float dt);
/* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity.
The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by :
velocity limits - vel_min, vel_max,
acceleration limits - accel_min, accel_max,
time constant - tc.
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
*/
void shape_vel_accel(float vel_input, float accel_input,
float vel, float& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float tc, float dt);
void shape_vel_accel_z(const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& vel, Vector3f& accel,
float vel_min, float vel_max,
float accel_min, float accel_max,
float tc, float dt);
/* shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
This function operates on the x and y axis of both Vector2f or Vector3f inputs.
The accel_max limit can be removed by setting it to zero.
*/
void shape_vel_accel_xy(Vector2f vel_input, const Vector2f& accel_input,
const Vector2f& vel, Vector2f& accel, float vel_max, float accel_max, float tc, float dt);
void shape_vel_accel_xy(const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& vel, Vector3f& accel, float vel_max, float accel_max, float tc, float dt);
/* shape_pos_vel calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@ -32,7 +92,36 @@ void shape_vel(float& vel_input, float vel, float& accel, float accel_max, float
The time constant must be positive.
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
*/
void shape_pos_vel(float& pos_input, float vel_input, float pos, float vel, float& accel, float vel_max, float vel_correction_max, float accel_max, float tc, float dt);
void shape_pos_vel_accel(float pos_input, float vel_input, float accel_input,
float pos, float vel, float& accel,
float vel_correction_max, float vel_min, float vel_max,
float accel_min, float accel_max, float tc, float dt);
void shape_pos_vel_accel_z(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
float vel_correction_max, float vel_min, float vel_max,
float accel_min, float accel_max, float tc, float dt);
/* shape_pos_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input position and velocity.
The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
The kinematic path is constrained by:
vel_max : maximum velocity
accel_max : maximum acceleration
tc : time constant
The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
The time constant also defines the time taken to achieve the maximum acceleration.
The time constant must be positive.
The function alters the input position to be the closest position that the system could reach zero acceleration in the minimum time.
This function operates only on the x and y axis of the Vector2f or Vector3f inputs.
The vel_max, vel_correction_max, and accel_max limits can be removed by setting the desired limit to zero.
*/
void shape_pos_vel_accel_xy(const Vector2f& pos_input, const Vector2f& vel_input, const Vector2f& accel_input,
const Vector2f& pos, const Vector2f& vel, Vector2f& accel,
float vel_correction_max, float vel_max, float accel_max, float tc, float dt);
void shape_pos_vel_accel_xy(const Vector3f& pos_input, const Vector3f& vel_input, const Vector3f& accel_input,
const Vector3f& pos, const Vector3f& vel, Vector3f& accel,
float vel_max, float vel_correction_max, float accel_max, float tc, float dt);
// proportional controller with piecewise sqrt sections to constrain second derivative
float sqrt_controller(float error, float p, float second_ord_lim, float dt);