Visual Servoing Platform  version 3.1.0
franka_generate_consecutive_motions.cpp
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #include <cmath>
4 #include <iostream>
5 
6 #include <visp3/core/vpConfig.h>
7 
8 #ifdef VISP_HAVE_FRANKA
9 
10 #include <franka/exception.h>
11 #include <franka/robot.h>
12 
25 int main(int argc, char **argv)
26 {
27  if (argc != 2) {
28  std::cerr << "Usage: ./generate_consecutive_motions <robot-hostname>" << std::endl;
29  return -1;
30  }
31 
32  try {
33  franka::Robot robot(argv[1]);
34 
35  // Set additional parameters always before the control loop, NEVER in the
36  // control loop! Set collision behavior.
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}});
41 
42  for (int i = 0; i < 5; i++) {
43  std::cout << "Executing motion." << std::endl;
44  try {
45  double time_max = 4.0;
46  double omega_max = 0.2;
47  double time = 0.0;
48  robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities {
49  time += time_step.toSec();
50 
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));
53 
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);
58  }
59  return velocities;
60  });
61  } catch (const franka::ControlException &e) {
62  std::cout << e.what() << std::endl;
63  std::cout << "Running error recovery..." << std::endl;
64  robot.automaticErrorRecovery();
65  }
66  }
67  } catch (const franka::Exception &e) {
68  std::cout << e.what() << std::endl;
69  return -1;
70  }
71 
72  std::cout << "Finished." << std::endl;
73 
74  return 0;
75 }
76 
77 #else
78 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
79 #endif