Skip to content

Commit

Permalink
fix interval selection
Browse files Browse the repository at this point in the history
  • Loading branch information
@pantor
pantor committed Jan 25, 2021
1 parent 5509f7e commit b940382
Show file tree
Hide file tree
Showing 15 changed files with 11,907 additions and 3,652 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@ Moreover, a range of additional parameter about the duration of the trajectory a

## Tests and Numerical Stability

The current test suite validates over 1.206.000 (random) trajectories. The numerical exactness is tested for the final position and final velocity to be within `1e-8`, for the velocity and acceleration limit to be withing `1e-9`, and for the final acceleration as well as jerk limit to be within a numerical error of `1e-12`. Ruckig presumes that the input values are within 5 orders of magnitude and that `jMax > 5e-2`. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness. The maximal supported trajectory duration is `1e12`.
The current test suite validates over 1.206.000 (random) trajectories. The numerical exactness is tested for the final position and final velocity to be within `1e-8`, for the velocity, acceleration and jerk limit to be withing `1e-12`, and for the final acceleration as well to be within a numerical error of `1e-12`. The maximal supported trajectory duration is `7e3`. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness.


## Development
Expand Down
18 changes: 0 additions & 18 deletions include/ruckig/parameter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,24 +68,6 @@ class InputParameter {
);
}

void scale(double factor) {
for (size_t dof = 0; dof < DOFs; ++dof) {
current_position[dof] *= factor;
current_velocity[dof] *= factor;
current_acceleration[dof] *= factor;
target_position[dof] *= factor;
target_velocity[dof] *= factor;
target_acceleration[dof] *= factor;
max_velocity[dof] *= factor;
max_acceleration[dof] *= factor;
max_jerk[dof] *= factor;

if (min_velocity) {
min_velocity.value()[dof] *= factor;
}
}
}

