Visual Servoing Platform  version 3.1.0
franka_joint_point_to_point_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 <algorithm>
4 #include <cmath>
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 
29 constexpr double kDeltaQMotionFinished = 1e-6;
30 
31 inline int sgn(double x)
32 {
33  if (x == 0) {
34  return 0;
35  }
36  return (x > 0) ? 1 : -1;
37 }
38 
39 std::array<double, 7> add(const std::array<double, 7> &a, const std::array<double, 7> &b)
40 {
41  std::array<double, 7> result;
42  for (size_t i = 0; i < a.size(); i++) {
43  result[i] = a[i] + b[i];
44  }
45  return result;
46 }
47 
48 std::array<double, 7> subtract(const std::array<double, 7> &a, const std::array<double, 7> &b)
49 {
50  std::array<double, 7> result;
51  for (size_t i = 0; i < a.size(); i++) {
52  result[i] = a[i] - b[i];
53  }
54  return result;
55 }
56 
57 bool calculateDesiredValues(double t, const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
58  const std::array<double, 7> &t_1, const std::array<double, 7> &t_2,
59  const std::array<double, 7> &t_f, const std::array<double, 7> &q_1,
60  std::array<double, 7> *delta_q_d);
61 
62 void calculateSynchronizedValues(const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
63  const std::array<double, 7> &ddq_max_start, const std::array<double, 7> &ddq_max_goal,
64  std::array<double, 7> *dq_max_sync, std::array<double, 7> *t_1_sync,
65  std::array<double, 7> *t_2_sync, std::array<double, 7> *t_f_sync,
66  std::array<double, 7> *q_1);
67 
68 int main(int argc, char **argv)
69 {
70  if (argc != 10) {
71  std::cerr << "Usage: ./generate_joint_pose_motion "
72  << "<robot-hostname> <goal-position> <speed-factor>" << std::endl
73  << "speed-factor must be between zero and one." << std::endl;
74  return -1;
75  }
76 
77  try {
78  franka::Robot robot(argv[1]);
79  std::array<double, 7> q_goal;
80  for (size_t i = 0; i < 7; i++) {
81  q_goal[i] = std::stod(argv[i + 2]);
82  }
83  double speed_factor = std::stod(argv[9]);
84 
85  // Set additional parameters always before the control loop, NEVER in the
86  // control loop! Set collision behavior.
87  robot.setCollisionBehavior(
88  {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
89  {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
90  {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
91  {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
92 
93  std::array<double, 7> q_start = robot.readOnce().q_d;
94 
95  std::array<double, 7> dq_max{{2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5}};
96  std::array<double, 7> ddq_max_start{{5, 5, 5, 5, 5, 5, 5}};
97  std::array<double, 7> ddq_max_goal{{5, 5, 5, 5, 5, 5, 5}};
98  for (size_t i = 0; i < 7; i++) {
99  dq_max[i] = speed_factor * dq_max[i];
100  ddq_max_start[i] = speed_factor * ddq_max_start[i];
101  ddq_max_goal[i] = speed_factor * ddq_max_goal[i];
102  }
103 
104  double time = 0.0;
105 
106  std::array<double, 7> dq_max_sync{};
107  std::array<double, 7> t_1_sync{};
108  std::array<double, 7> t_2_sync{};
109  std::array<double, 7> t_f_sync{};
110  std::array<double, 7> q_1{};
111  std::array<double, 7> delta_q = subtract(q_goal, q_start);
112 
113  calculateSynchronizedValues(delta_q, dq_max, ddq_max_start, ddq_max_goal, &dq_max_sync, &t_1_sync, &t_2_sync,
114  &t_f_sync, &q_1);
115  robot.control([=, &time](const franka::RobotState &, franka::Duration time_step) -> franka::JointPositions {
116  time += time_step.toSec();
117 
118  std::array<double, 7> delta_q_d;
119  bool motion_finished =
120  calculateDesiredValues(time, delta_q, dq_max_sync, t_1_sync, t_2_sync, t_f_sync, q_1, &delta_q_d);
121 
122  franka::JointPositions output = add(q_start, delta_q_d);
123  output.motion_finished = motion_finished;
124  return output;
125  });
126  std::cout << std::endl << "Motion finished" << std::endl;
127  } catch (const franka::Exception &e) {
128  std::cout << e.what() << std::endl;
129  return -1;
130  }
131 
132  return 0;
133 }
134 
135 bool calculateDesiredValues(double t, const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
136  const std::array<double, 7> &t_1, const std::array<double, 7> &t_2,
137  const std::array<double, 7> &t_f, const std::array<double, 7> &q_1,
138  std::array<double, 7> *delta_q_d)
139 {
140  std::array<int, 7> sign_delta_q;
141  std::array<double, 7> t_d = subtract(t_2, t_1);
142  std::array<double, 7> delta_t_2 = subtract(t_f, t_2);
143  std::array<bool, 7> joint_motion_finished{};
144 
145  for (size_t i = 0; i < 7; i++) {
146  sign_delta_q[i] = sgn(delta_q[i]);
147  if (std::abs(delta_q[i]) < kDeltaQMotionFinished) {
148  (*delta_q_d)[i] = 0;
149  joint_motion_finished[i] = true;
150  } else {
151  if (t < t_1[i]) {
152  (*delta_q_d)[i] =
153  -1.0 / std::pow(t_1[i], 3) * dq_max[i] * sign_delta_q[i] * (0.5 * t - t_1[i]) * std::pow(t, 3);
154  } else if (t >= t_1[i] && t < t_2[i]) {
155  (*delta_q_d)[i] = q_1[i] + (t - t_1[i]) * dq_max[i] * sign_delta_q[i];
156  } else if (t >= t_2[i] && t < t_f[i]) {
157  (*delta_q_d)[i] = delta_q[i] + 0.5 *
158  (1.0 / std::pow(delta_t_2[i], 3) * (t - t_1[i] - 2 * delta_t_2[i] - t_d[i]) *
159  std::pow((t - t_1[i] - t_d[i]), 3) +
160  (2.0 * t - 2.0 * t_1[i] - delta_t_2[i] - 2.0 * t_d[i])) *
161  dq_max[i] * sign_delta_q[i];
162  } else {
163  (*delta_q_d)[i] = delta_q[i];
164  joint_motion_finished[i] = true;
165  }
166  }
167  }
168  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), [](bool x) { return x; });
169 }
170 
171 void calculateSynchronizedValues(const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
172  const std::array<double, 7> &ddq_max_start, const std::array<double, 7> &ddq_max_goal,
173  std::array<double, 7> *dq_max_sync, std::array<double, 7> *t_1_sync,
174  std::array<double, 7> *t_2_sync, std::array<double, 7> *t_f_sync,
175  std::array<double, 7> *q_1)
176 {
177  std::array<double, 7> dq_max_reach = dq_max;
178  std::array<double, 7> t_f{};
179  std::array<double, 7> delta_t_2{};
180  std::array<double, 7> t_1{};
181  std::array<double, 7> delta_t_2_sync{};
182  int sign_delta_q[7];
183  for (size_t i = 0; i < 7; i++) {
184  sign_delta_q[i] = sgn(delta_q[i]);
185  if (std::abs(delta_q[i]) > kDeltaQMotionFinished) {
186  if (std::abs(delta_q[i]) < (3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_start[i]) +
187  3.0 / 4.0 * (std::pow(dq_max[i], 2) / ddq_max_goal[i]))) {
188  dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q[i] * sign_delta_q[i] * (ddq_max_start[i] * ddq_max_goal[i]) /
189  (ddq_max_start[i] + ddq_max_goal[i]));
190  }
191  t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start[i];
192  delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal[i];
193  t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q[i]) / dq_max_reach[i];
194  }
195  }
196 
197  double max_t_f = *std::max_element(t_f.begin(), t_f.end());
198  for (size_t i = 0; i < 7; i++) {
199  if (std::abs(delta_q[i]) > kDeltaQMotionFinished) {
200  double a = 1.5 / 2.0 * (ddq_max_goal[i] + ddq_max_start[i]);
201  double b = -1.0 * max_t_f * ddq_max_goal[i] * ddq_max_start[i];
202  double c = std::abs(delta_q[i]) * ddq_max_goal[i] * ddq_max_start[i];
203  double delta = b * b - 4.0 * a * c;
204  (*dq_max_sync)[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
205  (*t_1_sync)[i] = 1.5 * (*dq_max_sync)[i] / ddq_max_start[i];
206  delta_t_2_sync[i] = 1.5 * (*dq_max_sync)[i] / ddq_max_goal[i];
207  (*t_f_sync)[i] = (*t_1_sync)[i] / 2 + delta_t_2_sync[i] / 2 + std::abs(delta_q[i] / (*dq_max_sync)[i]);
208  (*t_2_sync)[i] = (*t_f_sync)[i] - delta_t_2_sync[i];
209  (*q_1)[i] = (*dq_max_sync)[i] * sign_delta_q[i] * (0.5 * (*t_1_sync)[i]);
210  }
211  }
212 }
213 
214 #else
215 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
216 #endif