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 #ifndef PR2_ARM_IK_H
00038 #define PR2_ARM_IK_H
00039
00040 #include <urdf_model/model.h>
00041 #include <Eigen/Core>
00042 #include <Eigen/LU>
00043 #include <kdl/chainiksolver.hpp>
00044 #include <moveit_msgs/GetPositionFK.h>
00045 #include <moveit_msgs/GetPositionIK.h>
00046 #include <moveit_msgs/GetKinematicSolverInfo.h>
00047
00048 namespace pr2_arm_kinematics
00049 {
00050 static const int NUM_JOINTS_ARM7DOF = 7;
00051
00052 static const double IK_EPS = 1e-5;
00053
00054 inline double distance(const urdf::Pose& transform)
00055 {
00056 return sqrt(transform.position.x * transform.position.x + transform.position.y * transform.position.y +
00057 transform.position.z * transform.position.z);
00058 }
00059
00060 inline bool solveQuadratic(const double& a, const double& b, const double& c, double* x1, double* x2)
00061 {
00062 double discriminant = b * b - 4 * a * c;
00063 if (fabs(a) < IK_EPS)
00064 {
00065 *x1 = -c / b;
00066 *x2 = *x1;
00067 return true;
00068 }
00069
00070 if (discriminant >= 0)
00071 {
00072 *x1 = (-b + sqrt(discriminant)) / (2 * a);
00073 *x2 = (-b - sqrt(discriminant)) / (2 * a);
00074 return true;
00075 }
00076 else if (fabs(discriminant) < IK_EPS)
00077 {
00078 *x1 = -b / (2 * a);
00079 *x2 = -b / (2 * a);
00080 return true;
00081 }
00082 else
00083 {
00084 *x1 = -b / (2 * a);
00085 *x2 = -b / (2 * a);
00086 return false;
00087 }
00088 }
00089
00090 inline bool solveCosineEqn(const double& a, const double& b, const double& c, double& soln1, double& soln2)
00091 {
00092 double theta1 = atan2(b, a);
00093 double denom = sqrt(a * a + b * b);
00094
00095 if (fabs(denom) < IK_EPS)
00096 {
00097 #ifdef DEBUG
00098 std::cout << "denom: " << denom << std::endl;
00099 #endif
00100 return false;
00101 }
00102 double rhs_ratio = c / denom;
00103 if (rhs_ratio < -1 || rhs_ratio > 1)
00104 {
00105 #ifdef DEBUG
00106 std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00107 #endif
00108 return false;
00109 }
00110 double acos_term = acos(rhs_ratio);
00111 soln1 = theta1 + acos_term;
00112 soln2 = theta1 - acos_term;
00113
00114 return true;
00115 }
00116
00117 class PR2ArmIK
00118 {
00119 public:
00125 PR2ArmIK();
00126 ~PR2ArmIK(){};
00127
00135 bool init(const urdf::ModelInterface& robot_model, const std::string& root_name, const std::string& tip_name);
00136
00142 void computeIKShoulderPan(const Eigen::Matrix4f& g_in, const double& shoulder_pan_initial_guess,
00143 std::vector<std::vector<double> >& solution) const;
00144
00150 void computeIKShoulderRoll(const Eigen::Matrix4f& g_in, const double& shoulder_roll_initial_guess,
00151 std::vector<std::vector<double> >& solution) const;
00152
00153
00154
00160 void getSolverInfo(moveit_msgs::KinematicSolverInfo& info);
00161
00165 moveit_msgs::KinematicSolverInfo solver_info_;
00166
00167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00168
00169 private:
00170 void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint, moveit_msgs::KinematicSolverInfo& info);
00171
00172 bool checkJointLimits(const std::vector<double>& joint_values) const;
00173
00174 bool checkJointLimits(const double& joint_value, const int& joint_num) const;
00175
00176 Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00177
00178 std::vector<double> angle_multipliers_;
00179
00180 std::vector<double> solution_;
00181
00182 double shoulder_upperarm_offset_, upperarm_elbow_offset_, elbow_wrist_offset_, shoulder_wrist_offset_,
00183 shoulder_elbow_offset_, torso_shoulder_offset_x_, torso_shoulder_offset_y_, torso_shoulder_offset_z_;
00184
00185 std::vector<double> min_angles_;
00186
00187 std::vector<double> max_angles_;
00188
00189 std::vector<bool> continuous_joint_;
00190 };
00191 }
00192 #endif // PR2_ARM_IK_H