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 MOVEIT_PR2_ARM_IK_H
00040 #define MOVEIT_PR2_ARM_IK_H
00041
00042 #include <urdf/model.h>
00043 #include <Eigen/Core>
00044 #include <Eigen/LU>
00045 #include <kdl/chainiksolver.hpp>
00046 #include <moveit/pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00047 #include <moveit/pr2_arm_kinematics/pr2_arm_kinematics_constants.h>
00048
00049
00050 namespace pr2_arm_kinematics
00051 {
00052 class PR2ArmIK
00053 {
00054 public:
00055
00061 PR2ArmIK();
00062 ~PR2ArmIK(){};
00063
00071 bool init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name);
00072
00078 void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution) const;
00079
00085 void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution) const;
00086
00087
00088
00089
00094 void getSolverInfo(moveit_msgs::KinematicSolverInfo &info);
00095
00099 moveit_msgs::KinematicSolverInfo solver_info_;
00100
00101 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00102
00103 private:
00104
00105 void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,moveit_msgs::KinematicSolverInfo &info);
00106
00107 bool checkJointLimits(const std::vector<double> &joint_values) const;
00108
00109 bool checkJointLimits(const double &joint_value, const int &joint_num) const;
00110
00111 Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00112
00113 std::vector<double> angle_multipliers_;
00114
00115 std::vector<double> solution_;
00116
00117 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_;
00118
00119 std::vector<double> min_angles_;
00120
00121 std::vector<double> max_angles_;
00122
00123 std::vector<bool> continuous_joint_;
00124
00125 };
00126 }
00127 #endif// PR2_ARM_IK_H