Даниил Грабарь
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.
37 lines
1.4 KiB
C++
37 lines
1.4 KiB
C++
#include "iiwa_planning/spline_limits.hpp"
|
|
#include <cassert>
|
|
#include <iostream>
|
|
#include <joint_trajectory_controller/trajectory.hpp>
|
|
|
|
int main() {
|
|
using namespace iiwa_planning;
|
|
std::vector<Sample> samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}};
|
|
std::vector<Limit> limits{{-2, 2, 1, 1, 2}};
|
|
const auto scale = limit_splines(samples, limits);
|
|
assert(scale > 1);
|
|
joint_trajectory_controller::Trajectory jtc;
|
|
trajectory_msgs::msg::JointTrajectoryPoint a, b, out;
|
|
a.positions = samples[0].position;
|
|
a.velocities = samples[0].velocity;
|
|
a.accelerations = samples[0].acceleration;
|
|
b.positions = samples[1].position;
|
|
b.velocities = samples[1].velocity;
|
|
b.accelerations = samples[1].acceleration;
|
|
const auto finish = rclcpp::Time(static_cast<int64_t>(samples[1].time * 1e9));
|
|
const auto start = rclcpp::Time(int64_t(0));
|
|
double prev = 0;
|
|
const int n = 10000;
|
|
for (int i = 0; i <= n; ++i) {
|
|
const auto now = rclcpp::Time(finish.nanoseconds() * i / n);
|
|
jtc.interpolate_between_points(start, a, finish, b, now, out);
|
|
assert(std::abs(out.velocities[0]) <= 1.000001);
|
|
assert(std::abs(out.accelerations[0]) <= 1.000001);
|
|
if (i)
|
|
assert(std::abs((out.accelerations[0] - prev) / (samples[1].time / n)) <=
|
|
2.00001);
|
|
prev = out.accelerations[0];
|
|
}
|
|
std::cout << "Installed JTC interpolation stays within "
|
|
"velocity/acceleration/jerk limits\n";
|
|
}
|