pr2_arm_ik_solver.h

Go to the documentation of this file.

//Software License Agreement (BSD License)

//Copyright (c) 2008, Willow Garage, Inc.
//All rights reserved.

//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:

// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
//   copyright notice, this list of conditions and the following
//   disclaimer in the documentation and/or other materials provided
//   with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
//   contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.

//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

#ifndef PR2_ARM_IK_SOLVER_H
#define PR2_ARM_IK_SOLVER_H

#include <urdf/model.h>
#include <Eigen/Core>
#include <kdl/chainiksolver.hpp>
#include <pr2_arm_kinematics/pr2_arm_ik.h>
#include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/PositionIKRequest.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf_conversions/tf_kdl.h>

namespace pr2_arm_kinematics
{

static const int NO_IK_SOLUTION = -1;
static const int TIMED_OUT = -2;

  class PR2ArmIKSolver : public KDL::ChainIkSolverPos
  {
    public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    PR2ArmIKSolver(const urdf::Model &robot_model, 
                   const std::string &root_frame_name,
                   const std::string &tip_frame_name,
                   const double &search_discretization_angle, 
                   const int &free_angle);

    ~PR2ArmIKSolver(){};

    PR2ArmIK pr2_arm_ik_;

    bool active_;

    int CartToJnt(const KDL::JntArray& q_init, 
                  const KDL::Frame& p_in, 
                  KDL::JntArray& q_out);

    int CartToJnt(const KDL::JntArray& q_init, 
                  const KDL::Frame& p_in, 
                  std::vector<KDL::JntArray> &q_out);
  
    int CartToJntSearch(const KDL::JntArray& q_in, 
                        const KDL::Frame& p_in, 
                        std::vector<KDL::JntArray> &q_out, 
                        const double &timeout);

    int CartToJntSearch(const KDL::JntArray& q_in, 
                        const KDL::Frame& p_in, 
                        KDL::JntArray &q_out, 
                        const double &timeout);

    int CartToJntSearch(const KDL::JntArray& q_in, 
                        const KDL::Frame& p_in, 
                        const double& consistency_limit, 
                        KDL::JntArray &q_out, 
                        const double &timeout);
    void getSolverInfo(kinematics_msgs::KinematicSolverInfo &response);

    int CartToJntSearch(const KDL::JntArray& q_in, 
                        const KDL::Frame& p_in, 
                        KDL::JntArray &q_out, 
                        const double &timeout, 
                        arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
                        const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
                        const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback);

    int CartToJntSearch(const KDL::JntArray& q_in, 
                        const KDL::Frame& p_in, 
                        KDL::JntArray &q_out, 
                        const double &timeout, 
                        const double& consistency_limit, 
                        arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
                        const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
                        const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback);



    std::string getFrameId();

    unsigned int getFreeAngle() const {
      return free_angle_;
    }

    void setFreeAngle(const unsigned int& free_angle) {
      free_angle_ = free_angle;
    }

  private:

    bool getCount(int &count, const int &max_count, const int &min_count);

    double search_discretization_angle_;

    int free_angle_;

    std::string root_frame_name_;
  };
}
#endif// PR2_ARM_IK_SOLVER_H