chomp_robot_model.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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 the Willow Garage 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 CHOMP_ROBOT_MODEL_H_
#define CHOMP_ROBOT_MODEL_H_

#include <planning_environment/models/collision_models.h>
#include <collision_proximity_planner/treefksolverjointposaxis.hpp>
#include <collision_proximity_planner/treefksolverjointposaxis_partial.hpp>
#include <ros/ros.h>
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <boost/shared_ptr.hpp>

#include <sensor_msgs/JointState.h>

#include <map>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <cstdlib>

namespace chomp
{

class ChompRobotModel
{
public:

  struct ChompJoint
  {
    const KDL::Joint* kdl_joint_;                               
    int kdl_joint_index_;                                       
    int chomp_joint_index_;                                     
    std::string joint_name_;                                    
    std::string link_name_;                                     
    bool wrap_around_;                                          
    bool has_joint_limits_;                                     
    double joint_limit_min_;                                    
    double joint_limit_max_;                                    
    double joint_update_limit_;                                 
  };

  struct ChompPlanningGroup
  {
    std::string name_;                                          
    int num_joints_;                                            
    std::vector<ChompJoint> chomp_joints_;                      
    std::vector<std::string> link_names_;                       
    boost::shared_ptr<KDL::TreeFkSolverJointPosAxisPartial> fk_solver_;           
    template <typename Derived>
    void getRandomState(Eigen::MatrixBase<Derived>& state_vec) const;

  };

  ChompRobotModel();
  virtual ~ChompRobotModel();

  bool init(const planning_environment::CollisionModels* model);

  const ChompPlanningGroup* getPlanningGroup(const std::string& group_name) const;

  int getNumKDLJoints() const;

  const KDL::Tree* getKDLTree() const;

  int urdfNameToKdlNumber(const std::string& urdf_name) const;

  const std::string kdlNumberToUrdfName(int kdl_number) const;

  const KDL::TreeFkSolverJointPosAxis* getForwardKinematicsSolver() const;

  const std::string& getReferenceFrame() const;

  template<typename T>
  void jointMsgToArray(T& msg_vector, Eigen::MatrixXd::RowXpr joint_array);

  template<typename T>
  void jointMsgToArray(T& msg_vector, KDL::JntArray& joint_array);

  void jointStateToArray(const sensor_msgs::JointState &joint_state, KDL::JntArray& joint_array);

  void jointStateToArray(const sensor_msgs::JointState &joint_state, Eigen::MatrixXd::RowXpr joint_array);

  void publishCollisionPoints(ros::Publisher& vis_marker);
  void getActiveJointInformation(const std::string &link_name, std::vector<int>& active_joints, int& segment_number);

private:
  ros::NodeHandle node_handle_,root_handle_;                                 
  KDL::Tree kdl_tree_;                                          
  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_;          
  std::map<std::string, ChompPlanningGroup> planning_groups_;   
  KDL::TreeFkSolverJointPosAxis *fk_solver_;                    
  std::string reference_frame_;                                 
  double max_radius_clearance_;                                 
  void getLinkInformation(const std::string link_name, std::vector<int>& active_joints, int& segment_number);


//  void getActiveJointsSegmentNumberForLink(std::string link_name, 
};


inline const ChompRobotModel::ChompPlanningGroup* ChompRobotModel::getPlanningGroup(const std::string& group_name) const
{
  std::map<std::string, ChompRobotModel::ChompPlanningGroup>::const_iterator it = planning_groups_.find(group_name);
  if (it == planning_groups_.end())
    return NULL;
  else
    return &(it->second);
}

inline int ChompRobotModel::getNumKDLJoints() const
{
  return num_kdl_joints_;
}

inline const KDL::Tree* ChompRobotModel::getKDLTree() const
{
  return &kdl_tree_;
}

inline int ChompRobotModel::urdfNameToKdlNumber(const std::string& urdf_name) const
{
  std::map<std::string, int>::const_iterator it = urdf_name_to_kdl_number_.find(urdf_name);
  if (it!=urdf_name_to_kdl_number_.end())
    return it->second;
  else
    return -1;
}

inline const std::string ChompRobotModel::kdlNumberToUrdfName(int kdl_number) const
{
  if (kdl_number<0 || kdl_number>=num_kdl_joints_)
    return std::string("");
  else
    return kdl_number_to_urdf_name_[kdl_number];
}

inline const KDL::TreeFkSolverJointPosAxis* ChompRobotModel::getForwardKinematicsSolver() const
{
  return fk_solver_;
}

inline const std::string& ChompRobotModel::getReferenceFrame() const
{
  return reference_frame_;
}

template <typename Derived>
void ChompRobotModel::ChompPlanningGroup::getRandomState(Eigen::MatrixBase<Derived>& state_vec) const
{
  for (int i=0; i<num_joints_; i++)
  {
    double min = chomp_joints_[i].joint_limit_min_;
    double max = chomp_joints_[i].joint_limit_max_;
    if (!chomp_joints_[i].has_joint_limits_)
    {
      min = -M_PI/2.0;
      max = M_PI/2.0;
    }
    state_vec(i) = ((((double)rand())/RAND_MAX) * (max-min)) + min;
  }
}

template<typename T>
void ChompRobotModel::jointMsgToArray(T& msg_vector, Eigen::MatrixXd::RowXpr joint_array)
{
  for (typename T::iterator it=msg_vector.begin(); it!=msg_vector.end(); it++)
  {
    std::string name = it->joint_name;
    int kdl_number = urdfNameToKdlNumber(name);
    if (kdl_number>=0)
      joint_array(kdl_number) = it->value[0];   //@TODO we assume a single joint value per joint now
  }
}

template<typename T>
void ChompRobotModel::jointMsgToArray(T& msg_vector, KDL::JntArray& joint_array)
{
  for (typename T::iterator it=msg_vector.begin(); it!=msg_vector.end(); it++)
  {
    std::string name = it->joint_name;
    int kdl_number = urdfNameToKdlNumber(name);
    if (kdl_number>=0)
      joint_array(kdl_number) = it->value[0];   //@TODO we assume a single joint value per joint now
  }
}

inline void ChompRobotModel::jointStateToArray(const sensor_msgs::JointState &joint_state, KDL::JntArray& joint_array)
{
  for(unsigned int i=0; i < joint_state.name.size(); i++)
  {
    std::string name = joint_state.name[i];
    int kdl_number = urdfNameToKdlNumber(name);
    if (kdl_number>=0)
      joint_array(kdl_number) = joint_state.position[i]; 
  }
}

inline void ChompRobotModel::jointStateToArray(const sensor_msgs::JointState &joint_state, Eigen::MatrixXd::RowXpr joint_array)
{
  for(unsigned int i=0; i < joint_state.name.size(); i++)
  {
    std::string name = joint_state.name[i];
    int kdl_number = urdfNameToKdlNumber(name);
    if (kdl_number>=0)
      joint_array(kdl_number) = joint_state.position[i]; 
  }
}

} // namespace chomp
#endif /* CHOMP_ROBOT_MODEL_H_ */