pr2_arm_ik.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #ifndef MOVEIT_PR2_ARM_IK_H
00038 #define MOVEIT_PR2_ARM_IK_H
00039 
00040 #include <urdf/model.h>
00041 #include <Eigen/Core>
00042 #include <Eigen/LU>// provides LU decomposition
00043 #include <kdl/chainiksolver.hpp>
00044 #include <moveit/pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00045 #include <moveit/pr2_arm_kinematics/pr2_arm_kinematics_constants.h>
00046 
00047 
00048 namespace pr2_arm_kinematics
00049 {
00050 class PR2ArmIK
00051 {
00052 public:
00053 
00059   PR2ArmIK();
00060   ~PR2ArmIK(){};
00061 
00069   bool init(const urdf::Model &robot_model, const std::string &root_name, const std::string &tip_name);
00070 
00076   void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess,std::vector<std::vector<double> > &solution) const;
00077 
00083   void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess,std::vector<std::vector<double> > &solution) const;
00084 
00085 
00086   //  std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
00087 
00092   void getSolverInfo(moveit_msgs::KinematicSolverInfo &info);
00093 
00097   moveit_msgs::KinematicSolverInfo solver_info_;
00098 
00099   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00100 
00101   private:
00102 
00103   void addJointToChainInfo(boost::shared_ptr<const urdf::Joint> joint,moveit_msgs::KinematicSolverInfo &info);
00104 
00105   bool checkJointLimits(const std::vector<double> &joint_values) const;
00106 
00107   bool checkJointLimits(const double &joint_value, const int &joint_num) const;
00108 
00109   Eigen::Matrix4f grhs_, gf_, home_inv_, home_;
00110 
00111   std::vector<double> angle_multipliers_;
00112 
00113   std::vector<double> solution_;
00114 
00115   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_;
00116 
00117   std::vector<double> min_angles_;
00118 
00119   std::vector<double> max_angles_;
00120 
00121   std::vector<bool> continuous_joint_;
00122 
00123 };
00124 }
00125 #endif// PR2_ARM_IK_H


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Sep 14 2015 14:17:49