Files
a7l20-cup-grasp/runtime/arm/cpp/totg_offline.cpp
T

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;
}
}