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 #include "goto_thread.h"
25 #include "openrave_thread.h"
26 #include "arm.h"
27 
28 #include <interfaces/JacoInterface.h>
29 #include <interfaces/JacoBimanualInterface.h>
30 #include <utils/math/angle.h>
31 #include <core/threading/mutex.h>
32 
33 #include <unistd.h>
34 
35 using namespace fawkes;
36 
37 /** @class JacoBimanualGotoThread "bimanual_goto_thread.h"
38  * Jaco Arm movement thread.
39  * This thread handles the movement of the arm.
40  *
41  * @author Bahram Maleki-Fard
42  */
43 
44 /** Constructor.
45  * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
46  */
48  : Thread("JacoBimanualGotoThread", Thread::OPMODE_CONTINUOUS)
49 {
50  __dual_arms = arms;
51  __final_mutex = NULL;
52  __final = true;
53 }
54 
55 
56 /** Destructor. */
58 {
59 }
60 
61 void
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 
72 void
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  */
90 void
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 ) {
134  logger->log_debug(name(), "target type mismatch, %i != %i", __arms.l.target->type, __arms.r.target->type);
135  __arms.l.target.clear();
136  __arms.r.target.clear();
137  usleep(30e3);
138  return;
139  }
140 
141  if( __arms.l.target->trajec_state == TRAJEC_IK_ERROR
142  || __arms.r.target->trajec_state == TRAJEC_IK_ERROR
143  || __arms.l.target->trajec_state == TRAJEC_PLANNING_ERROR
144  || __arms.r.target->trajec_state == TRAJEC_PLANNING_ERROR ) {
145  logger->log_warn(name(), "Trajectory could not be planned. Abort!");
146  // stop the current target and empty remaining queue, with appropriate error_code. This also sets "final" to true.
147  __dual_arms->iface->set_error_code( __arms.l.target->trajec_state );
148  stop();
149  return;
150  }
151 
152  if( __arms.l.target->trajec_state != __arms.r.target->trajec_state ) {
153  logger->log_debug(name(), "trajec state mismatch, %i != %i", __arms.l.target->trajec_state, __arms.r.target->trajec_state);
154  __arms.l.target.clear();
155  __arms.r.target.clear();
156  usleep(30e3);
157  return;
158  }
159 
160  switch( __arms.l.target->trajec_state ) {
161  case TRAJEC_SKIP:
162  // "regular" target. For now, we just process "GRIPPER", therefore do not
163  // change plotting
164  logger->log_debug(name(), "No planning for these targets. Process, using current finger positions...");
165 
166  if(__arms.l.target->type != TARGET_GRIPPER) {
167  logger->log_warn(name(), "Unknown target type %i, cannot process without planning!", __arms.l.target->type);
168  stop();
169  __dual_arms->iface->set_error_code( JacoInterface::ERROR_UNSPECIFIC );
170  } else {
171  _move_grippers();
172  logger->log_debug(name(), "...targets processed");
173  }
174  break;
175 
176  case TRAJEC_READY:
177  //logger->log_debug(name(), "Trajectories ready! Processing now.");
178  // update trajectory state
179  _lock_queues();
180  __arms.l.target->trajec_state = TRAJEC_EXECUTING;
181  __arms.r.target->trajec_state = TRAJEC_EXECUTING;
182  _unlock_queues();
183 
184  // process trajectories only if it actually "exists"
185  if( !__arms.l.target->trajec->empty() && !__arms.r.target->trajec->empty() ) {
186  // first let the openrave_thread show the trajectory in the viewer
187  __arms.l.arm->openrave_thread->plot_first();
188  __arms.r.arm->openrave_thread->plot_first();
189 
190  // enable plotting of current positions
191  __arms.l.arm->openrave_thread->plot_current(true);
192  __arms.r.arm->openrave_thread->plot_current(true);
193 
194  // then execute the trajectories
195  _exec_trajecs();
196  }
197 
198  break;
199 
200  default:
201  //logger->log_debug(name(), "Target is trajectory, but not ready yet!");
202  __arms.l.target.clear();
203  __arms.r.target.clear();
204  usleep(30e3);
205  break;
206  }
207 
208 }
209 
210 
211 /** Check if arm is final.
212  * @see JacoGotoThread::final
213  * @return "true" if arm is not moving anymore, "false" otherwise
214  */
215 bool
217 {
218  // Check if any movement has startet (__final would be false then)
219  __final_mutex->lock();
220  bool final = __final;
221  __final_mutex->unlock();
222  if( !final ) {
223  // There was some movement initiated. Check if it has finished
224  _check_final();
225  __final_mutex->lock();
226  final = __final;
227  __final_mutex->unlock();
228  }
229 
230  if( !final )
231  return false; // still moving
232 
233  // arm is not moving right now. Check if all targets have been processed
234  _lock_queues();
235  final = __arms.l.arm->target_queue->empty() && __arms.r.arm->target_queue->empty();
236  _unlock_queues();
237 
238  return final;
239 }
240 
241 /** Stops the current movement.
242  * This also stops any currently enqueued motion.
243  */
244 void
246 {
247  __arms.l.arm->goto_thread->stop();
248  __arms.r.arm->goto_thread->stop();
249 
250  __arms.l.target.clear();
251  __arms.r.target.clear();
252 
253  __final_mutex->lock();
254  __final = true;
255  __final_mutex->unlock();
256 }
257 
258 
259 /** Moves only the gripper of both arms
260  * @param l_f1 value of 1st finger of left arm
261  * @param l_f2 value of 2nd finger of left arm
262  * @param l_f3 value of 3rd finger of left arm
263  * @param r_f1 value of 1st finger of right arm
264  * @param r_f2 value of 2nd finger of right arm
265  * @param r_f3 value of 3rd finger of right arm
266  */
267 void
268 JacoBimanualGotoThread::move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3)
269 {
270  RefPtr<jaco_target_t> target_l(new jaco_target_t());
271  RefPtr<jaco_target_t> target_r(new jaco_target_t());
272  target_l->type = TARGET_GRIPPER;
273  target_r->type = TARGET_GRIPPER;
274  target_l->trajec_state = TRAJEC_SKIP;
275  target_r->trajec_state = TRAJEC_SKIP;
276  target_l->coord=true;
277  target_r->coord=true;
278 
279  target_l->fingers.push_back(l_f1);
280  target_l->fingers.push_back(l_f2);
281  target_l->fingers.push_back(l_f3);
282  target_r->fingers.push_back(l_f1);
283  target_r->fingers.push_back(l_f2);
284  target_r->fingers.push_back(l_f3);
285 
286  _lock_queues();
287  _enqueue_targets(target_l, target_r);
288  _unlock_queues();
289 }
290 
291 
292 /* PRIVATE METHODS */
293 inline void
294 JacoBimanualGotoThread::_lock_queues() const
295 {
296  __arms.l.arm->target_mutex->lock();
297  __arms.r.arm->target_mutex->lock();
298 }
299 
300 inline void
301 JacoBimanualGotoThread::_unlock_queues() const
302 {
303  __arms.l.arm->target_mutex->unlock();
304  __arms.r.arm->target_mutex->unlock();
305 }
306 
307 inline void
308 JacoBimanualGotoThread::_enqueue_targets(RefPtr<jaco_target_t> l, RefPtr<jaco_target_t> r)
309 {
310  __arms.l.arm->target_queue->push_back(l);
311  __arms.r.arm->target_queue->push_back(r);
312 }
313 
314 void
315 JacoBimanualGotoThread::_move_grippers()
316 {
317  __final_mutex->lock();
318  __final = false;
319  __final_mutex->unlock();
320 
321  for(unsigned int i=0; i<2; ++i) {
322  __v_arms[i]->finger_last[0] = __v_arms[i]->arm->iface->finger1();
323  __v_arms[i]->finger_last[1] = __v_arms[i]->arm->iface->finger2();
324  __v_arms[i]->finger_last[2] = __v_arms[i]->arm->iface->finger3();
325  __v_arms[i]->finger_last[3] = 0; // counter
326  }
327 
328  // process new target
329  try {
330  // only fingers moving. use current joint values for that
331  // we do this here and not in "move_gripper()" because we enqueue values. This ensures
332  // that we move the gripper with the current joint values, not with the ones we had
333  // when the target was enqueued!
334  for(unsigned int i=0; i<2; ++i) {
335  __v_arms[i]->target->pos.clear(); // just in case; should be empty anyway
336  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(0));
337  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(1));
338  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(2));
339  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(3));
340  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(4));
341  __v_arms[i]->target->pos.push_back(__v_arms[i]->arm->iface->joints(5));
342  __v_arms[i]->target->type = TARGET_ANGULAR;
343  }
344 
345  // just send the messages to the arm. nothing special here
346  __arms.l.arm->arm->goto_joints(__arms.l.target->pos, __arms.l.target->fingers);
347  __arms.r.arm->arm->goto_joints(__arms.r.target->pos, __arms.r.target->fingers);
348 
349  } catch( Exception &e ) {
350  logger->log_warn(name(), "Error sending commands to arm. Ex:%s", e.what_no_backtrace());
351  }
352 }
353 
354 void
355 JacoBimanualGotoThread::_exec_trajecs()
356 {
357  __final_mutex->lock();
358  __final = false;
359  __final_mutex->unlock();
360 
361  for(unsigned int i=0; i<2; ++i) {
362  if( __v_arms[i]->target->fingers.empty() ) {
363  // have no finger values. use current ones
364  __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger1());
365  __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger2());
366  __v_arms[i]->target->fingers.push_back(__v_arms[i]->arm->iface->finger3());
367  }
368  }
369 
370  try {
371  // stop old movement, if there was any
372  __arms.l.arm->arm->stop();
373  __arms.r.arm->arm->stop();
374 
375  // execute the trajectories
376  logger->log_debug(name(), "exec traj: send traj commands...");
377 
378  // find out which arm has the shorter trajectory
379  unsigned int first = 0;
380  unsigned int second = 1;
381  if( __v_arms[1]->target->trajec->size() < __v_arms[0]->target->trajec->size() ) {
382  first = 1;
383  second = 0;
384  }
385  JacoArm* arm_first = __v_arms[first]->arm->arm;
386  JacoArm* arm_second = __v_arms[second]->arm->arm;
387  jaco_trajec_t* trajec_first = *(__v_arms[first]->target->trajec);
388  jaco_trajec_t* trajec_second = *(__v_arms[second]->target->trajec);
389  unsigned int size_first = trajec_first->size();
390  unsigned int size_second = trajec_second->size();
391 
392  unsigned int it = 1; // iterator for the trajectories
393 
394  // send current position as initial trajec-point to arms
395  for(unsigned int i=0; i<2; ++i) {
397  cur.push_back( __v_arms[i]->arm->iface->joints(0) );
398  cur.push_back( __v_arms[i]->arm->iface->joints(1) );
399  cur.push_back( __v_arms[i]->arm->iface->joints(2) );
400  cur.push_back( __v_arms[i]->arm->iface->joints(3) );
401  cur.push_back( __v_arms[i]->arm->iface->joints(4) );
402  cur.push_back( __v_arms[i]->arm->iface->joints(5) );
403  __v_arms[i]->arm->arm->goto_joints(cur, __v_arms[i]->target->fingers, /*followup=*/false);
404  }
405 
406  // send rest of trajectory as followup trajectory points.
407  // Make sure to send the trajectory points alternatingly to the arm's
408  // internal FIFO trajectory queue.
409  while(it < size_first) {
410  arm_first->goto_joints(trajec_first->at(it), __v_arms[first]->target->fingers, /*followup=*/true);
411  arm_second->goto_joints(trajec_second->at(it), __v_arms[second]->target->fingers, /*followup=*/true);
412  ++it;
413  }
414 
415  // continue sending the rest of the longer trajectory
416  while(it < size_second) {
417  arm_second->goto_joints(trajec_second->at(it), __v_arms[second]->target->fingers, /*followup=*/true);
418  ++it;
419  }
420 
421  logger->log_debug(name(), "exec traj: ... DONE");
422 
423  } catch( Exception &e ) {
424  logger->log_warn(name(), "Error executing trajectory. Ex:%s", e.what_no_backtrace());
425  }
426 }
427 
428 void
429 JacoBimanualGotoThread::_check_final()
430 {
431  bool final = true;
432 
433  //logger->log_debug(name(), "check final");
434  for(unsigned int i=0; i<2; ++i) {
435  switch( __v_arms[i]->target->type ) {
436  case TARGET_ANGULAR:
437  //logger->log_debug(name(), "check[%u] final for TARGET ANGULAR", i);
438  for( unsigned int j=0; j<6; ++j ) {
439  final &= angle_distance(deg2rad(__v_arms[i]->target->pos.at(j)),
440  deg2rad(__v_arms[i]->arm->iface->joints(j))) < 0.05;
441  }
442  break;
443 
444  case TARGET_GRIPPER:
445  //logger->log_debug(name(), "check[%u] final for TARGET GRIPPER", i);
446  final &= __v_arms[i]->arm->arm->final();
447  break;
448 
449  default:
450  //logger->log_debug(name(), "check[%u] final for UNKNOWN!!!", i);
451  final &= false;
452  break;
453  }
454 
455  //logger->log_debug(name(), "check[%u] final (joints): %u", i, final);
456  }
457 
458 
459  if( final ) {
460  // check fingeres
461  for(unsigned int i=0; i<2; ++i) {
462  //logger->log_debug(name(), "check[%u] fingers for final", i);
463 
464  if( __v_arms[i]->finger_last[0] == __v_arms[i]->arm->iface->finger1() &&
465  __v_arms[i]->finger_last[1] == __v_arms[i]->arm->iface->finger2() &&
466  __v_arms[i]->finger_last[2] == __v_arms[i]->arm->iface->finger3() ) {
467  __v_arms[i]->finger_last[3] += 1;
468  } else {
469  __v_arms[i]->finger_last[0] = __v_arms[i]->arm->iface->finger1();
470  __v_arms[i]->finger_last[1] = __v_arms[i]->arm->iface->finger2();
471  __v_arms[i]->finger_last[2] = __v_arms[i]->arm->iface->finger3();
472  __v_arms[i]->finger_last[3] = 0; // counter
473  }
474  final &= __v_arms[i]->finger_last[3] > 10;
475  //logger->log_debug(name(), "check[%u] final (all): %u", i, final);
476  }
477  }
478 
479  __final_mutex->lock();
480  __final = final;
481  __final_mutex->unlock();
482 }
trajectory has been planned and is ready for execution.
Definition: types.h:74
virtual void finalize()
Finalize the thread.
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:115
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
virtual void loop()
The main loop of this thread.
virtual ~JacoBimanualGotoThread()
Destructor.
Thread class encapsulation of pthreads.
Definition: thread.h:42
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_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:113
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:114
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:86
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:46
only gripper movement.
Definition: types.h:64
virtual bool final()
Check if arm is final.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Jaco target struct, holding information on a target.
Definition: types.h:81
target with angular coordinates.
Definition: types.h:63
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void init()
Initialize the thread.
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 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 stop()
Stops the current movement.
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.
planner could not find IK solution for target
Definition: types.h:76
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
skip trajectory planning for this target.
Definition: types.h:71
void lock()
Lock this mutex.
Definition: mutex.cpp:89
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
Mutex mutual exclusion lock.
Definition: mutex.h:32
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
void set_error_code(const uint32_t new_error_code)
Set error_code value.
jaco_trajec_point_t fingers
target finger values.
Definition: types.h:84
Abstract class for a Kinova Jaco Arm that we want to control.
Definition: arm.h:38
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:128