6 #include <visp3/core/vpConfig.h> 8 #ifdef VISP_HAVE_FRANKA 10 #include <franka/exception.h> 11 #include <franka/robot.h> 25 int main(
int argc,
char **argv)
28 std::cerr <<
"Usage: ./generate_consecutive_motions <robot-hostname>" << std::endl;
33 franka::Robot robot(argv[1]);
37 robot.setCollisionBehavior({{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}},
38 {{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}},
39 {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}},
40 {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}});
42 for (
int i = 0; i < 5; i++) {
43 std::cout <<
"Executing motion." << std::endl;
45 double time_max = 4.0;
46 double omega_max = 0.2;
48 robot.control([=, &time](
const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities {
49 time += time_step.toSec();
51 double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
52 double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
54 franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}};
55 if (time >= 2 * time_max) {
56 std::cout << std::endl <<
"Finished motion." << std::endl;
57 return franka::MotionFinished(velocities);
61 }
catch (
const franka::ControlException &e) {
62 std::cout << e.what() << std::endl;
63 std::cout <<
"Running error recovery..." << std::endl;
64 robot.automaticErrorRecovery();
67 }
catch (
const franka::Exception &e) {
68 std::cout << e.what() << std::endl;
72 std::cout <<
"Finished." << std::endl;
78 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }