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