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
44using 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
57const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
58 "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>"
59 "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>"
60 "</_postprocessing>\n";
61const 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 */
76KatanaGotoOpenRaveThread::KatanaGotoOpenRaveThread(fawkes::RefPtr<fawkes::KatanaController> katana,
77 fawkes::Logger * logger,
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 */
110void
111KatanaGotoOpenRaveThread::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 */
135void
136KatanaGotoOpenRaveThread::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 */
161void
162KatanaGotoOpenRaveThread::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 */
172void
173KatanaGotoOpenRaveThread::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 */
183void
184KatanaGotoOpenRaveThread::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 */
194void
195KatanaGotoOpenRaveThread::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 */
204void
205KatanaGotoOpenRaveThread::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 */
218void
219KatanaGotoOpenRaveThread::set_plannerparams(const char *params, bool straight)
220{
221 if (straight) {
222 plannerparams_straight_ = params;
223 } else {
224 plannerparams_ = params;
225 }
226}
227
228void
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
288void
290{
291 _openrave->set_active_robot(NULL);
292 OR_robot_ = NULL;
293 OR_manip_ = NULL;
294}
295
296void
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 */
367bool
368KatanaGotoOpenRaveThread::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 */
479void
480KatanaGotoOpenRaveThread::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 */
507bool
508KatanaGotoOpenRaveThread::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 */
569bool
570KatanaGotoOpenRaveThread::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
Katana motion thread base class.
Definition: motion_thread.h:36
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:52
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:48
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:50
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:54
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual bool final()=0
Check if movement is final.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void stop()=0
Stop movement immediately.
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.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
At least one motor crashed.
Definition: exception.h:44
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Interface for a OpenRave connection creator.
Class containing information about all katana6M180 motors.
Definition: katana6M180.h:31
Class containing information about all neuronics-katana motors.
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void once()
Execute an action exactly once.
Definition: thread.cpp:1085
virtual void init()
Initialize the thread.
Definition: thread.cpp:343
virtual void finalize()
Finalize the thread.
Definition: thread.cpp:463
A class for handling time.
Definition: time.h:93
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:720
Fawkes library namespace.
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
Definition: conversion.h:56