24 #include "manipulator.h"
25 #include "environment.h"
27 #include <openrave-core.h>
28 #include <logging/logger.h>
29 #include <core/exceptions/software.h>
31 using namespace OpenRAVE;
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));