Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef PR2_ARM_IK_H
00040 #define PR2_ARM_IK_H
00041
00042 #include <urdf_model/model.h>
00043 #include <Eigen/Core>
00044 #include <Eigen/LU>
00045 #include <kdl/chainiksolver.hpp>
00046 #include <moveit_msgs/GetPositionFK.h>
00047 #include <moveit_msgs/GetPositionIK.h>
00048 #include <moveit_msgs/GetKinematicSolverInfo.h>
00049
00050 namespace pr2_arm_kinematics
00051 {
00052 static const int NUM_JOINTS_ARM7DOF = 7;
00053
00054 static const double IK_EPS = 1e-5;
00055
00056 inline double distance(const urdf::Pose &transform)
00057 {
00058 return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
00059 }
00060
00061
00062 inline bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00063 {
00064 double discriminant = b*b-4*a*c;
00065 if(fabs(a) < IK_EPS)
00066 {
00067 *x1 = -c/b;
00068 *x2 = *x1;
00069 return true;
00070 }
00071
00072 if (discriminant >= 0)
00073 {
00074 *x1 = (-b + sqrt(discriminant))/(2*a);
00075 *x2 = (-b - sqrt(discriminant))/(2*a);
00076 return true;
00077 }
00078 else if(fabs(discriminant) < IK_EPS)
00079 {
00080 *x1 = -b/(2*a);
00081 *x2 = -b/(2*a);
00082 return true;
00083 }
00084 else
00085 {
00086 *x1 = -b/(2*a);
00087 *x2 = -b/(2*a);
00088 return false;
00089 }
00090 }
00091
00092 inline bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
00093 {
00094 double theta1 = atan2(b,a);
00095 double denom = sqrt(a*a+b*b);
00096
00097 if(fabs(denom) < IK_EPS)
00098 {
00099 #ifdef DEBUG
00100 std::cout << "denom: " << denom << std::endl;
00101 #endif
00102 return false;
00103 }
00104 double rhs_ratio = c/denom;
00105 if(rhs_ratio < -1 || rhs_ratio > 1)
00106 {
00107 #ifdef DEBUG
00108 std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00109 #endif
00110 return false;
00111 }
00112 double acos_term = acos(rhs_ratio);
00113 soln1 = theta1 + acos_term;
00114 soln2 = theta1 - acos_term;
00115
00116 return true;
00117 }
00118
00119 class PR2ArmIK
00120 {
00121 public:
00122
00128 PR2ArmIK();
00129 ~PR2ArmIK(){};
00130
00138 bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name);
00139
00145 void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution) const;
00146
00152 void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution) const;
00153
00154
00155
00156
00161 void getSolverInfo(moveit_msgs::KinematicSolverInfo &info);
00162
00166 moveit_msgs::KinematicSolverInfo solver_info_;
00167
00168 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00169
00170 private:
00171
00172 void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,moveit_msgs::KinematicSolverInfo &info);
00173
00174 bool checkJointLimits(const std::vector<double> &joint_values) const;
00175
00176 bool checkJointLimits(const double &joint_value, const int &joint_num) const;
00177
00178 Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00179
00180 std::vector<double> angle_multipliers_;
00181
00182 std::vector<double> solution_;
00183
00184 double shoulder_upperarm_offset_, upperarm_elbow_offset_, elbow_wrist_offset_, shoulder_wrist_offset_, shoulder_elbow_offset_, torso_shoulder_offset_x_, torso_shoulder_offset_y_, torso_shoulder_offset_z_;
00185
00186 std::vector<double> min_angles_;
00187
00188 std::vector<double> max_angles_;
00189
00190 std::vector<bool> continuous_joint_;
00191
00192 };
00193 }
00194 #endif// PR2_ARM_IK_H