Fawkes API Fawkes Development Version
openrave_thread.cpp
1
2/***************************************************************************
3 * openrave_thread.cpp - Jaco plugin OpenRAVE Thread for single arm
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 "openrave_thread.h"
24
25#include "arm.h"
26#include "types.h"
27
28#include <core/threading/mutex.h>
29#include <interfaces/JacoInterface.h>
30#include <utils/math/angle.h>
31
32#include <cmath>
33#include <cstring>
34#include <stdio.h>
35#include <unistd.h>
36
37#ifdef HAVE_OPENRAVE
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>
42
43using namespace OpenRAVE;
44#endif
45
46using namespace fawkes;
47using namespace std;
48
49/** @class JacoOpenraveThread "openrave_thread.h"
50 * Jaco Arm thread for single-arm setup, integrating OpenRAVE
51 *
52 * @author Bahram Maleki-Fard
53 */
54
55/** Constructor.
56 * @param name thread name
57 * @param arm pointer to jaco_arm_t struct, to be used in this thread
58 * @param load_robot decide if this thread should load a robot. This should only
59 * be set to "true" if a separate OpenRaveRobot should be loaded (e.g. not the
60 * case when using 1 robot with 2 manipulators!)
61 */
62JacoOpenraveThread::JacoOpenraveThread(const char *name, jaco_arm_t *arm, bool load_robot)
64{
65 arm_ = arm;
66 load_robot_ = load_robot;
67#ifdef HAVE_OPENRAVE
68 planner_env_.env = NULL;
69 planner_env_.robot = NULL;
70 planner_env_.manip = NULL;
71
72 plotted_current_ = false;
73#endif
74}
75
76/** Get additional config entries. */
77void
78JacoOpenraveThread::_init()
79{
80 switch (arm_->config) {
81 case CONFIG_SINGLE:
82 cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/single");
83 break;
84
85 case CONFIG_LEFT:
86 cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/dual_left");
87 break;
88
89 case CONFIG_RIGHT:
90 cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/dual_right");
91 break;
92
93 default: throw fawkes::Exception("Could not read manipname from config."); break;
94 }
95}
96
97/** Load the robot into the environment. */
98void
99JacoOpenraveThread::_load_robot()
100{
101#ifdef HAVE_OPENRAVE
102
103 if (load_robot_) {
104 cfg_OR_robot_file_ = config->get_string("/hardware/jaco/openrave/robot_file");
105
106 try {
107 viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_, false);
108 } catch (Exception &e) {
109 throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)",
110 cfg_OR_robot_file_.c_str(),
112 }
113
114 try {
115 viewer_env_.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
116 viewer_env_.manip->add_motor(0, 0);
117 viewer_env_.manip->add_motor(1, 1);
118 viewer_env_.manip->add_motor(2, 2);
119 viewer_env_.manip->add_motor(3, 3);
120 viewer_env_.manip->add_motor(4, 4);
121 viewer_env_.manip->add_motor(5, 5);
122
123 // Set manipulator and offsets.
124 openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
125
126 openrave->get_environment()->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
127
128 } catch (Exception &e) {
129 finalize();
130 throw;
131 }
132
133 } else {
134 // robot was not loaded by this thread. So get them from openrave-environment now
135 try {
136 viewer_env_.robot = openrave->get_active_robot();
137 viewer_env_.manip = viewer_env_.robot->get_manipulator()->copy();
138 } catch (Exception &e) {
139 throw fawkes::Exception("%s: Could not get robot '%s' from openrave environment. (Error: %s)",
140 name(),
141 cfg_OR_robot_file_.c_str(),
143 }
144 }
145
146#endif //HAVE_OPENRAVE
147}
148
149/** Get pointers to the robot and manipulator in the viewer_env, and
150 * clone the environment.
151 */
152void
153JacoOpenraveThread::_post_init()
154{
155#ifdef HAVE_OPENRAVE
156 while (!robot_) {
157 robot_ = viewer_env_.robot->get_robot_ptr();
158 usleep(100);
159 }
160 while (!manip_) {
161 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
162 manip_ = robot_->SetActiveManipulator(cfg_manipname_);
163 usleep(100);
164 }
165
166 // create cloned environment for planning
167 logger->log_debug(name(), "Clone environment for planning");
168 openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
169
170 if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
171 throw fawkes::Exception("Could not clone properly, received a NULL pointer");
172 }
173
174 // set name of environment
175 switch (arm_->config) {
176 case CONFIG_SINGLE: planner_env_.env->set_name("Planner"); break;
177
178 case CONFIG_LEFT: planner_env_.env->set_name("Planner_Left"); break;
179
180 case CONFIG_RIGHT: planner_env_.env->set_name("Planner_Right"); break;
181 }
182
183 // set active manipulator in planning environment
184 {
185 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
186 RobotBase::ManipulatorPtr manip =
187 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
188 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
189 }
190
191 // Get chain of links from arm base to manipulator in viewer_env. Used for plotting joints
192 robot_->GetChain(manip_->GetBase()->GetIndex(), manip_->GetEndEffector()->GetIndex(), links_);
193
194#endif //HAVE_OPENRAVE
195}
196
197void
199{
200 arm_ = NULL;
201#ifdef HAVE_OPENRAVE
202 if (load_robot_)
203 openrave->set_active_robot(NULL);
204
205 planner_env_.robot = NULL;
206 planner_env_.manip = NULL;
207 planner_env_.env = NULL;
208
210#endif
211}
212
213/** Mani loop.
214 * It iterates over the target_queue to find the first target that needs
215 * trajectory planning. This can be done if it is the first target,
216 * or if the previous target has a known final configuration, which can
217 * be used as the current starting configuration.
218 * The result is stored in the struct of the current target, which can
219 * then be processed by the goto_thread
220 *
221 * @see JacoGotoThread#loop to see how goto_thread processes the queue
222 */
223void
225{
226#ifndef HAVE_OPENRAVE
227 usleep(30e3);
228#else
229 if (arm_ == NULL || arm_->arm == NULL) {
230 usleep(30e3);
231 return;
232 }
233
236 // get first target with type TARGET_TRAJEC that needs a planner
237 arm_->target_mutex->lock();
238 jaco_target_queue_t::iterator it;
239 for (it = arm_->target_queue->begin(); it != arm_->target_queue->end(); ++it) {
240 if ((*it)->trajec_state == TRAJEC_WAITING && !(*it)->coord) {
241 // have found a new target for path planning!
242 to = *it;
243 break;
244 }
245 }
246
247 if (to) {
248 // Check if there is a prior target that can be usd as the starting position in planning.
249 // The only target-types that can be used for that are those that contain joint positions,
250 // i.e. TARGET_ANGULAR and TARGET_TRAJEC
252 while (it != arm_->target_queue->begin()) {
253 --it;
254 if ((*it)->trajec_state == TRAJEC_READY || (*it)->trajec_state == TRAJEC_EXECUTING) {
256 from->pos = (*it)->trajec->back();
257 break;
258 } else if ((*it)->trajec_state == TRAJEC_SKIP && (*it)->type == TARGET_ANGULAR) {
259 from = *it;
260 break;
261 } else if (!((*it)->type == TARGET_GRIPPER)) {
262 // A previous target has unknown final configuration. Cannot plan for our target yet. Abort.
263 // TARGET_GRIPPER would be the only one we could skip without problems.
264 arm_->target_mutex->unlock();
265 usleep(30e3);
266 return;
267 }
268 }
269 arm_->target_mutex->unlock();
270
271 // if there was no prior target that can be used as a starting position, create one
272 if (!from) {
274 arm_->arm->get_joints(from->pos);
275 }
276
277 // run planner
278 _plan_path(from, to);
280
281 } else {
282 arm_->target_mutex->unlock();
284 usleep(30e3); // TODO: make this configurable
285 }
286#endif
287}
288
289void
291{
292#ifndef HAVE_OPENRAVE
293 return;
294#else
295 if (arm_ == NULL || arm_->iface == NULL || robot_ == NULL || manip_ == NULL)
296 return;
297
298 try {
299 joints_.clear();
300 joints_.push_back(arm_->iface->joints(0));
301 joints_.push_back(arm_->iface->joints(1));
302 joints_.push_back(arm_->iface->joints(2));
303 joints_.push_back(arm_->iface->joints(3));
304 joints_.push_back(arm_->iface->joints(4));
305 joints_.push_back(arm_->iface->joints(5));
306
307 // get target IK values in openrave format
308 viewer_env_.manip->set_angles_device(joints_);
309 viewer_env_.manip->get_angles(joints_);
310
311 {
312 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
313 robot_->SetDOFValues(joints_, 1, manip_->GetArmIndices());
314 }
315
316 joints_.clear();
317 joints_.push_back(deg2rad(arm_->iface->finger1() - 40.f));
318 joints_.push_back(deg2rad(arm_->iface->finger2() - 40.f));
319 joints_.push_back(deg2rad(arm_->iface->finger3() - 40.f));
320 {
321 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
322 robot_->SetDOFValues(joints_, 1, manip_->GetGripperIndices());
323 }
324
325 if (plot_current_) {
326 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
327
328 if (!plotted_current_) {
329 // new plotting command. Erase previous plot
330 graph_current_.clear();
331 }
332
333 if (cfg_OR_plot_cur_manip_) {
334 OpenRAVE::Vector color(0.f, 1.f, 0.f, 1.f);
335 const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
336 float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
337 graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
338 }
339
340 if (cfg_OR_plot_cur_joints_) {
341 OpenRAVE::Vector color(0.2f, 1.f, 0.f, 1.f);
342 for (unsigned int i = 0; i < links_.size(); ++i) {
343 const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
344 float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
345 graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
346 }
347 }
348 }
349 plotted_current_ = plot_current_;
350
351 } catch (openrave_exception &e) {
352 throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
353 }
354#endif
355}
356
357/** Solve IK and add target to the queue.
358 *
359 * The IK is solved, ignoring collisions of the end-effector with the environment.
360 * We do this to generally decide if IK is generally solvable. Collision checking
361 * is done in a later step in JacoOpenraveThread::_plan_path .
362 *
363 * If IK is solvable, the target is enqueued in the target_queue.
364 *
365 * @param x x-coordinate of target position
366 * @param y y-coordinate of target position
367 * @param z z-coordinate of target position
368 * @param e1 1st euler rotation of target orientation
369 * @param e2 2nd euler rotation of target orientation
370 * @param e3 3rd euler rotation of target orientation
371 * @param plan decide if we want to plan a trajectory for this or not
372 * @return "true", if IK could be solved. "false" otherwise
373 */
374bool
375JacoOpenraveThread::add_target(float x, float y, float z, float e1, float e2, float e3, bool plan)
376{
377 bool solvable = false; // need to define it here outside the ifdef-scope
378
379#ifdef HAVE_OPENRAVE
380 try {
381 // update planner params; set correct DOF and stuff
382 planner_env_.robot->get_planner_params();
383
384 if (plan) {
385 // get IK from openrave. Ignore collisions with env though, as this is only for IK check and env might change at the
386 // time we start planning. There will be separate IK checks though for planning!
387 planner_env_.robot->enable_ik_comparison(false);
388 solvable = planner_env_.robot->set_target_euler(
389 EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
390
391 if (solvable) {
392 // add this to the target queue for planning
393 logger->log_debug(name(), "Adding to target_queue for later planning");
394
395 // create new target for the queue
397 target->type = TARGET_CARTESIAN;
398 target->trajec_state = TRAJEC_WAITING;
399 target->coord = false;
400 target->pos.push_back(x);
401 target->pos.push_back(y);
402 target->pos.push_back(z);
403 target->pos.push_back(e1);
404 target->pos.push_back(e2);
405 target->pos.push_back(e3);
406
407 arm_->target_mutex->lock();
408 arm_->target_queue->push_back(target);
409 arm_->target_mutex->unlock();
410 } else {
411 logger->log_warn(name(), "No IK solution found for target.");
412 }
413
414 } else {
415 // don't plan, consider this the final configuration
416
417 // get IK from openrave. Do not ignore collisions this time, because we skip planning
418 // and go straight to this configuration!
419 solvable = planner_env_.robot->set_target_euler(EULER_ZXZ, x, y, z, e1, e2, e3);
420
421 if (solvable) {
422 logger->log_debug(name(), "Skip planning, add this as TARGET_ANGULAR");
423
424 // create new target for the queue
426 target->type = TARGET_ANGULAR;
427 target->trajec_state = TRAJEC_SKIP;
428 target->coord = false;
429 // get target IK values
430 planner_env_.robot->get_target().manip->get_angles_device(target->pos);
431
432 arm_->target_mutex->lock();
433 arm_->target_queue->push_back(target);
434 arm_->target_mutex->unlock();
435 } else {
436 logger->log_warn(name(), "No IK solution found for target.");
437 }
438 }
439
440 } catch (openrave_exception &e) {
441 throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
442 }
443#endif
444
445 return solvable;
446}
447
448/** Add target joint values to the queue.
449 *
450 * Use this method with caution, as for now there are no checks for validity
451 * of the target joint values. This will be added soon.
452 * Collision checking with the environment is done in a later step
453 * in JacoOpenraveThread::_plan_path .
454 *
455 * @param j1 target angle of 1st joint
456 * @param j2 target angle of 2nd joint
457 * @param j3 target angle of 3rd joint
458 * @param j4 target angle of 4th joint
459 * @param j5 target angle of 5th joint
460 * @param j6 target angle of 6th joint
461 * @param plan decide if we want to plan a trajectory for this or not
462 * @return "true", if the target joints are valid and not in self-collision,
463 * "false" otherwise.
464 * CAUTION: Self-collision is not checked yet, this feature will be added soon.
465 */
466bool
468 float j2,
469 float j3,
470 float j4,
471 float j5,
472 float j6,
473 bool plan)
474{
475 bool joints_valid = false; // need to define it here outside the ifdef-scope
476
477#ifdef HAVE_OPENRAVE
478 try {
479 // update planner params; set correct DOF and stuff
480 planner_env_.robot->get_planner_params();
481
482 //TODO: need some kind cheking for self-collision, i.e. if the joint values are "valid".
483 // For now expect the user to know what he does, when he sets joint angles directly
484 joints_valid = true;
485
486 // create new target for the queue
488 target->type = TARGET_ANGULAR;
489 target->trajec_state = plan ? TRAJEC_WAITING : TRAJEC_SKIP;
490 target->coord = false;
491 target->pos.push_back(j1);
492 target->pos.push_back(j2);
493 target->pos.push_back(j3);
494 target->pos.push_back(j4);
495 target->pos.push_back(j5);
496 target->pos.push_back(j6);
497
498 arm_->target_mutex->lock();
499 arm_->target_queue->push_back(target);
500 arm_->target_mutex->unlock();
501
502 } catch (openrave_exception &e) {
503 throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
504 }
505#endif
506
507 return joints_valid;
508}
509
510/** Flush the target_queue and add this one.
511 * see JacoOpenraveThread#add_target for that.
512 *
513 * @param x x-coordinate of target position
514 * @param y y-coordinate of target position
515 * @param z z-coordinate of target position
516 * @param e1 1st euler rotation of target orientation
517 * @param e2 2nd euler rotation of target orientation
518 * @param e3 3rd euler rotation of target orientation
519 * @param plan decide if we want to plan a trajectory for this or not
520 * @return "true", if IK could be solved. "false" otherwise
521 */
522bool
523JacoOpenraveThread::set_target(float x, float y, float z, float e1, float e2, float e3, bool plan)
524{
525 arm_->target_mutex->lock();
526 arm_->target_queue->clear();
527 arm_->target_mutex->unlock();
528 return add_target(x, y, z, e1, e2, e3, plan);
529}
530
531/** Flush the target_queue and add this one.
532 * see JacoOpenraveThread#add_target_ang for that.
533 *
534 * @param j1 target angle of 1st joint
535 * @param j2 target angle of 2nd joint
536 * @param j3 target angle of 3rd joint
537 * @param j4 target angle of 4th joint
538 * @param j5 target angle of 5th joint
539 * @param j6 target angle of 6th joint
540 * @param plan decide if we want to plan a trajectory for this or not
541 * @return "true", if IK could be solved. "false" otherwise
542 */
543bool
545 float j2,
546 float j3,
547 float j4,
548 float j5,
549 float j6,
550 bool plan)
551{
552 arm_->target_mutex->lock();
553 arm_->target_queue->clear();
554 arm_->target_mutex->unlock();
555 return add_target_ang(j1, j2, j3, j4, j5, j6, plan);
556}
557
558void
559JacoOpenraveThread::_plan_path(RefPtr<jaco_target_t> &from, RefPtr<jaco_target_t> &to)
560{
561#ifdef HAVE_OPENRAVE
562 // update state of the trajectory
563 arm_->target_mutex->lock();
564 to->trajec_state = TRAJEC_PLANNING;
565 arm_->target_mutex->unlock();
566
567 // Update bodies in planner-environment
568 // clone robot state, ignoring grabbed bodies
569 {
570 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
571 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
572 planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
573 planner_env_.env->delete_all_objects();
574
575 /*
576 // Old method. Somehow we encountered problems. OpenRAVE internal bug?
577 RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
578 0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
579 saver.Restore( planner_env_.robot->get_robot_ptr() );
580 */
581 // New method. Simply set the DOF values as they are in viewer_env_
582 vector<dReal> dofs;
583 viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
584 planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
585 }
586
587 // then clone all objects
588 planner_env_.env->clone_objects(viewer_env_.env);
589
590 // restore robot state
591 {
592 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
593
594 // Set active manipulator and active DOFs (need for planner and IK solver!)
595 RobotBase::ManipulatorPtr manip =
596 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
597 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
598
599 // update robot state with attached objects
600 {
601 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
602 /*
603 // Old method. Somehow we encountered problems. OpenRAVE internal bug?
604 RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
605 KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
606 saver.Restore( planner_env_.robot->get_robot_ptr() );
607 */
608 // New method. Grab all bodies in planner_env_ that are grabbed in viewer_env_ by this manipulator
609 vector<RobotBase::GrabbedInfoPtr> grabbed;
610 viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
611 for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
612 ++it) {
614 "compare _robotlinkname '%s' with our manip link '%s'",
615 (*it)->_robotlinkname.c_str(),
616 manip->GetEndEffector()->GetName().c_str());
617 if ((*it)->_robotlinkname == manip->GetEndEffector()->GetName()) {
618 logger->log_debug(name(), "attach '%s'!", (*it)->_grabbedname.c_str());
619 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
620 planner_env_.env,
621 cfg_manipname_.c_str());
622 }
623 }
624 }
625 }
626
627 // Set target point for planner. Check again for IK, avoiding collisions with the environment
628 //logger->log_debug(name(), "setting target %f %f %f %f %f %f",
629 // to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5));
630 planner_env_.robot->enable_ik_comparison(true);
631 if (to->type == TARGET_CARTESIAN) {
632 if (!planner_env_.robot->set_target_euler(EULER_ZXZ,
633 to->pos.at(0),
634 to->pos.at(1),
635 to->pos.at(2),
636 to->pos.at(3),
637 to->pos.at(4),
638 to->pos.at(5))) {
639 logger->log_warn(name(), "Planning failed, second IK check failed");
640 arm_->target_mutex->lock();
641 to->trajec_state = TRAJEC_PLANNING_ERROR;
642 arm_->target_mutex->unlock();
643 return;
644
645 } else {
646 // set target angles. This changes the internal target type to ANGLES (see openrave/robot.*)
647 // and will use BaseManipulation's MoveActiveJoints. Otherwise it will use MoveToHandPosition,
648 // which does not have the filtering of IK solutions for the closest one as we have.
649 vector<float> target;
650 planner_env_.robot->get_target().manip->get_angles(target);
651 planner_env_.robot->set_target_angles(target);
652 }
653
654 } else {
655 vector<float> target;
656 //TODO: need some kind cheking for env-collision, i.e. if the target is colllision-free.
657 // For now expect the user to know what he does, when he sets joint angles directly
658 planner_env_.robot->get_target().manip->set_angles_device(to->pos);
659
660 planner_env_.robot->get_target().manip->get_angles(target);
661 planner_env_.robot->set_target_angles(target);
662 }
663
664 // Set starting point for planner
665 //logger->log_debug(name(), "setting start %f %f %f %f %f %f",
666 // from->pos.at(0), from->pos.at(1), from->pos.at(2), from->pos.at(3), from->pos.at(4), from->pos.at(5));
667 planner_env_.manip->set_angles_device(from->pos);
668
669 // Set planning parameters
670 planner_env_.robot->set_target_plannerparams(plannerparams_);
671
672 // Run planner
673 try {
674 planner_env_.env->run_planner(planner_env_.robot, cfg_OR_sampling_);
675 } catch (fawkes::Exception &e) {
676 logger->log_warn(name(), "Planning failed: %s", e.what_no_backtrace());
677 // TODO: better handling!
678 // for now just skip planning, so the target_queue can be processed
679 arm_->target_mutex->lock();
680 //to->type = TARGET_ANGULAR;
681 to->trajec_state = TRAJEC_PLANNING_ERROR;
682 arm_->target_mutex->unlock();
683 return;
684 }
685
686 // add trajectory to queue
687 //logger->log_debug(name(), "plan successful, adding to queue");
688 arm_->trajec_mutex->lock();
689 // we can do the following becaouse get_trajectory_device() returns a new object, thus
690 // can be safely deleted by RefPtr auto-deletion
691 to->trajec = RefPtr<jaco_trajec_t>(planner_env_.robot->get_trajectory_device());
692 arm_->trajec_mutex->unlock();
693
694 // update target.
695 arm_->target_mutex->lock();
696 //change target type to ANGULAR and set target->pos accordingly. This makes final-checking
697 // in goto_thread much easier
698 to->type = TARGET_ANGULAR;
699 to->pos = to->trajec->back();
700 // update trajectory state
701 to->trajec_state = TRAJEC_READY;
702 arm_->target_mutex->unlock();
703#endif
704}
705
706/** Plot the first target of the queue in the viewer_env */
707void
709{
710#ifdef HAVE_OPENRAVE
711 if (!cfg_OR_use_viewer_ || (!cfg_OR_plot_traj_manip_ && !cfg_OR_plot_traj_joints_))
712 return;
713
714 graph_handle_.clear();
715
716 // check if there is a target to be plotted
717 arm_->target_mutex->lock();
718 if (arm_->target_queue->empty()) {
719 arm_->target_mutex->unlock();
720 return;
721 }
722
723 // get RefPtr to first target in queue
724 RefPtr<jaco_target_t> target = arm_->target_queue->front();
725 arm_->target_mutex->unlock();
726
727 // only plot trajectories
728 if (target->trajec_state != TRAJEC_READY && target->trajec_state != TRAJEC_EXECUTING)
729 return;
730
731 // plot the trajectory (if possible)
732 arm_->trajec_mutex->lock();
733 if (!target->trajec) {
734 arm_->trajec_mutex->unlock();
735 return;
736 }
737
738 // remove all GraphHandlerPtr and currently drawn plots
739 graph_handle_.clear();
740 {
741 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
742
743 // save the state, do not modifiy currently active robot!
744 RobotBasePtr tmp_robot = viewer_env_.robot->get_robot_ptr();
745 RobotBase::RobotStateSaver saver(tmp_robot);
746
747 std::vector<dReal> joints;
748 OpenRaveManipulatorPtr manip = viewer_env_.manip->copy();
749
750 OpenRAVE::Vector color_m(arm_->trajec_color[0],
751 arm_->trajec_color[1],
752 arm_->trajec_color[2],
753 arm_->trajec_color[3]);
754 OpenRAVE::Vector color_j(arm_->trajec_color[0] / 1.4f,
755 0.2f,
756 arm_->trajec_color[2] / 1.4f,
757 arm_->trajec_color[3] / 1.4f);
758
759 for (jaco_trajec_t::iterator it = target->trajec->begin(); it != target->trajec->end(); ++it) {
760 manip->set_angles_device((*it));
761 manip->get_angles(joints);
762
763 tmp_robot->SetDOFValues(joints, 1, manip_->GetArmIndices());
764
765 if (cfg_OR_plot_traj_manip_) {
766 const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
767 float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
768 graph_handle_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_m));
769 }
770
771 if (cfg_OR_plot_traj_joints_) {
772 for (unsigned int i = 0; i < links_.size(); ++i) {
773 const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
774 float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
775 graph_handle_.push_back(
776 viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_j));
777 }
778 }
779 }
780 } // robot state is restored
781
782 arm_->trajec_mutex->unlock();
783
784#endif //HAVE_OPENRAVE
785}
Base Jaco Arm thread, integrating OpenRAVE.
fawkes::Mutex * planning_mutex_
mutex, used to lock when planning.
virtual void finalize()
Finalize the thread.
virtual void loop()
Mani loop.
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
virtual bool set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Flush the target_queue and add this one.
virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Flush the target_queue and add this one.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void finalize()
Finalize the thread.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
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 get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
float finger1() const
Get finger1 value.
float * joints() const
Get joints 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
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Class containing information about all Kinova Jaco motors.
Definition: kinova_jaco.h:31
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:141
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:447
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
@ TARGET_GRIPPER
only gripper movement.
Definition: types.h:61
@ TARGET_CARTESIAN
target with cartesian coordinates.
Definition: types.h:59
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
@ CONFIG_SINGLE
we only have one arm.
Definition: types.h:52
@ CONFIG_LEFT
this arm is the left one out of two.
Definition: types.h:53
@ CONFIG_RIGHT
this arm is the right one out of two.
Definition: types.h:54
@ 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_PLANNING
planner is planning the trajectory.
Definition: types.h:70
@ 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
@ EULER_ZXZ
ZXZ rotation.
Definition: types.h:45
Jaco struct containing all components required for one arm.
Definition: types.h:93
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:103
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
Jaco target struct, holding information on a target.
Definition: types.h:79