Даниил Грабарь
0a091ad521
Use shared limits for all seven axes across MoveIt, URDF and FRI. Apply Ruckig smoothing and continuous JTC spline checks, stretching trajectory timing to respect velocity, acceleration and jerk limits. Require fresh stationary feedback for direct trajectories and protect preparation from concurrent submissions and stop requests. Add regression tests, dependencies and bilingual FRI documentation. Include the updated LIN speed-sweep scenario and current 60 ms filter configuration. Acceleration and jerk limits remain project settings; physical high-speed twitching is still under investigation.
59 lines
1.8 KiB
C++
59 lines
1.8 KiB
C++
#include "iiwa_planning/spline_limits.hpp"
|
|
#include <cassert>
|
|
#include <cmath>
|
|
#include <iostream>
|
|
using namespace iiwa_planning;
|
|
int main() {
|
|
// Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5.
|
|
std::vector<Sample> points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}};
|
|
std::vector<Limit> limits{{-2, 2, 2, 1, 2}};
|
|
const double scale = limit_splines(points, limits);
|
|
assert(scale > 3.0);
|
|
assert(std::abs(points.back().position[0] - 1) < 1e-12);
|
|
assert(limit_splines(points, limits) <= 1.000001);
|
|
// Consistent time scaling keeps the geometric quintic unchanged.
|
|
auto p = quintic(0, 1, 0, 0, 0, 0, 1);
|
|
auto q = quintic(0, 1, 0, 0, 0, 0, points.back().time);
|
|
for (int i = 0; i <= 5; ++i)
|
|
assert(std::abs(p[i] - q[i]) < 1e-10);
|
|
bool bad = false;
|
|
try {
|
|
std::vector<Sample> s{{0, {0}, {0}, {0}}, {0, {1}, {0}, {0}}};
|
|
limit_splines(s, limits);
|
|
} catch (const std::exception &) {
|
|
bad = true;
|
|
}
|
|
assert(bad);
|
|
bad = false;
|
|
// Positions are in range but large endpoint velocities overshoot between
|
|
// them.
|
|
try {
|
|
std::vector<Sample> s{{0, {0}, {0}, {0}},
|
|
{1, {0}, {12}, {0}},
|
|
{2, {0}, {-12}, {0}},
|
|
{3, {0}, {0}, {0}}};
|
|
limit_splines(s, limits);
|
|
} catch (const std::exception &) {
|
|
bad = true;
|
|
}
|
|
assert(bad);
|
|
bad = false;
|
|
try {
|
|
std::vector<Sample> s{{0, {NAN}, {0}, {0}}, {1, {0}, {0}, {0}}};
|
|
limit_splines(s, limits);
|
|
} catch (const std::exception &) {
|
|
bad = true;
|
|
}
|
|
assert(bad);
|
|
// Non-rest boundaries must not slip past the finite-segment jerk check.
|
|
bad = false;
|
|
try {
|
|
std::vector<Sample> s{{0, {0}, {0}, {1}}, {1, {1}, {0}, {0}}};
|
|
limit_splines(s, limits);
|
|
} catch (const std::exception &) {
|
|
bad = true;
|
|
}
|
|
assert(bad);
|
|
std::cout << "spline limits OK\n";
|
|
}
|