24 #include "manipulator.h"
25 #include "environment.h"
27 #include <openrave-core.h>
28 #include <logging/logger.h>
29 #include <core/exceptions/software.h>
66 this->
load(filename, env);
72 delete __target.
manip;
76 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
77 __robot->GetEnv()->Remove(__mod_basemanip);
78 __robot->GetEnv()->Remove(__robot);
79 }
catch(
const openrave_exception &e) {
81 {__logger->
log_warn(
"OpenRAVE Robot",
"Could not unload robot properly from environment. Ex:%s", e.what());}
89 __traj =
new std::vector< std::vector<dReal> >();
91 __trans_offset_x = 0.f;
92 __trans_offset_y = 0.f;
93 __trans_offset_z = 0.f;
106 __robot = env->
get_env_ptr()->ReadRobotXMLFile(filename);
111 {__logger->
log_debug(
"OpenRAVE Robot",
"Robot loaded.");}
123 __name = __robot->GetName();
124 __robot->SetActiveManipulator(__robot->GetManipulators().at(0)->GetName());
125 __arm = __robot->GetActiveManipulator();
126 __robot->SetActiveDOFs(__arm->GetArmIndices());
128 if(__robot->GetActiveDOF() == 0)
129 {
throw fawkes::Exception(
"OpenRAVE Robot: Robot not added to environment yet. Need to do that first, otherwise planner will fail.");}
133 PlannerBase::PlannerParametersPtr params(
new PlannerBase::PlannerParameters());
134 __planner_params = params;
135 __planner_params->_nMaxIterations = 4000;
136 __planner_params->SetRobotActiveJoints(__robot);
137 __planner_params->vgoalconfig.resize(__robot->GetActiveDOF());
138 }
catch(
const openrave_exception &e) {
139 throw fawkes::Exception(
"OpenRAVE Robot: Could not create PlannerParameters. Ex:%s", e.what());
144 __mod_basemanip = RaveCreateModule(__robot->GetEnv(),
"basemanipulation");
145 __robot->GetEnv()->AddModule( __mod_basemanip, __robot->GetName());
146 }
catch(
const openrave_exception &e) {
147 throw fawkes::Exception(
"OpenRAVE Robot: Cannot load BaseManipulation Module. Ex:%s", e.what());
151 {__logger->
log_debug(
"OpenRAVE Robot",
"Robot ready.");}
163 __trans_offset_x = trans_x;
164 __trans_offset_y = trans_y;
165 __trans_offset_z = trans_z;
179 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
181 std::vector<dReal> angles;
183 __robot->SetActiveDOFValues(angles);
186 __arm = __robot->GetActiveManipulator();
187 Transform trans = __arm->GetEndEffectorTransform();
188 __trans_offset_x = trans.trans[0] - device_trans_x;
189 __trans_offset_y = trans.trans[1] - device_trans_y;
190 __trans_offset_z = trans.trans[2] - device_trans_z;
206 __display_planned_movements = display_movements;
214 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
215 std::vector<dReal> angles;
216 __robot->GetActiveDOFValues(angles);
224 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
225 std::vector<dReal> angles;
227 __robot->SetActiveDOFValues(angles);
236 return __display_planned_movements;
255 __target.
x = trans_x;
256 __target.
y = trans_y;
257 __target.
z = trans_z;
277 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
278 __arm = __robot->GetActiveManipulator();
279 Transform trans = __arm->GetEndEffectorTransform();
282 trans_y - trans.trans[1],
283 trans_z - trans.trans[2]);
300 Vector trans(trans_x, trans_y, trans_z);
301 Vector rot(quat_w, quat_x, quat_y, quat_z);
303 return set_target_transform(trans, rot, no_offset);
320 Vector trans(trans_x, trans_y, trans_z);
321 Vector aa(angle, axisX, axisY, axisZ);
322 Vector rot = quatFromAxisAngle(aa);
324 return set_target_transform(trans, rot, no_offset);
341 Vector trans(trans_x, trans_y, trans_z);
342 std::vector<float> rot(9, 0.f);
346 __logger->
log_debug(
"TEST ZXZ",
"%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
353 __logger->
log_debug(
"TEST ZYZ",
"%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
392 __robot->ReleaseAllGrabbed();
395 float alpha = atan2(trans_y - __trans_offset_y, trans_x - __trans_offset_x);
396 Vector quat_y = quatFromAxisAngle(
Vector(0.f, M_PI/2, 0.f));
397 Vector quat_x = quatFromAxisAngle(
Vector(-alpha, 0.f, 0.f));
398 Vector quat_z = quatFromAxisAngle(
Vector(0.f, 0.f, rot_x));
400 Vector quat_xY = quatMultiply (quat_y, quat_x);
401 Vector quat_xYZ = quatMultiply (quat_xY, quat_z);
403 Vector trans(trans_x, trans_y, trans_z);
405 if( set_target_transform(trans, quat_xYZ,
true) )
409 Vector quatPosY=quatFromAxisAngle(
Vector(0.f, 0.017f, 0.f));
410 Vector quatNegY=quatFromAxisAngle(
Vector(0.f, -0.017f, 0.f));
415 unsigned int count = 0;
416 bool foundIK =
false;
418 while( (!foundIK) && (count <= 45)) {
421 quatPos = quatMultiply(quatPos, quatPosY);
422 quatNeg = quatMultiply(quatNeg, quatNegY);
424 quat_xYZ = quatMultiply(quatPos, quat_z);
425 foundIK = set_target_transform(trans, quat_xYZ,
true);
427 quat_xYZ = quatMultiply(quatNeg, quat_z);
428 foundIK = set_target_transform(trans, quat_xYZ,
true);
446 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
447 __arm = __robot->GetActiveManipulator();
448 std::vector<OpenRAVE::dReal> target_angles;
452 __target.
solvable = __arm->FindIKSolution(ik_param,target_angles,
true);
488 OpenRAVE::RobotBasePtr
515 OpenRAVE::PlannerBase::PlannerParametersPtr
518 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
519 __manip->
get_angles(__planner_params->vinitialconfig);
522 __robot->SetActiveDOFValues(__planner_params->vinitialconfig);
524 return __planner_params;
531 std::vector< std::vector<dReal> >*
541 std::vector< std::vector<float> >*
544 std::vector< std::vector<float> >* traj =
new std::vector< std::vector<float> >();
546 std::vector<float> v;
548 for(
unsigned int i=0; i<__traj->size(); i++) {
559 OpenRAVE::ModuleBasePtr
562 return __mod_basemanip;
574 bool success =
false;
576 success = __robot->Grab(
object);
577 }
catch(
const OpenRAVE::openrave_exception &e) {
579 __logger->
log_warn(
"OpenRAVE Robot",
"Could not attach Object. Ex:%s", e.what());
593 OpenRAVE::KinBodyPtr body = env->
get_env_ptr()->GetKinBody(name);
606 __robot->Release(
object);
607 }
catch(
const OpenRAVE::openrave_exception &e) {
609 __logger->
log_warn(
"OpenRAVE Robot",
"Could not release Object. Ex:%s", e.what());
623 OpenRAVE::KinBodyPtr body = env->
get_env_ptr()->GetKinBody(name);
635 __robot->ReleaseAllGrabbed();
636 }
catch(
const OpenRAVE::openrave_exception &e) {
638 __logger->
log_warn(
"OpenRAVE Robot",
"Could not release all objects. Ex:%s", e.what());
661 OpenRaveRobot::set_target_transform(OpenRAVE::Vector& trans, OpenRAVE::Vector& rotQuat,
bool no_offset)
664 target.trans = trans;
665 target.rot = rotQuat;
668 target.trans[0] += __trans_offset_x;
669 target.trans[1] += __trans_offset_y;
670 target.trans[2] += __trans_offset_z;
674 __target.
x = target.trans[0];
675 __target.
y = target.trans[1];
676 __target.
z = target.trans[2];
677 __target.
qw = target.rot[0];
678 __target.
qx = target.rot[1];
679 __target.
qy = target.rot[2];
680 __target.
qz = target.rot[3];
683 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
684 __arm = __robot->GetActiveManipulator();
685 if( __arm->GetIkSolver()->Supports(IKP_Transform6D) ) {
686 __logger->
log_debug(
"OR TMP",
"6D suppport");
688 std::vector<OpenRAVE::dReal> target_angles;
690 __target.
ikparam = IkParameterization(target);
691 __target.
solvable = __arm->FindIKSolution(__target.
ikparam,target_angles,
true);
694 }
else if( __arm->GetIkSolver()->Supports(IKP_TranslationDirection5D) ) {
695 __logger->
log_debug(
"OR TMP",
"5D suppport");
697 std::vector<OpenRAVE::dReal> target_angles;
699 __target.
ikparam = get_5dof_ikparam(target);
703 __logger->
log_debug(
"OR TMP",
"No IK suppport");
726 if( rotations.size() != 9 ) {
731 {__logger->
log_error(
"OpenRAVE Robot",
"Bad size of rotations vector. Is %i, expected 9", rotations.size());}
735 Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
736 Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
737 Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
739 __logger->
log_debug(
"TEST",
"Rot1: %f %f %f", r1[0], r1[1], r1[2]);
740 __logger->
log_debug(
"TEST",
"Rot2: %f %f %f", r2[0], r2[1], r2[2]);
741 __logger->
log_debug(
"TEST",
"Rot3: %f %f %f", r3[0], r3[1], r3[2]);
743 Vector q1 = quatFromAxisAngle(r1);
744 Vector q2 = quatFromAxisAngle(r2);
745 Vector q3 = quatFromAxisAngle(r3);
747 Vector q12 = quatMultiply (q1, q2);
748 Vector quat = quatMultiply (q12, q3);
750 return set_target_transform(trans, quat, no_offset);
757 OpenRAVE::IkParameterization
758 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform& trans)
774 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
777 RobotBasePtr tmp_robot = __robot;
778 RobotBase::RobotStateSaver saver(tmp_robot);
781 std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
782 tmp_robot->SetActiveDOFValues(zero_joints);
786 Transform cur_pos = __arm->GetEndEffectorTransform();
787 Vector v1 = quatFromAxisAngle(Vector(-M_PI/2, 0, 0));
788 v1 = quatMultiply(cur_pos.rot, v1);
791 v1 = quatInverse(v1);
792 TransformMatrix mat = matrixFromQuat(v1);
793 dir = mat.rotate(dir);
797 TransformMatrix mat = matrixFromQuat(trans.rot);
798 dir = mat.rotate(dir);
800 IkParameterization ikparam = __arm->GetIkParameterization(IKP_TranslationDirection5D);
801 ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
virtual bool display_planned_movements() const
Getter for __display_planned_movements.
Target: OpenRAVE::IkParameterization string.
virtual void update_manipulator()
Update motor values from OpenRAVE model.
float qx
x value of quaternion
virtual void set_target_plannerparams(std::string ¶ms)
Set additional planner parameters.
float x
translation on x-axis
Target: relative endeffector translation, based on arm extension.
Fawkes library namespace.
float qz
z value of quaternion
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
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...
virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false)
Set target, given relative transition.
virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, bool no_offset=false)
Set target, given transition, and Euler-rotation.
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
target_type_t type
target type
Target: absolute endeffector translation and rotation.
virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x)
Set target by giving position of an object.
std::string plannerparams
additional string to be passed to planner, i.e.
virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param)
Set target by giving IkParameterizaion of target.
float z
translation on z-axis
virtual bool attach_object(OpenRAVE::KinBodyPtr object)
Attach a kinbody to the robot.
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...
OpenRaveManipulator * manip
target manipulator configuration
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...
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
float y
translation on y-axis
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Base class for exceptions in Fawkes.
virtual bool set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, bool no_offset=false)
Set target, given transition, and rotation as axis-angle.
OpenRaveEnvironment class.
virtual void update_model()
Update/Set OpenRAVE motor angles.
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, bool no_offset=false)
Set target, given transition, and rotation as quaternion.
Target: relative endeffector translation, based on robot's coordinate system.
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
void angles_or_to_device(std::vector< T_from > &from, std::vector< T_to > &to) const
Transform OpenRAVE motor angles to real device angles.
Struct containing information about the current target.
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironment *env)
Load robot from xml file.
Class containing information about all manipulator motors.
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.
float qw
w value of quaternion
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
euler_rotation_t
Euler rotations.
virtual OpenRaveManipulator * get_manipulator() const
Get manipulator.
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 forma...
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
virtual ~OpenRaveRobot()
Destructor.
bool solvable
target IK solvable
virtual void set_ready()
Set robot ready for usage.
Expected parameter is missing.
virtual bool set_target_straight(float trans_x, float trans_y, float trans_z)
Set target for a straight movement, given transition.
virtual void set_manipulator(fawkes::OpenRaveManipulator *manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
float qy
y value of quaternion
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
virtual target_t get_target() const
Get target.