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