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 }
JacoBimanualGotoThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual void stop()
Stops the current movement.
virtual void init()
Initialize the thread.
virtual void loop()
The main loop of this thread.
virtual bool final()
Check if arm is final.
virtual ~JacoBimanualGotoThread()
Destructor.
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 finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Abstract class for a Kinova Jaco Arm that we want to control.
Definition: arm.h:36
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)=0
Move the arm to given configuration.
void set_error_code(const uint32_t new_error_code)
Set error_code 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
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_GRIPPER
only gripper movement.
Definition: types.h:61
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:43
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_IK_ERROR
planner could not find IK solution for target
Definition: types.h:73
@ 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 a dual-arm setup.
Definition: types.h:113
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:115
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:114
Jaco target struct, holding information on a target.
Definition: types.h:79