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 
36 using 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 
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(),
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) {
156  logger->log_debug(name(),
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
170  logger->log_debug(name(),
171  "No planning for these targets. Process, using current finger positions...");
172 
173  if (arms_.l.target->type != TARGET_GRIPPER) {
174  logger->log_warn(name(),
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  */
222 bool
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  */
251 void
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  */
273 void
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 */
303 inline void
304 JacoBimanualGotoThread::_lock_queues() const
305 {
306  arms_.l.arm->target_mutex->lock();
307  arms_.r.arm->target_mutex->lock();
308 }
309 
310 inline void
311 JacoBimanualGotoThread::_unlock_queues() const
312 {
313  arms_.l.arm->target_mutex->unlock();
314  arms_.r.arm->target_mutex->unlock();
315 }
316 
317 inline void
318 JacoBimanualGotoThread::_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 
324 void
325 JacoBimanualGotoThread::_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 
364 void
365 JacoBimanualGotoThread::_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 
444 void
445 JacoBimanualGotoThread::_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 }
trajectory has been planned and is ready for execution.
Definition: types.h:71
virtual void finalize()
Finalize the thread.
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
virtual void loop()
The main loop of this thread.
virtual ~JacoBimanualGotoThread()
Destructor.
Thread class encapsulation of pthreads.
Definition: thread.h:45
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_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:114
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:115
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:84
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:43
only gripper movement.
Definition: types.h:61
virtual bool final()
Check if arm is final.
Base class for exceptions in Fawkes.
Definition: exception.h:35
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
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:663
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:85
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:73
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:68
void lock()
Lock this mutex.
Definition: mutex.cpp:87
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:36
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:82
Abstract class for a Kinova Jaco Arm that we want to control.
Definition: arm.h:35
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:123