Fawkes API  Fawkes Development Version
goto_openrave_thread.cpp
1 
2 /***************************************************************************
3  * goto_openrave_thread.cpp - Katana goto one-time thread using openrave lib
4  *
5  * Created: Wed Jun 10 11:45:31 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  * 2010 Bahram Maleki-Fard
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "goto_openrave_thread.h"
25 #include "conversion.h"
26 #include "controller.h"
27 #include "exception.h"
28 
29 #include <cstdlib>
30 
31 #include <utils/time/time.h>
32 
33 #ifdef HAVE_OPENRAVE
34 
35 #include <plugins/openrave/robot.h>
36 #include <plugins/openrave/environment.h>
37 #include <plugins/openrave/manipulators/katana6M180.h>
38 #include <plugins/openrave/manipulators/neuronics_katana.h>
39 #include <plugins/openrave/types.h>
40 
41 #include <vector>
42 #endif
43 using namespace fawkes;
44 
45 /** @class KatanaGotoOpenRaveThread "goto_openrave_thread.h"
46  * Katana collision-free goto thread.
47  * This thread moves the arm into a specified position,
48  * using IK and path-planning from OpenRAVE.
49  * @author Tim Niemueller (goto_thread.h/cpp)
50  * @author Bahram Maleki-Fard (OpenRAVE extension)
51  */
52 
53 #ifdef HAVE_OPENRAVE
54 
55 /// @cond SELFEXPLAINING
56 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
57  "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>"
58  "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>"
59  "</_postprocessing>\n";
60 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS_STRAIGHT =
61  "maxdeviationangle 0.05";
62 /// @endcond
63 
64 
65 /** Constructor.
66  * @param katana katana controller base class
67  * @param logger logger
68  * @param openrave pointer to OpenRaveConnector aspect
69  * @param poll_interval_ms interval in ms between two checks if the
70  * final position has been reached
71  * @param robot_file path to robot's xml-file
72  * @param arm_model arm model used in robot_file, either "5dof" or "6dof_dummy"
73  * @param autoload_IK true, if IK databas should be automatically generated (recommended)
74  * @param use_viewer true, if viewer should be started (default: false)
75  */
76 KatanaGotoOpenRaveThread::KatanaGotoOpenRaveThread(fawkes::RefPtr<fawkes::KatanaController> katana,
77  fawkes::Logger *logger,
78  fawkes::OpenRaveConnector* openrave,
79  unsigned int poll_interval_ms,
80  std::string robot_file,
81  std::string arm_model,
82  bool autoload_IK,
83  bool use_viewer)
84  : KatanaMotionThread("KatanaGotoOpenRaveThread", katana, logger),
85  __OR_robot( 0 ),
86  __OR_manip( 0 ),
87  __target_object( "" ),
88  __target_traj( 0 ),
89  __cfg_robot_file( robot_file ),
90  __cfg_arm_model( arm_model ),
91  __cfg_autoload_IK( autoload_IK ),
92  __cfg_use_viewer( use_viewer ),
93  __is_target_object( 0 ),
94  __has_target_quaternion( 0 ),
95  __move_straight( 0 ),
96  __is_arm_extension( 0 ),
97  __plannerparams( "default" ),
98  __plannerparams_straight( "default" ),
99  _openrave( openrave )
100 {
101 }
102 
103 
104 /** Set target position.
105  * @param x X coordinate relative to base
106  * @param y Y coordinate relative to base
107  * @param z Z coordinate relative to base
108  * @param phi Phi Euler angle of tool
109  * @param theta Theta Euler angle of tool
110  * @param psi Psi Euler angle of tool
111  */
112 void
113 KatanaGotoOpenRaveThread::set_target(float x, float y, float z,
114  float phi, float theta, float psi)
115 {
116  __x = x;
117  __y = y;
118  __z = z;
119  __phi = (phi);
120  __theta = (theta);
121  __psi = (psi);
122 
123  __has_target_quaternion = false;
124  __is_target_object = false;
125  __move_straight = false;
126  __is_arm_extension = false;
127 }
128 
129 /** Set target position.
130  * @param x X coordinate relative to base
131  * @param y Y coordinate relative to base
132  * @param z Z coordinate relative to base
133  * @param quat_x x value of quaternion for tool's rotation
134  * @param quat_y y value of quaternion for tool's rotation
135  * @param quat_z z value of quaternion for tool's rotation
136  * @param quat_w w value of quaternion for tool's rotation
137  */
138 void
139 KatanaGotoOpenRaveThread::set_target(float x, float y, float z,
140  float quat_x, float quat_y, float quat_z, float quat_w)
141 {
142  __x = x;
143  __y = y;
144  __z = z;
145  __quat_x = quat_x;
146  __quat_y = quat_y;
147  __quat_z = quat_z;
148  __quat_w = quat_w;
149 
150  __has_target_quaternion = true;
151  __is_target_object = false;
152  __move_straight = false;
153  __is_arm_extension = false;
154 }
155 
156 /** Set target position.
157  * @param object_name name of the object (kinbody) in OpenRaveEnvironment
158  */
159 void
160 KatanaGotoOpenRaveThread::set_target(const std::string& object_name, float rot_x)
161 {
162  __target_object = object_name;
163 
164  __is_target_object = true;
165 }
166 
167 /** Set theta error
168  * @param error error in radians
169  */
170 void
171 KatanaGotoOpenRaveThread::set_theta_error(float error)
172 {
173  __theta_error = error;
174 }
175 
176 /** Set if arm should move straight.
177  * Make sure to call this after(!) a "set_target" method, as they
178  * set "__move_straight" attribute to its default value.
179  * @param move_straight true, if arm should move straight
180  */
181 void
182 KatanaGotoOpenRaveThread::set_move_straight(bool move_straight)
183 {
184  __move_straight = move_straight;
185 }
186 
187 /** Set if target is taken as arm extension.
188  * Make sure to call this after(!) a "set_target" method, as they
189  * set "__move_straight" attribute to its default value.
190  * @param arm_extension true, if target is regarded as arm extension
191  */
192 void
193 KatanaGotoOpenRaveThread::set_arm_extension(bool arm_extension)
194 {
195  __is_arm_extension = arm_extension;
196 }
197 
198 /** Set plannerparams.
199  * @param params plannerparameters. For further information, check openrave plugin, or OpenRAVE documentaiton.
200  * @param straight true, if these params are for straight movement
201  */
202 void
203 KatanaGotoOpenRaveThread::set_plannerparams(std::string& params, bool straight)
204 {
205  if( straight ) {
206  __plannerparams_straight = params;
207  } else {
208  __plannerparams = params;
209  }
210 }
211 
212 /** Set plannerparams.
213  * @param params plannerparameters. For further information, check openrave plugin, or OpenRAVE documentaiton.
214  * @param straight true, if these params are for straight movement
215  */
216 void
217 KatanaGotoOpenRaveThread::set_plannerparams(const char* params, bool straight)
218 {
219  if( straight ) {
220  __plannerparams_straight = params;
221  } else {
222  __plannerparams = params;
223  }
224 }
225 
226 void
228 {
229  try {
230  __OR_robot = _openrave->add_robot(__cfg_robot_file, false);
231 
232  // configure manipulator
233  // TODO: from config parameters? neccessary?
234  if( __cfg_arm_model == "5dof" ) {
235  __OR_manip = new OpenRaveManipulatorNeuronicsKatana(5, 5);
236  __OR_manip->add_motor(0,0);
237  __OR_manip->add_motor(1,1);
238  __OR_manip->add_motor(2,2);
239  __OR_manip->add_motor(3,3);
240  __OR_manip->add_motor(4,4);
241 
242  // Set manipulator and offsets.
243  // offsetZ: katana.kinbody is 0.165 above ground; coordinate system of real katana has origin in intersection of j1 and j2 (i.e. start of link L2: 0.2015 on z-axis)
244  // offsetX: katana.kinbody is setup 0.0725 on +x axis
245  _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
246  __OR_robot->get_robot_ptr()->SetActiveManipulator("arm_kni");
247 
248  if( __cfg_autoload_IK ) {
249  _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_TranslationDirection5D);
250  }
251  } else if ( __cfg_arm_model == "6dof_dummy" ) {
252  __OR_manip = new OpenRaveManipulatorKatana6M180(6, 5);
253  __OR_manip->add_motor(0,0);
254  __OR_manip->add_motor(1,1);
255  __OR_manip->add_motor(2,2);
256  __OR_manip->add_motor(4,3);
257  __OR_manip->add_motor(5,4);
258 
259  // Set manipulator and offsets.
260  // offsetZ: katana.kinbody is 0.165 above ground; coordinate system of real katana has origin in intersection of j1 and j2 (i.e. start of link L2: 0.2015 on z-axis)
261  // offsetX: katana.kinbody is setup 0.0725 on +x axis
262  _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
263 
264  if( __cfg_autoload_IK ) {
265  _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_Transform6D);
266  }
267  } else {
268  throw fawkes::Exception("Unknown entry for 'arm_model':%s", __cfg_arm_model.c_str());
269  }
270 
271  } catch (Exception& e) {
272  // TODO: not just simple throw....
273  throw;
274  }
275 
276  if( __cfg_use_viewer)
277  _openrave->start_viewer();
278 }
279 
280 void
282 {
283  delete(__OR_robot);
284  __OR_robot = NULL;
285 
286  delete(__OR_manip);
287  __OR_manip = NULL;
288 }
289 
290 void
292 {
293 #ifndef EARLY_PLANNING
294  if( !plan_target() ) {
295  _finished = true;
296  return;
297  }
298 #else
300  _finished = true;
301  return;
302  }
303 #endif
304 
305  // Get trajectories and move katana along them
306  __target_traj = __OR_robot->get_trajectory_device();
307  Time time_now, time_last = Time();
308  try {
309  bool final = false;
310  __it = __target_traj->begin();
311  while (!final) {
312  time_last.stamp_systime();
313  final = move_katana();
314  update_openrave_data();
315  time_now.stamp_systime();
316 
317  // Wait before sending next command. W.it until 5ms before reached time for next traj point
318  // CAUTION! In order for this to work correctly, you need to assure that OpenRAVE model of the
319  // arm and the real device have the same velocity, i.e. need the same amount of time to complete
320  // a movement. Otherwise sampling over time and waiting does not make much sense.
321  // Disable the following line if requirement not fulfilled.
322 
323  //usleep(1000*1000*(sampling + time_last.in_sec() - time_now.in_sec() - 0.005f));
324  }
325 
326  // check if encoders are close enough to target position
327  final = false;
328  while (!final) {
329  final = true;
330  update_openrave_data();
331  try {
332  final = _katana->final();
334  _logger->log_warn("KatanaMotorControlTrhead", e.what());
336  break;
337  }
338  }
339 
340  } catch (fawkes::Exception &e) {
341  _logger->log_warn("KatanaGotoThread", "Moving along trajectory failed (ignoring): %s", e.what());
342  _finished = true;
344  _katana->stop();
345  return;
346  }
347 
348 
349  // TODO: Check for finished motion
350  // Can't use current version like in goto_thread, because target is
351  // not set in libkni! can check endeffector position instead, or
352  // check last angle values from __target_traj with katana-arm values
353 
354  _finished = true;
355 }
356 
357 
358 /** Peform path-planning on target.
359  * @return true if ik solvable and path planned, false otherwise
360  */
361 bool
362 KatanaGotoOpenRaveThread::plan_target()
363 {
364  // Fetch motor encoder values
365  if( !update_motor_data() ) {
366  _logger->log_warn("KatanaGotoThread", "Fetching current motor values failed");
368  return false;
369  }
370 
371  // Set starting point for planner, convert encoder values to angles if necessary
372  if( !_katana->joint_angles() ) {
373  encToRad(__motor_encoders, __motor_angles);
374  }
375  __OR_manip->set_angles_device(__motor_angles);
376 
377  // Checking if target has IK solution
378  if( __plannerparams.compare("default") == 0 ) {
379  __plannerparams = DEFAULT_PLANNERPARAMS;
380  }
381  if( __is_target_object) {
382  _logger->log_debug(name(), "Check IK for object (%s)", __target_object.c_str());
383 
384  if( !_openrave->set_target_object(__target_object, __OR_robot) ) {
385  _logger->log_warn("KatanaGotoThread", "Initiating goto failed, no IK solution found");
387  return false;
388  }
389  }
390  else {
391  bool success = false;
392  try {
393  if( __has_target_quaternion ) {
394  _logger->log_debug(name(), "Check IK(%f,%f,%f | %f,%f,%f,%f)",
395  __x, __y, __z, __quat_x, __quat_y, __quat_z, __quat_w);
396  success = __OR_robot->set_target_quat(__x, __y, __z, __quat_w, __quat_x, __quat_y, __quat_z);
397  } else if( __move_straight ) {
398  _logger->log_debug(name(), "Check IK(%f,%f,%f), straight movement",
399  __x, __y, __z);
400  if( __is_arm_extension ) {
401  success = __OR_robot->set_target_rel(__x, __y, __z, true);
402  } else {
403  success = __OR_robot->set_target_straight(__x, __y, __z);
404  }
405  if( __plannerparams_straight.compare("default") == 0 ) {
406  __plannerparams_straight = DEFAULT_PLANNERPARAMS_STRAIGHT;
407  }
408  } else {
409  float theta_error = 0.0f;
410  while( !success && (theta_error <= __theta_error)) {
411  _logger->log_debug(name(), "Check IK(%f,%f,%f | %f,%f,%f)",
412  __x, __y, __z, __phi, __theta+theta_error, __psi);
413  success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta+theta_error, __psi);
414  if( !success ) {
415  _logger->log_debug(name(), "Check IK(%f,%f,%f | %f,%f,%f)",
416  __x, __y, __z, __phi, __theta-theta_error, __psi);
417  success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta-theta_error, __psi);
418  }
419 
420  theta_error += 0.01;
421  }
422  }
423  } catch(OpenRAVE::openrave_exception &e) {
424  _logger->log_debug(name(), "OpenRAVE exception:%s", e.what());
425  }
426 
427  if( !success ) {
428  _logger->log_warn("KatanaGotoThread", "Initiating goto failed, no IK solution found");
430  return false;
431  }
432  }
433  if( __move_straight ) {
434  __OR_robot->set_target_plannerparams(__plannerparams_straight);
435  } else {
436  __OR_robot->set_target_plannerparams(__plannerparams);
437  }
438 
439  // Run planner
440  float sampling = 0.04f; //maybe catch from config? or "learning" depending on performance?
441  try {
442  _openrave->run_planner(__OR_robot, sampling);
443  } catch (fawkes::Exception &e) {
444  _logger->log_warn("KatanaGotoThread", "Planner failed (ignoring): %s", e.what());
446  return false;
447  }
448 
450  return true;
451 }
452 
453 /** Update data of arm in OpenRAVE model */
454 void
455 KatanaGotoOpenRaveThread::update_openrave_data()
456 {
457  // Fetch motor encoder values
458  if( !update_motor_data() ) {
459  _logger->log_warn("KatanaGotoThread", "Fetching current motor values failed");
460  _finished = true;
461  return;
462  }
463 
464  // Convert encoder values to angles if necessary
465  if( !_katana->joint_angles() ) {
466  encToRad(__motor_encoders, __motor_angles);
467  }
468  __OR_manip->set_angles_device(__motor_angles);
469 
470  std::vector<OpenRAVE::dReal> angles;
471  __OR_manip->get_angles(angles);
472 
473  {
474  OpenRAVE::EnvironmentMutex::scoped_lock lock(__OR_robot->get_robot_ptr()->GetEnv()->GetMutex());
475  __OR_robot->get_robot_ptr()->SetActiveDOFValues(angles);
476  }
477 }
478 
479 /** Update motors and fetch current encoder values.
480  * @return true if succesful, false otherwise
481  */
482 bool
483 KatanaGotoOpenRaveThread::update_motor_data()
484 {
485  short num_errors = 0;
486 
487  // update motors
488  while (1) {
489  //usleep(__poll_interval_usec);
490  try {
491  _katana->read_sensor_data(); // update sensor data
492  _katana->read_motor_data(); // update motor data
493  } catch (Exception &e) {
494  if (++num_errors <= 10) {
495  _logger->log_warn("KatanaGotoThread", "Receiving motor data failed, retrying");
496  continue;
497  } else {
498  _logger->log_warn("KatanaGotoThread", "Receiving motor data failed too often, aborting");
500  return 0;
501  }
502  }
503  break;
504  }
505 
506  // fetch joint values
507  num_errors = 0;
508  while (1) {
509  //usleep(__poll_interval_usec);
510  try {
511  if( _katana->joint_angles()) {
512  _katana->get_angles(__motor_angles, false); //fetch encoder values, param refreshEncoders=false
513  } else {
514  _katana->get_encoders(__motor_encoders, false); //fetch encoder values, param refreshEncoders=false
515  }
516  } catch (Exception &e) {
517  if (++num_errors <= 10) {
518  _logger->log_warn("KatanaGotoThread", "Receiving motor %s failed, retrying", _katana->joint_angles() ? "angles" : "encoders");
519  continue;
520  } else {
521  _logger->log_warn("KatanaGotoThread", "Receiving motor %s failed, aborting", _katana->joint_angles() ? "angles" : "encoders");
523  return 0;
524  }
525  }
526  break;
527  }
528 
529  return 1;
530 }
531 
532 
533 /** Realize next trajectory point.
534  * Take the next point from the current trajectory __target_traj, set its
535  * joint values and advance the iterator.
536  * @return true if the trajectory is finished, i.e. there are no more points
537  * left, false otherwise.
538  */
539 bool
540 KatanaGotoOpenRaveThread::move_katana()
541 {
542  if( _katana->joint_angles() ) {
543  _katana->move_to(*__it, /*blocking*/false);
544  } else {
545  std::vector<int> enc;
546  _katana->get_encoders(enc);
547  _katana->move_to(enc, /*blocking*/false);
548  }
549 
550  return (++__it == __target_traj->end());
551 }
552 
553 #endif
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void once()
Execute an action exactly once.
Definition: thread.cpp:1065
Class containing information about all katana6M180 motors.
Definition: katana6M180.h:33
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:800
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
Fawkes library namespace.
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:50
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:54
A class for handling time.
Definition: time.h:91
Katana motion thread base class.
Definition: motion_thread.h:37
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
Definition: conversion.h:56
Class containing information about all neuronics-katana motors.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void finalize()
Finalize the thread.
Definition: thread.cpp:469
static const uint32_t ERROR_NONE
ERROR_NONE constant.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)=0
Move endeffctor to given coordinates.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
Interface for a OpenRave connection creator.
At least one motor crashed.
Definition: exception.h:45
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:52
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:56
virtual void init()
Initialize the thread.
Definition: thread.cpp:346
virtual bool final()=0
Check if movement is final.
virtual void stop()=0
Stop movement immediately.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
Interface for logging.
Definition: logger.h:34