8 #include <visp3/core/vpConfig.h> 10 #ifdef VISP_HAVE_FRANKA 12 #include <franka/exception.h> 13 #include <franka/robot.h> 29 constexpr
double kDeltaQMotionFinished = 1e-6;
31 inline int sgn(
double x)
36 return (x > 0) ? 1 : -1;
39 std::array<double, 7> add(
const std::array<double, 7> &a,
const std::array<double, 7> &b)
41 std::array<double, 7> result;
42 for (
size_t i = 0; i < a.size(); i++) {
43 result[i] = a[i] + b[i];
48 std::array<double, 7> subtract(
const std::array<double, 7> &a,
const std::array<double, 7> &b)
50 std::array<double, 7> result;
51 for (
size_t i = 0; i < a.size(); i++) {
52 result[i] = a[i] - b[i];
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);
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);
68 int main(
int argc,
char **argv)
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;
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]);
83 double speed_factor = std::stod(argv[9]);
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}});
93 std::array<double, 7> q_start = robot.readOnce().q_d;
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];
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);
113 calculateSynchronizedValues(delta_q, dq_max, ddq_max_start, ddq_max_goal, &dq_max_sync, &t_1_sync, &t_2_sync,
115 robot.control([=, &time](
const franka::RobotState &, franka::Duration time_step) -> franka::JointPositions {
116 time += time_step.toSec();
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);
122 franka::JointPositions output = add(q_start, delta_q_d);
123 output.motion_finished = motion_finished;
126 std::cout << std::endl <<
"Motion finished" << std::endl;
127 }
catch (
const franka::Exception &e) {
128 std::cout << e.what() << std::endl;
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)
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{};
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) {
149 joint_motion_finished[i] =
true;
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];
163 (*delta_q_d)[i] = delta_q[i];
164 joint_motion_finished[i] =
true;
168 return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), [](
bool x) {
return x; });
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)
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{};
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]));
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];
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]);
215 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }