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