56 lines
2.6 KiB
C++
56 lines
2.6 KiB
C++
// Offline-only adapter to the INSTALLED MoveIt TOTG implementation.
|
|
// No ROS node, executor, RViz, CAN, or controller connection is created.
|
|
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
|
|
#include <cmath>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
#include <stdexcept>
|
|
#include <vector>
|
|
|
|
int main() {
|
|
try {
|
|
size_t count; double velocity, acceleration, tolerance, sample_dt;
|
|
if (!(std::cin >> count >> velocity >> acceleration >> tolerance >> sample_dt) ||
|
|
count < 2 || count > 10000 || !std::isfinite(velocity) || velocity <= 0 ||
|
|
!std::isfinite(acceleration) || acceleration <= 0 ||
|
|
!std::isfinite(tolerance) || tolerance <= 0 || tolerance > .01 ||
|
|
!std::isfinite(sample_dt) || sample_dt < .001 || sample_dt > 1.)
|
|
throw std::runtime_error("Invalid TOTG input");
|
|
std::vector<Eigen::VectorXd> points;
|
|
for (size_t i=0; i<count; ++i) {
|
|
Eigen::VectorXd q(7);
|
|
for (int j=0; j<7; ++j) {
|
|
if (!(std::cin >> q[j]) || !std::isfinite(q[j]))
|
|
throw std::runtime_error("Nonfinite/incomplete waypoint");
|
|
}
|
|
if (points.empty() || (q-points.back()).norm() > 1e-10) points.push_back(q);
|
|
}
|
|
if (points.size()<2) throw std::runtime_error("No moving geometric path");
|
|
auto path=trajectory_processing::Path::create(points, tolerance);
|
|
if (!path) throw std::runtime_error("MoveIt TOTG path failed");
|
|
auto trajectory=trajectory_processing::Trajectory::create(*path,
|
|
Eigen::VectorXd::Constant(7,velocity), Eigen::VectorXd::Constant(7,acceleration), .001);
|
|
if (!trajectory) throw std::runtime_error("MoveIt TOTG parameterization failed");
|
|
const double duration=trajectory->getDuration();
|
|
if (!std::isfinite(duration) || duration<=0 || duration>1800)
|
|
throw std::runtime_error("Invalid TOTG duration");
|
|
size_t n=static_cast<size_t>(std::ceil(duration/sample_dt));
|
|
// Uniform intervals including the exact endpoint avoid a tiny final fit
|
|
// interval. These are fit knots, not the final 100 Hz execution samples.
|
|
std::cout << std::setprecision(17) << duration << ' ' << path->getLength() << '\n';
|
|
for (size_t i=0; i<=n; ++i) {
|
|
double t=duration*double(i)/double(n);
|
|
auto q=trajectory->getPosition(t);
|
|
auto v=trajectory->getVelocity(t);
|
|
auto a=trajectory->getAcceleration(t);
|
|
std::cout << t;
|
|
for (const auto& value:{q,v,a})
|
|
for (int j=0; j<7; ++j) std::cout << ' ' << value[j];
|
|
std::cout << '\n';
|
|
}
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "TOTG failed: " << e.what() << '\n';
|
|
return 1;
|
|
}
|
|
}
|