Fawkes API Fawkes Development Version
goto_thread.cpp
1
2/***************************************************************************
3 * goto_thread.cpp - Kinova Jaco plugin movement thread
4 *
5 * Created: Thu Jun 20 15:04:20 2013
6 * Copyright 2013 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 "goto_thread.h"
24
25#include "arm.h"
26#include "openrave_thread.h"
27
28#include <core/threading/mutex.h>
29#include <interfaces/JacoInterface.h>
30#include <utils/math/angle.h>
31
32#include <unistd.h>
33
34using namespace fawkes;
35
36/** @class JacoGotoThread "goto_thread.h"
37 * Jaco Arm movement thread.
38 * This thread handles the movement of the arm.
39 *
40 * @author Bahram Maleki-Fard
41 */
42
43/** Constructor.
44 * @param name thread name
45 * @param arm pointer to jaco_arm_t struct, to be used in this thread
46 */
48: Thread(name, Thread::OPMODE_CONTINUOUS)
49{
50 arm_ = arm;
51 final_mutex_ = NULL;
52
53 final_ = true;
54
55 wait_status_check_ = 0; //wait loops to check for jaco_retract_mode_t again
56}
57
58/** Destructor. */
60{
61}
62
63/** Initialize. */
64void
66{
67 final_mutex_ = new Mutex();
68}
69
70/** Finalize. */
71void
73{
74 delete final_mutex_;
75 final_mutex_ = NULL;
76 arm_ = NULL;
77}
78
79/** Check if arm is final.
80 * Checks if the movements started by this thread have finished, and
81 * if the target_queue has been fully processed. Otherwise the arm
82 * is probably still moving and therefore not final.
83 *
84 * @return "true" if arm is not moving anymore, "false" otherwise
85 */
86bool
88{
89 // Check if any movement has startet (final_ would be false then)
90 final_mutex_->lock();
91 bool final = final_;
92 final_mutex_->unlock();
93 if (!final) {
94 // There was some movement initiated. Check if it has finished
95 _check_final();
96 final_mutex_->lock();
97 final = final_;
98 final_mutex_->unlock();
99 }
100
101 if (!final)
102 return false; // still moving
103
104 // arm is not moving right now. Check if all targets have been processed
105 arm_->target_mutex->lock();
106 final = arm_->target_queue->empty();
107 arm_->target_mutex->unlock();
108
109 if (final)
110 arm_->openrave_thread->plot_current(false);
111
112 return final;
113}
114
115/** Set new target, given cartesian coordinates.
116 * This target is added to the queue, skipping trajectory planning.
117 * CAUTION: This also means: no collision avoidance!
118 *
119 * @param x x-coordinate of target position
120 * @param y y-coordinate of target position
121 * @param z z-coordinate of target position
122 * @param e1 1st euler rotation of target orientation
123 * @param e2 2nd euler rotation of target orientation
124 * @param e3 3rd euler rotation of target orientation
125 * @param f1 value of 1st finger
126 * @param f2 value of 2nd finger
127 * @param f3 value of 3rd finger
128 */
129void
131 float y,
132 float z,
133 float e1,
134 float e2,
135 float e3,
136 float f1,
137 float f2,
138 float f3)
139{
141 target->type = TARGET_CARTESIAN;
142 target->trajec_state = TRAJEC_SKIP;
143 target->coord = false;
144
145 target->pos.push_back(x);
146 target->pos.push_back(y);
147 target->pos.push_back(z);
148 target->pos.push_back(e1);
149 target->pos.push_back(e2);
150 target->pos.push_back(e3);
151
152 if (f1 > 0.f && f2 > 0.f && f3 > 0.f) {
153 target->fingers.push_back(f1);
154 target->fingers.push_back(f2);
155 target->fingers.push_back(f3);
156 }
157 arm_->target_mutex->lock();
158 arm_->target_queue->push_back(target);
159 arm_->target_mutex->unlock();
160}
161
162/** Set new target, given joint positions
163 * This target is added to the queue, skipping trajectory planning.
164 * CAUTION: This also means: no collision avoidance!
165 *
166 * @param j1 angular position of 1st joint (in degree)
167 * @param j2 angular position of 2nd joint (in degree)
168 * @param j3 angular position of 3rd joint (in degree)
169 * @param j4 angular position of 4th joint (in degree)
170 * @param j5 angular position of 5th joint (in degree)
171 * @param j6 angular position of 6th joint (in degree)
172 * @param f1 value of 1st finger
173 * @param f2 value of 2nd finger
174 * @param f3 value of 3rd finger
175 */
176void
178 float j2,
179 float j3,
180 float j4,
181 float j5,
182 float j6,
183 float f1,
184 float f2,
185 float f3)
186{
188 target->type = TARGET_ANGULAR;
189 target->trajec_state = TRAJEC_SKIP;
190 target->coord = false;
191
192 target->pos.push_back(j1);
193 target->pos.push_back(j2);
194 target->pos.push_back(j3);
195 target->pos.push_back(j4);
196 target->pos.push_back(j5);
197 target->pos.push_back(j6);
198
199 if (f1 > 0.f && f2 > 0.f && f3 > 0.f) {
200 target->fingers.push_back(f1);
201 target->fingers.push_back(f2);
202 target->fingers.push_back(f3);
203 }
204 arm_->target_mutex->lock();
205 arm_->target_queue->push_back(target);
206 arm_->target_mutex->unlock();
207}
208
209/** Moves the arm to the "READY" position.
210 * This target is added to the queue, skipping trajectory planning.
211 * CAUTION: This also means: no collision avoidance!
212 */
213void
215{
217 target->type = TARGET_READY;
218 arm_->target_mutex->lock();
219 arm_->target_queue->push_back(target);
220 arm_->target_mutex->unlock();
221}
222
223/** Moves the arm to the "RETRACT" position.
224 * This target is added to the queue, skipping trajectory planning.
225 * CAUTION: This also means: no collision avoidance!
226 */
227void
229{
231 target->type = TARGET_RETRACT;
232 arm_->target_mutex->lock();
233 arm_->target_queue->push_back(target);
234 arm_->target_mutex->unlock();
235}
236
237/** Moves only the gripper.
238 * @param f1 value of 1st finger
239 * @param f2 value of 2nd finger
240 * @param f3 value of 3rd finger
241 */
242void
243JacoGotoThread::move_gripper(float f1, float f2, float f3)
244{
246 target->type = TARGET_GRIPPER;
247
248 target->fingers.push_back(f1);
249 target->fingers.push_back(f2);
250 target->fingers.push_back(f3);
251
252 arm_->target_mutex->lock();
253 arm_->target_queue->push_back(target);
254 arm_->target_mutex->unlock();
255}
256
257/** Stops the current movement.
258 * This also stops any currently enqueued motion.
259 */
260void
262{
263 try {
264 arm_->arm->stop();
265
266 arm_->target_mutex->lock();
267 arm_->target_queue->clear();
268 arm_->target_mutex->unlock();
269
270 target_.clear();
271
272 final_mutex_->lock();
273 final_ = true;
274 final_mutex_->unlock();
275
276 } catch (Exception &e) {
277 logger->log_warn(name(), "Error sending stop command to arm. Ex:%s", e.what());
278 }
279}
280
281/** The main loop of this thread.
282 * It gets the first target in the target_queue and checks if it is ready
283 * to be processed. The target is removed from the queue if the movement is
284 * "final" (see final()), which is when this method starts processing
285 * the queue again.
286 */
287void
289{
290 final_mutex_->lock();
291 bool final = final_;
292 final_mutex_->unlock();
293
294 if (arm_ == NULL || arm_->arm == NULL || !final) {
295 usleep(30e3);
296 return;
297 }
298
299 // Current target has been processed. Unref, if still refed
300 if (target_) {
301 target_.clear();
302 // trajectory has been processed. remove that target from queue.
303 // This will automatically delete the trajectory as well as soon
304 // as we leave this block (thanks to refptr)
305 arm_->target_mutex->lock();
306 arm_->target_queue->pop_front();
307 arm_->target_mutex->unlock();
308 }
309
310 // Check for new targets
311 arm_->target_mutex->lock();
312 if (!arm_->target_queue->empty()) {
313 // get RefPtr to first target in queue
314 target_ = arm_->target_queue->front();
315 }
316 arm_->target_mutex->unlock();
317 if (!target_ || target_->coord) {
318 //no new target in queue, or target needs coordination of both arms,
319 // which is not what this thread does
320 target_.clear();
321 usleep(30e3);
322 return;
323 }
324
325 switch (target_->trajec_state) {
326 case TRAJEC_SKIP:
327 // "regular" target
329 name(), "No planning for this new target. Process, using current finger positions...");
330
331 if (target_->type != TARGET_GRIPPER) {
332 // arm moves! clear previously drawn trajectory plot
334
335 // also enable ploting current joint positions
336 arm_->openrave_thread->plot_current(true);
337 }
338 _goto_target();
339 logger->log_debug(name(), "...target processed");
340 break;
341
342 case TRAJEC_READY:
343 logger->log_debug(name(), "Trajectory ready! Processing now.");
344 // update trajectory state
345 arm_->target_mutex->lock();
346 target_->trajec_state = TRAJEC_EXECUTING;
347 arm_->target_mutex->unlock();
348
349 // process trajectory only if it actually "exists"
350 if (!target_->trajec->empty()) {
351 // first let the openrave_thread show the trajectory in the viewer
353
354 // enable plotting current positions
355 arm_->openrave_thread->plot_current(true);
356
357 // then execute the trajectory
358 _exec_trajec(*(target_->trajec));
359 }
360 break;
361
363 logger->log_debug(name(), "Trajectory could not be planned. Abort!");
364 // stop the current and remaining queue, with appropriate error_code. This also sets "final" to true.
365 stop();
366 arm_->iface->set_error_code(JacoInterface::ERROR_PLANNING);
367 break;
368
369 default:
370 //logger->log_debug("Target is trajectory, but not ready yet!");
371 target_.clear();
372 usleep(30e3);
373 break;
374 }
375}
376
377/* PRIVATE METHODS */
378void
379JacoGotoThread::_check_final()
380{
381 bool check_fingers = false;
382 bool final = true;
383
384 //logger->log_debug(name(), "check final");
385 switch (target_->type) {
386 case TARGET_READY:
387 case TARGET_RETRACT:
388 if (wait_status_check_ == 0) {
389 //logger->log_debug(name(), "check final for TARGET_MODE");
390 final_mutex_->lock();
391 final_ = arm_->arm->final();
392 final_mutex_->unlock();
393 } else if (wait_status_check_ >= 10) {
394 wait_status_check_ = 0;
395 } else {
396 ++wait_status_check_;
397 }
398 break;
399
400 case TARGET_ANGULAR:
401 //logger->log_debug(name(), "check final for TARGET ANGULAR");
402 //final = arm_->arm->final();
403 for (unsigned int i = 0; i < 6; ++i) {
404 final &=
405 (angle_distance(deg2rad(target_->pos.at(i)), deg2rad(arm_->iface->joints(i))) < 0.05);
406 }
407 final_mutex_->lock();
408 final_ = final;
409 final_mutex_->unlock();
410 check_fingers = true;
411 break;
412
413 case TARGET_GRIPPER:
414 //logger->log_debug(name(), "check final for TARGET GRIPPER");
415 final_mutex_->lock();
416 final_ = arm_->arm->final();
417 final_mutex_->unlock();
418 check_fingers = true;
419 break;
420
421 default: //TARGET_CARTESIAN
422 //logger->log_debug(name(), "check final for TARGET CARTESIAN");
423 //final = arm_->arm->final();
424 final &= (angle_distance(target_->pos.at(0), arm_->iface->x()) < 0.01);
425 final &= (angle_distance(target_->pos.at(1), arm_->iface->y()) < 0.01);
426 final &= (angle_distance(target_->pos.at(2), arm_->iface->z()) < 0.01);
427 final &= (angle_distance(target_->pos.at(3), arm_->iface->euler1()) < 0.1);
428 final &= (angle_distance(target_->pos.at(4), arm_->iface->euler2()) < 0.1);
429 final &= (angle_distance(target_->pos.at(5), arm_->iface->euler3()) < 0.1);
430 final_mutex_->lock();
431 final_ = final;
432 final_mutex_->unlock();
433
434 check_fingers = true;
435 break;
436 }
437
438 final_mutex_->lock();
439 final = final_;
440 final_mutex_->unlock();
441 //logger->log_debug(name(), "check final: %u", final);
442
443 if (check_fingers && final) {
444 //logger->log_debug(name(), "check fingeres for final");
445
446 // also check fingeres
447 if (finger_last_[0] == arm_->iface->finger1() && finger_last_[1] == arm_->iface->finger2()
448 && finger_last_[2] == arm_->iface->finger3()) {
449 finger_last_[3] += 1;
450 } else {
451 finger_last_[0] = arm_->iface->finger1();
452 finger_last_[1] = arm_->iface->finger2();
453 finger_last_[2] = arm_->iface->finger3();
454 finger_last_[3] = 0; // counter
455 }
456 final_mutex_->lock();
457 final_ &= finger_last_[3] > 10;
458 final_mutex_->unlock();
459 }
460}
461
462void
463JacoGotoThread::_goto_target()
464{
465 finger_last_[0] = arm_->iface->finger1();
466 finger_last_[1] = arm_->iface->finger2();
467 finger_last_[2] = arm_->iface->finger3();
468 finger_last_[3] = 0; // counter
469
470 final_mutex_->lock();
471 final_ = false;
472 final_mutex_->unlock();
473
474 // process new target
475 try {
476 arm_->arm->stop(); // stop old movement, if there was any
477 //arm_->arm->start_api_ctrl();
478
479 if (target_->type == TARGET_GRIPPER) {
480 // only fingers moving. use current joint values for that
481 // we do this here and not in "move_gripper()" because we enqueue values. This ensures
482 // that we move the gripper with the current joint values, not with the ones we had
483 // when the target was enqueued!
484 target_->pos.clear(); // just in case; should be empty anyway
485 target_->pos.push_back(arm_->iface->joints(0));
486 target_->pos.push_back(arm_->iface->joints(1));
487 target_->pos.push_back(arm_->iface->joints(2));
488 target_->pos.push_back(arm_->iface->joints(3));
489 target_->pos.push_back(arm_->iface->joints(4));
490 target_->pos.push_back(arm_->iface->joints(5));
491 target_->type = TARGET_ANGULAR;
492 }
493
494 switch (target_->type) {
495 case TARGET_ANGULAR:
496 logger->log_debug(name(), "target_type: TARGET_ANGULAR");
497 if (target_->fingers.empty()) {
498 // have no finger values. use current ones
499 target_->fingers.push_back(arm_->iface->finger1());
500 target_->fingers.push_back(arm_->iface->finger2());
501 target_->fingers.push_back(arm_->iface->finger3());
502 }
503 arm_->arm->goto_joints(target_->pos, target_->fingers);
504 break;
505
506 case TARGET_READY:
507 logger->log_debug(name(), "loop: target_type: TARGET_READY");
508 wait_status_check_ = 0;
509 arm_->arm->goto_ready();
510 break;
511
512 case TARGET_RETRACT:
513 logger->log_debug(name(), "target_type: TARGET_RETRACT");
514 wait_status_check_ = 0;
515 arm_->arm->goto_retract();
516 break;
517
518 default: //TARGET_CARTESIAN
519 logger->log_debug(name(), "target_type: TARGET_CARTESIAN");
520 if (target_->fingers.empty()) {
521 // have no finger values. use current ones
522 target_->fingers.push_back(arm_->iface->finger1());
523 target_->fingers.push_back(arm_->iface->finger2());
524 target_->fingers.push_back(arm_->iface->finger3());
525 }
526 arm_->arm->goto_coords(target_->pos, target_->fingers);
527 break;
528 }
529
530 } catch (Exception &e) {
531 logger->log_warn(name(), "Error sending command to arm. Ex:%s", e.what_no_backtrace());
532 }
533}
534
535void
536JacoGotoThread::_exec_trajec(jaco_trajec_t *trajec)
537{
538 final_mutex_->lock();
539 final_ = false;
540 final_mutex_->unlock();
541
542 if (target_->fingers.empty()) {
543 // have no finger values. use current ones
544 target_->fingers.push_back(arm_->iface->finger1());
545 target_->fingers.push_back(arm_->iface->finger2());
546 target_->fingers.push_back(arm_->iface->finger3());
547 }
548
549 try {
550 // stop old movement
551 arm_->arm->stop();
552
553 // execute the trajectory
554 logger->log_debug(name(), "exec traj: send traj commands...");
555 arm_->arm->goto_trajec(trajec, target_->fingers);
556 logger->log_debug(name(), "exec traj: ... DONE");
557
558 } catch (Exception &e) {
559 logger->log_warn(name(), "Error executing trajectory. Ex:%s", e.what_no_backtrace());
560 }
561}
virtual void loop()
The main loop of this thread.
virtual bool final()
Check if arm is final.
Definition: goto_thread.cpp:87
virtual void finalize()
Finalize.
Definition: goto_thread.cpp:72
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual void set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given joint positions This target is added to the queue, skipping trajectory planning...
JacoGotoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: goto_thread.cpp:47
virtual void init()
Initialize.
Definition: goto_thread.cpp:65
virtual void set_target(float x, float y, float z, float e1, float e2, float e3, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given cartesian coordinates.
virtual void stop()
Stops the current movement.
virtual void pos_ready()
Moves the arm to the "READY" position.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual ~JacoGotoThread()
Destructor.
Definition: goto_thread.cpp:59
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
virtual void stop()=0
Stop the current movement.
virtual void goto_retract()=0
Move the arm to RETRACT position.
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)=0
Move the arm to given configuration.
virtual bool final()=0
Check if movement is final.
virtual void goto_ready()=0
Move the arm to READY position.
virtual void goto_trajec(std::vector< std::vector< float > > *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
float x() const
Get x value.
float euler3() const
Get euler3 value.
float finger1() const
Get finger1 value.
float * joints() const
Get joints value.
float euler2() const
Get euler2 value.
float z() const
Get z value.
float euler1() const
Get euler1 value.
float y() const
Get y 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
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:447
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_READY
target is the READY position of the Jaco arm.
Definition: types.h:62
@ TARGET_GRIPPER
only gripper movement.
Definition: types.h:61
@ TARGET_CARTESIAN
target with cartesian coordinates.
Definition: types.h:59
@ TARGET_RETRACT
target is the RETRACT position of the Jaco arm.
Definition: types.h:63
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
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_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 one arm.
Definition: types.h:93
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
Jaco target struct, holding information on a target.
Definition: types.h:79