sbpl_arm_model.h

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Maxim Likhachev
 * 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 the University of Pennsylvania 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.
 */

#include <string>
#include <map>
#include <vector>
#include <sbpl/config.h>
#include <angles/angles.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolvervel_pinv_givens.hpp>
#include <kdl/treefksolverpos_recursive.hpp>
#include <urdf/model.h>
#include <ros/ros.h>
#include <pr2_arm_kinematics/pr2_arm_ik_solver.h>

using namespace std;

#ifndef _SBPL_ARM_MODEL_
#define _SBPL_ARM_MODEL_

struct ArmJoint
{
  bool continuous;
  double min;
  double max;
  std::string name;
};

struct ArmLink
{
  short unsigned int radius_c;
  short unsigned int length_c;
  int ind_chain;
  int ind_jnt1;
  int ind_jnt2;
  double length;
  double radius;
  std::string name;
};


class SBPLArmModel{

  public:

    SBPLArmModel(FILE* arm_file);
    
    ~SBPLArmModel();
    
    int num_joints_;

    int num_links_;

    double resolution_;
    
    bool initKDLChain(const std::string &fKDL);

    bool initKDLChainFromParamServer();

    bool getArmDescription(FILE* fCfg);

    double getAxisAngle(const double R1[3][3], const double R2[3][3]);

    void RPY2Rot(double roll, double pitch, double yaw, double Rot[3][3]);
    
    bool computeFK(const std::vector<double> angles, int frame_num, KDL::Frame *frame_out);

    bool computeFK(const std::vector<double> angles, int f_num, std::vector<double> &xyzrpy);

    bool computeFK(const std::vector<double> angles, int frame_num, std::vector<double> &xyzrpy, int sol_num);
    
    bool computeEndEffPose(const std::vector<double> angles, double R_target[3][3], double *x, double *y, double *z, double *axis_angle);

    bool computeIK(const std::vector<double> pose, const std::vector<double> start, std::vector<double> &solution);

    bool getJointPositions(const std::vector<double> angles, const double R_target[3][3], std::vector<std::vector<double> > &links, double *axis_angle);
  
    bool getJointPositions(const std::vector<double> angles, std::vector<std::vector<double> > &links, KDL::Frame &f_out);

    bool computePlanningJointPos(const std::vector<double> angles, double *x, double *y, double *z);
   
    bool getPlanningJointPose(const std::vector<double> angles, std::vector<double> &pose);

    bool getPlanningJointPose(const std::vector<double> angles, double R_target[3][3], std::vector<double> &pose, double *axis_angle);

    bool checkJointLimits(std::vector<double> angles, bool verbose);

    void printArmDescription(FILE* fOut);

    int getMaxLinkRadius();

    short unsigned int getLinkRadiusCells(int link);
    
    double getLinkRadius(int link);

    short unsigned int getPlanningJointRadius();
   
    void setResolution(double resolution);

    void setRefFrameTransform(KDL::Frame f, std::string &name);

    void getArmChainRootLinkName(std::string &name);

    std::vector<std::vector<double> >  getCollisionCuboids();

    void setDebugFile(FILE* file);

    void getJointLimits(int joint_num, double* min, double *max);

  private:

          std::string robot_description_;

    std::string chain_root_name_;

    std::string chain_tip_name_;

    std::string reference_frame_;

    urdf::Model robot_model_;

    KDL::Tree kdl_tree_;

    int planning_joint_;

    std::string planning_joint_name_;

    short unsigned int max_radius_;

          std::vector<ArmJoint> joints_;

          std::vector<ArmLink> links_;

    std::vector<int> joint_indeces_;

    KDL::Frame transform_;
    
    KDL::Frame transform_inverse_;

    std::vector<std::vector<double> > collision_cuboids_;

    int num_collision_cuboids_;

    FILE* fOut_;

    //from CHOMP
    void parseKDLTree();
          int num_kdl_joints_;
    std::map<std::string, std::string> joint_segment_mapping_;    
    std::map<std::string, std::string> segment_joint_mapping_;    
    std::vector<std::string> kdl_number_to_urdf_name_;            
    std::map<std::string, int> urdf_name_to_kdl_number_;          
    KDL::JntArray jnt_pos_in_;
          KDL::JntArray jnt_pos_out_;
          KDL::Frame p_out_;
          
    KDL::Chain chain_;
          KDL::ChainFkSolverPos_recursive *fk_solver_;

    KDL::Chain ik_chain_;
          KDL::ChainFkSolverPos_recursive *ik_fk_solver_;
          KDL::ChainIkSolverPos_NR *ik_solver_;
    KDL::ChainIkSolverVel_pinv_givens *ik_solver_vel_;
          KDL::JntArray ik_jnt_pos_in_;
          KDL::JntArray ik_jnt_pos_out_;
         
    pr2_arm_kinematics::PR2ArmIKSolver* pr2_arm_ik_solver_;

    double frameToAxisAngle(const KDL::Frame frame, const double R_target[3][3]);
   
    void getRPY(double Rot[3][3], double* roll, double* pitch, double* yaw, int solution_number);
};


inline short unsigned int SBPLArmModel::getLinkRadiusCells(int link)
{
  if(link < num_links_)
    return links_[link].radius_c;
  return 0;
}

inline double SBPLArmModel::getLinkRadius(int link)
{
  if(link < num_links_)
    return links_[link].radius;
  return 0;
}

inline int SBPLArmModel::getMaxLinkRadius()
{
  return max_radius_;
}

inline std::vector<std::vector<double> >  SBPLArmModel::getCollisionCuboids()
{
  return collision_cuboids_;
}

#endif

---

sbpl_arm_planner
Author(s): Benjamin Cohen/bcohen@seas.upenn.edu
autogenerated on Wed Feb 29 11:46:16 2012