Skip to content

Commit

Permalink
add min_acceleration
Browse files Browse the repository at this point in the history
  • Loading branch information
@pantor
pantor committed Feb 9, 2021
1 parent 0d4c539 commit d0ad9f8
Show file tree
Hide file tree
Showing 12 changed files with 672 additions and 736 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ add_library(ruckig SHARED
target_compile_features(ruckig PUBLIC cxx_std_17)
target_include_directories(ruckig PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(ruckig PUBLIC)
target_compile_options(ruckig PRIVATE -Werror -Wall -Wextra)
# target_compile_options(ruckig PRIVATE -Werror -Wall -Wextra)


if(Reflexxes)
Expand Down
4 changes: 3 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,9 +106,11 @@ Vector max_velocity;
Vector max_acceleration;
Vector max_jerk;

std::optional<Vector> min_velocity; // If not given, the negative maximum velocity will be used.
std::optional<Vector> min_acceleration; // If not given, the negative maximum acceleration will be used.

std::array<bool, DOFs> enabled; // Initialized to true
std::optional<double> minimum_duration;
std::optional<Vector> min_velocity; // If not given, the negative maximum velocity will be used.
```

The members are implemented using the C++ standard libraries `array` and `optional` type. Note that there are some range constraints due to numerical reasons, you can read below for more details. To check the input in front of a calculation step, the `ruckig.validate_input(input)` method returns `false` if an input is not valid. Of course, the target state needs to be within the given dynamic limits. Additionally, the target acceleration needs to fulfill
Expand Down
7 changes: 6 additions & 1 deletion include/ruckig/parameter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,11 @@ class InputParameter {
Vector current_position, current_velocity {}, current_acceleration {};
Vector target_position, target_velocity {}, target_acceleration {};
Vector max_velocity, max_acceleration, max_jerk;
std::optional<Vector> min_velocity;
std::optional<Vector> min_acceleration;

std::array<bool, DOFs> enabled;
std::optional<double> minimum_duration;
std::optional<Vector> min_velocity;

InputParameter() {
std::fill(enabled.begin(), enabled.end(), true);
Expand All @@ -70,6 +71,7 @@ class InputParameter {
|| enabled != rhs.enabled
|| minimum_duration != rhs.minimum_duration
|| min_velocity != rhs.min_velocity
|| min_acceleration != rhs.min_acceleration
|| type != rhs.type
);
}
Expand All @@ -88,6 +90,9 @@ class InputParameter {
if (min_velocity) {
ss << "inp.min_velocity = [" << join(min_velocity.value()) << "]\n";
}
if (min_acceleration) {
ss << "inp.min_acceleration = [" << join(min_acceleration.value()) << "]\n";
}
return ss.str();
}
};
Expand Down
9 changes: 5 additions & 4 deletions include/ruckig/ruckig.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class Ruckig {

std::array<Block, DOFs> blocks;
std::array<double, DOFs> p0s, v0s, a0s; // Starting point of profiles without brake trajectory
std::array<double, DOFs> inp_min_velocity;
std::array<double, DOFs> inp_min_velocity, inp_min_acceleration;
for (size_t dof = 0; dof < DOFs; ++dof) {
Profile& p = trajectory.profiles[dof];

Expand All @@ -102,9 +102,10 @@ class Ruckig {
}

inp_min_velocity[dof] = inp.min_velocity ? inp.min_velocity.value()[dof] : -inp.max_velocity[dof];
inp_min_acceleration[dof] = inp.min_acceleration ? inp.min_acceleration.value()[dof] : -inp.max_acceleration[dof];

// Calculate brake (if input exceeds or will exceed limits)
Brake::get_brake_trajectory(inp.current_velocity[dof], inp.current_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof], p.t_brakes, p.j_brakes);
Brake::get_brake_trajectory(inp.current_velocity[dof], inp.current_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof], p.t_brakes, p.j_brakes);

p.t_brake = p.t_brakes[0] + p.t_brakes[1];
p0s[dof] = inp.current_position[dof];
Expand All @@ -119,7 +120,7 @@ class Ruckig {
std::tie(p0s[dof], v0s[dof], a0s[dof]) = Profile::integrate(p.t_brakes[i], p0s[dof], v0s[dof], a0s[dof], p.j_brakes[i]);
}

Step1 step1 {p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof]};
Step1 step1 {p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]};
bool found_profile = step1.get_profile(p, blocks[dof]);
if (!found_profile) {
if constexpr (throw_error) {
Expand Down Expand Up @@ -155,7 +156,7 @@ class Ruckig {
Profile& p = trajectory.profiles[dof];
const double t_profile = trajectory.duration - p.t_brake.value_or(0.0);

Step2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp.max_jerk[dof]};
Step2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]};
bool found_time_synchronization = step2.get_profile(p);
if (!found_time_synchronization) {
if constexpr (throw_error) {
Expand Down
77 changes: 34 additions & 43 deletions include/ruckig/steps.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,11 @@ struct Block {
class Brake {
static constexpr double eps {2e-14};

static void acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
static void velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
static void acceleration_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
static void velocity_brake(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);

public:
static void get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
static void get_brake_trajectory(double v0, double a0, double vMax, double vMin, double aMax, double aMin, double jMax, std::array<double, 2>& t_brake, std::array<double, 2>& j_brake);
};


Expand All @@ -52,14 +52,13 @@ class Step1 {

double p0, v0, a0;
double pf, vf, af;
double vMax, vMin, aMax, jMax;
double vMax, vMin, aMax, aMin, jMax;

// Pre-calculated expressions
double pd;
double v0_v0, vf_vf;
double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6;
double af_af, af_p3, af_p4, af_p5, af_p6;
double aMax_aMax;
double jMax_jMax;

// Only a single velocity-limited profile can be valid
Expand All @@ -69,23 +68,14 @@ class Step1 {
std::array<Profile, 5> valid_profiles;
size_t valid_profile_counter;

void time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
void time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
void time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax);
void time_up_vel(Profile& profile, double vMax, double aMax, double jMax);
void time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax);
void time_up_acc1(Profile& profile, double vMax, double aMax, double jMax);
void time_up_acc0(Profile& profile, double vMax, double aMax, double jMax);
void time_up_none(Profile& profile, double vMax, double aMax, double jMax);

inline void time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_vel(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_acc1(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_acc0(Profile& profile, double vMin, double aMax, double jMax);
inline void time_down_none(Profile& profile, double vMin, double aMax, double jMax);
void time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax);
void time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax);

inline void add_profile(Profile profile, double jMax) {
profile.direction = (jMax > 0) ? Profile::Direction::UP : Profile::Direction::DOWN;
Expand Down Expand Up @@ -114,7 +104,7 @@ class Step1 {
bool calculate_block(Block& block) const;

public:
explicit Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax);
explicit Step1(double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax);

bool get_profile(const Profile& input, Block& block);
};
Expand All @@ -128,7 +118,7 @@ class Step2 {
double tf;
double p0, v0, a0;
double pf, vf, af;
double vMax, vMin, aMax, jMax;
double vMax, vMin, aMax, aMin, jMax;

// Pre-calculated expressions
double pd;
Expand All @@ -138,30 +128,31 @@ class Step2 {
double v0_v0, vf_vf;
double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6;
double af_af, af_p3, af_p4, af_p5, af_p6;
double aMax_aMax;
double jMax_jMax;
double g1, g2;

bool time_up_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_acc1_vel(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_acc0_vel(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_vel(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_acc0_acc1(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_acc1(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_acc0(Profile& profile, double vMax, double aMax, double jMax);
bool time_up_none(Profile& profile, double vMax, double aMax, double jMax);

inline bool time_down_acc0_acc1_vel(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_acc1_vel(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_acc0_vel(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_vel(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_acc0_acc1(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_acc1(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_acc0(Profile& profile, double vMin, double aMax, double jMax);
inline bool time_down_none(Profile& profile, double vMin, double aMax, double jMax);
bool time_acc0_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_acc1_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_acc0_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_vel(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_acc0_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_acc1(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_acc0(Profile& profile, double vMax, double aMax, double aMin, double jMax);
bool time_none(Profile& profile, double vMax, double aMax, double aMin, double jMax);

inline bool check_all(Profile& profile, double vMax, double aMax, double aMin, double jMax) {
return time_acc0_acc1_vel(profile, vMax, aMax, aMin, jMax)
|| time_acc0_vel(profile, vMax, aMax, aMin, jMax)
|| time_acc1_vel(profile, vMax, aMax, aMin, jMax)
|| time_vel(profile, vMax, aMax, aMin, jMax)
|| time_acc0(profile, vMax, aMax, aMin, jMax)
|| time_acc1(profile, vMax, aMax, aMin, jMax)
|| time_acc0_acc1(profile, vMax, aMax, aMin, jMax)
|| time_none(profile, vMax, aMax, aMin, jMax);
}

public:
explicit Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double jMax);
explicit Step2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax);

bool get_profile(Profile& profile);
};
Expand Down
24 changes: 11 additions & 13 deletions include/ruckig/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ struct Profile {
std::array<double, 2> t_brakes, j_brakes, a_brakes, v_brakes, p_brakes;

template<JerkSigns jerk_signs, Limits limits>
bool check(double pf, double vf, double af, double jf, double vMax, double aMax) {
bool check(double pf, double vf, double af, double jf, double vMax, double aMax, double aMin) {
if (t[0] < 0) {
return false;
}
Expand Down Expand Up @@ -81,7 +81,6 @@ struct Profile {
}

const double vMaxAbs = std::abs(vMax) + 1e-12;
const double aMaxAbs = std::abs(aMax) + 1e-12;

for (size_t i = 0; i < 7; ++i) {
a[i+1] = a[i] + t[i] * j[i];
Expand All @@ -105,29 +104,28 @@ struct Profile {
this->jerk_signs = jerk_signs;
this->limits = limits;

const double aUppLim = ((aMax > 0) ? aMax : aMin) + 1e-12;
const double aLowLim = ((aMax > 0) ? aMin : aMax) - 1e-12;

// Velocity limit can be broken in the beginning if both initial velocity and acceleration are too high
// std::cout << std::setprecision(15) << "target: " << std::abs(p[7]-pf) << " " << std::abs(v[7] - vf) << " " << std::abs(a[7] - af) << " T: " << t_sum[6] << " " << to_string() << std::endl;
return std::abs(p[7] - pf) < 1e-8
&& std::abs(v[7] - vf) < 1e-8
&& std::abs(a[7] - af) < 1e-12 // This is not really needed, but we want to double check
&& std::abs(v[3]) < vMaxAbs
&& std::abs(v[4]) < vMaxAbs
&& std::abs(v[5]) < vMaxAbs
&& std::abs(v[6]) < vMaxAbs
&& std::abs(a[1]) < aMaxAbs
&& std::abs(a[3]) < aMaxAbs
&& std::abs(a[5]) < aMaxAbs;
&& std::abs(v[3]) < vMaxAbs && std::abs(v[4]) < vMaxAbs && std::abs(v[5]) < vMaxAbs && std::abs(v[6]) < vMaxAbs
&& a[1] > aLowLim && a[3] > aLowLim && a[5] > aLowLim
&& a[1] < aUppLim && a[3] < aUppLim && a[5] < aUppLim;
}

template<JerkSigns jerk_signs, Limits limits>
inline bool check([[maybe_unused]] double tf, double pf, double vf, double af, double jf, double vMax, double aMax) {
inline bool check([[maybe_unused]] double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double aMin) {
// Time doesn't need to be checked as every profile has a: tf - ... equation
return check<jerk_signs, limits>(pf, vf, af, jf, vMax, aMax); // && (std::abs(t_sum[6] - tf) < 1e-8);
return check<jerk_signs, limits>(pf, vf, af, jf, vMax, aMax, aMin); // && (std::abs(t_sum[6] - tf) < 1e-8);
}

template<JerkSigns jerk_signs, Limits limits>
inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double jMax) {
return (std::abs(jf) < std::abs(jMax) + 1e-12) && check<jerk_signs, limits>(tf, pf, vf, af, jf, vMax, aMax);
inline bool check(double tf, double pf, double vf, double af, double jf, double vMax, double aMax, double aMin, double jMax) {
return (std::abs(jf) < std::abs(jMax) + 1e-12) && check<jerk_signs, limits>(tf, pf, vf, af, jf, vMax, aMax, aMin);
}

//! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration.
Expand Down
Loading

0 comments on commit d0ad9f8

Please sign in to comment.