An example showing how to execute consecutive motions with error recovery.
#include <cmath>
#include <iostream>
int main(int argc, char** argv) {
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
return -1;
}
try {
std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
std::cout << "WARNING: This example will move the robot! "
<< "Please make sure to have the user stop button at hand!" << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
std::cout << "Finished moving to initial joint configuration." << std::endl;
{{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
{{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
{{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}},
{{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}});
for (size_t i = 0; i < 5; i++) {
std::cout << "Executing motion." << std::endl;
try {
double time_max = 4.0;
double omega_max = 0.2;
double time = 0.0;
double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
if (time >= 2 * time_max) {
std::cout << std::endl << "Finished motion." << std::endl;
}
return velocities;
});
std::cout << e.what() << std::endl;
std::cout << "Running error recovery..." << std::endl;
}
}
std::cout << e.what() << std::endl;
return -1;
}
std::cout << "Finished." << std::endl;
return 0;
}