Visual Servoing Platform  version 3.1.0
franka_generate_joint_velocity_motion.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_joint_velocity_motion <robot-hostname>" << std::endl;
29  return -1;
30  }
31  try {
32  franka::Robot robot(argv[1]);
33 
34  // Set additional parameters always before the control loop, NEVER in the
35  // control loop! Set collision behavior.
36  robot.setCollisionBehavior(
37  {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
38  {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
39  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
40  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
41 
42  double time_max = 4.0;
43  double omega_max = 0.2;
44  double time = 0.0;
45  robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities {
46  time += time_step.toSec();
47 
48  double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
49  double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
50 
51  franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}};
52 
53  if (time >= 2 * time_max) {
54  std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
55  return franka::MotionFinished(velocities);
56  }
57  return velocities;
58  });
59  } catch (const franka::Exception &e) {
60  std::cout << e.what() << std::endl;
61  return -1;
62  }
63 
64  return 0;
65 }
66 
67 #else
68 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
69 #endif