Visual Servoing Platform  version 3.1.0
franka_execute_trajectory.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 <fstream>
5 #include <iostream>
6 #include <iterator>
7 #include <vector>
8 
9 #include <visp3/core/vpConfig.h>
10 
11 #ifdef VISP_HAVE_FRANKA
12 
13 #include <franka/exception.h>
14 #include <franka/robot.h>
15 
26 template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
27 {
28  ostream << "[";
29  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
30  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
31  ostream << "]";
32  return ostream;
33 }
34 
35 int main(int argc, char **argv)
36 {
37  if (argc != 4) {
38  std::cerr << "Usage: " << argv[0] << " <robot-hostname> <trajectory-csv> <output>" << std::endl;
39  return -1;
40  }
41 
42  std::cout << "Loading csv trajectory" << std::endl;
43  std::fstream csv_file_stream;
44  csv_file_stream.open(argv[2], std::fstream::in);
45 
46  std::vector<std::array<double, 7> > samples;
47  while (csv_file_stream) {
48  std::array<double, 7> q;
49  char delimiter;
50  for (int i = 0; i < 7; i++) {
51  csv_file_stream >> q[i] >> delimiter;
52  }
53  samples.push_back(q);
54  }
55  std::cout << "Read " << samples.size() << " samples" << std::endl;
56 
57  std::vector<franka::RobotState> states;
58  try {
59  franka::Robot robot(argv[1]);
60 
61  // Set additional parameters always before the control loop, NEVER in the
62  // control loop! Set collision behavior.
63  robot.setCollisionBehavior(
64  {{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}},
65  {{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}},
66  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
67  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
68 
69  // Set the joint impedance.
70  robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
71 
72  size_t index = 0;
73  robot.control([&](const franka::RobotState &robot_state, franka::Duration time_step) -> franka::JointPositions {
74  states.push_back(robot_state);
75 
76  index += time_step.toMSec();
77 
78  if (index >= samples.size() - 1) {
79  return franka::MotionFinished(franka::JointPositions(samples.back()));
80  }
81  return samples[index];
82  });
83  } catch (const franka::ControlException &e) {
84  std::cout << e.what() << std::endl;
85  } catch (const franka::Exception &e) {
86  std::cout << e.what() << std::endl;
87  return -1;
88  }
89 
90  std::cout << "Logging results to file: " << argv[3] << std::endl;
91  std::fstream output_stream;
92  output_stream.open(argv[3], std::fstream::out);
93  for (size_t s = 0; s < states.size(); s++) {
94  output_stream << "Sample: #" << s << std::endl;
95  output_stream << "q_d: \t" << samples[s] << std::endl;
96  output_stream << "Robot state:" << std::endl;
97  output_stream << "q: \t" << states[s].q << std::endl;
98  output_stream << "q_d: \t" << states[s].q_d << std::endl;
99  output_stream << "dq: \t" << states[s].dq << std::endl;
100  output_stream << "______" << std::endl;
101  }
102 
103  return 0;
104 }
105 
106 #else
107 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
108 #endif