Fawkes API Fawkes Development Version
bimanual_goto_thread.cpp
1
2/***************************************************************************
3 * bimaual_goto_thread.cpp - Jaco plugin movement thread for coordinated bimanual manipulation
4 *
5 * Created: Mon Sep 29 23:17:12 2014
6 * Copyright 2014 Bahram Maleki-Fard
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "bimanual_goto_thread.h"
24
25#include "arm.h"
26#include "goto_thread.h"
27#include "openrave_thread.h"
28
29#include <core/threading/mutex.h>
30#include <interfaces/JacoBimanualInterface.h>
31#include <interfaces/JacoInterface.h>
32#include <utils/math/angle.h>
33
34#include <unistd.h>
35
36using namespace fawkes;
37
38/** @class JacoBimanualGotoThread "bimanual_goto_thread.h"
39 * Jaco Arm movement thread.
40 * This thread handles the movement of the arm.
41 *
42 * @author Bahram Maleki-Fard
43 */
44
45/** Constructor.
46 * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
47 */
49: Thread("JacoBimanualGotoThread", Thread::OPMODE_CONTINUOUS)
50{
51 dual_arms_ = arms;
52 final_mutex_ = NULL;
53 final_ = true;
54}
55
56/** Destructor. */
58{
59}
60
61void
63{
64 arms_.l.arm = dual_arms_->left;
65 arms_.r.arm = dual_arms_->right;
66
67 final_mutex_ = new Mutex();
68 v_arms_[0] = &(arms_.l);
69 v_arms_[1] = &(arms_.r);
70}
71
72void
74{
75 dual_arms_ = NULL;
76
77 v_arms_[0] = NULL;
78 v_arms_[1] = NULL;
79
80 arms_.l.arm = NULL;
81 arms_.r.arm = NULL;
82
83 delete final_mutex_;
84 final_mutex_ = NULL;
85}
86
87/** The main loop of this thread.
88 * @see JacoGotoThread::loop
89 */
90void
92{
93 final_mutex_->lock();
94 bool final = final_;
95 final_mutex_->unlock();
96
97 if (arms_.l.arm == NULL || arms_.r.arm == NULL || !final) {
98 usleep(30e3);
99 return;
100 }
101
102 // Current targets have been processed. Unref, if still refed
103 if (arms_.l.target && arms_.r.target) {
104 arms_.l.target.clear();
105 arms_.r.target.clear();
106 // trajectories hav been processed. remove those targets from queues.
107 // This will automatically delete the trajectories as well as soon
108 // as we leave this block (thanks to refptr)
109 _lock_queues();
110 arms_.l.arm->target_queue->pop_front();
111 arms_.r.arm->target_queue->pop_front();
112 _unlock_queues();
113 }
114
115 // Check for new targets
116 _lock_queues();
117 if (!arms_.l.arm->target_queue->empty() && !arms_.r.arm->target_queue->empty()) {
118 // get RefPtr to first target in queue
119 arms_.l.target = arms_.l.arm->target_queue->front();
120 arms_.r.target = arms_.r.arm->target_queue->front();
121 }
122 _unlock_queues();
123
124 if (!arms_.l.target || !arms_.r.target || !arms_.l.target->coord || !arms_.r.target->coord) {
125 //no new target in queue, or at least one target is not meant for
126 // coordinated manipulation
127 arms_.l.target.clear();
128 arms_.r.target.clear();
129 usleep(30e3);
130 return;
131 }
132
133 if (arms_.l.target->type != arms_.r.target->type) {
135 "target type mismatch, %i != %i",
136 arms_.l.target->type,
137 arms_.r.target->type);
138 arms_.l.target.clear();
139 arms_.r.target.clear();
140 usleep(30e3);
141 return;
142 }
143
144 if (arms_.l.target->trajec_state == TRAJEC_IK_ERROR
145 || arms_.r.target->trajec_state == TRAJEC_IK_ERROR
146 || arms_.l.target->trajec_state == TRAJEC_PLANNING_ERROR
147 || arms_.r.target->trajec_state == TRAJEC_PLANNING_ERROR) {
148 logger->log_warn(name(), "Trajectory could not be planned. Abort!");
149 // stop the current target and empty remaining queue, with appropriate error_code. This also sets "final" to true.
150 dual_arms_->iface->set_error_code(arms_.l.target->trajec_state);
151 stop();
152 return;
153 }
154
155 if (arms_.l.target->trajec_state != arms_.r.target->trajec_state) {
157 "trajec state mismatch, %i != %i",
158 arms_.l.target->trajec_state,
159 arms_.r.target->trajec_state);
160 arms_.l.target.clear();
161 arms_.r.target.clear();
162 usleep(30e3);
163 return;
164 }
165
166 switch (arms_.l.target->trajec_state) {
167 case TRAJEC_SKIP:
168 // "regular" target. For now, we just process "GRIPPER", therefore do not
169 // change plotting
171 "No planning for these targets. Process, using current finger positions...");
172
173 if (arms_.l.target->type != TARGET_GRIPPER) {
175 "Unknown target type %i, cannot process without planning!",
176 arms_.l.target->type);
177 stop();
178 dual_arms_->iface->set_error_code(JacoInterface::ERROR_UNSPECIFIC);
179 } else {
180 _move_grippers();
181 logger->log_debug(name(), "...targets processed");
182 }
183 break;
184
185 case TRAJEC_READY:
186 //logger->log_debug(name(), "Trajectories ready! Processing now.");
187 // update trajectory state
188 _lock_queues();
189 arms_.l.target->trajec_state = TRAJEC_EXECUTING;
190 arms_.r.target->trajec_state = TRAJEC_EXECUTING;
191 _unlock_queues();
192
193 // process trajectories only if it actually "exists"
194 if (!arms_.l.target->trajec->empty() && !arms_.r.target->trajec->empty()) {
195 // first let the openrave_thread show the trajectory in the viewer
196 arms_.l.arm->openrave_thread->plot_first();
197 arms_.r.arm->openrave_thread->plot_first();
198
199 // enable plotting of current positions
200 arms_.l.arm->openrave_thread->plot_current(true);
201 arms_.r.arm->openrave_thread->plot_current(true);
202
203 // then execute the trajectories
204 _exec_trajecs();
205 }
206
207 break;
208
209 default:
210 //logger->log_debug(name(), "Target is trajectory, but not ready yet!");
211 arms_.l.target.clear();
212 arms_.r.target.clear();
213 usleep(30e3);
214 break;
215 }
216}
217
218/** Check if arm is final.
219 * @see JacoGotoThread::final
220 * @return "true" if arm is not moving anymore, "false" otherwise
221 */
222bool
224{
225 // Check if any movement has startet (final_ would be false then)
226 final_mutex_->lock();
227 bool final = final_;
228 final_mutex_->unlock();
229 if (!final) {
230 // There was some movement initiated. Check if it has finished
231 _check_final();
232 final_mutex_->lock();
233 final = final_;
234 final_mutex_->unlock();
235 }
236
237 if (!final)
238 return false; // still moving
239
240 // arm is not moving right now. Check if all targets have been processed
241 _lock_queues();
242 final = arms_.l.arm->target_queue->empty() && arms_.r.arm->target_queue->empty();
243 _unlock_queues();
244
245 return final;
246}
247
248/** Stops the current movement.
249 * This also stops any currently enqueued motion.
250 */
251void
253{
254 arms_.l.arm->goto_thread->stop();
255 arms_.r.arm->goto_thread->stop();
256
257 arms_.l.target.clear();
258 arms_.r.target.clear();
259
260 final_mutex_->lock();
261 final_ = true;
262 final_mutex_->unlock();
263}
264
265/** Moves only the gripper of both arms
266 * @param l_f1 value of 1st finger of left arm
267 * @param l_f2 value of 2nd finger of left arm
268 * @param l_f3 value of 3rd finger of left arm
269 * @param r_f1 value of 1st finger of right arm
270 * @param r_f2 value of 2nd finger of right arm
271 * @param r_f3 value of 3rd finger of right arm
272 */
273void
275 float l_f2,
276 float l_f3,
277 float r_f1,
278 float r_f2,
279 float r_f3)
280{
281 RefPtr<jaco_target_t> target_l(new jaco_target_t());
282 RefPtr<jaco_target_t> target_r(new jaco_target_t());
283 target_l->type = TARGET_GRIPPER;
284 target_r->type = TARGET_GRIPPER;
285 target_l->trajec_state = TRAJEC_SKIP;
286 target_r->trajec_state = TRAJEC_SKIP;
287 target_l->coord = true;
288 target_r->coord = true;
289
290 target_l->fingers.push_back(l_f1);
291 target_l->fingers.push_back(l_f2);
292 target_l->fingers.push_back(l_f3);
293 target_r->fingers.push_back(l_f1);
294 target_r->fingers.push_back(l_f2);
295 target_r->fingers.push_back(l_f3);
296
297 _lock_queues();
298 _enqueue_targets(target_l, target_r);
299 _unlock_queues();
300}
301
302/* PRIVATE METHODS */
303inline void
304JacoBimanualGotoThread::_lock_queues() const
305{
306 arms_.l.arm->target_mutex->lock();
307 arms_.r.arm->target_mutex->lock();
308}
309
310inline void
311JacoBimanualGotoThread::_unlock_queues() const
312{
313 arms_.l.arm->target_mutex->unlock();
314 arms_.r.arm->target_mutex->unlock();
315}
316
317inline void
318JacoBimanualGotoThread::_enqueue_targets(RefPtr<jaco_target_t> l, RefPtr<jaco_target_t> r)
319{
320 arms_.l.arm->target_queue->push_back(l);
321 arms_.r.arm->target_queue->push_back(r);
322}
323
324void
325JacoBimanualGotoThread::_move_grippers()
326{
327 final_mutex_->lock();
328 final_ = false;
329 final_mutex_->unlock();
330
331 for (unsigned int i = 0; i < 2; ++i) {
332 v_arms_[i]->finger_last[0] = v_arms_[i]->arm->iface->finger1();
333 v_arms_[i]->finger_last[1] = v_arms_[i]->arm->iface->finger2();
334 v_arms_[i]->finger_last[2] = v_arms_[i]->arm->iface->finger3();
335 v_arms_[i]->finger_last[3] = 0; // counter
336 }
337
338 // process new target
339 try {
340 // only fingers moving. use current joint values for that
341 // we do this here and not in "move_gripper()" because we enqueue values. This ensures
342 // that we move the gripper with the current joint values, not with the ones we had
343 // when the target was enqueued!
344 for (unsigned int i = 0; i < 2; ++i) {
345 v_arms_[i]->target->pos.clear(); // just in case; should be empty anyway
346 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(0));
347 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(1));
348 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(2));
349 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(3));
350 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(4));
351 v_arms_[i]->target->pos.push_back(v_arms_[i]->arm->iface->joints(5));
352 v_arms_[i]->target->type = TARGET_ANGULAR;
353 }
354
355 // just send the messages to the arm. nothing special here
356 arms_.l.arm->arm->goto_joints(arms_.l.target->pos, arms_.l.target->fingers);
357 arms_.r.arm->arm->goto_joints(arms_.r.target->pos, arms_.r.target->fingers);
358
359 } catch (Exception &e) {
360 logger->log_warn(name(), "Error sending commands to arm. Ex:%s", e.what_no_backtrace());
361 }
362}
363
364void
365JacoBimanualGotoThread::_exec_trajecs()
366{
367 final_mutex_->lock();
368 final_ = false;
369 final_mutex_->unlock();
370
371 for (unsigned int i = 0; i < 2; ++i) {
372 if (v_arms_[i]->target->fingers.empty()) {
373 // have no finger values. use current ones
374 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger1());
375 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger2());
376 v_arms_[i]->target->fingers.push_back(v_arms_[i]->arm->iface->finger3());
377 }
378 }
379
380 try {
381 // stop old movement, if there was any
382 arms_.l.arm->arm->stop();
383 arms_.r.arm->arm->stop();
384
385 // execute the trajectories
386 logger->log_debug(name(), "exec traj: send traj commands...");
387
388 // find out which arm has the shorter trajectory
389 unsigned int first = 0;
390 unsigned int second = 1;
391 if (v_arms_[1]->target->trajec->size() < v_arms_[0]->target->trajec->size()) {
392 first = 1;
393 second = 0;
394 }
395 JacoArm * arm_first = v_arms_[first]->arm->arm;
396 JacoArm * arm_second = v_arms_[second]->arm->arm;
397 jaco_trajec_t *trajec_first = *(v_arms_[first]->target->trajec);
398 jaco_trajec_t *trajec_second = *(v_arms_[second]->target->trajec);
399 unsigned int size_first = trajec_first->size();
400 unsigned int size_second = trajec_second->size();
401
402 unsigned int it = 1; // iterator for the trajectories
403
404 // send current position as initial trajec-point to arms
405 for (unsigned int i = 0; i < 2; ++i) {
407 cur.push_back(v_arms_[i]->arm->iface->joints(0));
408 cur.push_back(v_arms_[i]->arm->iface->joints(1));
409 cur.push_back(v_arms_[i]->arm->iface->joints(2));
410 cur.push_back(v_arms_[i]->arm->iface->joints(3));
411 cur.push_back(v_arms_[i]->arm->iface->joints(4));
412 cur.push_back(v_arms_[i]->arm->iface->joints(5));
413 v_arms_[i]->arm->arm->goto_joints(cur, v_arms_[i]->target->fingers, /*followup=*/false);
414 }
415
416 // send rest of trajectory as followup trajectory points.
417 // Make sure to send the trajectory points alternatingly to the arm's
418 // internal FIFO trajectory queue.
419 while (it < size_first) {
420 arm_first->goto_joints(trajec_first->at(it),
421 v_arms_[first]->target->fingers,
422 /*followup=*/true);
423 arm_second->goto_joints(trajec_second->at(it),
424 v_arms_[second]->target->fingers,
425 /*followup=*/true);
426 ++it;
427 }
428
429 // continue sending the rest of the longer trajectory
430 while (it < size_second) {
431 arm_second->goto_joints(trajec_second->at(it),
432 v_arms_[second]->target->fingers,
433 /*followup=*/true);
434 ++it;
435 }
436
437 logger->log_debug(name(), "exec traj: ... DONE");
438
439 } catch (Exception &e) {
440 logger->log_warn(name(), "Error executing trajectory. Ex:%s", e.what_no_backtrace());
441 }
442}
443
444void
445JacoBimanualGotoThread::_check_final()
446{
447 bool final = true;
448
449 //logger->log_debug(name(), "check final");
450 for (unsigned int i = 0; i < 2; ++i) {
451 switch (v_arms_[i]->target->type) {
452 case TARGET_ANGULAR:
453 //logger->log_debug(name(), "check[%u] final for TARGET ANGULAR", i);
454 for (unsigned int j = 0; j < 6; ++j) {
455 final &= angle_distance(deg2rad(v_arms_[i]->target->pos.at(j)),
456 deg2rad(v_arms_[i]->arm->iface->joints(j)))
457 < 0.05;
458 }
459 break;
460
461 case TARGET_GRIPPER:
462 //logger->log_debug(name(), "check[%u] final for TARGET GRIPPER", i);
463 final &= v_arms_[i]->arm->arm->final();
464 break;
465
466 default:
467 //logger->log_debug(name(), "check[%u] final for UNKNOWN!!!", i);
468 final &= false;
469 break;
470 }
471
472 //logger->log_debug(name(), "check[%u] final (joints): %u", i, final);
473 }
474
475 if (final) {
476 // check fingeres
477 for (unsigned int i = 0; i < 2; ++i) {
478 //logger->log_debug(name(), "check[%u] fingers for final", i);
479
480 if (v_arms_[i]->finger_last[0] == v_arms_[i]->arm->iface->finger1()
481 && v_arms_[i]->finger_last[1] == v_arms_[i]->arm->iface->finger2()
482 && v_arms_[i]->finger_last[2] == v_arms_[i]->arm->iface->finger3()) {
483 v_arms_[i]->finger_last[3] += 1;
484 } else {
485 v_arms_[i]->finger_last[0] = v_arms_[i]->arm->iface->finger1();
486 v_arms_[i]->finger_last[1] = v_arms_[i]->arm->iface->finger2();
487 v_arms_[i]->finger_last[2] = v_arms_[i]->arm->iface->finger3();
488 v_arms_[i]->finger_last[3] = 0; // counter
489 }
490 final &= v_arms_[i]->finger_last[3] > 10;
491 //logger->log_debug(name(), "check[%u] final (all): %u", i, final);
492 }
493 }
494
495 final_mutex_->lock();
496 final_ = final;
497 final_mutex_->unlock();
498}
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual void stop()
Stops the current movement.
virtual void init()
Initialize the thread.
virtual void loop()
The main loop of this thread.
virtual bool final()
Check if arm is final.
virtual ~JacoBimanualGotoThread()
Destructor.
virtual void move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3)
Moves only the gripper of both arms.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Abstract class for a Kinova Jaco Arm that we want to control.
Definition: arm.h:36
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:123
@ TARGET_GRIPPER
only gripper movement.
Definition: types.h:61
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:46
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Definition: types.h:48
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
Definition: types.h:71
@ TRAJEC_IK_ERROR
planner could not find IK solution for target
Definition: types.h:73
@ TRAJEC_PLANNING_ERROR
planner could not plan a collision-free trajectory.
Definition: types.h:74
@ TRAJEC_EXECUTING
trajectory is being executed.
Definition: types.h:72
@ TRAJEC_SKIP
skip trajectory planning for this target.
Definition: types.h:68
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:115
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:114
Jaco target struct, holding information on a target.
Definition: types.h:79