25 #include "environment.h"
26 #include "manipulator.h"
28 #include <core/exceptions/software.h>
29 #include <logging/logger.h>
31 #include <openrave-core.h>
33 using namespace OpenRAVE;
48 : logger_(logger), name_(
""), manip_(0), find_best_ik_(1)
69 display_planned_movements_(false)
72 this->
load(filename, env);
82 : logger_(src.logger_), name_(src.name_), find_best_ik_(src.find_best_ik_)
85 traj_ =
new std::vector<std::vector<dReal>>();
87 trans_offset_x_ = src.trans_offset_x_;
88 trans_offset_y_ = src.trans_offset_y_;
89 trans_offset_z_ = src.trans_offset_z_;
92 EnvironmentMutex::scoped_lock lock(new_env->get_env_ptr()->GetMutex());
94 robot_ = new_env->get_env_ptr()->GetRobot(name);
104 logger_->
log_debug(this->name(),
"Robot loaded.");
113 EnvironmentMutex::scoped_lock lock(src.
get_robot_ptr()->GetEnv()->GetMutex());
114 arm_ = robot_->SetActiveManipulator(src.
get_robot_ptr()->GetActiveManipulator()->GetName());
116 robot_->SetActiveDOFs(arm_->GetArmIndices());
120 display_planned_movements_ =
false;
123 logger_->
log_debug(this->name(),
"Robot '%s' cloned.", name.c_str());
130 target_.
manip = NULL;
134 EnvironmentBasePtr env = robot_->GetEnv();
135 EnvironmentMutex::scoped_lock lock(env->GetMutex());
136 env->Remove(mod_basemanip_);
139 logger_->
log_warn(name(),
"Robot unloaded from environment");
140 }
catch (
const openrave_exception &e) {
143 "Could not unload robot properly from environment. Ex:%s",
152 OpenRaveRobot::build_name_str()
158 n << RaveGetEnvironmentId(robot_->GetEnv()) <<
":";
165 OpenRaveRobot::name()
const
167 return name_str_.c_str();
172 OpenRaveRobot::init()
174 traj_ =
new std::vector<std::vector<dReal>>();
176 trans_offset_x_ = 0.f;
177 trans_offset_y_ = 0.f;
178 trans_offset_z_ = 0.f;
190 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
194 robot_ = env->get_env_ptr()->ReadRobotXMLFile(filename);
198 "%s: Robot could not be loaded. Check xml file/path '%s'.", name(), filename.c_str());
200 name_ = robot_->GetName();
204 logger_->
log_debug(name(),
"Robot loaded.");
216 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
218 robot_->SetActiveManipulator(robot_->GetManipulators().at(0)->GetName());
219 arm_ = robot_->GetActiveManipulator();
220 robot_->SetActiveDOFs(arm_->GetArmIndices());
222 if (robot_->GetActiveDOF() == 0)
224 "%s: Robot not added to environment yet. Need to do that first, otherwise planner will fail.",
229 PlannerBase::PlannerParametersPtr params(
new PlannerBase::PlannerParameters());
230 planner_params_ = params;
231 planner_params_->_nMaxIterations = 4000;
232 planner_params_->SetRobotActiveJoints(
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());
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());
248 logger_->
log_debug(name(),
"Robot ready.");
260 trans_offset_x_ = trans_x;
261 trans_offset_y_ = trans_y;
262 trans_offset_z_ = trans_z;
276 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
278 std::vector<dReal> angles;
280 robot_->SetActiveDOFValues(angles);
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;
303 display_planned_movements_ = display_movements;
311 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
312 std::vector<dReal> angles;
313 robot_->GetActiveDOFValues(angles);
321 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
322 std::vector<dReal> angles;
324 robot_->SetActiveDOFValues(angles);
333 return display_planned_movements_;
345 find_best_ik_ = enable;
388 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
389 arm_ = robot_->GetActiveManipulator();
390 trans = arm_->GetEndEffectorTransform();
394 trans_y - trans.trans[1],
395 trans_z - trans.trans[2]);
418 IkFilterOptions filter,
421 Vector trans(trans_x, trans_y, trans_z);
422 Vector rot(quat_w, quat_x, quat_y, quat_z);
424 return set_target_transform(trans, rot, filter, no_offset);
447 IkFilterOptions filter,
450 Vector trans(trans_x, trans_y, trans_z);
451 Vector aa(angle, axisX, axisY, axisZ);
452 Vector rot = quatFromAxisAngle(aa);
454 return set_target_transform(trans, rot, filter, no_offset);
477 OpenRAVE::IkFilterOptions filter,
480 Vector trans(trans_x, trans_y, trans_z);
481 std::vector<float> rot(9, 0.f);
487 name(),
"Target EULER_ZXZ: %f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
551 IkFilterOptions filter)
558 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
559 robot_->ReleaseAllGrabbed();
564 atan2(trans_y - trans_offset_y_,
565 trans_x - trans_offset_x_);
567 quatFromAxisAngle(Vector(0.f, M_PI / 2, 0.f));
568 Vector quat_x = quatFromAxisAngle(
569 Vector(-alpha, 0.f, 0.f));
571 quatFromAxisAngle(Vector(0.f, 0.f, rot_x));
573 Vector quat_xY = quatMultiply(quat_y, quat_x);
574 Vector quat_xYZ = quatMultiply(quat_xY, quat_z);
576 Vector trans(trans_x, trans_y, trans_z);
578 if (set_target_transform(trans, quat_xYZ, filter,
true))
582 Vector quatPosY = quatFromAxisAngle(Vector(0.f, 0.017f, 0.f));
583 Vector quatNegY = quatFromAxisAngle(Vector(0.f, -0.017f, 0.f));
585 Vector quatPos(quat_xY);
586 Vector quatNeg(quat_xY);
588 unsigned int count = 0;
589 bool foundIK =
false;
591 while ((!foundIK) && (count <= 45)) {
594 quatPos = quatMultiply(quatPos, quatPosY);
595 quatNeg = quatMultiply(quatNeg, quatNegY);
597 quat_xYZ = quatMultiply(quatPos, quat_z);
598 foundIK = set_target_transform(trans, quat_xYZ, filter,
true);
600 quat_xYZ = quatMultiply(quatNeg, quat_z);
601 foundIK = set_target_transform(trans, quat_xYZ, filter,
true);
620 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
621 arm_ = robot_->GetActiveManipulator();
691 OpenRAVE::RobotBasePtr
718 OpenRAVE::PlannerBase::PlannerParametersPtr
721 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
723 planner_params_->SetRobotActiveJoints(robot_);
724 planner_params_->vgoalconfig.resize(robot_->GetActiveDOF());
726 manip_->
get_angles(planner_params_->vinitialconfig);
729 robot_->SetActiveDOFValues(planner_params_->vinitialconfig);
731 return planner_params_;
738 std::vector<std::vector<dReal>> *
748 std::vector<std::vector<float>> *
751 std::vector<std::vector<float>> *traj =
new std::vector<std::vector<float>>();
753 std::vector<float> v;
755 for (
unsigned int i = 0; i < traj_->size(); i++) {
766 OpenRAVE::ModuleBasePtr
769 return mod_basemanip_;
782 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
784 bool success =
false;
788 RobotBase::ManipulatorPtr manip = robot_->SetActiveManipulator(manip_name);
792 "Could not attach Object, could not get manipulator '%s'",
797 success = robot_->Grab(
object, manip->GetEndEffector());
802 success = robot_->Grab(
object);
804 }
catch (
const OpenRAVE::openrave_exception &e) {
806 logger_->
log_warn(name(),
"Could not attach Object. Ex:%s", e.what());
821 const char * manip_name)
823 OpenRAVE::KinBodyPtr body;
825 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
826 body = env->get_env_ptr()->GetKinBody(name);
840 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
841 robot_->Release(
object);
842 }
catch (
const OpenRAVE::openrave_exception &e) {
844 logger_->
log_warn(name(),
"Could not release Object. Ex:%s", e.what());
858 OpenRAVE::KinBodyPtr body;
860 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
861 body = env->get_env_ptr()->GetKinBody(name);
874 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
875 robot_->ReleaseAllGrabbed();
876 }
catch (
const OpenRAVE::openrave_exception &e) {
878 logger_->
log_warn(name(),
"Could not release all objects. Ex:%s", e.what());
898 OpenRaveRobot::set_target_transform(Vector & trans,
899 OpenRAVE::Vector &rotQuat,
900 IkFilterOptions filter,
904 target.trans = trans;
905 target.rot = rotQuat;
908 target.trans[0] += trans_offset_x_;
909 target.trans[1] += trans_offset_y_;
910 target.trans[2] += trans_offset_z_;
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];
923 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
924 arm_ = robot_->GetActiveManipulator();
925 if (arm_->GetIkSolver()->Supports(IKP_Transform6D)) {
927 logger_->
log_debug(name(),
"6D suppport for arm %s", arm_->GetName().c_str());
929 target_.
ikparam = IkParameterization(target);
932 }
else if (arm_->GetIkSolver()->Supports(IKP_TranslationDirection5D)) {
934 logger_->
log_debug(name(),
"5D suppport");
936 target_.
ikparam = get_5dof_ikparam(target);
941 logger_->
log_debug(name(),
"No IK suppport");
964 std::vector<float> & rotations,
965 OpenRAVE::IkFilterOptions filter,
968 if (rotations.size() != 9) {
974 "Bad size of rotations vector. Is %i, expected 9",
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));
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]);
989 Vector q1 = quatFromAxisAngle(r1);
990 Vector q2 = quatFromAxisAngle(r2);
991 Vector q3 = quatFromAxisAngle(r3);
993 Vector q12 = quatMultiply(q1, q2);
994 Vector quat = quatMultiply(q12, q3);
996 return set_target_transform(trans, quat, filter, no_offset);
1003 OpenRAVE::IkParameterization
1004 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform &trans)
1020 EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
1021 Vector dir(1, 0, 0);
1023 RobotBasePtr tmp_robot = robot_;
1024 RobotBase::RobotStateSaver saver(
1028 std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
1029 tmp_robot->SetActiveDOFValues(zero_joints);
1033 Transform cur_pos = arm_->GetEndEffectorTransform();
1034 Vector v1 = quatFromAxisAngle(Vector(-M_PI / 2, 0, 0));
1035 v1 = quatMultiply(cur_pos.rot, v1);
1038 v1 = quatInverse(v1);
1039 TransformMatrix mat = matrixFromQuat(v1);
1040 dir = mat.rotate(dir);
1044 TransformMatrix mat = matrixFromQuat(trans.rot);
1045 dir = mat.rotate(dir);
1047 IkParameterization ikparam = arm_->GetIkParameterization(IKP_TranslationDirection5D);
1048 ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
1058 OpenRaveRobot::solve_ik(IkFilterOptions filter)
1060 if (!find_best_ik_) {
1061 std::vector<dReal> solution;
1062 target_.
solvable = arm_->FindIKSolution(target_.
ikparam, solution, filter);
1066 std::vector<std::vector<dReal>> solutions;
1069 target_.
solvable = arm_->FindIKSolutions(target_.
ikparam, solutions, filter);
1074 std::vector<std::vector<dReal>>::iterator sol;
1075 std::vector<dReal> cur;
1076 std::vector<dReal> diff;
1078 arm_->GetArmDOFValues(cur);
1080 for (sol = solutions.begin(); sol != solutions.end(); ++sol) {
1082 robot_->SubtractActiveDOFValues(diff, *sol);
1084 float sol_dist = 0.f;
1085 for (
unsigned int i = 0; i < diff.size(); ++i) {
1086 sol_dist += fabs(diff[i]);
1089 (*sol)[i] = cur[i] - diff[i];
1092 if (sol_dist < dist) {