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 #ifndef PR2_ARM_IK_H
00034 #define PR2_ARM_IK_H
00035
00036 #include <urdf/model.h>
00037 #include <Eigen/Core>
00038 #include <Eigen/LU>
00039 #include <kdl/chainiksolver.hpp>
00040 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00041 #include <pr2_arm_kinematics/pr2_arm_kinematics_constants.h>
00042
00043
00044 namespace pr2_arm_kinematics
00045 {
00046 class PR2ArmIK
00047 {
00048 public:
00049
00055 PR2ArmIK();
00056 ~PR2ArmIK(){};
00057
00065 bool init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name);
00066
00072 void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution);
00073
00079 void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution);
00080
00081
00082
00083
00088 void getSolverInfo(kinematics_msgs::KinematicSolverInfo &info);
00089
00093 kinematics_msgs::KinematicSolverInfo solver_info_;
00094
00095 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00096
00097 private:
00098
00099 void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,kinematics_msgs::KinematicSolverInfo &info);
00100
00101 bool checkJointLimits(const std::vector<double> &joint_values);
00102
00103 bool checkJointLimits(const double &joint_value, const int &joint_num);
00104
00105 Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00106
00107 std::vector<double> angle_multipliers_;
00108
00109 std::vector<double> solution_;
00110
00111 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_;
00112
00113 std::vector<double> min_angles_;
00114
00115 std::vector<double> max_angles_;
00116
00117 std::vector<bool> continuous_joint_;
00118
00119 };
00120 }
00121 #endif// PR2_ARM_IK_H