std::string to_string() const {
std::stringstream ss;
ss << "\ninp.current_position = [" << join(current_position) << "]\n";
Expand Down
28 changes: 19 additions & 9 deletions include/ruckig/profile.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ struct Profile {
//! Allow up to two segments of braking before the "correct" profile starts
std::array<double, 2> t_brakes, j_brakes, a_brakes, v_brakes, p_brakes;

template<Teeth teeth, Limits limits>
template<Teeth teeth, Limits limits, bool add_t2_t4 = false>
bool check(double pf, double vf, double af, double jf, double vMax, double aMax) {
if constexpr (teeth == Teeth::UDDU) {
j = {jf, 0, -jf, 0, -jf, 0, jf};
Expand All @@ -42,21 +42,33 @@ struct Profile {
if (t[i+1] < 0) {
return false;
}

t_sum[i+1] = t_sum[i] + t[i+1];
}

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

for (size_t i = 0; i < 7; i += 1) {
a[i+1] = a[i] + t[i] * j[i];
v[i+1] = v[i] + t[i] * (a[i] + t[i] * j[i] / 2);
p[i+1] = p[i] + t[i] * (v[i] + t[i] * (a[i] / 2 + t[i] * j[i] / 6));

if (i > 1 && a[i+1] * a[i] < -1e-15) {
const double v_a_zero = v[i] - (a[i] * a[i]) / (2 * j[i]);
if (std::abs(v_a_zero) > vMaxAbs) {
return false;
}
}

if constexpr (limits == Limits::ACC0_ACC1_VEL || limits == Limits::ACC0_VEL || limits == Limits::ACC1_VEL || limits == Limits::VEL) {
if (i == 2) {
a[i+1] = 0.0;
a[3] = 0.0;
}
}
}

if (t_sum[6] > 1e12) { // For numerical reasons, equal to around 32000 years in SI units...
if (t_sum[6] > 1e12) { // For numerical reasons
return false;
}

Expand All @@ -65,8 +77,6 @@ struct Profile {

// 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;
const double vMaxAbs = std::abs(vMax) + 1e-9;
const double aMaxAbs = std::abs(aMax) + 1e-9;
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
Expand All @@ -92,11 +102,11 @@ struct Profile {
}

//! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration.
static std::tuple<double, double, double> integrate(double t, double p0, double v0, double a0, double j, double scale = 1.0) {
static std::tuple<double, double, double> integrate(double t, double p0, double v0, double a0, double j) {
return {
scale * (p0 + t * (v0 + t * (a0 / 2 + t * j / 6))),
scale * (v0 + t * (a0 + t * j / 2)),
scale * (a0 + t * j),
p0 + t * (v0 + t * (a0 / 2 + t * j / 6)),
v0 + t * (a0 + t * j / 2),
a0 + t * j,
};
}

Expand Down
41 changes: 13 additions & 28 deletions include/ruckig/ruckig.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,6 @@ class Ruckig {
//! Current input, only for comparison for recalculation
InputParameter<DOFs> current_input;

//! Normalized input for calculating the trajectory
InputParameter<DOFs> normalized_input;

//! Scale that normalizes the input
double scale;

//! Current time in [s]
double t;

Expand All @@ -41,19 +35,6 @@ class Ruckig {
return (std::abs(a) < std::abs(b));
}

void normalize_input(const InputParameter<DOFs>& input) {
const auto [vMax_min, vMax_max] = std::minmax_element(input.max_velocity.cbegin(), input.max_velocity.cend(), abs_compare);
const auto [aMax_min, aMax_max] = std::minmax_element(input.max_acceleration.cbegin(), input.max_acceleration.cend(), abs_compare);
const auto [jMax_min, jMax_max] = std::minmax_element(input.max_jerk.cbegin(), input.max_jerk.cend(), abs_compare);

const double min_value = std::min({*vMax_min, *aMax_min, *jMax_min});
const double max_value = std::max({*vMax_max, *aMax_max, *jMax_max});
scale = 1.0; // / min_value;

normalized_input = input;
// normalized_input.scale(scale);
}

bool synchronize(const std::array<Block, DOFs>& blocks, std::optional<double> t_min, double& t_sync, size_t& limiting_dof, std::array<Profile, DOFs>& profiles) {
if (DOFs == 1 && !t_min) {
limiting_dof = 0;
Expand Down Expand Up @@ -108,8 +89,7 @@ class Ruckig {
return Result::ErrorInvalidInput;
}

normalize_input(input);
InputParameter<DOFs>& inp = normalized_input;
InputParameter<DOFs>& inp = current_input;

std::array<Block, DOFs> blocks;
std::array<double, DOFs> p0s, v0s, a0s; // Starting point of profiles without brake trajectory
Expand Down Expand Up @@ -239,7 +219,12 @@ class Ruckig {
return false;
}

double max_target_acceleration = std::sqrt(2 * input.max_jerk[dof] * (input.max_velocity[dof] - std::abs(input.target_velocity[dof])));
double max_target_acceleration;
if (input.min_velocity && input.target_velocity[dof] < 0) {
max_target_acceleration = std::sqrt(-2 * input.max_jerk[dof] * (input.min_velocity.value()[dof] - input.target_velocity[dof]));
} else {
max_target_acceleration = std::sqrt(2 * input.max_jerk[dof] * (input.max_velocity[dof] - std::abs(input.target_velocity[dof])));
}
if (std::abs(input.target_acceleration[dof]) > max_target_acceleration) {
return false;
}
Expand Down Expand Up @@ -277,15 +262,15 @@ class Ruckig {
if (time + delta_time > tf) {
// Keep constant acceleration
for (size_t dof = 0; dof < DOFs; ++dof) {
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(time - tf, normalized_input.target_position[dof], normalized_input.target_velocity[dof], normalized_input.target_acceleration[dof], 0, 1./scale);
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(time - tf, current_input.target_position[dof], current_input.target_velocity[dof], current_input.target_acceleration[dof], 0);
}
return;
}

for (size_t dof = 0; dof < DOFs; ++dof) {
if (!normalized_input.enabled[dof]) {
if (!current_input.enabled[dof]) {
// Keep constant acceleration
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(time, normalized_input.current_position[dof], normalized_input.current_velocity[dof], normalized_input.current_acceleration[dof], 0, 1./scale);
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(time, current_input.current_position[dof], current_input.current_velocity[dof], current_input.current_acceleration[dof], 0);
}

const auto& p = profiles[dof];
Expand All @@ -298,7 +283,7 @@ class Ruckig {
t_diff -= p.t_brakes[index - 1];
}

std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff, p.p_brakes[index], p.v_brakes[index], p.a_brakes[index], p.j_brakes[index], 1./scale);
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff, p.p_brakes[index], p.v_brakes[index], p.a_brakes[index], p.j_brakes[index]);
continue;
} else {
t_diff -= p.t_brake.value();
Expand All @@ -308,7 +293,7 @@ class Ruckig {
// Non-time synchronization
if (t_diff >= p.t_sum[6]) {
// Keep constant acceleration
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff - p.t_sum[6], normalized_input.target_position[dof], normalized_input.target_velocity[dof], normalized_input.target_acceleration[dof], 0, 1./scale);
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff - p.t_sum[6], current_input.target_position[dof], current_input.target_velocity[dof], current_input.target_acceleration[dof], 0);
continue;
}

Expand All @@ -319,7 +304,7 @@ class Ruckig {
t_diff -= p.t_sum[index - 1];
}

std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff, p.p[index], p.v[index], p.a[index], p.j[index], 1./scale);
std::tie(output.new_position[dof], output.new_velocity[dof], output.new_acceleration[dof]) = Profile::integrate(t_diff, p.p[index], p.v[index], p.a[index], p.j[index]);
}
}

Expand Down
10 changes: 10 additions & 0 deletions include/ruckig/steps.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ class Step1 {
double aMax_aMax;
double jMax_jMax;

bool has_up_vel {false}, has_down_vel {false};

// Max 6 valid profiles
std::array<Profile, 6> valid_profiles;
size_t valid_profile_counter;
Expand All @@ -84,6 +86,14 @@ class Step1 {

inline void add_profile(Profile profile, double jMax) {
profile.direction = (jMax > 0) ? Profile::Direction::UP : Profile::Direction::DOWN;

if (profile.limits == Limits::ACC0_ACC1_VEL || profile.limits == Limits::ACC0_VEL || profile.limits == Limits::ACC1_VEL || profile.limits == Limits::VEL) {
switch (profile.direction) {
case Profile::Direction::UP: has_up_vel = true; break;
case Profile::Direction::DOWN: has_down_vel = true; break;
}
}

valid_profiles[valid_profile_counter] = profile;
++valid_profile_counter;
}
Expand Down
Loading

0 comments on commit b940382

Please sign in to comment.