Skip to content

Commit 005fdcd

Browse files
pantorpantor
committed
extend phase synchronization to velocity control
1 parent 73907c8 commit 005fdcd

File tree

5 files changed

+207
-86
lines changed

5 files changed

+207
-86
lines changed

‎include/ruckig/calculator_target.hpp

Lines changed: 79 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,7 @@ class TargetCalculator {
2828

2929
constexpr static double eps {std::numeric_limits<double>::epsilon()};
3030

31-
Vector<double> new_max_jerk; // For phase synchronization
32-
Vector<double> pd;
31+
Vector<double> new_max_jerk, pd; // For phase synchronization
3332
VectorIntervals<double> possible_t_syncs;
3433
VectorIntervals<size_t> idx;
3534

@@ -41,49 +40,76 @@ class TargetCalculator {
4140

4241
//! Is the trajectory (in principle) phase synchronizable?
4342
bool is_input_collinear(const InputParameter<DOFs>& inp, const Vector<double>& jMax, Profile::Direction limiting_direction, size_t limiting_dof) {
44-
// Get scaling factor of first DoF
45-
bool pd_found_nonzero {false};
46-
double v0_scale, a0_scale, vf_scale, af_scale;
47-
for (size_t dof = 0; dof < pd.size(); ++dof) {
43+
// Check that vectors pd, v0, a0, vf, af are collinear
44+
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
45+
pd[dof] = inp.target_position[dof] - inp.current_position[dof];
46+
}
47+
48+
const Vector<double>* scale_vector = nullptr;
49+
std::optional<size_t> scale_dof; // Need to find a scale DOF because limiting DOF might not be phase synchronized
50+
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
4851
if (inp_per_dof_synchronization[dof] != Synchronization::Phase) {
4952
continue;
5053
}
5154

52-
pd[dof] = inp.target_position[dof] - inp.current_position[dof];
53-
54-
if (!pd_found_nonzero && std::abs(pd[dof]) > eps) {
55-
v0_scale = inp.current_velocity[dof] / pd[dof];
56-
a0_scale = inp.current_acceleration[dof] / pd[dof];
57-
vf_scale = inp.target_velocity[dof] / pd[dof];
58-
af_scale = inp.target_acceleration[dof] / pd[dof];
59-
pd_found_nonzero = true;
55+
if (inp_per_dof_control_interface[dof] == ControlInterface::Position && std::abs(pd[dof]) > eps) {
56+
scale_vector = &pd;
57+
scale_dof = dof;
58+
break;
59+
60+
} else if (std::abs(inp.current_velocity[dof]) > eps) {
61+
scale_vector = &inp.current_velocity;
62+
scale_dof = dof;
63+
break;
64+
65+
} else if (std::abs(inp.current_acceleration[dof]) > eps) {
66+
scale_vector = &inp.current_acceleration;
67+
scale_dof = dof;
68+
break;
69+
70+
} else if (std::abs(inp.target_velocity[dof]) > eps) {
71+
scale_vector = &inp.target_velocity;
72+
scale_dof = dof;
73+
break;
74+
75+
} else if (std::abs(inp.target_acceleration[dof]) > eps) {
76+
scale_vector = &inp.target_acceleration;
77+
scale_dof = dof;
78+
break;
6079
}
6180
}
6281

63-
if (!pd_found_nonzero) { // position difference is zero everywhere...
64-
return false;
82+
if (!scale_dof) {
83+
return false; // Zero everywhere is in theory collinear, but that trivial case is better handled elsewhere
6584
}
6685

86+
const double scale = scale_vector->operator[](*scale_dof);
87+
const double pd_scale = pd[*scale_dof] / scale;
88+
const double v0_scale = inp.current_velocity[*scale_dof] / scale;
89+
const double vf_scale = inp.target_velocity[*scale_dof] / scale;
90+
const double a0_scale = inp.current_acceleration[*scale_dof] / scale;
91+
const double af_scale = inp.target_acceleration[*scale_dof] / scale;
92+
93+
const double scale_limiting = scale_vector->operator[](limiting_dof);
6794
const double max_jerk_limiting = (limiting_direction == Profile::Direction::UP) ? jMax[limiting_dof] : -jMax[limiting_dof];
68-
constexpr double eps_colinear {10 * eps};
6995

70-
for (size_t dof = 0; dof < pd.size(); ++dof) {
71-
if (dof == limiting_dof || inp_per_dof_synchronization[dof] != Synchronization::Phase) {
96+
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
97+
if (inp_per_dof_synchronization[dof] != Synchronization::Phase) {
7298
continue;
7399
}
74100

75-
// Are the vectors collinear?
101+
const double current_scale = scale_vector->operator[](dof);
76102
if (
77-
std::abs(inp.current_velocity[dof] - v0_scale * pd[dof]) > eps_colinear
78-
|| std::abs(inp.current_acceleration[dof] - a0_scale * pd[dof]) > eps_colinear
79-
|| std::abs(inp.target_velocity[dof] - vf_scale * pd[dof]) > eps_colinear
80-
|| std::abs(inp.target_acceleration[dof] - af_scale * pd[dof]) > eps_colinear
103+
(inp_per_dof_control_interface[dof] == ControlInterface::Position && std::abs(pd[dof] - pd_scale * current_scale) > eps)
104+
|| std::abs(inp.current_velocity[dof] - v0_scale * current_scale) > eps
105+
|| std::abs(inp.current_acceleration[dof] - a0_scale * current_scale) > eps
106+
|| std::abs(inp.target_velocity[dof] - vf_scale * current_scale) > eps
107+
|| std::abs(inp.target_acceleration[dof] - af_scale * current_scale) > eps
81108
) {
82109
return false;
83110
}
84111

85-
const double scale = pd[dof] / pd[limiting_dof];
86-
new_max_jerk[dof] = scale * max_jerk_limiting;
112+
new_max_jerk[dof] = max_jerk_limiting * current_scale / scale_limiting;
87113
}
88114

89115
return true;
@@ -232,7 +258,7 @@ class TargetCalculator {
232258
case ControlInterface::Velocity: {
233259
p.brake.get_velocity_brake_trajectory(inp.current_acceleration[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]);
234260
// p.accel.get_velocity_brake_trajectory(inp.target_acceleration[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]);
235-
p.set_boundary(inp.current_position[dof], inp.current_velocity[dof], inp.current_acceleration[dof], inp.target_velocity[dof], inp.target_acceleration[dof]);
261+
p.set_boundary_for_velocity(inp.current_position[dof], inp.current_velocity[dof], inp.current_acceleration[dof], inp.target_velocity[dof], inp.target_acceleration[dof]);
236262
} break;
237263
}
238264

@@ -304,7 +330,7 @@ class TargetCalculator {
304330
}
305331

306332
// Phase Synchronization
307-
if (!discrete_duration && limiting_dof && std::any_of(inp_per_dof_synchronization.begin(), inp_per_dof_synchronization.end(), [](Synchronization s){ return s == Synchronization::Phase; }) && std::all_of(inp_per_dof_control_interface.begin(), inp_per_dof_control_interface.end(), [](ControlInterface s){ return s == ControlInterface::Position; })) {
333+
if (limiting_dof && std::any_of(inp_per_dof_synchronization.begin(), inp_per_dof_synchronization.end(), [](Synchronization s){ return s == Synchronization::Phase; })) {
308334
const Profile& p_limiting = traj.profiles[0][limiting_dof.value()];
309335
if (is_input_collinear(inp, inp.max_jerk, p_limiting.direction, limiting_dof.value())) {
310336
bool found_time_synchronization {true};
@@ -320,15 +346,33 @@ class TargetCalculator {
320346
p.jerk_signs = p_limiting.jerk_signs;
321347

322348
// Profile::Limits::NONE is a small hack, as there is no specialization for that in the check function
323-
switch (p.jerk_signs) {
324-
case Profile::JerkSigns::UDDU: {
325-
if (!p.check_with_timing<Profile::JerkSigns::UDDU, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
326-
found_time_synchronization = false;
349+
switch (inp_per_dof_control_interface[dof]) {
350+
case ControlInterface::Position: {
351+
switch (p.jerk_signs) {
352+
case Profile::JerkSigns::UDDU: {
353+
if (!p.check_with_timing<Profile::JerkSigns::UDDU, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
354+
found_time_synchronization = false;
355+
}
356+
} break;
357+
case Profile::JerkSigns::UDUD: {
358+
if (!p.check_with_timing<Profile::JerkSigns::UDUD, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
359+
found_time_synchronization = false;
360+
}
361+
} break;
327362
}
328363
} break;
329-
case Profile::JerkSigns::UDUD: {
330-
if (!p.check_with_timing<Profile::JerkSigns::UDUD, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
331-
found_time_synchronization = false;
364+
case ControlInterface::Velocity: {
365+
switch (p.jerk_signs) {
366+
case Profile::JerkSigns::UDDU: {
367+
if (!p.check_for_velocity_with_timing<Profile::JerkSigns::UDDU, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
368+
found_time_synchronization = false;
369+
}
370+
} break;
371+
case Profile::JerkSigns::UDUD: {
372+
if (!p.check_for_velocity_with_timing<Profile::JerkSigns::UDUD, Profile::Limits::NONE>(t_profile, new_max_jerk[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof])) {
373+
found_time_synchronization = false;
374+
}
375+
} break;
332376
}
333377
} break;
334378
}

‎include/ruckig/profile.hpp

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,19 @@ class Profile {
110110
return check_for_velocity<jerk_signs, limits>(jf, aMax, aMin); // && (std::abs(t_sum.back() - tf) < t_precision);
111111
}
112112

113+
template<JerkSigns jerk_signs, Limits limits>
114+
inline bool check_for_velocity_with_timing(double tf, double jf, double aMax, double aMin, double jMax) {
115+
return (std::abs(jf) < std::abs(jMax) + j_eps) && check_for_velocity_with_timing<jerk_signs, limits>(tf, jf, aMax, aMin);
116+
}
117+
118+
inline void set_boundary_for_velocity(double p0_new, double v0_new, double a0_new, double vf_new, double af_new) {
119+
a[0] = a0_new;
120+
v[0] = v0_new;
121+
p[0] = p0_new;
122+
af = af_new;
123+
vf = vf_new;
124+
}
125+
113126
// For position interface
114127
template<JerkSigns jerk_signs, Limits limits, bool set_limits = false>
115128
bool check(double jf, double vMax, double vMin, double aMax, double aMin) {
@@ -221,7 +234,6 @@ class Profile {
221234
return (std::abs(jf) < std::abs(jMax) + j_eps) && check_with_timing<jerk_signs, limits>(tf, jf, vMax, vMin, aMax, aMin);
222235
}
223236

224-
//! Set boundary values for the position interface
225237
inline void set_boundary(double p0_new, double v0_new, double a0_new, double pf_new, double vf_new, double af_new) {
226238
a[0] = a0_new;
227239
v[0] = v0_new;
@@ -231,16 +243,8 @@ class Profile {
231243
pf = pf_new;
232244
}
233245

234-
//! Set boundary values for the velocity interface
235-
inline void set_boundary(double p0_new, double v0_new, double a0_new, double vf_new, double af_new) {
236-
a[0] = a0_new;
237-
v[0] = v0_new;
238-
p[0] = p0_new;
239-
af = af_new;
240-
vf = vf_new;
241-
}
242246

243-
inline static void check_position_extremum(double t_ext, double t_sum, double t, double p, double v, double a, double j, PositionExtrema& ext) {
247+
static void check_position_extremum(double t_ext, double t_sum, double t, double p, double v, double a, double j, PositionExtrema& ext) {
244248
if (0 < t_ext && t_ext < t) {
245249
double p_ext, a_ext;
246250
std::tie(p_ext, std::ignore, a_ext) = integrate(t_ext, p, v, a, j);

‎include/ruckig/utils.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
namespace ruckig {
1010

1111
template<class Vector>
12-
std::string join(const Vector& array) {
12+
inline std::string join(const Vector& array) {
1313
std::ostringstream ss;
1414
for (size_t i = 0; i < array.size(); ++i) {
1515
if (i) ss << ", ";
@@ -19,7 +19,7 @@ std::string join(const Vector& array) {
1919
}
2020

2121
//! Integrate with constant jerk for duration t. Returns new position, new velocity, and new acceleration.
22-
static inline std::tuple<double, double, double> integrate(double t, double p0, double v0, double a0, double j) {
22+
inline std::tuple<double, double, double> integrate(double t, double p0, double v0, double a0, double j) {
2323
return std::make_tuple(
2424
p0 + t * (v0 + t * (a0 / 2 + t * j / 6)),
2525
v0 + t * (a0 + t * j / 2),

‎test/test-target-known.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ using namespace ruckig;
77

88

99
template<size_t DOFs, class OTGType>
10-
inline void check_duration(OTGType& otg, InputParameter<DOFs>& input, double duration) {
10+
void check_duration(OTGType& otg, InputParameter<DOFs>& input, double duration) {
1111
OutputParameter<DOFs> output;
1212

1313
while (otg.update(input, output) == Result::Working) {

0 commit comments

Comments
 (0)