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) {
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
Target: OpenRAVE::IkParameterization string.
virtual void update_manipulator()
Update motor values from OpenRAVE model.
float qx
x value of quaternion
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.
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.
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.
float qz
z value of quaternion
virtual target_t get_target() const
Get target.
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 OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false)
Set target, given relative transition.
target_type_t type
target type
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.
Target: absolute endeffector translation and rotation.
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
std::string plannerparams
additional string to be passed to planner, i.e.
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
float z
translation on z-axis
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.
float y
translation on y-axis
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Base class for exceptions in Fawkes.
virtual void update_model()
Update/Set OpenRAVE motor angles.
Target: relative endeffector translation, based on robot's coordinate system.
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
virtual void enable_ik_comparison(bool enable)
Activate/Deactive IK comparison.
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.
Struct containing information about the current target.
virtual void set_target_raw(std::string &cmd)
Set raw command for BaseManipulation module.
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.
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.
float qw
w value of quaternion
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name=NULL)
Attach a kinbody to the robot.
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
euler_rotation_t
Euler rotations.
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
Load robot from xml file.
RefPtr<> is a reference-counting shared smartpointer.
OpenRaveManipulatorPtr manip
target manipulator configuration
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual OpenRaveManipulatorPtr get_manipulator() const
Get manipulator.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
virtual ~OpenRaveRobot()
Destructor.
bool solvable
target IK solvable
void angles_or_to_device(std::vector< T_from > &from, std::vector< T_to > &to) const
Transform OpenRAVE motor angles to real device angles.
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 bool display_planned_movements() const
Getter for display_planned_movements_.
Target: motor joint values.
float qy
y value of quaternion
virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving IkParameterizaion of target.
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
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.