Visual Servoing Platform  version 3.1.0
franka_joint_impedance_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 
4 #include <iostream>
5 
6 #include <visp3/core/vpConfig.h>
7 
8 #ifdef VISP_HAVE_FRANKA
9 
10 #include <array>
11 #include <atomic>
12 #include <cmath>
13 #include <functional>
14 #include <iterator>
15 #include <mutex>
16 #include <thread>
17 
18 #include <franka/duration.h>
19 #include <franka/exception.h>
20 #include <franka/model.h>
21 #include <franka/robot.h>
22 
23 namespace
24 {
25 template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
26 {
27  ostream << "[";
28  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
29  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
30  ostream << "]";
31  return ostream;
32 }
33 } // anonymous namespace
34 
51 int main(int argc, char **argv)
52 {
53  // Check whether the required arguments were passed.
54  if (argc != 5) {
55  std::cerr << "Usage: ./" << argv[0] << " <robot-hostname>"
56  << " <radius in [m]>"
57  << " <vel_max in [m/s]>"
58  << " <print_rate in [Hz]>" << std::endl;
59  return -1;
60  }
61 
62  // Set and initialize trajectory parameters.
63  const double radius = std::stod(argv[2]);
64  const double vel_max = std::stod(argv[3]);
65  const double acceleration_time = 2.0;
66  double vel_current = 0.0;
67  double angle = 0.0;
68 
69  double time = 0.0;
70  const double run_time = 20.0;
71 
72  // Set print rate for comparing commanded vs. measured torques.
73  double print_rate = std::stod(argv[4]);
74  if (print_rate < 0.0) {
75  std::cerr << "print_rate too small, must be >= 0.0" << std::endl;
76  return -1;
77  }
78 
79  // Initialize data fields for the print thread.
80  struct {
81  std::mutex mutex;
82  bool has_data;
83  std::array<double, 7> tau_d_last;
84  franka::RobotState robot_state;
85  std::array<double, 7> gravity;
86  } print_data{};
87  std::atomic_bool running{true};
88 
89  // Start print thread.
90  std::thread print_thread([print_rate, &print_data, &running]() {
91  while (running) {
92  // Sleep to achieve the desired print rate.
93  std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0))));
94 
95  // Try to lock data to avoid read write collisions.
96  if (print_data.mutex.try_lock() && print_data.has_data) {
97  std::array<double, 7> tau_error{};
98  double error_rms(0.0);
99  std::array<double, 7> tau_d_actual{};
100  for (size_t i = 0; i < 7; ++i) {
101  tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i];
102  tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i];
103  error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / tau_error.size();
104  }
105  // Print data to console
106  std::cout << "tau_error [Nm]: " << tau_error << std::endl
107  << "tau_commanded [Nm]: " << tau_d_actual << std::endl
108  << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl
109  << "root mean square of tau_error [Nm]: " << error_rms << std::endl
110  << "-----------------------" << std::endl;
111  print_data.has_data = false;
112  print_data.mutex.unlock();
113  }
114  }
115  });
116 
117  try {
118  // Connect to robot.
119  franka::Robot robot(argv[1]);
120 
121  // Set collision behavior.
122  robot.setCollisionBehavior(
123  {{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}},
124  {{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}},
125  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
126  {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
127 
128  // Load the kinematics and dynamics model.
129  franka::Model model = robot.loadModel();
130 
131  // Read the initial pose to start the motion from there.
132  std::array<double, 16> initial_pose = robot.readOnce().O_T_EE;
133 
134  // Define callback function to send Cartesian pose goals to get inverse
135  // kinematics solved.
136  std::function<franka::CartesianPose(const franka::RobotState &, franka::Duration)> cartesian_pose_callback =
137  [=, &time, &vel_current, &running, &angle](const franka::RobotState & /*state*/,
138  franka::Duration period) -> franka::CartesianPose {
139  // Update time.
140  time += period.toSec();
141 
142  // Compute Cartesian velocity.
143  if (vel_current < vel_max && time < run_time) {
144  vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
145  }
146  if (vel_current > 0.0 && time > run_time) {
147  vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
148  }
149  vel_current = std::fmax(vel_current, 0.0);
150  vel_current = std::fmin(vel_current, vel_max);
151 
152  // Compute new angle for our circular trajectory.
153  angle += period.toSec() * vel_current / std::fabs(radius);
154  if (angle > 2 * M_PI) {
155  angle -= 2 * M_PI;
156  }
157 
158  // Compute relative y and z positions of desired pose.
159  double delta_y = radius * (1 - std::cos(angle));
160  double delta_z = radius * std::sin(angle);
161  franka::CartesianPose pose_desired = initial_pose;
162  pose_desired.O_T_EE[13] += delta_y;
163  pose_desired.O_T_EE[14] += delta_z;
164 
165  // Send desired pose.
166  if (time >= run_time + acceleration_time) {
167  running = false;
168  return franka::MotionFinished(pose_desired);
169  }
170 
171  return pose_desired;
172  };
173 
174  // Set gains for the joint impedance control.
175  // Stiffness
176  const std::array<double, 7> k_gains = {{1000.0, 1000.0, 1000.0, 1000.0, 500.0, 300.0, 100.0}};
177  // Damping
178  const std::array<double, 7> d_gains = {{100.0, 100.0, 100.0, 100.0, 50.0, 30.0, 10.0}};
179 
180  // Define callback for the joint torque control loop.
181  std::function<franka::Torques(const franka::RobotState &, franka::Duration)> impedance_control_callback =
182  [&print_data, &model, k_gains, d_gains](const franka::RobotState &state,
183  franka::Duration /*period*/) -> franka::Torques {
184  // Read current coriolis terms from model.
185  std::array<double, 7> coriolis =
186  model.coriolis(state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}});
187 
188  // Compute torque command from joint impedance control law.
189  // Note: The answer to our Cartesian pose inverse kinematics is always
190  // in state.q_d with one time step delay.
191  std::array<double, 7> tau_d;
192  for (size_t i = 0; i < 7; i++) {
193  tau_d[i] = k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
194  }
195 
196  // Update data to print.
197  if (print_data.mutex.try_lock()) {
198  print_data.has_data = true;
199  print_data.robot_state = state;
200  print_data.tau_d_last = tau_d;
201  print_data.gravity = model.gravity(state, 0.0, {{0.0, 0.0, 0.0}});
202  print_data.mutex.unlock();
203  }
204 
205  // Send torque command.
206  return tau_d;
207  };
208 
209  // Start real-time control loop.
210  robot.control(impedance_control_callback, cartesian_pose_callback);
211 
212  } catch (const franka::Exception &ex) {
213  running = false;
214  std::cerr << ex.what() << std::endl;
215  }
216 
217  if (print_thread.joinable()) {
218  print_thread.join();
219  }
220  return 0;
221 }
222 
223 #else
224 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
225 #endif