24 #include "manipulator.h" 25 #include "environment.h" 27 #include <openrave-core.h> 28 #include <logging/logger.h> 29 #include <core/exceptions/software.h> 68 this->
load(filename, env);
77 __logger( src.__logger ),
79 __find_best_ik( src.__find_best_ik )
82 __traj =
new std::vector< std::vector<dReal> >();
84 __trans_offset_x = src.__trans_offset_x;
85 __trans_offset_y = src.__trans_offset_y;
86 __trans_offset_z = src.__trans_offset_z;
89 EnvironmentMutex::scoped_lock lock(new_env->get_env_ptr()->GetMutex());
91 __robot = new_env->get_env_ptr()->GetRobot( name );
98 __logger->
log_debug(this->name(),
"Robot loaded.");
106 EnvironmentMutex::scoped_lock lock(src.
get_robot_ptr()->GetEnv()->GetMutex());
107 __arm = __robot->SetActiveManipulator( src.
get_robot_ptr()->GetActiveManipulator()->GetName() );
109 __robot->SetActiveDOFs(__arm->GetArmIndices());
113 __display_planned_movements =
false;
116 __logger->
log_debug(this->name(),
"Robot '%s' cloned.", name.c_str());
122 __target.
manip = NULL;
126 EnvironmentBasePtr env = __robot->GetEnv();
127 EnvironmentMutex::scoped_lock lock(env->GetMutex());
128 env->Remove(__mod_basemanip);
129 env->Remove(__robot);
131 __logger->
log_warn(name(),
"Robot unloaded from environment");
132 }
catch(
const openrave_exception &e) {
134 __logger->
log_warn(name(),
"Could not unload robot properly from environment. Ex:%s", e.what());
142 OpenRaveRobot::build_name_str()
145 n <<
"OpenRaveRobot" <<
"[";
147 n << RaveGetEnvironmentId(__robot->GetEnv()) <<
":";
149 __name_str = n.str();
154 OpenRaveRobot::name()
const 156 return __name_str.c_str();
161 OpenRaveRobot::init()
163 __traj =
new std::vector< std::vector<dReal> >();
165 __trans_offset_x = 0.f;
166 __trans_offset_y = 0.f;
167 __trans_offset_z = 0.f;
180 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
184 __robot = env->get_env_ptr()->ReadRobotXMLFile(filename);
189 __name = __robot->GetName();
193 __logger->
log_debug(name(),
"Robot loaded.");
205 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
207 __robot->SetActiveManipulator(__robot->GetManipulators().at(0)->GetName());
208 __arm = __robot->GetActiveManipulator();
209 __robot->SetActiveDOFs(__arm->GetArmIndices());
211 if(__robot->GetActiveDOF() == 0)
212 throw fawkes::Exception(
"%s: Robot not added to environment yet. Need to do that first, otherwise planner will fail.", name());
216 PlannerBase::PlannerParametersPtr params(
new PlannerBase::PlannerParameters());
217 __planner_params = params;
218 __planner_params->_nMaxIterations = 4000;
219 __planner_params->SetRobotActiveJoints(__robot);
220 __planner_params->vgoalconfig.resize(__robot->GetActiveDOF());
221 }
catch(
const openrave_exception &e) {
222 throw fawkes::Exception(
"%s: Could not create PlannerParameters. Ex:%s", name(), e.what());
227 __mod_basemanip = RaveCreateModule(__robot->GetEnv(),
"basemanipulation");
228 __robot->GetEnv()->AddModule( __mod_basemanip, __robot->GetName());
229 }
catch(
const openrave_exception &e) {
230 throw fawkes::Exception(
"%s: Cannot load BaseManipulation Module. Ex:%s", name(), e.what());
234 __logger->
log_debug(name(),
"Robot ready.");
246 __trans_offset_x = trans_x;
247 __trans_offset_y = trans_y;
248 __trans_offset_z = trans_z;
262 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
264 std::vector<dReal> angles;
266 __robot->SetActiveDOFValues(angles);
269 __arm = __robot->GetActiveManipulator();
270 Transform trans = __arm->GetEndEffectorTransform();
271 __trans_offset_x = trans.trans[0] - device_trans_x;
272 __trans_offset_y = trans.trans[1] - device_trans_y;
273 __trans_offset_z = trans.trans[2] - device_trans_z;
289 __display_planned_movements = display_movements;
297 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
298 std::vector<dReal> angles;
299 __robot->GetActiveDOFValues(angles);
307 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
308 std::vector<dReal> angles;
310 __robot->SetActiveDOFValues(angles);
319 return __display_planned_movements;
331 __find_best_ik = enable;
350 __target.
x = trans_x;
351 __target.
y = trans_y;
352 __target.
z = trans_z;
374 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
375 __arm = __robot->GetActiveManipulator();
376 trans = __arm->GetEndEffectorTransform();
380 trans_y - trans.trans[1],
381 trans_z - trans.trans[2]);
398 float quat_w,
float quat_x,
float quat_y,
float quat_z,
399 IkFilterOptions filter,
bool no_offset)
401 Vector trans(trans_x, trans_y, trans_z);
402 Vector rot(quat_w, quat_x, quat_y, quat_z);
404 return set_target_transform(trans, rot, filter, no_offset);
421 float angle,
float axisX,
float axisY,
float axisZ,
422 IkFilterOptions filter,
bool no_offset)
424 Vector trans(trans_x, trans_y, trans_z);
425 Vector aa(angle, axisX, axisY, axisZ);
426 Vector rot = quatFromAxisAngle(aa);
428 return set_target_transform(trans, rot, filter, no_offset);
445 float trans_x,
float trans_y,
float trans_z,
446 float phi,
float theta,
float psi,
447 OpenRAVE::IkFilterOptions filter,
bool no_offset)
449 Vector trans(trans_x, trans_y, trans_z);
450 std::vector<float> rot(9, 0.f);
455 __logger->
log_debug(name(),
"Target EULER_ZXZ: %f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
463 __logger->
log_debug(name(),
"Target EULER_ZYZ:",
"%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
471 __logger->
log_debug(name(),
"Target EULER_ZYX:",
"%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
506 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
507 __robot->ReleaseAllGrabbed();
511 float alpha = atan2(trans_y - __trans_offset_y, trans_x - __trans_offset_x);
512 Vector quat_y = quatFromAxisAngle(Vector(0.f, M_PI/2, 0.f));
513 Vector quat_x = quatFromAxisAngle(Vector(-alpha, 0.f, 0.f));
514 Vector quat_z = quatFromAxisAngle(Vector(0.f, 0.f, rot_x));
516 Vector quat_xY = quatMultiply (quat_y, quat_x);
517 Vector quat_xYZ = quatMultiply (quat_xY, quat_z);
519 Vector trans(trans_x, trans_y, trans_z);
521 if( set_target_transform(trans, quat_xYZ, filter,
true) )
525 Vector quatPosY=quatFromAxisAngle(Vector(0.f, 0.017f, 0.f));
526 Vector quatNegY=quatFromAxisAngle(Vector(0.f, -0.017f, 0.f));
528 Vector quatPos(quat_xY);
529 Vector quatNeg(quat_xY);
531 unsigned int count = 0;
532 bool foundIK =
false;
534 while( (!foundIK) && (count <= 45)) {
537 quatPos = quatMultiply(quatPos, quatPosY);
538 quatNeg = quatMultiply(quatNeg, quatNegY);
540 quat_xYZ = quatMultiply(quatPos, quat_z);
541 foundIK = set_target_transform(trans, quat_xYZ, filter,
true);
543 quat_xYZ = quatMultiply(quatNeg, quat_z);
544 foundIK = set_target_transform(trans, quat_xYZ, filter,
true);
563 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
564 __arm = __robot->GetActiveManipulator();
637 OpenRAVE::RobotBasePtr
664 OpenRAVE::PlannerBase::PlannerParametersPtr
667 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
669 __planner_params->SetRobotActiveJoints(__robot);
670 __planner_params->vgoalconfig.resize(__robot->GetActiveDOF());
672 __manip->
get_angles(__planner_params->vinitialconfig);
675 __robot->SetActiveDOFValues(__planner_params->vinitialconfig);
677 return __planner_params;
684 std::vector< std::vector<dReal> >*
694 std::vector< std::vector<float> >*
697 std::vector< std::vector<float> >* traj =
new std::vector< std::vector<float> >();
699 std::vector<float> v;
701 for(
unsigned int i=0; i<__traj->size(); i++) {
712 OpenRAVE::ModuleBasePtr
715 return __mod_basemanip;
729 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
731 bool success =
false;
735 RobotBase::ManipulatorPtr manip = __robot->SetActiveManipulator(manip_name);
738 __logger->
log_warn(name(),
"Could not attach Object, could not get manipulator '%s'", manip_name);
742 success = __robot->Grab(
object, manip->GetEndEffector());
747 success = __robot->Grab(
object);
749 }
catch(
const OpenRAVE::openrave_exception &e) {
751 __logger->
log_warn(name(),
"Could not attach Object. Ex:%s", e.what());
766 OpenRAVE::KinBodyPtr body;
768 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
769 body = env->get_env_ptr()->GetKinBody(name);
783 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
784 __robot->Release(
object);
785 }
catch(
const OpenRAVE::openrave_exception &e) {
787 __logger->
log_warn(name(),
"Could not release Object. Ex:%s", e.what());
801 OpenRAVE::KinBodyPtr body;
803 EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
804 body = env->get_env_ptr()->GetKinBody(name);
817 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
818 __robot->ReleaseAllGrabbed();
819 }
catch(
const OpenRAVE::openrave_exception &e) {
821 __logger->
log_warn(name(),
"Could not release all objects. Ex:%s", e.what());
844 OpenRaveRobot::set_target_transform(Vector& trans, OpenRAVE::Vector& rotQuat, IkFilterOptions filter,
bool no_offset)
847 target.trans = trans;
848 target.rot = rotQuat;
851 target.trans[0] += __trans_offset_x;
852 target.trans[1] += __trans_offset_y;
853 target.trans[2] += __trans_offset_z;
857 __target.
x = target.trans[0];
858 __target.
y = target.trans[1];
859 __target.
z = target.trans[2];
860 __target.
qw = target.rot[0];
861 __target.
qx = target.rot[1];
862 __target.
qy = target.rot[2];
863 __target.
qz = target.rot[3];
866 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
867 __arm = __robot->GetActiveManipulator();
868 if( __arm->GetIkSolver()->Supports(IKP_Transform6D) ) {
870 __logger->
log_debug(name(),
"6D suppport for arm %s", __arm->GetName().c_str());
872 __target.
ikparam = IkParameterization(target);
875 }
else if( __arm->GetIkSolver()->Supports(IKP_TranslationDirection5D) ) {
877 __logger->
log_debug(name(),
"5D suppport");
879 __target.
ikparam = get_5dof_ikparam(target);
884 __logger->
log_debug(name(),
"No IK suppport");
908 if( rotations.size() != 9 ) {
913 __logger->
log_error(name(),
"Bad size of rotations vector. Is %i, expected 9", rotations.size());
917 Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
918 Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
919 Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
922 __logger->
log_debug(name(),
"TEST Rot1: %f %f %f", r1[0], r1[1], r1[2]);
923 __logger->
log_debug(name(),
"TEST Rot2: %f %f %f", r2[0], r2[1], r2[2]);
924 __logger->
log_debug(name(),
"TEST Rot3: %f %f %f", r3[0], r3[1], r3[2]);
927 Vector q1 = quatFromAxisAngle(r1);
928 Vector q2 = quatFromAxisAngle(r2);
929 Vector q3 = quatFromAxisAngle(r3);
931 Vector q12 = quatMultiply (q1, q2);
932 Vector quat = quatMultiply (q12, q3);
934 return set_target_transform(trans, quat, filter, no_offset);
941 OpenRAVE::IkParameterization
942 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform& trans)
958 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
961 RobotBasePtr tmp_robot = __robot;
962 RobotBase::RobotStateSaver saver(tmp_robot);
965 std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
966 tmp_robot->SetActiveDOFValues(zero_joints);
970 Transform cur_pos = __arm->GetEndEffectorTransform();
971 Vector v1 = quatFromAxisAngle(Vector(-M_PI/2, 0, 0));
972 v1 = quatMultiply(cur_pos.rot, v1);
975 v1 = quatInverse(v1);
976 TransformMatrix mat = matrixFromQuat(v1);
977 dir = mat.rotate(dir);
981 TransformMatrix mat = matrixFromQuat(trans.rot);
982 dir = mat.rotate(dir);
984 IkParameterization ikparam = __arm->GetIkParameterization(IKP_TranslationDirection5D);
985 ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
995 OpenRaveRobot::solve_ik(IkFilterOptions filter)
997 if( !__find_best_ik ) {
998 std::vector<dReal> solution;
999 __target.
solvable = __arm->FindIKSolution(__target.
ikparam,solution,filter);
1003 std::vector< std::vector<dReal> > solutions;
1006 __target.
solvable = __arm->FindIKSolutions(__target.
ikparam,solutions,filter);
1011 std::vector< std::vector<dReal> >::iterator sol;
1012 std::vector<dReal> cur;
1013 std::vector<dReal> diff;
1015 __arm->GetArmDOFValues(cur);
1017 for( sol=solutions.begin(); sol!=solutions.end(); ++sol ) {
1019 __robot->SubtractActiveDOFValues(diff, *sol);
1021 float sol_dist = 0.f;
1022 for(
unsigned int i=0; i<diff.size(); ++i ) {
1023 sol_dist += fabs(diff[i]);
1026 (*sol)[i] = cur[i] - diff[i];
1029 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 forma...
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.