Visual Servoing Platform  version 3.1.0
franka_motion_with_control.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 <iomanip>
5 #include <iostream>
6 #include <vector>
7 
8 #include <visp3/core/vpConfig.h>
9 
10 #ifdef VISP_HAVE_FRANKA
11 
12 #include <franka/exception.h>
13 #include <franka/robot.h>
14 
28 namespace
29 {
30 
31 class Controller
32 {
33 public:
34  Controller(size_t dq_filter_size, const std::array<double, 7> &K_P, // NOLINT
35  const std::array<double, 7> &K_D) // NOLINT
36  : dq_current_filter_position_(0), dq_filter_size_(dq_filter_size), K_P_(K_P), K_D_(K_D)
37  {
38  std::fill(dq_d_.begin(), dq_d_.end(), 0);
39  dq_buffer_.reset(new double[dq_filter_size_ * 7]);
40  std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7], 0);
41  }
42 
43  inline franka::Torques step(const franka::RobotState &state)
44  {
45  updateDQFilter(state);
46 
47  std::array<double, 7> tau_J_d; // NOLINT
48  for (size_t i = 0; i < 7; i++) {
49  tau_J_d[i] = K_P_[i] * (state.q_d[i] - state.q[i]) + K_D_[i] * (dq_d_[i] - getDQFiltered(i));
50  }
51  return tau_J_d;
52  }
53 
54  void updateDQFilter(const franka::RobotState &state)
55  {
56  for (size_t i = 0; i < 7; i++) {
57  dq_buffer_.get()[dq_current_filter_position_ * 7 + i] = state.dq[i];
58  }
59  dq_current_filter_position_ = (dq_current_filter_position_ + 1) % dq_filter_size_;
60  }
61 
62  double getDQFiltered(size_t index) const
63  {
64  double value = 0;
65  for (size_t i = index; i < 7 * dq_filter_size_; i += 7) {
66  value += dq_buffer_.get()[i];
67  }
68  return value / dq_filter_size_;
69  }
70 
71 private:
72  size_t dq_current_filter_position_;
73  size_t dq_filter_size_;
74 
75  const std::array<double, 7> K_P_; // NOLINT
76  const std::array<double, 7> K_D_; // NOLINT
77 
78  std::array<double, 7> dq_d_;
79  std::unique_ptr<double[]> dq_buffer_;
80 };
81 
82 std::vector<double> generateTrajectory(double a_max)
83 {
84  // Generating a motion with smooth velocity and acceleration.
85  // Squared sine is used for the acceleration/deceleration phase.
86  std::vector<double> trajectory;
87  constexpr double kTimeStep = 0.001; // [s]
88  constexpr double kAccelerationTime = 1; // time spend accelerating and decelerating [s]
89  constexpr double kConstantVelocityTime = 1; // time spend with constant speed [s]
90  // obtained during the speed up
91  // and slow down [rad/s^2]
92  double a = 0; // [rad/s^2]
93  double v = 0; // [rad/s]
94  double t = 0; // [s]
95  while (t < (2 * kAccelerationTime + kConstantVelocityTime)) {
96  if (t <= kAccelerationTime) {
97  a = pow(sin(t * M_PI / kAccelerationTime), 2) * a_max;
98  } else if (t <= (kAccelerationTime + kConstantVelocityTime)) {
99  a = 0;
100  } else {
101  const double deceleration_time =
102  (kAccelerationTime + kConstantVelocityTime) - t; // time spent in the deceleration phase
103  a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max;
104  }
105  v += a * kTimeStep;
106  t += kTimeStep;
107  trajectory.push_back(v);
108  }
109  return trajectory;
110 }
111 
112 } // anonymous namespace
113 
114 int main(int argc, char **argv)
115 {
116  if (argc != 7) {
117  std::cerr << "Usage: ./" << argv[0] << " <robot-hostname>"
118  << " <filter size>"
119  << " <K_P>"
120  << " <K_D>"
121  << " <joint>"
122  << " <a_max>" << std::endl;
123  return -1;
124  }
125 
126  size_t filter_size = std::stoul(argv[2]);
127  std::array<double, 7> K_P; // NOLINT
128  std::array<double, 7> K_D; // NOLINT
129  for (size_t i = 0; i < 7; i++) {
130  K_P[i] = std::stod(argv[3]);
131  K_D[i] = std::stod(argv[4]);
132  }
133 
134  std::cout << "Initializing controller: " << std::endl;
135  for (size_t i = 0; i < 7; i++) {
136  std::cout << i + 1 << ": K_P = " << K_P[i] << "\tK_D = " << K_D[i] << std::endl;
137  }
138  std::cout << "dq filter size: " << filter_size << std::endl;
139  Controller controller(filter_size, K_P, K_D);
140 
141  try {
142  franka::Robot robot(argv[1]);
143 
144  // Set additional parameters always before the control loop, NEVER in the
145  // control loop! Set collision behavior.
146  robot.setCollisionBehavior(
147  {{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}},
148  {{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}},
149  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
150  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
151 
152  size_t index = 0;
153  int joint_number = std::stoi(argv[5]);
154  std::vector<double> trajectory = generateTrajectory(std::stod(argv[6]));
155 
156  robot.control([&](const franka::RobotState &robot_state,
157  franka::Duration) -> franka::Torques { return controller.step(robot_state); },
158  [&](const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities {
159  index += time_step.toMSec();
160 
161  if (index >= trajectory.size()) {
162  index = trajectory.size() - 1;
163  }
164 
165  franka::JointVelocities velocities{{0, 0, 0, 0, 0, 0, 0}};
166  velocities.dq[joint_number] = trajectory[index];
167 
168  if (index >= trajectory.size() - 1) {
169  return franka::MotionFinished(velocities);
170  }
171  return velocities;
172  });
173  } catch (const franka::Exception &e) {
174  std::cout << e.what() << std::endl;
175  return -1;
176  }
177 
178  return 0;
179 }
180 
181 #else
182 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
183 #endif