Fawkes API Fawkes Development Version
robot.cpp
1
2/***************************************************************************
3 * robot.cpp - Fawkes to OpenRAVE Robot Handler
4 *
5 * Created: Mon Sep 20 14:50:34 2010
6 * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
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 "robot.h"
24
25#include "environment.h"
26#include "manipulator.h"
27
28#include <core/exceptions/software.h>
29#include <logging/logger.h>
30
31#include <openrave-core.h>
32
33using namespace OpenRAVE;
34namespace fawkes {
35
36/** @class OpenRaveRobot <plugins/openrave/robot.h>
37* Class handling interaction with the OpenRAVE::RobotBase class.
38* This class mainly handles robot specific tasks, like setting a
39* target, looking for IK solutions and handling planning parameters
40* for the robot.
41* @author Bahram Maleki-Fard
42*/
43
44/** Constructor
45 * @param logger pointer to fawkes logger
46 */
48: logger_(logger), name_(""), manip_(0), find_best_ik_(1)
49{
50 init();
51}
52/** Constructor
53 * @param filename path to robot's xml file
54 * @param env pointer to OpenRaveEnvironment object
55 * @param logger pointer to fawkes logger
56 */
57OpenRaveRobot::OpenRaveRobot(const std::string & filename,
59 fawkes::Logger * logger)
60: logger_(logger),
61 name_(""),
62 name_str_(""),
63 manip_(0),
64 target_(TARGET_NONE),
65 find_best_ik_(1),
66 trans_offset_x_(0),
67 trans_offset_y_(0),
68 trans_offset_z_(0),
69 display_planned_movements_(false)
70{
71 init();
72 this->load(filename, env);
73}
74
75/** Copy Constructor.
76 * @param src The OpenRaveRobot to clone
77 * @param new_env Pointer to the new OpenRaveEnvironment. We need this to set robot_
78 * to the correct robot in the new OpenRAVE-environment.
79 */
81 const fawkes::OpenRaveEnvironmentPtr &new_env)
82: logger_(src.logger_), name_(src.name_), find_best_ik_(src.find_best_ik_)
83{
84 build_name_str();
85 traj_ = new std::vector<std::vector<dReal>>();
86
87 trans_offset_x_ = src.trans_offset_x_;
88 trans_offset_y_ = src.trans_offset_y_;
89 trans_offset_z_ = src.trans_offset_z_;
90
91 // Get correct robot from environment.
92 EnvironmentMutex::scoped_lock lock(new_env->get_env_ptr()->GetMutex());
93 std::string name = src.get_robot_ptr()->GetName();
94 robot_ = new_env->get_env_ptr()->GetRobot(name);
95
96 if (!robot_)
97 throw fawkes::IllegalArgumentException("%s: Robot '%s' could not be loaded. "
98 "Check name.",
99 this->name(),
100 name.c_str());
101
102 build_name_str();
103 if (logger_) {
104 logger_->log_debug(this->name(), "Robot loaded.");
105 }
106
107 // Initialize robot
108 set_ready();
109
110 // Set the same manipulator "active" as it was in previous environment
111 // "set_ready()" just takes the first manipulator it finds
112 {
113 EnvironmentMutex::scoped_lock lock(src.get_robot_ptr()->GetEnv()->GetMutex());
114 arm_ = robot_->SetActiveManipulator(src.get_robot_ptr()->GetActiveManipulator()->GetName());
115 }
116 robot_->SetActiveDOFs(arm_->GetArmIndices());
117
118 manip_ = src.get_manipulator()->copy();
119 target_.manip = manip_->copy();
120 display_planned_movements_ = false;
121
122 if (logger_) {
123 logger_->log_debug(this->name(), "Robot '%s' cloned.", name.c_str());
124 }
125}
126
127/** Destructor */
129{
130 target_.manip = NULL;
131
132 //unload everything related to this robot from environment
133 try {
134 EnvironmentBasePtr env = robot_->GetEnv();
135 EnvironmentMutex::scoped_lock lock(env->GetMutex());
136 env->Remove(mod_basemanip_);
137 env->Remove(robot_);
138 if (logger_)
139 logger_->log_warn(name(), "Robot unloaded from environment");
140 } catch (const openrave_exception &e) {
141 if (logger_)
142 logger_->log_warn(name(),
143 "Could not unload robot properly from environment. Ex:%s",
144 e.what());
145 }
146}
147
148/** Build name string to use in logging messages.
149 * Nothing important, but helpful for debugging etc.
150 */
151void
152OpenRaveRobot::build_name_str()
153{
154 std::stringstream n;
155 n << "OpenRaveRobot"
156 << "[";
157 if (robot_)
158 n << RaveGetEnvironmentId(robot_->GetEnv()) << ":";
159 n << name_ << "]";
160 name_str_ = n.str();
161}
162
163/** Get the name string to use in logging messages etc. */
164const char *
165OpenRaveRobot::name() const
166{
167 return name_str_.c_str();
168}
169
170/** Inittialize object attributes */
171void
172OpenRaveRobot::init()
173{
174 traj_ = new std::vector<std::vector<dReal>>();
175
176 trans_offset_x_ = 0.f;
177 trans_offset_y_ = 0.f;
178 trans_offset_z_ = 0.f;
179
180 build_name_str();
181}
182
183/** Load robot from xml file
184 * @param filename path to robot's xml file
185 * @param env pointer to OpenRaveEnvironment object
186 */
187void
188OpenRaveRobot::load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
189{
190 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
191
192 // TODO: implementing without usage of 'environment'
193 // openrave_exception handling is done in OpenRAVE (see environment-core.h)
194 robot_ = env->get_env_ptr()->ReadRobotXMLFile(filename);
195
196 if (!robot_)
198 "%s: Robot could not be loaded. Check xml file/path '%s'.", name(), filename.c_str());
199
200 name_ = robot_->GetName();
201 build_name_str();
202
203 if (logger_)
204 logger_->log_debug(name(), "Robot loaded.");
205}
206
207/** Set robot ready for usage.
208 * Here: Set active DOFs and create plannerParameters.
209 * CAUTION: Only successful after added to environment. Otherwise no active DOF will be recognized. */
210void
212{
213 if (!robot_)
214 throw fawkes::Exception("%s: Robot not loaded properly yet.", name());
215
216 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
217
218 robot_->SetActiveManipulator(robot_->GetManipulators().at(0)->GetName());
219 arm_ = robot_->GetActiveManipulator();
220 robot_->SetActiveDOFs(arm_->GetArmIndices());
221
222 if (robot_->GetActiveDOF() == 0)
223 throw fawkes::Exception(
224 "%s: Robot not added to environment yet. Need to do that first, otherwise planner will fail.",
225 name());
226
227 // create planner parameters
228 try {
229 PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
230 planner_params_ = params;
231 planner_params_->_nMaxIterations = 4000; // max iterations before failure
232 planner_params_->SetRobotActiveJoints(
233 robot_); // set planning configuration space to current active dofs
234 planner_params_->vgoalconfig.resize(robot_->GetActiveDOF());
235 } catch (const openrave_exception &e) {
236 throw fawkes::Exception("%s: Could not create PlannerParameters. Ex:%s", name(), e.what());
237 }
238
239 // create and load BaseManipulation module
240 try {
241 mod_basemanip_ = RaveCreateModule(robot_->GetEnv(), "basemanipulation");
242 robot_->GetEnv()->AddModule(mod_basemanip_, robot_->GetName());
243 } catch (const openrave_exception &e) {
244 throw fawkes::Exception("%s: Cannot load BaseManipulation Module. Ex:%s", name(), e.what());
245 }
246
247 if (logger_)
248 logger_->log_debug(name(), "Robot ready.");
249}
250
251/** Directly set transition offset between coordinate systems
252 * of real device and OpenRAVE model.
253 * @param trans_x transition offset on x-axis
254 * @param trans_y transition offset on y-axis
255 * @param trans_z transition offset on z-axis
256 */
257void
258OpenRaveRobot::set_offset(float trans_x, float trans_y, float trans_z)
259{
260 trans_offset_x_ = trans_x;
261 trans_offset_y_ = trans_y;
262 trans_offset_z_ = trans_z;
263}
264
265/** Calculate transition offset between coordinate systems
266 * of real device and OpenRAVE model.
267 * Sets model's angles to current device's angles (from manip_),
268 * and compares transitions.
269 * @param device_trans_x transition on x-axis (real device)
270 * @param device_trans_y transition on y-axis (real device)
271 * @param device_trans_z transition on z-axis (real device)
272 */
273void
274OpenRaveRobot::calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
275{
276 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
277 // get device's current angles, and set them for OpenRAVE model
278 std::vector<dReal> angles;
279 manip_->get_angles(angles);
280 robot_->SetActiveDOFValues(angles);
281
282 // get model's current transition and compare
283 arm_ = robot_->GetActiveManipulator();
284 Transform trans = arm_->GetEndEffectorTransform();
285 trans_offset_x_ = trans.trans[0] - device_trans_x;
286 trans_offset_y_ = trans.trans[1] - device_trans_y;
287 trans_offset_z_ = trans.trans[2] - device_trans_z;
288}
289
290/** Set pointer to OpenRaveManipulator object.
291 * Make sure this is called AFTER all manipulator settings have
292 * been set (assures that target_.manip has the same settings).
293 * @param manip pointer to OpenRaveManipulator object
294 * @param display_movements true, if movements should be displayed in viewer.
295 * Better be "false" if want to sync OpenRAVE models with device
296 */
297void
299{
300 manip_ = manip;
301 target_.manip = manip_->copy();
302
303 display_planned_movements_ = display_movements;
304}
305
306/** Update motor values from OpenRAVE model.
307 * Can be used to sync real device with OpenRAVE model*/
308void
310{
311 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
312 std::vector<dReal> angles;
313 robot_->GetActiveDOFValues(angles);
314 manip_->set_angles(angles);
315}
316
317/** Update/Set OpenRAVE motor angles */
318void
320{
321 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
322 std::vector<dReal> angles;
323 manip_->get_angles(angles);
324 robot_->SetActiveDOFValues(angles);
325}
326
327/** Getter for display_planned_movements_.
328 * @return return value
329 */
330bool
332{
333 return display_planned_movements_;
334}
335
336/** Activate/Deactive IK comparison.
337 * When activated, we don't just take the first returned IK solution, but
338 * compare them all to find the best, i.e. the one that is "closest" to our
339 * current configuration.
340 * @param enable Sets the state of the comparison. Enabled by default.
341 */
342void
344{
345 find_best_ik_ = enable;
346}
347
348/** Set target, given relative transition.
349 * This is the prefered method to set a target for straight manipulator movement.
350 * @param trans_x x-transition
351 * @param trans_y y-transition
352 * @param trans_z z-transition
353 * @param is_extension true, if base coordination system lies in arm extension
354 * @return true if solvable, false otherwise
355 */
356bool
357OpenRaveRobot::set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension)
358{
359 if (is_extension) {
360 target_.type = TARGET_RELATIVE_EXT;
361 } else {
362 target_.type = TARGET_RELATIVE;
363 }
364 target_.x = trans_x;
365 target_.y = trans_y;
366 target_.z = trans_z;
367
368 // Not sure how to check IK solvability yet. Would be nice to have this
369 // checked before planning a path.
370 target_.solvable = true;
371
372 return target_.solvable;
373}
374
375/** Set target for a straight movement, given transition.
376 * This is the a wrapper for "set_target_rel", to be able to call for a
377 * straight arm movement by giving non-relative transition.
378 * @param trans_x x-transition
379 * @param trans_y y-transition
380 * @param trans_z z-transition
381 * @return true if solvable, false otherwise
382 */
383bool
384OpenRaveRobot::set_target_straight(float trans_x, float trans_y, float trans_z)
385{
386 Transform trans;
387 {
388 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
389 arm_ = robot_->GetActiveManipulator();
390 trans = arm_->GetEndEffectorTransform();
391 }
392
393 return set_target_rel(trans_x - trans.trans[0],
394 trans_y - trans.trans[1],
395 trans_z - trans.trans[2]);
396}
397
398/** Set target, given transition, and rotation as quaternion.
399 * @param trans_x x-transition
400 * @param trans_y y-transition
401 * @param trans_z z-transition
402 * @param quat_w quaternion skalar
403 * @param quat_x quaternion 1st value
404 * @param quat_y quaternion 2nd value
405 * @param quat_z quaternion 3rd value
406 * @param filter IK filter options (see OpenRAVE doc for details)
407 * @param no_offset if true, do not include manipulator offset (default: false)
408 * @return true if solvable, false otherwise
409 */
410bool
412 float trans_y,
413 float trans_z,
414 float quat_w,
415 float quat_x,
416 float quat_y,
417 float quat_z,
418 IkFilterOptions filter,
419 bool no_offset)
420{
421 Vector trans(trans_x, trans_y, trans_z);
422 Vector rot(quat_w, quat_x, quat_y, quat_z);
423
424 return set_target_transform(trans, rot, filter, no_offset);
425}
426
427/** Set target, given transition, and rotation as axis-angle.
428 * @param trans_x x-transition
429 * @param trans_y y-transition
430 * @param trans_z z-transition
431 * @param angle axis-angle angle
432 * @param axisX axis-angle x-axis value
433 * @param axisY axis-angle y-axis value
434 * @param axisZ axis-angle z-axis value
435 * @param filter IK filter options (see OpenRAVE doc for details)
436 * @param no_offset if true, do not include manipulator offset (default: false)
437 * @return true if solvable, false otherwise
438 */
439bool
441 float trans_y,
442 float trans_z,
443 float angle,
444 float axisX,
445 float axisY,
446 float axisZ,
447 IkFilterOptions filter,
448 bool no_offset)
449{
450 Vector trans(trans_x, trans_y, trans_z);
451 Vector aa(angle, axisX, axisY, axisZ);
452 Vector rot = quatFromAxisAngle(aa);
453
454 return set_target_transform(trans, rot, filter, no_offset);
455}
456
457/** Set target, given transition, and Euler-rotation.
458 * @param type Euler-rotation type (ZXZ, ZYZ, ...)
459 * @param trans_x x-transition
460 * @param trans_y y-transition
461 * @param trans_z z-transition
462 * @param phi 1st rotation
463 * @param theta 2nd rotation
464 * @param psi 3rd rotation
465 * @param filter IK filter options (see OpenRAVE doc for details)
466 * @param no_offset if true, do not include manipulator offset (default: false)
467 * @return true if solvable, false otherwise
468 */
469bool
471 float trans_x,
472 float trans_y,
473 float trans_z,
474 float phi,
475 float theta,
476 float psi,
477 OpenRAVE::IkFilterOptions filter,
478 bool no_offset)
479{
480 Vector trans(trans_x, trans_y, trans_z);
481 std::vector<float> rot(9, 0.f); //rotations vector
482
483 switch (type) {
484 case (EULER_ZXZ):
485 if (logger_)
486 logger_->log_debug(
487 name(), "Target EULER_ZXZ: %f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
488 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
489 rot.at(3) = theta; //2nd row, 1st value; rotation on x-axis
490 rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
491 break;
492
493 case (EULER_ZYZ):
494 if (logger_)
495 logger_->log_debug(name(),
496 "Target EULER_ZYZ:",
497 "%f %f %f %f %f %f",
498 trans_x,
499 trans_y,
500 trans_z,
501 phi,
502 theta,
503 psi);
504 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
505 rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
506 rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
507 break;
508
509 case (EULER_ZYX):
510 if (logger_)
511 logger_->log_debug(name(),
512 "Target EULER_ZYX:",
513 "%f %f %f %f %f %f",
514 trans_x,
515 trans_y,
516 trans_z,
517 phi,
518 theta,
519 psi);
520 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
521 rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
522 rot.at(6) = psi; //3rd row, 1st value; rotation on x-axis
523 break;
524
525 default:
526 target_.type = TARGET_NONE;
527 target_.solvable = false;
528 return false;
529 }
530
531 return set_target_euler(trans, rot, filter, no_offset);
532}
533
534/** Set target by giving position of an object.
535 * Currently the object should be cylindric, and stand upright. It may
536 * also be rotated on its x-axis, but that rotation needs to be given in an argument
537 * to calculate correct position for end-effector. This is only temporary until
538 * proper grasp planning for 5DOF in OpenRAVE is provided.
539 * @param trans_x x-transition of object
540 * @param trans_y y-transition of object
541 * @param trans_z z-transition of object
542 * @param rot_x rotation of object on x-axis (radians) (default: 0.f, i.e. upright)
543 * @param filter IK filter options (see OpenRAVE doc for details)
544 * @return true if solvable, false otherwise
545 */
546bool
548 float trans_y,
549 float trans_z,
550 float rot_x,
551 IkFilterOptions filter)
552{
553 // This is about 2 times faster than using setTargetEuler each time, especially when it comes
554 // to the while loop (whole loop: ~56ms vs ~99ms)
555
556 // release all attached/grabbed bodys
557 {
558 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
559 robot_->ReleaseAllGrabbed();
560 }
561
562 // quaternion defining consecutiv rotations on axis
563 float alpha =
564 atan2(trans_y - trans_offset_y_,
565 trans_x - trans_offset_x_); //angle to rotate left/right when manipulator points to +x
566 Vector quat_y =
567 quatFromAxisAngle(Vector(0.f, M_PI / 2, 0.f)); //1st, rotate down -> manipulator points to +x
568 Vector quat_x = quatFromAxisAngle(
569 Vector(-alpha, 0.f, 0.f)); //2nd, rotate left/right -> manipulator points to object
570 Vector quat_z =
571 quatFromAxisAngle(Vector(0.f, 0.f, rot_x)); //last, rotate wrist -> manipulator ready to grab
572
573 Vector quat_xY = quatMultiply(quat_y, quat_x);
574 Vector quat_xYZ = quatMultiply(quat_xY, quat_z);
575
576 Vector trans(trans_x, trans_y, trans_z);
577
578 if (set_target_transform(trans, quat_xYZ, filter, true))
579 return true;
580
581 //try varying 2nd rotation (quat_y) until a valid IK is found. Max angle: 45° (~0.79 rad)
582 Vector quatPosY = quatFromAxisAngle(Vector(0.f, 0.017f, 0.f)); //rotate up for 1°
583 Vector quatNegY = quatFromAxisAngle(Vector(0.f, -0.017f, 0.f)); //rotate down for 1°
584
585 Vector quatPos(quat_xY); //starting position, after first 2 rotations
586 Vector quatNeg(quat_xY);
587
588 unsigned int count = 0;
589 bool foundIK = false;
590
591 while ((!foundIK) && (count <= 45)) {
592 count++;
593
594 quatPos = quatMultiply(quatPos, quatPosY); //move up ~1°
595 quatNeg = quatMultiply(quatNeg, quatNegY); //move down ~1°
596
597 quat_xYZ = quatMultiply(quatPos, quat_z); //apply wrist rotation
598 foundIK = set_target_transform(trans, quat_xYZ, filter, true);
599 if (!foundIK) {
600 quat_xYZ = quatMultiply(quatNeg, quat_z);
601 foundIK = set_target_transform(trans, quat_xYZ, filter, true);
602 }
603 }
604
605 return foundIK;
606}
607
608/** Set target by giving IkParameterizaion of target.
609 * OpenRAVE::IkParameterization is the desired type to be calculated with
610 * by OpenRAVE. Each oter type (i.e. Transform) is implicitly transformed
611 * to an IkParameterization before continuing to check for Ik solution and
612 * planning, i.e. by the BaseManipulation module.
613 * @param ik_param the OpenRAVE::IkParameterization of the target
614 * @param filter IK filter options (see OpenRAVE doc for details)
615 * @return true if solvable, false otherwise
616 */
617bool
618OpenRaveRobot::set_target_ikparam(IkParameterization ik_param, IkFilterOptions filter)
619{
620 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
621 arm_ = robot_->GetActiveManipulator();
622
623 target_.ikparam = ik_param;
624 target_.type = TARGET_IKPARAM;
625 solve_ik(filter);
626
627 return target_.solvable;
628}
629
630/** Set additional planner parameters.
631 * BaseManipulation module accepts many arguments that can be passed.
632 * Planner parameters can be important to plan a path according to ones
633 * needs, e.g. set deviations, optimizer iterations, etc.
634 * Do not mistake it with the single argument "plannerparams" of BaseManipulation.
635 * @param params complete string of additional arguments.
636 */
637void
639{
640 target_.plannerparams = params;
641}
642
643/** Set additional planner parameters.
644 * @param params complete string of additional arguments.
645 */
646void
648{
649 target_.plannerparams = params;
650}
651
652/** Set raw command for BaseManipulation module.
653 * BaseManipulation module accepts many arguments that can be passed.
654 * Basic commands are covered by the other set_target_ methods. In case something
655 * is not covered, or you want to send a custom command, use this method.
656 * Remember that plannerparams set by "set_target_plannerparams" are still added
657 * to the planner, so make sure you don't send duplicate entries both in plannerparams
658 * and in the raw command string.
659 * @param cmd complete command string.
660 */
661void
663{
664 target_.raw_cmd = cmd;
665}
666
667/** Set raw command for BaseManipulation module.
668 * @param cmd complete command string.
669 */
670void
672{
673 target_.raw_cmd = cmd;
674}
675
676/** Set target angles directly.
677 * @param angles vector with angle values
678 */
679void
680OpenRaveRobot::set_target_angles(std::vector<float> &angles)
681{
682 target_.manip->set_angles(angles);
683 target_.type = TARGET_JOINTS;
684 target_.solvable = true; //no IK check done though!
685}
686
687/* ################### getters ##################*/
688/** Returns RobotBasePtr for uses in other classes.
689 * @return RobotBasePtr of current robot
690 */
691OpenRAVE::RobotBasePtr
693{
694 return robot_;
695}
696
697/** Get target.
698 * @return target struct
699 */
702{
703 return target_;
704}
705
706/** Get manipulator.
707 * @return pointer to currentl used OpenRaveManipulator
708 */
711{
712 return manip_;
713}
714
715/** Updates planner parameters and return pointer to it
716 * @return PlannerParametersPtr or robot's planner params
717 */
718OpenRAVE::PlannerBase::PlannerParametersPtr
720{
721 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
722 // set planning configuration space to current active dofs
723 planner_params_->SetRobotActiveJoints(robot_);
724 planner_params_->vgoalconfig.resize(robot_->GetActiveDOF());
725
726 manip_->get_angles(planner_params_->vinitialconfig);
727 target_.manip->get_angles(planner_params_->vgoalconfig);
728
729 robot_->SetActiveDOFValues(planner_params_->vinitialconfig);
730
731 return planner_params_;
732}
733
734/** Return pointer to trajectory of motion from
735 * manip_ to target_.manip with OpenRAVE-model angle format
736 * @return pointer to trajectory
737 */
738std::vector<std::vector<dReal>> *
740{
741 return traj_;
742}
743
744/** Return pointer to trajectory of motion from
745 * manip_ to target_.manip with device angle format
746 * @return pointer to trajectory
747 */
748std::vector<std::vector<float>> *
750{
751 std::vector<std::vector<float>> *traj = new std::vector<std::vector<float>>();
752
753 std::vector<float> v;
754
755 for (unsigned int i = 0; i < traj_->size(); i++) {
756 manip_->angles_or_to_device(traj_->at(i), v);
757 traj->push_back(v);
758 }
759
760 return traj;
761}
762
763/** Return BaseManipulation Module-Pointer.
764 * @return ModuleBasePtr
765 */
766OpenRAVE::ModuleBasePtr
768{
769 return mod_basemanip_;
770}
771
772/* ###### attach / release kinbodys ###### */
773/** Attach a kinbody to the robot.
774 * @param object KinbodyPtr of object to be attached
775 * @param manip_name name of the manipulator to attach the object to.
776 * If non given, the currently active manipulator is taken.
777 * @return true if successful
778 */
779bool
780OpenRaveRobot::attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name)
781{
782 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
783
784 bool success = false;
785 try {
786 if (manip_name) {
787 // try attaching to given manipulator
788 RobotBase::ManipulatorPtr manip = robot_->SetActiveManipulator(manip_name);
789 if (!manip) {
790 if (logger_)
791 logger_->log_warn(name(),
792 "Could not attach Object, could not get manipulator '%s'",
793 manip_name);
794 return false;
795
796 } else {
797 success = robot_->Grab(object, manip->GetEndEffector());
798 }
799
800 } else {
801 // use currently active manipulator
802 success = robot_->Grab(object);
803 }
804 } catch (const OpenRAVE::openrave_exception &e) {
805 if (logger_)
806 logger_->log_warn(name(), "Could not attach Object. Ex:%s", e.what());
807 return false;
808 }
809
810 return success;
811}
812/** Attach a kinbody to the robot.
813 * @param name name of the object
814 * @param env pointer to OpenRaveEnvironment object
815 * @param manip_name name of the manipulator to attach the object to
816 * @return true if successful
817 */
818bool
821 const char * manip_name)
822{
823 OpenRAVE::KinBodyPtr body;
824 {
825 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
826 body = env->get_env_ptr()->GetKinBody(name);
827 }
828
829 return attach_object(body, manip_name);
830}
831
832/** Release a kinbody from the robot.
833 * @param object KinbodyPtr of object to be released
834 * @return true if successful
835 */
836bool
837OpenRaveRobot::release_object(OpenRAVE::KinBodyPtr object)
838{
839 try {
840 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
841 robot_->Release(object);
842 } catch (const OpenRAVE::openrave_exception &e) {
843 if (logger_)
844 logger_->log_warn(name(), "Could not release Object. Ex:%s", e.what());
845 return false;
846 }
847
848 return true;
849}
850/** Release a kinbody from the robot.
851 * @param name name of the object
852 * @param env pointer to OpenRaveEnvironment object
853 * @return true if successful
854 */
855bool
857{
858 OpenRAVE::KinBodyPtr body;
859 {
860 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
861 body = env->get_env_ptr()->GetKinBody(name);
862 }
863
864 return release_object(body);
865}
866
867/** Release all grabbed kinbodys from the robot.
868 * @return true if successful
869 */
870bool
872{
873 try {
874 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
875 robot_->ReleaseAllGrabbed();
876 } catch (const OpenRAVE::openrave_exception &e) {
877 if (logger_)
878 logger_->log_warn(name(), "Could not release all objects. Ex:%s", e.what());
879 return false;
880 }
881
882 return true;
883}
884
885/* ########################################
886 ###------------- private ------------###
887 ########################################*/
888
889/** Set target, given transformation (transition, and rotation as quaternion).
890 * Check IK solvability for target Transform. If solvable,
891 * then set target angles to manipulator configuration target_.manip
892 * @param trans transformation vector
893 * @param rotQuat rotation vector; a quaternion
894 * @param no_offset if true, do not include manipulator offset (default: false)
895 * @return true if solvable, false otherwise
896 */
897bool
898OpenRaveRobot::set_target_transform(Vector & trans,
899 OpenRAVE::Vector &rotQuat,
900 IkFilterOptions filter,
901 bool no_offset)
902{
903 Transform target;
904 target.trans = trans;
905 target.rot = rotQuat;
906
907 if (!no_offset) {
908 target.trans[0] += trans_offset_x_;
909 target.trans[1] += trans_offset_y_;
910 target.trans[2] += trans_offset_z_;
911 }
912
913 target_.type = TARGET_TRANSFORM;
914 target_.x = target.trans[0];
915 target_.y = target.trans[1];
916 target_.z = target.trans[2];
917 target_.qw = target.rot[0];
918 target_.qx = target.rot[1];
919 target_.qy = target.rot[2];
920 target_.qz = target.rot[3];
921
922 // check for supported IK types
923 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
924 arm_ = robot_->GetActiveManipulator();
925 if (arm_->GetIkSolver()->Supports(IKP_Transform6D)) {
926 if (logger_)
927 logger_->log_debug(name(), "6D suppport for arm %s", arm_->GetName().c_str());
928 // arm supports 6D ik. Perfect!
929 target_.ikparam = IkParameterization(target);
930 solve_ik(filter);
931
932 } else if (arm_->GetIkSolver()->Supports(IKP_TranslationDirection5D)) {
933 if (logger_)
934 logger_->log_debug(name(), "5D suppport");
935 // arm has only 5 DOF.
936 target_.ikparam = get_5dof_ikparam(target);
937 target_.solvable = set_target_ikparam(target_.ikparam, filter);
938
939 } else {
940 if (logger_)
941 logger_->log_debug(name(), "No IK suppport");
942 //other IK types not supported yet
943 target_.solvable = false;
944 }
945
946 return target_.solvable;
947}
948
949/** Set target, given 3 consecutive axis rotations.
950 * Axis rotations are given as 1 vector representing a 3x3 matrix,
951 * (left to right, top to bottom) where each row represents
952 * one rotation over one axis (axis-angle notation).
953 * See public setTargetEuler methods to get a better understanding.
954 *
955 * Check IK solvability for target Transform. If solvable,
956 * then set target angles to manipulator configuration target_.manip
957 * @param rotations 3x3 matrix given as one row.
958 * @param filter IK filter options (see OpenRAVE doc for details)
959 * @param no_offset if true, do not include manipulator offset (default: false)
960 * @return true if solvable, false otherwise
961 */
962bool
963OpenRaveRobot::set_target_euler(OpenRAVE::Vector & trans,
964 std::vector<float> & rotations,
965 OpenRAVE::IkFilterOptions filter,
966 bool no_offset)
967{
968 if (rotations.size() != 9) {
969 target_.type = TARGET_NONE;
970 target_.solvable = false;
971
972 if (logger_)
973 logger_->log_error(name(),
974 "Bad size of rotations vector. Is %i, expected 9",
975 rotations.size());
976 return false;
977 }
978
979 Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
980 Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
981 Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
982
983 if (logger_) {
984 logger_->log_debug(name(), "TEST Rot1: %f %f %f", r1[0], r1[1], r1[2]);
985 logger_->log_debug(name(), "TEST Rot2: %f %f %f", r2[0], r2[1], r2[2]);
986 logger_->log_debug(name(), "TEST Rot3: %f %f %f", r3[0], r3[1], r3[2]);
987 }
988
989 Vector q1 = quatFromAxisAngle(r1);
990 Vector q2 = quatFromAxisAngle(r2);
991 Vector q3 = quatFromAxisAngle(r3);
992
993 Vector q12 = quatMultiply(q1, q2);
994 Vector quat = quatMultiply(q12, q3);
995
996 return set_target_transform(trans, quat, filter, no_offset);
997}
998
999/** Get IkParameterization for a 5DOF arm given a 6D Transform.
1000 * @param trans The 6D OpenRAVE::Transform
1001 * @return the calculated 5DOF IkParameterization
1002 */
1003OpenRAVE::IkParameterization
1004OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform &trans)
1005{
1006 /* The initial pose (that means NOT all joints=0, but the manipulator's coordinate-system
1007 matching the world-coordinate-system) of an arm in OpenRAVE has its gripper pointing to the z-axis.
1008 Imagine a tube between the grippers. That tube lies on the y-axis.
1009 For 5DOF-IK one needs another manipulator definition, that has it's z-axis lying on that
1010 'tube', i.e. it needs to be lying between the fingers. That is achieved by rotating the
1011 coordinate-system first by +-90° around z-axis, then +90° on the rotated x-axis.
1012 */
1013
1014 // get direction vector for TranslationDirection5D
1015 /* Rotate Vector(0, +-1, 0) by target.rot. First need to figure out which of "+-"
1016 Now if the first rotation on z-axis was +90°, we need a (0,-1,0) direction vector.
1017 If it was -90°, we need (0, 1, 0). So just take the inverse of the first rotation
1018 and apply it to (1,0,0)
1019 */
1020 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
1021 Vector dir(1, 0, 0);
1022 {
1023 RobotBasePtr tmp_robot = robot_;
1024 RobotBase::RobotStateSaver saver(
1025 tmp_robot); // save the state, do not modifiy currently active robot!
1026
1027 //reset robot joints
1028 std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
1029 tmp_robot->SetActiveDOFValues(zero_joints);
1030
1031 // revert the rotations for the 5DOF manipulator specifition. See long comment above.
1032 // First rotate back -90° on x-axis (revert 2nd rotation)
1033 Transform cur_pos = arm_->GetEndEffectorTransform();
1034 Vector v1 = quatFromAxisAngle(Vector(-M_PI / 2, 0, 0));
1035 v1 = quatMultiply(cur_pos.rot, v1);
1036
1037 // Now get the inverse of 1st rotation and get our (0, +-1, 0) direction
1038 v1 = quatInverse(v1);
1039 TransformMatrix mat = matrixFromQuat(v1);
1040 dir = mat.rotate(dir);
1041 } // robot state is restored
1042
1043 // now rotate direction by target
1044 TransformMatrix mat = matrixFromQuat(trans.rot);
1045 dir = mat.rotate(dir);
1046
1047 IkParameterization ikparam = arm_->GetIkParameterization(IKP_TranslationDirection5D);
1048 ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
1049
1050 return ikparam;
1051}
1052
1053/** Find IK solution that is closest to current configuration.
1054 * This method checks and updates the internal target_ variable.
1055 * @return true if solvable, false otherwise.
1056 */
1057bool
1058OpenRaveRobot::solve_ik(IkFilterOptions filter)
1059{
1060 if (!find_best_ik_) {
1061 std::vector<dReal> solution;
1062 target_.solvable = arm_->FindIKSolution(target_.ikparam, solution, filter);
1063 target_.manip->set_angles(solution);
1064
1065 } else {
1066 std::vector<std::vector<dReal>> solutions;
1067
1068 // get all IK solutions
1069 target_.solvable = arm_->FindIKSolutions(target_.ikparam, solutions, filter);
1070 if (!target_.solvable)
1071 return false;
1072
1073 // pick closest solution to current configuration
1074 std::vector<std::vector<dReal>>::iterator sol;
1075 std::vector<dReal> cur;
1076 std::vector<dReal> diff;
1077 float dist = 100.f;
1078 arm_->GetArmDOFValues(cur);
1079
1080 for (sol = solutions.begin(); sol != solutions.end(); ++sol) {
1081 diff = cur;
1082 robot_->SubtractActiveDOFValues(diff, *sol);
1083
1084 float sol_dist = 0.f;
1085 for (unsigned int i = 0; i < diff.size(); ++i) {
1086 sol_dist += fabs(diff[i]);
1087 // use cur+diff instead of sol, to have better angles
1088 // for circular joints. Otherwise planner might have problems
1089 (*sol)[i] = cur[i] - diff[i];
1090 }
1091
1092 if (sol_dist < dist) {
1093 // found a solution that is closer
1094 dist = sol_dist;
1095 target_.manip->set_angles(*sol);
1096 }
1097 }
1098 }
1099
1100 return target_.solvable;
1101}
1102
1103} // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
Expected parameter is missing.
Definition: software.h:80
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.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
void angles_or_to_device(std::vector< T_from > &from, std::vector< T_to > &to) const
Transform OpenRAVE motor angles to real device angles.
Definition: manipulator.h:110
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Definition: manipulator.h:126
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
OpenRAVE Robot class.
Definition: robot.h:38
virtual void set_target_plannerparams(std::string &params)
Set additional planner parameters.
Definition: robot.cpp:638
virtual void set_offset(float trans_x, float trans_y, float trans_z)
Directly set transition offset between coordinate systems of real device and OpenRAVE model.
Definition: robot.cpp:258
virtual std::vector< std::vector< OpenRAVE::dReal > > * get_trajectory() const
Return pointer to trajectory of motion from manip_ to target_.manip with OpenRAVE-model angle format.
Definition: robot.cpp:739
virtual void set_target_raw(std::string &cmd)
Set raw command for BaseManipulation module.
Definition: robot.cpp:662
virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving IkParameterizaion of target.
Definition: robot.cpp:618
virtual void update_manipulator()
Update motor values from OpenRAVE model.
Definition: robot.cpp:309
virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false)
Set target, given relative transition.
Definition: robot.cpp:357
virtual bool set_target_quat(float trans_x, float trans_y, float trans_z, float quat_w, float quat_x, float quat_y, float quat_z, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as quaternion.
Definition: robot.cpp:411
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
Definition: robot.cpp:298
virtual target_t get_target() const
Get target.
Definition: robot.cpp:701
virtual bool set_target_straight(float trans_x, float trans_y, float trans_z)
Set target for a straight movement, given transition.
Definition: robot.cpp:384
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
Definition: robot.cpp:871
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
Definition: robot.cpp:47
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
Load robot from xml file.
Definition: robot.cpp:188
virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and Euler-rotation.
Definition: robot.cpp:470
virtual void set_ready()
Set robot ready for usage.
Definition: robot.cpp:211
virtual bool set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as axis-angle.
Definition: robot.cpp:440
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Definition: robot.cpp:719
virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving position of an object.
Definition: robot.cpp:547
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
Definition: robot.cpp:837
virtual bool display_planned_movements() const
Getter for display_planned_movements_.
Definition: robot.cpp:331
virtual ~OpenRaveRobot()
Destructor.
Definition: robot.cpp:128
virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name=NULL)
Attach a kinbody to the robot.
Definition: robot.cpp:780
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
Definition: robot.cpp:692
virtual std::vector< std::vector< float > > * get_trajectory_device() const
Return pointer to trajectory of motion from manip_ to target_.manip with device angle format.
Definition: robot.cpp:749
virtual void calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
Calculate transition offset between coordinate systems of real device and OpenRAVE model.
Definition: robot.cpp:274
virtual void enable_ik_comparison(bool enable)
Activate/Deactive IK comparison.
Definition: robot.cpp:343
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
Definition: robot.cpp:767
virtual OpenRaveManipulatorPtr get_manipulator() const
Get manipulator.
Definition: robot.cpp:710
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
Definition: robot.cpp:680
virtual void update_model()
Update/Set OpenRAVE motor angles.
Definition: robot.cpp:319
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Fawkes library namespace.
@ TARGET_TRANSFORM
Target: absolute endeffector translation and rotation.
Definition: types.h:54
@ TARGET_IKPARAM
Target: OpenRAVE::IkParameterization string.
Definition: types.h:57
@ TARGET_RELATIVE
Target: relative endeffector translation, based on robot's coordinate system.
Definition: types.h:55
@ TARGET_JOINTS
Target: motor joint values.
Definition: types.h:53
@ TARGET_NONE
No valid target.
Definition: types.h:52
@ TARGET_RELATIVE_EXT
Target: relative endeffector translation, based on arm extension.
Definition: types.h:56
euler_rotation_t
Euler rotations.
Definition: types.h:44
@ EULER_ZXZ
ZXZ rotation.
Definition: types.h:45
@ EULER_ZYX
ZYX rotation.
Definition: types.h:47
@ EULER_ZYZ
ZYZ rotation.
Definition: types.h:46
Struct containing information about the current target.
Definition: types.h:72
float qz
z value of quaternion
Definition: types.h:78
float x
translation on x-axis
Definition: types.h:73
float qy
y value of quaternion
Definition: types.h:77
OpenRaveManipulatorPtr manip
target manipulator configuration
Definition: types.h:81
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Definition: types.h:84
float qw
w value of quaternion
Definition: types.h:79
bool solvable
target IK solvable
Definition: types.h:80
target_type_t type
target type
Definition: types.h:82
float z
translation on z-axis
Definition: types.h:75
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
Definition: types.h:88
float y
translation on y-axis
Definition: types.h:74
std::string plannerparams
additional string to be passed to planner, i.e.
Definition: types.h:86
float qx
x value of quaternion
Definition: types.h:76