23 #include "bimanual_goto_thread.h"
26 #include "goto_thread.h"
27 #include "openrave_thread.h"
29 #include <core/threading/mutex.h>
30 #include <interfaces/JacoBimanualInterface.h>
31 #include <interfaces/JacoInterface.h>
32 #include <utils/math/angle.h>
49 :
Thread(
"JacoBimanualGotoThread",
Thread::OPMODE_CONTINUOUS)
64 arms_.l.arm = dual_arms_->
left;
65 arms_.r.arm = dual_arms_->
right;
67 final_mutex_ =
new Mutex();
68 v_arms_[0] = &(arms_.l);
69 v_arms_[1] = &(arms_.r);
97 if (arms_.l.arm == NULL || arms_.r.arm == NULL || !
final) {
103 if (arms_.l.target && arms_.r.target) {
104 arms_.l.target.clear();
105 arms_.r.target.clear();
110 arms_.l.arm->target_queue->pop_front();
111 arms_.r.arm->target_queue->pop_front();
117 if (!arms_.l.arm->target_queue->empty() && !arms_.r.arm->target_queue->empty()) {
119 arms_.l.target = arms_.l.arm->target_queue->front();
120 arms_.r.target = arms_.r.arm->target_queue->front();
124 if (!arms_.l.target || !arms_.r.target || !arms_.l.target->coord || !arms_.r.target->coord) {
127 arms_.l.target.clear();
128 arms_.r.target.clear();
133 if (arms_.l.target->type != arms_.r.target->type) {
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();
155 if (arms_.l.target->trajec_state != arms_.r.target->trajec_state) {
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();
166 switch (arms_.l.target->trajec_state) {
171 "No planning for these targets. Process, using current finger positions...");
175 "Unknown target type %i, cannot process without planning!",
176 arms_.l.target->type);
194 if (!arms_.l.target->trajec->empty() && !arms_.r.target->trajec->empty()) {
196 arms_.l.arm->openrave_thread->plot_first();
197 arms_.r.arm->openrave_thread->plot_first();
200 arms_.l.arm->openrave_thread->plot_current(
true);
201 arms_.r.arm->openrave_thread->plot_current(
true);
211 arms_.l.target.clear();
212 arms_.r.target.clear();
226 final_mutex_->
lock();
232 final_mutex_->
lock();
242 final = arms_.l.arm->target_queue->empty() && arms_.r.arm->target_queue->empty();
254 arms_.l.arm->goto_thread->stop();
255 arms_.r.arm->goto_thread->stop();
257 arms_.l.target.clear();
258 arms_.r.target.clear();
260 final_mutex_->
lock();
287 target_l->
coord =
true;
288 target_r->
coord =
true;
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);
298 _enqueue_targets(target_l, target_r);
304 JacoBimanualGotoThread::_lock_queues()
const
306 arms_.l.arm->target_mutex->lock();
307 arms_.r.arm->target_mutex->lock();
311 JacoBimanualGotoThread::_unlock_queues()
const
313 arms_.l.arm->target_mutex->unlock();
314 arms_.r.arm->target_mutex->unlock();
320 arms_.l.arm->target_queue->push_back(l);
321 arms_.r.arm->target_queue->push_back(r);
325 JacoBimanualGotoThread::_move_grippers()
327 final_mutex_->
lock();
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;
344 for (
unsigned int i = 0; i < 2; ++i) {
345 v_arms_[i]->target->pos.clear();
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));
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);
365 JacoBimanualGotoThread::_exec_trajecs()
367 final_mutex_->
lock();
371 for (
unsigned int i = 0; i < 2; ++i) {
372 if (v_arms_[i]->target->fingers.empty()) {
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());
382 arms_.l.arm->arm->stop();
383 arms_.r.arm->arm->stop();
389 unsigned int first = 0;
390 unsigned int second = 1;
391 if (v_arms_[1]->target->trajec->size() < v_arms_[0]->target->trajec->size()) {
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();
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,
false);
419 while (it < size_first) {
421 v_arms_[first]->target->fingers,
424 v_arms_[second]->target->fingers,
430 while (it < size_second) {
432 v_arms_[second]->target->fingers,
445 JacoBimanualGotoThread::_check_final()
450 for (
unsigned int i = 0; i < 2; ++i) {
451 switch (v_arms_[i]->target->type) {
454 for (
unsigned int j = 0; j < 6; ++j) {
456 deg2rad(v_arms_[i]->arm->iface->joints(j)))
463 final &= v_arms_[i]->arm->arm->final();
477 for (
unsigned int i = 0; i < 2; ++i) {
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;
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;
490 final &= v_arms_[i]->finger_last[3] > 10;
495 final_mutex_->
lock();