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 
34 using 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. */
64 void
66 {
67  final_mutex_ = new Mutex();
68 }
69 
70 /** Finalize. */
71 void
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  */
86 bool
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  */
129 void
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 {
140  RefPtr<jaco_target_t> target(new jaco_target_t());
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  */
176 void
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 {
187  RefPtr<jaco_target_t> target(new jaco_target_t());
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  */
213 void
215 {
216  RefPtr<jaco_target_t> target(new jaco_target_t());
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  */
227 void
229 {
230  RefPtr<jaco_target_t> target(new jaco_target_t());
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  */
242 void
243 JacoGotoThread::move_gripper(float f1, float f2, float f3)
244 {
245  RefPtr<jaco_target_t> target(new jaco_target_t());
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  */
260 void
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  */
287 void
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
328  logger->log_debug(
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
333  arm_->openrave_thread->plot_first();
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
352  arm_->openrave_thread->plot_first();
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 */
378 void
379 JacoGotoThread::_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 
462 void
463 JacoGotoThread::_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 
535 void
536 JacoGotoThread::_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 }
trajectory has been planned and is ready for execution.
Definition: types.h:71
float z() const
Get z value.
Jaco struct containing all components required for one arm.
Definition: types.h:92
JacoGotoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: goto_thread.cpp:47
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
virtual void goto_retract()=0
Move the arm to RETRACT position.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
virtual void init()
Initialize.
Definition: goto_thread.cpp:65
float finger2() const
Get finger2 value.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
Thread class encapsulation of pthreads.
Definition: thread.h:45
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.
trajectory is being executed.
Definition: types.h:72
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Definition: types.h:48
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
jaco_target_type_t type
target type.
Definition: types.h:80
planner could not plan a collision-free trajectory.
Definition: types.h:74
jaco_trajec_point_t pos
target position (interpreted depending on target type).
Definition: types.h:81
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:84
only gripper movement.
Definition: types.h:61
float euler3() const
Get euler3 value.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
virtual ~JacoGotoThread()
Destructor.
Definition: goto_thread.cpp:59
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
virtual void pos_ready()
Moves the arm to the "READY" position.
float euler2() const
Get euler2 value.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:445
float finger1() const
Get finger1 value.
virtual void stop()
Stops the current movement.
float x() const
Get x value.
float * joints() const
Get joints value.
Base class for exceptions in Fawkes.
Definition: exception.h:35
virtual void goto_trajec(std::vector< std::vector< float >> *trajec, std::vector< float > &fingers)=0
Move the arm along the given trajectory.
target with cartesian coordinates.
Definition: types.h:59
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
virtual void finalize()
Finalize.
Definition: goto_thread.cpp:72
virtual bool final()
Check if arm is final.
Definition: goto_thread.cpp:87
Jaco target struct, holding information on a target.
Definition: types.h:78
target with angular coordinates.
Definition: types.h:60
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void goto_ready()=0
Move the arm to READY position.
float euler1() const
Get euler1 value.
bool coord
this target needs to be coordinated with targets of other arms.
Definition: types.h:85
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)=0
Move the arm to given configuration.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
skip trajectory planning for this target.
Definition: types.h:68
virtual void loop()
The main loop of this thread.
virtual bool final()=0
Check if movement is final.
void lock()
Lock this mutex.
Definition: mutex.cpp:87
target is the RETRACT position of the Jaco arm.
Definition: types.h:63
virtual void stop()=0
Stop the current movement.
Mutex mutual exclusion lock.
Definition: mutex.h:32
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float finger3() const
Get finger3 value.
target is the READY position of the Jaco arm.
Definition: types.h:62
jaco_trajec_point_t fingers
target finger values.
Definition: types.h:82
virtual void plot_current(bool enable)
Enable/Disable plotting of the current arm position.
float y() const
Get y value.
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:123
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...