Fawkes API Fawkes Development Version
bimanual_openrave_thread.cpp
1
2/***************************************************************************
3 * bimanual_openrave_thread.cpp - Jaco plugin OpenRAVE Thread for bimanual manipulation
4 *
5 * Created: Mon Jul 28 19:43:20 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_openrave_thread.h"
24
25#include "arm.h"
26#include "types.h"
27
28#include <core/threading/mutex.h>
29
30#include <algorithm>
31#include <cmath>
32#include <cstring>
33#include <stdio.h>
34#include <unistd.h>
35
36#ifdef HAVE_OPENRAVE
37# include <openrave/openrave.h>
38# include <plugins/openrave/environment.h>
39# include <plugins/openrave/manipulator.h>
40# include <plugins/openrave/manipulators/kinova_jaco.h>
41# include <plugins/openrave/robot.h>
42using namespace OpenRAVE;
43#endif
44
45using namespace fawkes;
46using namespace std;
47
48/** @class JacoBimanualOpenraveThread "bimanual_openrave_thread.h"
49 * Jaco Arm thread for dual-arm setup, integrating OpenRAVE
50 *
51 * @author Bahram Maleki-Fard
52 */
53
54/** Constructor.
55 * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
56 */
58: JacoOpenraveBaseThread("JacoBimanualOpenraveThread")
59{
60 arms_.left.arm = arms->left;
61 arms_.right.arm = arms->right;
62#ifdef HAVE_OPENRAVE
63 planner_env_.env = NULL;
64 planner_env_.robot = NULL;
65 planner_env_.manip = NULL;
66#endif
67
68 __constrained = false;
69}
70
71void
72JacoBimanualOpenraveThread::_init()
73{
74#ifdef HAVE_OPENRAVE
75 arms_.left.manipname = config->get_string("/hardware/jaco/openrave/manipname/dual_left");
76 arms_.right.manipname = config->get_string("/hardware/jaco/openrave/manipname/dual_right");
77#endif
78}
79
80void
81JacoBimanualOpenraveThread::_load_robot()
82{
83#ifdef HAVE_OPENRAVE
84 cfg_OR_robot_file_ = config->get_string("/hardware/jaco/openrave/robot_dual_file");
85
86 try {
87 //viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_, false);
88 // manually add robot; the automatic needs to be altered
89 viewer_env_.robot = new OpenRaveRobot(logger);
90 viewer_env_.robot->load(cfg_OR_robot_file_, viewer_env_.env);
91 viewer_env_.env->add_robot(viewer_env_.robot);
92 viewer_env_.robot->set_ready();
93 openrave->set_active_robot(viewer_env_.robot);
94 } catch (Exception &e) {
95 throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)",
96 cfg_OR_robot_file_.c_str(),
98 }
99
100 try {
101 viewer_env_.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
102 viewer_env_.manip->add_motor(0, 0);
103 viewer_env_.manip->add_motor(1, 1);
104 viewer_env_.manip->add_motor(2, 2);
105 viewer_env_.manip->add_motor(3, 3);
106 viewer_env_.manip->add_motor(4, 4);
107 viewer_env_.manip->add_motor(5, 5);
108
109 // Set manipulator and offsets.
110 openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
111
112 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
113
114 arms_.right.manip =
115 viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
116 if (cfg_OR_auto_load_ik_) {
117 logger->log_debug(name(), "load IK for right arm");
118 viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
119 }
120
121 arms_.left.manip =
122 viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
123 if (cfg_OR_auto_load_ik_) {
124 logger->log_debug(name(), "load IK for left arm");
125 viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
126 }
127
128 } catch (Exception &e) {
129 finalize();
130 throw;
131 }
132
133 // create cloned environment for planning
134 logger->log_debug(name(), "Clone environment for planning");
135 openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
136
137 if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
138 throw fawkes::Exception("Could not clone properly, received a NULL pointer");
139 }
140
141 // set name of env
142 planner_env_.env->set_name("Planner_Bimanual");
143
144 // set manips to those of planner env
145 arms_.right.manip =
146 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
147 arms_.left.manip =
148 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
149
150 // initial modules for dualmanipulation
151 _init_dualmanipulation();
152#endif
153}
154
155void
156JacoBimanualOpenraveThread::_init_dualmanipulation()
157{
158#ifdef HAVE_OPENRAVE
159 // load dualmanipulation module
160 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
161 mod_dualmanip_ = RaveCreateModule(planner_env_.env->get_env_ptr(), "dualmanipulation");
162 planner_env_.env->get_env_ptr()->Add(mod_dualmanip_,
163 true,
164 planner_env_.robot->get_robot_ptr()->GetName());
165
166 // load MultiManipIkSolver stuff
167 // Get all the links that are affecte by left/right manipulator
168 vector<int> arm_idx_l = arms_.left.manip->GetArmIndices();
169 vector<int> arm_idx_r = arms_.right.manip->GetArmIndices();
170 vector<int> grp_idx = arms_.left.manip->GetGripperIndices();
171 arm_idx_l.reserve(arm_idx_l.size() + grp_idx.size());
172 arm_idx_l.insert(arm_idx_l.end(), grp_idx.begin(), grp_idx.end());
173 grp_idx = arms_.right.manip->GetGripperIndices();
174 arm_idx_r.reserve(arm_idx_r.size() + grp_idx.size());
175 arm_idx_r.insert(arm_idx_r.end(), grp_idx.begin(), grp_idx.end());
176
177 RobotBasePtr robot = planner_env_.robot->get_robot_ptr();
178 vector<KinBody::LinkPtr> all_links = robot->GetLinks();
179 for (vector<KinBody::LinkPtr>::iterator link = all_links.begin(); link != all_links.end();
180 ++link) {
181 bool affected = false;
182 for (vector<int>::iterator idx = arm_idx_l.begin(); idx != arm_idx_l.end(); ++idx) {
183 if (robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),
184 (*link)->GetIndex())) {
185 // this link is affected by left manipulator
186 links_left_.insert(*link);
187 arm_idx_l.erase(idx); // no need to check this one again
188 affected = true;
189 break;
190 }
191 }
192
193 if (affected)
194 continue;
195
196 for (vector<int>::iterator idx = arm_idx_r.begin(); idx != arm_idx_r.end(); ++idx) {
197 if (robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),
198 (*link)->GetIndex())) {
199 // this link is affected by right manipulator
200 links_right_.insert(*link);
201 arm_idx_r.erase(idx); // no need to check this one again
202 break;
203 }
204 }
205 }
206#endif
207}
208
209void
211{
212 arms_.left.arm = NULL;
213 arms_.right.arm = NULL;
214#ifdef HAVE_OPENRAVE
215 openrave->set_active_robot(NULL);
216
217 planner_env_.env->get_env_ptr()->Remove(mod_dualmanip_);
218 planner_env_.robot = NULL;
219 planner_env_.manip = NULL;
220 planner_env_.env = NULL;
221#endif
222
224}
225
226void
228{
229#ifndef HAVE_OPENRAVE
230 usleep(30e3);
231#else
232 if (arms_.left.arm == NULL || arms_.right.arm == NULL) {
233 usleep(30e3);
234 return;
235 }
236
237 // get first target in queues
238 arms_.left.arm->target_mutex->lock();
239 arms_.right.arm->target_mutex->lock();
240 if (!arms_.left.arm->target_queue->empty() && !arms_.right.arm->target_queue->empty()) {
241 arms_.left.target = arms_.left.arm->target_queue->front();
242 arms_.right.target = arms_.right.arm->target_queue->front();
243 }
244 arms_.left.arm->target_mutex->unlock();
245 arms_.right.arm->target_mutex->unlock();
246
247 if (!arms_.left.target || !arms_.right.target || !arms_.left.target->coord
248 || !arms_.right.target->coord || arms_.left.target->trajec_state != TRAJEC_WAITING
249 || arms_.right.target->trajec_state != TRAJEC_WAITING) {
250 //no new target in queue, or target is not meant for coordinated bimanual manipulation
251 usleep(30e3);
252 return;
253 }
254
255 // copy environment first
256 _copy_env();
257
258 // get suiting IK solutions
259 vector<float> sol_l, sol_r;
260 bool solvable = _solve_multi_ik(sol_l, sol_r);
261 arms_.left.arm->target_mutex->lock();
262 arms_.right.arm->target_mutex->lock();
263 if (!solvable) {
264 arms_.left.target->trajec_state = TRAJEC_IK_ERROR;
265 arms_.right.target->trajec_state = TRAJEC_IK_ERROR;
266 arms_.left.arm->target_mutex->unlock();
267 arms_.right.arm->target_mutex->unlock();
268 usleep(30e3);
269 return;
270 } else {
271 arms_.left.target->type = TARGET_ANGULAR;
272 arms_.left.target->pos = sol_l;
273 arms_.right.target->type = TARGET_ANGULAR;
274 arms_.right.target->pos = sol_r;
275 arms_.left.arm->target_mutex->unlock();
276 arms_.right.arm->target_mutex->unlock();
277
278 // run path planner
279 _plan_path();
280 }
281
282#endif
283}
284
285/** Enable/Disable constrained planning.
286 * Enabling it will constrain the movement, which means it is tried to
287 * maintain the distance of the grippers to each other. This should be
288 * activated when moving an object with both hands, but disabled in
289 * situations when the arms do not need to hold the object simultaneously
290 * at all times.
291 *
292 * @param enable Enables/Disables the state.
293 */
294void
296{
297 __constrained = enable;
298}
299
300/** Add target for coordinated manipulation to the queue.
301 *
302 * This adds targets to the queues for both left and right arms. It sets
303 * the target->coord flag to "true", which means it will not be processed
304 * by the threads for uncoordinated manipulation!
305 *
306 * @param l_x x-coordinate of target position of left arm
307 * @param l_y y-coordinate of target position of left arm
308 * @param l_z z-coordinate of target position of left arm
309 * @param l_e1 1st euler rotation of target orientation of left arm
310 * @param l_e2 2nd euler rotation of target orientation of left arm
311 * @param l_e3 3rd euler rotation of target orientation of left arm
312 * @param r_x x-coordinate of target position of right arm
313 * @param r_y y-coordinate of target position of right arm
314 * @param r_z z-coordinate of target position of right arm
315 * @param r_e1 1st euler rotation of target orientation of right arm
316 * @param r_e2 2nd euler rotation of target orientation of right arm
317 * @param r_e3 3rd euler rotation of target orientation of right arm
318 * @return "true", if target could be added to queue.
319 */
320bool
322 float l_y,
323 float l_z,
324 float l_e1,
325 float l_e2,
326 float l_e3,
327 float r_x,
328 float r_y,
329 float r_z,
330 float r_e1,
331 float r_e2,
332 float r_e3)
333{
334#ifdef HAVE_OPENRAVE
335 // no IK checking yet, just enqueue until they can be processed
336 // create new targets for the queues
337 RefPtr<jaco_target_t> target_l(new jaco_target_t());
338 target_l->type = TARGET_CARTESIAN;
339 target_l->trajec_state = TRAJEC_WAITING;
340 target_l->coord = true;
341 target_l->pos.push_back(l_x);
342 target_l->pos.push_back(l_y);
343 target_l->pos.push_back(l_z);
344 target_l->pos.push_back(l_e1);
345 target_l->pos.push_back(l_e2);
346 target_l->pos.push_back(l_e3);
347
348 RefPtr<jaco_target_t> target_r(new jaco_target_t());
349 target_r->type = TARGET_CARTESIAN;
350 target_r->trajec_state = TRAJEC_WAITING;
351 target_r->coord = true;
352 target_r->pos.push_back(r_x);
353 target_r->pos.push_back(r_y);
354 target_r->pos.push_back(r_z);
355 target_r->pos.push_back(r_e1);
356 target_r->pos.push_back(r_e2);
357 target_r->pos.push_back(r_e3);
358
359 arms_.left.arm->target_mutex->lock();
360 arms_.right.arm->target_mutex->lock();
361 arms_.left.arm->target_queue->push_back(target_l);
362 arms_.right.arm->target_queue->push_back(target_r);
363 arms_.left.arm->target_mutex->unlock();
364 arms_.right.arm->target_mutex->unlock();
365
366 return true;
367#else
368 return false;
369#endif
370}
371
372void
374{
375 // do nothing, this thread is only for plannning!
376}
377
378void
380{
381}
382
383void
384JacoBimanualOpenraveThread::_set_trajec_state(jaco_trajec_state_t state)
385{
386#ifdef HAVE_OPENRAVE
387 arms_.left.arm->target_mutex->lock();
388 arms_.right.arm->target_mutex->lock();
389 arms_.left.target->trajec_state = state;
390 arms_.right.target->trajec_state = state;
391 arms_.left.arm->target_mutex->unlock();
392 arms_.right.arm->target_mutex->unlock();
393#endif
394}
395
396void
397JacoBimanualOpenraveThread::_copy_env()
398{
399#ifdef HAVE_OPENRAVE
400 // Update bodies in planner-environment
401 // clone robot state, ignoring grabbed bodies
402 {
403 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
404 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
405 planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
406 planner_env_.env->delete_all_objects();
407
408 /*
409 // Old method. Somehow we encountered problems. OpenRAVE internal bug?
410 RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
411 0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
412 saver.Restore( planner_env_.robot->get_robot_ptr() );
413 */
414 // New method. Simply set the DOF values as they are in viewer_env_
415 vector<dReal> dofs;
416 viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
417 planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
418 }
419
420 // then clone all objects
421 planner_env_.env->clone_objects(viewer_env_.env);
422
423 // update robot state with attached objects
424 {
425 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
426 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
427 /*
428 // Old method. Somehow we encountered problems. OpenRAVE internal bug?
429 RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
430 KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
431 saver.Restore( planner_env_.robot->get_robot_ptr() );
432 */
433 // New method. Grab all bodies in planner_env_ that are grabbed in viewer_env_ by this manipulator
434 vector<RobotBase::GrabbedInfoPtr> grabbed;
435 viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
436 for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
437 ++it) {
439 "compare _robotlinkname '%s' with our manip links '%s' and '%s'",
440 (*it)->_robotlinkname.c_str(),
441 arms_.left.manip->GetEndEffector()->GetName().c_str(),
442 arms_.right.manip->GetEndEffector()->GetName().c_str());
443 if ((*it)->_robotlinkname == arms_.left.manip->GetEndEffector()->GetName()) {
445 "attach '%s' to '%s'!",
446 (*it)->_grabbedname.c_str(),
447 arms_.left.manip->GetEndEffector()->GetName().c_str());
448 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
449 planner_env_.env,
450 arms_.left.manipname.c_str());
451
452 } else if ((*it)->_robotlinkname == arms_.right.manip->GetEndEffector()->GetName()) {
454 "attach '%s' to '%s'!",
455 (*it)->_grabbedname.c_str(),
456 arms_.right.manip->GetEndEffector()->GetName().c_str());
457 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
458 planner_env_.env,
459 arms_.right.manipname.c_str());
460 }
461 }
462 }
463#endif
464}
465
466bool
467JacoBimanualOpenraveThread::_plan_path()
468{
469#ifdef HAVE_OPENRAVE
470 _set_trajec_state(TRAJEC_PLANNING);
471
472 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
473
474 // Set active DOFs
475 vector<int> dofs = arms_.left.manip->GetArmIndices();
476 vector<int> dofs_r = arms_.right.manip->GetArmIndices();
477 dofs.reserve(dofs.size() + dofs_r.size());
478 dofs.insert(dofs.end(), dofs_r.begin(), dofs_r.end());
479 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(dofs);
480
481 // setup command for dualmanipulation module
482 stringstream cmdin, cmdout;
483 cmdin << std::setprecision(numeric_limits<dReal>::digits10 + 1);
484 cmdout << std::setprecision(numeric_limits<dReal>::digits10 + 1);
485
486 vector<dReal> sol;
487 cmdin << "MoveAllJoints goal";
488 planner_env_.manip->set_angles_device(arms_.left.target->pos);
489 planner_env_.manip->get_angles(sol);
490 for (size_t i = 0; i < sol.size(); ++i) {
491 cmdin << " " << sol[i];
492 }
493 planner_env_.manip->set_angles_device(arms_.right.target->pos);
494 planner_env_.manip->get_angles(sol);
495 for (size_t i = 0; i < sol.size(); ++i) {
496 cmdin << " " << sol[i];
497 }
498
499 //add additional planner parameters
500 if (!plannerparams_.empty()) {
501 cmdin << " " << plannerparams_;
502 }
503
504 //constrain planning if required
505 if (__constrained) {
506 cmdin << " constrainterrorthresh 1";
507 }
508
509 cmdin << " execute 0";
510 cmdin << " outputtraj";
511 //logger->log_debug(name(), "Planner: dualmanip cmdin:%s", cmdin.str().c_str());
512
513 // plan path
514 bool success = false;
515 try {
516 success = mod_dualmanip_->SendCommand(cmdout, cmdin);
517 } catch (openrave_exception &e) {
518 logger->log_debug(name(), "Planner: dualmanip command failed. Ex:%s", e.what());
519 }
520
521 if (!success) {
522 logger->log_warn(name(), "Planner: planning failed");
523 _set_trajec_state(TRAJEC_PLANNING_ERROR);
524 arms_.left.arm->target_mutex->lock();
525 arms_.right.arm->target_mutex->lock();
526 arms_.left.arm->target_mutex->unlock();
527 arms_.right.arm->target_mutex->unlock();
528 return false;
529
530 } else {
531 //logger->log_debug(name(), "Planner: path planned. cmdout:%s", cmdout.str().c_str());
532
533 // read returned trajectory
534 ConfigurationSpecification cfg_spec =
535 planner_env_.robot->get_robot_ptr()->GetActiveConfigurationSpecification();
536 TrajectoryBasePtr traj = RaveCreateTrajectory(planner_env_.env->get_env_ptr(), "");
537 traj->Init(cfg_spec);
538 if (!traj->deserialize(cmdout)) {
539 logger->log_warn(name(), "Planner: Cannot read trajectory data.");
540 _set_trajec_state(TRAJEC_PLANNING_ERROR);
541 arms_.left.arm->target_mutex->lock();
542 arms_.right.arm->target_mutex->lock();
543 arms_.left.arm->target_mutex->unlock();
544 arms_.right.arm->target_mutex->unlock();
545 return false;
546
547 } else {
548 // sampling trajectory and setting target trajectory
549 jaco_trajec_t * trajec_l = new jaco_trajec_t();
550 jaco_trajec_t * trajec_r = new jaco_trajec_t();
551 jaco_trajec_point_t p; // point we will add to trajectories
552 vector<dReal> tmp_p;
553 int arm_dof = cfg_spec.GetDOF() / 2;
554
555 for (dReal time = 0; time <= traj->GetDuration(); time += (dReal)cfg_OR_sampling_) {
556 vector<dReal> point;
557 traj->Sample(point, time);
558
559 tmp_p = vector<dReal>(point.begin(), point.begin() + arm_dof);
560 planner_env_.manip->angles_or_to_device(tmp_p, p);
561 trajec_l->push_back(p);
562
563 tmp_p = vector<dReal>(point.begin() + arm_dof, point.begin() + 2 * arm_dof);
564 planner_env_.manip->angles_or_to_device(tmp_p, p);
565 trajec_r->push_back(p);
566 }
567
568 arms_.left.arm->target_mutex->lock();
569 arms_.right.arm->target_mutex->lock();
570 arms_.left.target->trajec = RefPtr<jaco_trajec_t>(trajec_l);
571 arms_.right.target->trajec = RefPtr<jaco_trajec_t>(trajec_r);
572 // update target.
573 // set target->pos accordingly. This makes final-checking in goto_thread much easier
574 arms_.left.target->pos = trajec_l->back();
575 arms_.right.target->pos = trajec_r->back();
576 arms_.left.target->trajec_state = TRAJEC_READY;
577 arms_.right.target->trajec_state = TRAJEC_READY;
578 arms_.left.arm->target_mutex->unlock();
579 arms_.right.arm->target_mutex->unlock();
580
581 return true;
582 }
583 }
584#endif
585
586 return false;
587}
588
589bool
590JacoBimanualOpenraveThread::_solve_multi_ik(vector<float> &left, vector<float> &right)
591{
592#ifndef HAVE_OPENRAVE
593 return false;
594#else
595 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
596
597 // robot ptr for convenienc
598 RobotBasePtr robot = planner_env_.robot->get_robot_ptr();
599
600 // get grabbed bodies
601 vector<KinBodyPtr> grabbed;
602 robot->GetGrabbed(grabbed);
603
604 // save state of grabbed bodies
605 vector<KinBody::KinBodyStateSaver> statesaver;
606 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
607 statesaver.push_back(KinBody::KinBodyStateSaver(*body));
608 }
609
610 // get IK solutions for both arms
611 vector<vector<dReal>> solutions_l, solutions_r;
612 {
613 // save state of robot
614 RobotBase::RobotStateSaver robot_saver(robot);
615
616 // Find IK solutions for left arm
617 // Disable all links of right manipulator
618 for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
619 ++body) {
620 (*body)->Enable(false);
621 }
622 // Enable only grabbed bodies of this manipulator
623 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
624 (*body)->Enable(arms_.left.manip->IsGrabbing(*body));
625 }
626 // Get Ik Solutions.
627 robot->SetActiveManipulator(arms_.left.manip);
628 robot->SetActiveDOFs(arms_.left.manip->GetArmIndices());
629 planner_env_.robot->set_target_euler(EULER_ZXZ,
630 arms_.left.target->pos.at(0),
631 arms_.left.target->pos.at(1),
632 arms_.left.target->pos.at(2),
633 arms_.left.target->pos.at(3),
634 arms_.left.target->pos.at(4),
635 arms_.left.target->pos.at(5));
636 IkParameterization param = planner_env_.robot->get_target().ikparam;
637 arms_.left.manip->FindIKSolutions(param, solutions_l, IKFO_CheckEnvCollisions);
638 if (solutions_l.empty()) {
639 logger->log_warn(name(), "No IK solutions found for left arm");
640 return false;
641 } else {
642 logger->log_debug(name(), "IK solution found for left arm");
643 }
644
645 // now same for right arm. but enable links of right manipulator first
646 for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
647 ++body) {
648 (*body)->Enable(true);
649 }
650 // Disable all links of left manipulator
651 for (set<KinBody::LinkPtr>::iterator body = links_left_.begin(); body != links_left_.end();
652 ++body) {
653 (*body)->Enable(false);
654 }
655 // Enable only grabbed bodies of this manipulator
656 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
657 (*body)->Enable(arms_.right.manip->IsGrabbing(*body));
658 }
659 // Get Ik Solutions.
660 robot->SetActiveManipulator(arms_.right.manip);
661 robot->SetActiveDOFs(arms_.right.manip->GetArmIndices());
662 planner_env_.robot->set_target_euler(EULER_ZXZ,
663 arms_.right.target->pos.at(0),
664 arms_.right.target->pos.at(1),
665 arms_.right.target->pos.at(2),
666 arms_.right.target->pos.at(3),
667 arms_.right.target->pos.at(4),
668 arms_.right.target->pos.at(5));
669 param = planner_env_.robot->get_target().ikparam;
670 arms_.right.manip->FindIKSolutions(param, solutions_r, IKFO_CheckEnvCollisions);
671 if (solutions_r.empty()) {
672 logger->log_warn(name(), "No IK solutions found for right arm");
673 return false;
674 } else {
675 logger->log_debug(name(), "IK solution found for right arm");
676 }
677 } // robot state-saver destroyed
678
679 // restore kinbody states
680 for (vector<KinBody::KinBodyStateSaver>::iterator s = statesaver.begin(); s != statesaver.end();
681 ++s) {
682 (*s).Restore();
683 }
684
685 // finally find the closest solutions without collision and store them
686 bool solution_found = false;
687 {
688 // save state of robot
689 RobotBase::RobotStateSaver robot_saver(robot);
690 vector<vector<dReal>>::iterator sol_l, sol_r;
691
692 float dist = 100.f;
693 vector<dReal> cur_l, cur_r;
694 vector<dReal> diff_l, diff_r;
695 arms_.left.manip->GetArmDOFValues(cur_l);
696 arms_.right.manip->GetArmDOFValues(cur_r);
697
698 // try each combination to find closest non-colliding
699 for (sol_l = solutions_l.begin(); sol_l != solutions_l.end(); ++sol_l) {
700 for (sol_r = solutions_r.begin(); sol_r != solutions_r.end(); ++sol_r) {
701 // set joints for robot model
702 robot->SetDOFValues(*sol_l, 1, arms_.left.manip->GetArmIndices());
703 robot->SetDOFValues(*sol_r, 1, arms_.right.manip->GetArmIndices());
704
705 // check for collisions
706 if (!robot->CheckSelfCollision() && !robot->GetEnv()->CheckCollision(robot)) {
707 //logger->log_debug(name(), "Collision-free solution found!");
708 // calculate distance
709 float dist_l = 0.f;
710 float dist_r = 0.f;
711 diff_l = cur_l;
712 diff_r = cur_r;
713 robot->SubtractDOFValues(diff_l, (*sol_l), arms_.left.manip->GetArmIndices());
714 robot->SubtractDOFValues(diff_r, (*sol_r), arms_.right.manip->GetArmIndices());
715 for (unsigned int i = 0; i < diff_l.size(); ++i) {
716 dist_l += fabs(diff_l[i]);
717 // use cur+diff instead of sol, to have better angles
718 // for circular joints. Otherwise planner might have problems
719 (*sol_l)[i] = cur_l[i] - diff_l[i];
720 }
721 //logger->log_debug(name(), "Distance left: %f", dist_l);
722 for (unsigned int i = 0; i < diff_r.size(); ++i) {
723 dist_r += fabs(diff_r[i]);
724 (*sol_r)[i] = cur_r[i] - diff_r[i];
725 }
726 //logger->log_debug(name(), "Distance right: %f", dist_r);
727
728 if (dist_l + dist_r < dist) {
729 //logger->log_debug(name(), "Dist %f is closer that previous one (%f). Take this!", dist_l+dist_r, dist);
730 dist = dist_l + dist_r;
731 solution_found = true;
732 left.clear();
733 right.clear();
734 planner_env_.manip->set_angles(*sol_l);
735 planner_env_.manip->get_angles_device(left);
736 planner_env_.manip->set_angles(*sol_r);
737 planner_env_.manip->get_angles_device(right);
738 }
739 } else {
740 //logger->log_debug(name(), "Skipping solution because of collision!");
741 }
742 }
743 }
744 } // robot state-saver destroyed
745
746 return solution_found;
747#endif
748}
virtual void finalize()
Finalize the thread.
virtual bool add_target(float l_x, float l_y, float l_z, float l_e1, float l_e2, float l_e3, float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3)
Add target for coordinated manipulation to the queue.
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
virtual void loop()
Code to execute in the thread.
Base Jaco Arm thread, integrating OpenRAVE.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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
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
Class containing information about all Kinova Jaco motors.
Definition: kinova_jaco.h:31
OpenRAVE Robot class.
Definition: robot.h:38
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
@ TARGET_CARTESIAN
target with cartesian coordinates.
Definition: types.h:59
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:46
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Definition: types.h:48
@ TRAJEC_WAITING
new trajectory target, wait for planner to process.
Definition: types.h:69
@ 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
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
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