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 #include "arm.h"
25 #include "types.h"
26 
27 #include <interfaces/JacoInterface.h>
28 #include <core/threading/mutex.h>
29 #include <utils/math/angle.h>
30 
31 #include <cmath>
32 #include <stdio.h>
33 #include <cstring>
34 #include <unistd.h>
35 
36 #ifdef HAVE_OPENRAVE
37  #include <plugins/openrave/environment.h>
38  #include <plugins/openrave/robot.h>
39  #include <plugins/openrave/manipulator.h>
40  #include <plugins/openrave/manipulators/kinova_jaco.h>
41 
42  using namespace OpenRAVE;
43 #endif
44 
45 using namespace fawkes;
46 using namespace std;
47 
48 /** @class JacoOpenraveThread "openrave_thread.h"
49  * Jaco Arm thread for single-arm setup, integrating OpenRAVE
50  *
51  * @author Bahram Maleki-Fard
52  */
53 
54 /** Constructor.
55  * @param name thread name
56  * @param arm pointer to jaco_arm_t struct, to be used in this thread
57  * @param load_robot decide if this thread should load a robot. This should only
58  * be set to "true" if a separate OpenRaveRobot should be loaded (e.g. not the
59  * case when using 1 robot with 2 manipulators!)
60  */
61 JacoOpenraveThread::JacoOpenraveThread(const char *name, jaco_arm_t* arm, bool load_robot)
63 {
64  __arm = arm;
65  __load_robot = load_robot;
66 #ifdef HAVE_OPENRAVE
67  __planner_env.env = NULL;
68  __planner_env.robot = NULL;
69  __planner_env.manip = NULL;
70 
71  __plotted_current = false;
72 #endif
73 }
74 
75 /** Get additional config entries. */
76 void
77 JacoOpenraveThread::_init()
78 {
79  switch( __arm->config ) {
80  case CONFIG_SINGLE:
81  __cfg_manipname = config->get_string("/hardware/jaco/openrave/manipname/single");
82  break;
83 
84  case CONFIG_LEFT:
85  __cfg_manipname = config->get_string("/hardware/jaco/openrave/manipname/dual_left");
86  break;
87 
88  case CONFIG_RIGHT:
89  __cfg_manipname = config->get_string("/hardware/jaco/openrave/manipname/dual_right");
90  break;
91 
92  default:
93  throw fawkes::Exception("Could not read manipname from config.");
94  break;
95  }
96 }
97 
98 /** Load the robot into the environment. */
99 void
100 JacoOpenraveThread::_load_robot()
101 {
102 #ifdef HAVE_OPENRAVE
103 
104  if(__load_robot) {
105  __cfg_OR_robot_file = config->get_string("/hardware/jaco/openrave/robot_file");
106 
107  try {
108  __viewer_env.robot = openrave->add_robot(__cfg_OR_robot_file, false);
109  } catch (Exception& e) {
110  throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)", __cfg_OR_robot_file.c_str(), e.what_no_backtrace());
111  }
112 
113  try {
114  __viewer_env.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
115  __viewer_env.manip->add_motor(0,0);
116  __viewer_env.manip->add_motor(1,1);
117  __viewer_env.manip->add_motor(2,2);
118  __viewer_env.manip->add_motor(3,3);
119  __viewer_env.manip->add_motor(4,4);
120  __viewer_env.manip->add_motor(5,5);
121 
122  // Set manipulator and offsets.
123  openrave->set_manipulator(__viewer_env.robot, __viewer_env.manip, 0.f, 0.f, 0.f);
124 
125  openrave->get_environment()->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
126 
127  } catch (Exception& e) {
128  finalize();
129  throw;
130  }
131 
132  } else {
133  // robot was not loaded by this thread. So get them from openrave-environment now
134  try {
135  __viewer_env.robot = openrave->get_active_robot();
136  __viewer_env.manip = __viewer_env.robot->get_manipulator()->copy();
137  } catch (Exception& e) {
138  throw fawkes::Exception("%s: Could not get robot '%s' from openrave environment. (Error: %s)", name(), __cfg_OR_robot_file.c_str(), e.what_no_backtrace());
139  }
140  }
141 
142 #endif //HAVE_OPENRAVE
143 }
144 
145 /** Get pointers to the robot and manipulator in the viewer_env, and
146  * clone the environment.
147  */
148 void
149 JacoOpenraveThread::_post_init()
150 {
151 #ifdef HAVE_OPENRAVE
152  while( !__robot ) {
153  __robot = __viewer_env.robot->get_robot_ptr();
154  usleep(100);
155  }
156  while( !__manip ) {
157  EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
158  __manip = __robot->SetActiveManipulator(__cfg_manipname);
159  usleep(100);
160  }
161 
162  // create cloned environment for planning
163  logger->log_debug(name(), "Clone environment for planning");
164  openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip);
165 
166  if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) {
167  throw fawkes::Exception("Could not clone properly, received a NULL pointer");
168  }
169 
170  // set name of environment
171  switch( __arm->config ) {
172  case CONFIG_SINGLE:
173  __planner_env.env->set_name("Planner");
174  break;
175 
176  case CONFIG_LEFT:
177  __planner_env.env->set_name("Planner_Left");
178  break;
179 
180  case CONFIG_RIGHT:
181  __planner_env.env->set_name("Planner_Right");
182  break;
183  }
184 
185  // set active manipulator in planning environment
186  {
187  EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
188  RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
189  __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
190  }
191 
192  // Get chain of links from arm base to manipulator in viewer_env. Used for plotting joints
193  __robot->GetChain( __manip->GetBase()->GetIndex(), __manip->GetEndEffector()->GetIndex(), __links);
194 
195 #endif //HAVE_OPENRAVE
196 }
197 
198 void
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  */
223 void
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 ) {
255  from = RefPtr<jaco_target_t>(new jaco_target_t());
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 ) {
273  from = RefPtr<jaco_target_t>(new jaco_target_t());
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 
289 void
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  */
374 bool
375 JacoOpenraveThread::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(EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
389 
390  if( solvable ) {
391  // add this to the target queue for planning
392  logger->log_debug(name(), "Adding to target_queue for later planning");
393 
394  // create new target for the queue
395  RefPtr<jaco_target_t> target(new jaco_target_t());
396  target->type = TARGET_CARTESIAN;
397  target->trajec_state = TRAJEC_WAITING;
398  target->coord=false;
399  target->pos.push_back(x);
400  target->pos.push_back(y);
401  target->pos.push_back(z);
402  target->pos.push_back(e1);
403  target->pos.push_back(e2);
404  target->pos.push_back(e3);
405 
406  __arm->target_mutex->lock();
407  __arm->target_queue->push_back(target);
408  __arm->target_mutex->unlock();
409  } else {
410  logger->log_warn(name(), "No IK solution found for target.");
411  }
412 
413  } else {
414  // don't plan, consider this the final configuration
415 
416  // get IK from openrave. Do not ignore collisions this time, because we skip planning
417  // and go straight to this configuration!
418  solvable = __planner_env.robot->set_target_euler(EULER_ZXZ, x, y, z, e1, e2, e3);
419 
420  if( solvable ) {
421  logger->log_debug(name(), "Skip planning, add this as TARGET_ANGULAR");
422 
423  // create new target for the queue
424  RefPtr<jaco_target_t> target(new jaco_target_t());
425  target->type = TARGET_ANGULAR;
426  target->trajec_state = TRAJEC_SKIP;
427  target->coord=false;
428  // get target IK values
429  __planner_env.robot->get_target().manip->get_angles_device(target->pos);
430 
431  __arm->target_mutex->lock();
432  __arm->target_queue->push_back(target);
433  __arm->target_mutex->unlock();
434  } else {
435  logger->log_warn(name(), "No IK solution found for target.");
436  }
437  }
438 
439  } catch( openrave_exception &e) {
440  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
441  }
442 #endif
443 
444  return solvable;
445 }
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  */
466 bool
467 JacoOpenraveThread::add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan)
468 {
469  bool joints_valid = false; // need to define it here outside the ifdef-scope
470 
471 #ifdef HAVE_OPENRAVE
472  try {
473  // update planner params; set correct DOF and stuff
474  __planner_env.robot->get_planner_params();
475 
476  //TODO: need some kind cheking for self-collision, i.e. if the joint values are "valid".
477  // For now expect the user to know what he does, when he sets joint angles directly
478  joints_valid = true;
479 
480  // create new target for the queue
481  RefPtr<jaco_target_t> target(new jaco_target_t());
482  target->type = TARGET_ANGULAR;
483  target->trajec_state = plan ? TRAJEC_WAITING : TRAJEC_SKIP;
484  target->coord=false;
485  target->pos.push_back(j1);
486  target->pos.push_back(j2);
487  target->pos.push_back(j3);
488  target->pos.push_back(j4);
489  target->pos.push_back(j5);
490  target->pos.push_back(j6);
491 
492  __arm->target_mutex->lock();
493  __arm->target_queue->push_back(target);
494  __arm->target_mutex->unlock();
495 
496  } catch( openrave_exception &e) {
497  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
498  }
499 #endif
500 
501  return joints_valid;
502 }
503 
504 
505 /** Flush the target_queue and add this one.
506  * see JacoOpenraveThread#add_target for that.
507  *
508  * @param x x-coordinate of target position
509  * @param y y-coordinate of target position
510  * @param z z-coordinate of target position
511  * @param e1 1st euler rotation of target orientation
512  * @param e2 2nd euler rotation of target orientation
513  * @param e3 3rd euler rotation of target orientation
514  * @param plan decide if we want to plan a trajectory for this or not
515  * @return "true", if IK could be solved. "false" otherwise
516  */
517 bool
518 JacoOpenraveThread::set_target(float x, float y, float z, float e1, float e2, float e3, bool plan)
519 {
520  __arm->target_mutex->lock();
521  __arm->target_queue->clear();
522  __arm->target_mutex->unlock();
523  return add_target(x, y, z, e1, e2, e3, plan);
524 }
525 
526 
527 /** Flush the target_queue and add this one.
528  * see JacoOpenraveThread#add_target_ang for that.
529  *
530  * @param j1 target angle of 1st joint
531  * @param j2 target angle of 2nd joint
532  * @param j3 target angle of 3rd joint
533  * @param j4 target angle of 4th joint
534  * @param j5 target angle of 5th joint
535  * @param j6 target angle of 6th joint
536  * @param plan decide if we want to plan a trajectory for this or not
537  * @return "true", if IK could be solved. "false" otherwise
538  */
539 bool
540 JacoOpenraveThread::set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan)
541 {
542  __arm->target_mutex->lock();
543  __arm->target_queue->clear();
544  __arm->target_mutex->unlock();
545  return add_target_ang(j1, j2, j3, j4, j5, j6, plan);
546 }
547 
548 
549 void
550 JacoOpenraveThread::_plan_path(RefPtr<jaco_target_t> &from, RefPtr<jaco_target_t> &to)
551 {
552 #ifdef HAVE_OPENRAVE
553  // update state of the trajectory
554  __arm->target_mutex->lock();
556  __arm->target_mutex->unlock();
557 
558  // Update bodies in planner-environment
559  // clone robot state, ignoring grabbed bodies
560  {
561  EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
562  EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
563  __planner_env.robot->get_robot_ptr()->ReleaseAllGrabbed();
564  __planner_env.env->delete_all_objects();
565 
566  /*
567  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
568  RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
569  0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
570  saver.Restore( __planner_env.robot->get_robot_ptr() );
571  */
572  // New method. Simply set the DOF values as they are in __viewer_env
573  vector<dReal> dofs;
574  __viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs);
575  __planner_env.robot->get_robot_ptr()->SetDOFValues(dofs);
576  }
577 
578  // then clone all objects
579  __planner_env.env->clone_objects( __viewer_env.env );
580 
581  // restore robot state
582  {
583  EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
584 
585  // Set active manipulator and active DOFs (need for planner and IK solver!)
586  RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
587  __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
588 
589  // update robot state with attached objects
590  {
591  EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
592  /*
593  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
594  RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
595  KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
596  saver.Restore( __planner_env.robot->get_robot_ptr() );
597  */
598  // New method. Grab all bodies in __planner_env that are grabbed in __viewer_env by this manipulator
599  vector<RobotBase::GrabbedInfoPtr> grabbed;
600  __viewer_env.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
601  for( vector<RobotBase::GrabbedInfoPtr>::iterator it=grabbed.begin(); it!=grabbed.end(); ++it ) {
602  logger->log_debug(name(), "compare _robotlinkname '%s' with our manip link '%s'",
603  (*it)->_robotlinkname.c_str(), manip->GetEndEffector()->GetName().c_str());
604  if( (*it)->_robotlinkname == manip->GetEndEffector()->GetName() ) {
605  logger->log_debug(name(), "attach '%s'!", (*it)->_grabbedname.c_str());
606  __planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __cfg_manipname.c_str());
607  }
608  }
609  }
610  }
611 
612  // Set target point for planner. Check again for IK, avoiding collisions with the environment
613  //logger->log_debug(name(), "setting target %f %f %f %f %f %f",
614  // to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5));
615  __planner_env.robot->enable_ik_comparison(true);
616  if( to->type == TARGET_CARTESIAN ) {
617  if( !__planner_env.robot->set_target_euler(EULER_ZXZ, to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5)) ) {
618  logger->log_warn(name(), "Planning failed, second IK check failed");
619  __arm->target_mutex->lock();
621  __arm->target_mutex->unlock();
622  return;
623 
624  } else {
625  // set target angles. This changes the internal target type to ANGLES (see openrave/robot.*)
626  // and will use BaseManipulation's MoveActiveJoints. Otherwise it will use MoveToHandPosition,
627  // which does not have the filtering of IK solutions for the closest one as we have.
628  vector<float> target;
629  __planner_env.robot->get_target().manip->get_angles(target);
630  __planner_env.robot->set_target_angles(target);
631  }
632 
633  } else {
634  vector<float> target;
635  //TODO: need some kind cheking for env-collision, i.e. if the target is colllision-free.
636  // For now expect the user to know what he does, when he sets joint angles directly
637  __planner_env.robot->get_target().manip->set_angles_device(to->pos);
638 
639  __planner_env.robot->get_target().manip->get_angles(target);
640  __planner_env.robot->set_target_angles(target);
641  }
642 
643  // Set starting point for planner
644  //logger->log_debug(name(), "setting start %f %f %f %f %f %f",
645  // from->pos.at(0), from->pos.at(1), from->pos.at(2), from->pos.at(3), from->pos.at(4), from->pos.at(5));
646  __planner_env.manip->set_angles_device(from->pos);
647 
648  // Set planning parameters
649  __planner_env.robot->set_target_plannerparams(__plannerparams);
650 
651  // Run planner
652  try {
653  __planner_env.env->run_planner(__planner_env.robot, __cfg_OR_sampling);
654  } catch (fawkes::Exception &e) {
655  logger->log_warn(name(), "Planning failed: %s", e.what_no_backtrace());
656  // TODO: better handling!
657  // for now just skip planning, so the target_queue can be processed
658  __arm->target_mutex->lock();
659  //to->type = TARGET_ANGULAR;
661  __arm->target_mutex->unlock();
662  return;
663  }
664 
665  // add trajectory to queue
666  //logger->log_debug(name(), "plan successful, adding to queue");
667  __arm->trajec_mutex->lock();
668  // we can do the following becaouse get_trajectory_device() returns a new object, thus
669  // can be safely deleted by RefPtr auto-deletion
670  to->trajec = RefPtr<jaco_trajec_t>( __planner_env.robot->get_trajectory_device() );
671  __arm->trajec_mutex->unlock();
672 
673  // update target.
674  __arm->target_mutex->lock();
675  //change target type to ANGULAR and set target->pos accordingly. This makes final-checking
676  // in goto_thread much easier
677  to->type = TARGET_ANGULAR;
678  to->pos = to->trajec->back();
679  // update trajectory state
681  __arm->target_mutex->unlock();
682 #endif
683 }
684 
685 
686 
687 /** Plot the first target of the queue in the viewer_env */
688 void
690 {
691 #ifdef HAVE_OPENRAVE
692  if( !__cfg_OR_use_viewer || (!__cfg_OR_plot_traj_manip && !__cfg_OR_plot_traj_joints))
693  return;
694 
695  __graph_handle.clear();
696 
697  // check if there is a target to be plotted
698  __arm->target_mutex->lock();
699  if( __arm->target_queue->empty() ) {
700  __arm->target_mutex->unlock();
701  return;
702  }
703 
704  // get RefPtr to first target in queue
705  RefPtr<jaco_target_t> target = __arm->target_queue->front();
706  __arm->target_mutex->unlock();
707 
708 
709  // only plot trajectories
710  if( target->trajec_state != TRAJEC_READY && target->trajec_state != TRAJEC_EXECUTING )
711  return;
712 
713  // plot the trajectory (if possible)
714  __arm->trajec_mutex->lock();
715  if( !target->trajec ) {
716  __arm->trajec_mutex->unlock();
717  return;
718  }
719 
720  // remove all GraphHandlerPtr and currently drawn plots
721  __graph_handle.clear();
722  {
723  EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
724 
725  // save the state, do not modifiy currently active robot!
726  RobotBasePtr tmp_robot = __viewer_env.robot->get_robot_ptr();
727  RobotBase::RobotStateSaver saver(tmp_robot);
728 
729  std::vector<dReal> joints;
730  OpenRaveManipulatorPtr manip = __viewer_env.manip->copy();
731 
732  OpenRAVE::Vector color_m(__arm->trajec_color[0],
733  __arm->trajec_color[1],
734  __arm->trajec_color[2],
735  __arm->trajec_color[3]);
736  OpenRAVE::Vector color_j(__arm->trajec_color[0] / 1.4f,
737  0.2f,
738  __arm->trajec_color[2] / 1.4f,
739  __arm->trajec_color[3] / 1.4f);
740 
741  for(jaco_trajec_t::iterator it = target->trajec->begin(); it!=target->trajec->end(); ++it) {
742  manip->set_angles_device((*it));
743  manip->get_angles(joints);
744 
745  tmp_robot->SetDOFValues(joints, 1, __manip->GetArmIndices());
746 
747  if( __cfg_OR_plot_traj_manip ) {
748  const OpenRAVE::Vector &trans = __manip->GetEndEffectorTransform().trans;
749  float transa[4] = { (float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w };
750  __graph_handle.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 3.f, color_m));
751  }
752 
753  if( __cfg_OR_plot_traj_joints ) {
754  for(unsigned int i=0; i<__links.size(); ++i) {
755  const OpenRAVE::Vector &trans = __links[i]->GetTransform().trans;
756  float transa[4] = { (float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w };
757  __graph_handle.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 3.f, color_j));
758  }
759  }
760  }
761  } // robot state is restored
762 
763  __arm->trajec_mutex->unlock();
764 
765 #endif //HAVE_OPENRAVE
766 }
trajectory has been planned and is ready for execution.
Definition: types.h:74
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:141
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
Jaco struct containing all components required for one arm.
Definition: types.h:95
ZXZ rotation.
Definition: types.h:48
fawkes::Mutex * __planning_mutex
mutex, used to lock when planning.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
fawkes::RefPtr< jaco_trajec_t > trajec
trajectory, if target is TARGET_TRAJEC.
Definition: types.h:85
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:97
float finger2() const
Get finger2 value.
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
STL namespace.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:104
trajectory is being executed.
Definition: types.h:75
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
jaco_target_type_t type
target type.
Definition: types.h:82
planner could not plan a collision-free trajectory.
Definition: types.h:77
jaco_trajec_point_t pos
target position (interpreted depending on target type).
Definition: types.h:83
virtual void finalize()
Finalize the thread.
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:86
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.
only gripper movement.
Definition: types.h:64
this arm is the right one out of two.
Definition: types.h:57
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:98
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
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.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:457
float finger1() const
Get finger1 value.
Class containing information about all Kinova Jaco motors.
Definition: kinova_jaco.h:33
float * joints() const
Get joints value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:103
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
target with cartesian coordinates.
Definition: types.h:62
Jaco target struct, holding information on a target.
Definition: types.h:81
target with angular coordinates.
Definition: types.h:63
const char * name() const
Get name of thread.
Definition: thread.h:95
jaco_arm_config_t config
configuration for this arm
Definition: types.h:96
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
planner is planning the trajectory.
Definition: types.h:73
bool coord
this target needs to be coordinated with targets of other arms.
Definition: types.h:87
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:106
this arm is the left one out of two.
Definition: types.h:56
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.
virtual void finalize()
Finalize the thread.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
skip trajectory planning for this target.
Definition: types.h:71
new trajectory target, wait for planner to process.
Definition: types.h:72
void lock()
Lock this mutex.
Definition: mutex.cpp:89
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.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
virtual void loop()
Mani loop.
float finger3() const
Get finger3 value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
we only have one arm.
Definition: types.h:55
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.
Base Jaco Arm thread, integrating OpenRAVE.