collision_proximity_planner: collision_proximity_planner: chomp_robot

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 the 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 
00037 #include <collision_proximity_planner/chomp_robot_model.h>
00038 #include <kdl_parser/kdl_parser.hpp>
00039 #include <ros/ros.h>
00040 #include <cstdio>
00041 #include <iostream>
00042 #include <visualization_msgs/MarkerArray.h>
00043 
00044 using namespace std;
00045 using namespace arm_navigation_msgs;
00046 
00047 namespace chomp
00048 {
00049 
00050 ChompRobotModel::ChompRobotModel():
00051   node_handle_("~")
00052 {
00053 }
00054 
00055 ChompRobotModel::~ChompRobotModel()
00056 {
00057 }
00058 
00059 bool ChompRobotModel::init(const planning_environment::CollisionModels* models)
00060 {
00061   reference_frame_ = models->getRobotFrameId();
00062 
00063   // get the urdf as a string:
00064   string urdf_string;
00065   if (!node_handle_.getParam(models->getDescription(), urdf_string))
00066   {
00067     return false;
00068   }
00069 
00070   // get some other params:
00071   double joint_update_limit;
00072   node_handle_.param("joint_update_limit", joint_update_limit, 0.05);
00073 
00074   // Construct the KDL tree
00075   if (!kdl_parser::treeFromString(urdf_string, kdl_tree_))
00076   {
00077     ROS_ERROR("Failed to construct KDL tree from URDF.");
00078     return false;
00079   }
00080   num_kdl_joints_ = kdl_tree_.getNrOfJoints();
00081 
00082   // create the joint_segment_mapping, which used to be created by the URDF -> KDL parser
00083   // but not any more, but the rest of the code depends on it, so we simply generate the mapping here:
00084   KDL::SegmentMap segment_map = kdl_tree_.getSegments();
00085 
00086   for (KDL::SegmentMap::const_iterator it = segment_map.begin(); it != segment_map.end(); ++it)
00087   {
00088     if (it->second.segment.getJoint().getType() != KDL::Joint::None)
00089     {
00090       std::string joint_name = it->second.segment.getJoint().getName();
00091       std::string segment_name = it->first;
00092       joint_segment_mapping_.insert(make_pair(joint_name, segment_name));
00093     }
00094   }
00095 
00096   // create the fk solver:
00097   fk_solver_ = new KDL::TreeFkSolverJointPosAxis(kdl_tree_, reference_frame_);
00098 
00099   kdl_number_to_urdf_name_.resize(num_kdl_joints_);
00100   // Create the inverse mapping - KDL segment to joint name
00101   // (at the same time) Create a mapping from KDL numbers to URDF joint names and vice versa
00102   for (map<string, string>::iterator it = joint_segment_mapping_.begin(); it!= joint_segment_mapping_.end(); ++it)
00103   {
00104     std::string joint_name = it->first;
00105     std::string segment_name = it->second;
00106   //  std::cout << joint_name << " -> " << segment_name << std::endl;
00107     segment_joint_mapping_.insert(make_pair(segment_name, joint_name));
00108     int kdl_number = kdl_tree_.getSegment(segment_name)->second.q_nr;
00109     if (kdl_tree_.getSegment(segment_name)->second.segment.getJoint().getType() != KDL::Joint::None)
00110     {
00111   //    std::cout << "Kdl number is " << kdl_number << std::endl;
00112       kdl_number_to_urdf_name_[kdl_number] = joint_name;
00113       urdf_name_to_kdl_number_.insert(make_pair(joint_name, kdl_number));
00114     }
00115   }
00116 
00117   // initialize the planning groups
00118   const std::map<std::string, planning_models::KinematicModel::JointModelGroup*> groups = models->getKinematicModel()->getJointModelGroupMap();  
00119   for(std::map<std::string, planning_models::KinematicModel::JointModelGroup*>::const_iterator it = groups.begin();
00120       it != groups.end();
00121       it++) {
00122     ChompPlanningGroup group;
00123     ROS_INFO_STREAM("Making group for " << it->first);
00124     group.name_ = it->first;
00125     int num_links = it->second->getGroupLinkModels().size();
00126     group.num_joints_ = 0;
00127     group.link_names_.resize(num_links);
00128     std::vector<bool> active_joints;
00129     active_joints.resize(num_kdl_joints_, false);
00130     for (unsigned int i=0; i < it->second->getJointModelNames().size(); i++)
00131     {
00132       std::string joint_name = it->second->getJointModelNames()[i];
00133       map<string, string>::iterator link_name_it = joint_segment_mapping_.find(joint_name);
00134       if (link_name_it == joint_segment_mapping_.end())
00135       {
00136         ROS_WARN("Joint name %s did not have containing KDL segment.", joint_name.c_str());
00137         continue;
00138       }
00139       std::string link_name = link_name_it->second;
00140       group.link_names_[i] = link_name;
00141       const KDL::Segment* segment = &(kdl_tree_.getSegment(link_name)->second.segment);
00142       KDL::Joint::JointType joint_type =  segment->getJoint().getType();
00143       if (joint_type != KDL::Joint::None)
00144       {
00145         ChompJoint joint;
00146         joint.chomp_joint_index_ = group.num_joints_;
00147         joint.kdl_joint_index_ = kdl_tree_.getSegment(link_name)->second.q_nr;
00148         joint.kdl_joint_ = &(segment->getJoint());
00149         joint.link_name_ = link_name;
00150         joint.joint_name_ = segment_joint_mapping_[link_name];
00151         joint.joint_update_limit_ = joint_update_limit;
00152         const planning_models::KinematicModel::JointModel* kin_model_joint = models->getKinematicModel()->getJointModel(joint.joint_name_);
00153         if (const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(kin_model_joint))
00154         {
00155           joint.wrap_around_ = revolute_joint->continuous_;
00156           joint.has_joint_limits_ = !(joint.wrap_around_);
00157           std::pair<double,double> bounds;
00158           revolute_joint->getVariableBounds(revolute_joint->getName(), bounds);
00159           joint.joint_limit_min_ = bounds.first;
00160           joint.joint_limit_max_ = bounds.second;
00161           ROS_DEBUG_STREAM("Setting bounds for joint " << revolute_joint->getName() << " to " << joint.joint_limit_min_ << " " << joint.joint_limit_max_);
00162         }
00163         else if (const planning_models::KinematicModel::PrismaticJointModel* prismatic_joint = dynamic_cast<const planning_models::KinematicModel::PrismaticJointModel*>(kin_model_joint))
00164         {
00165           joint.wrap_around_ = false;
00166           joint.has_joint_limits_ = true;
00167           std::pair<double,double> bounds;
00168           prismatic_joint->getVariableBounds(prismatic_joint->getName(), bounds);
00169           joint.joint_limit_min_ = bounds.first;
00170           joint.joint_limit_max_ = bounds.second;
00171           ROS_DEBUG_STREAM("Setting bounds for joint " << prismatic_joint->getName() << " to " << joint.joint_limit_min_ << " " << joint.joint_limit_max_);
00172         }
00173         else
00174         {
00175           ROS_WARN("CHOMP cannot handle floating or planar joints yet.");
00176         }
00177 
00178         group.num_joints_++;
00179         group.chomp_joints_.push_back(joint);
00180         active_joints[joint.kdl_joint_index_] = true;
00181       }
00182 
00183     }
00184     group.fk_solver_.reset(new KDL::TreeFkSolverJointPosAxisPartial(kdl_tree_, reference_frame_, active_joints));
00185     planning_groups_.insert(make_pair(it->first, group));
00186   }
00187 
00188   // test it:
00189 /*  KDL::JntArray q_in(kdl_tree_.getNrOfJoints());
00190   std::vector<KDL::Frame> segment_frames;
00191   std::vector<KDL::Vector> joint_axis;
00192   std::vector<KDL::Vector> joint_pos;
00193 
00194   ros::WallTime start_time = ros::WallTime::now();
00195 
00196   boost::shared_ptr<KDL::TreeFkSolverJointPosAxisPartial> fks = planning_groups_["right_arm"].fk_solver_;
00197   double q=0.0;
00198   int n = kdl_tree_.getNrOfJoints();
00199   for (int i=0; i<100000; i++)
00200   {
00201     for (int j=0; j<n; j++)
00202     {
00203       q_in(j) += q;
00204       q+=0.1;
00205     }
00206     if (i==0)
00207       fks->JntToCartFull(q_in, joint_pos, joint_axis, segment_frames);
00208     else
00209       fks->JntToCartPartial(q_in, joint_pos, joint_axis, segment_frames);
00210   }
00211   ROS_INFO("100000 FK calls in %f wall-seconds.", (ros::WallTime::now() - start_time).toSec());
00212 */
00213 
00214   ROS_INFO("Initialized chomp robot model in %s reference frame.", reference_frame_.c_str());
00215 
00216   return true;
00217 }
00218 
00219 void ChompRobotModel::getLinkInformation(const std::string link_name, std::vector<int>& active_joints, int& segment_number)
00220 {
00221   ROS_DEBUG_STREAM("Link info for " << link_name);
00222 
00223   // identify the joints that contribute to this link
00224   active_joints.clear();
00225   KDL::SegmentMap::const_iterator segment_iter = kdl_tree_.getSegment(link_name);
00226 
00227   // go up the tree until we find the root:
00228   while (segment_iter != kdl_tree_.getRootSegment())
00229   {
00230     KDL::Joint::JointType joint_type =  segment_iter->second.segment.getJoint().getType();
00231     if (joint_type != KDL::Joint::None)
00232     {
00233       active_joints.push_back(segment_iter->second.q_nr);
00234       ROS_DEBUG_STREAM("Adding parent " << segment_iter->second.segment.getJoint().getName());
00235     }
00236     segment_iter = segment_iter->second.parent;
00237   }
00238   ROS_DEBUG(" ");
00239 
00240   segment_number = fk_solver_->segmentNameToIndex(link_name);
00241 
00242 }
00243 
00244 void ChompRobotModel::getActiveJointInformation(const std::string &link_name, 
00245                                                 std::vector<int>& active_joints, 
00246                                                 int& segment_number)
00247 {
00248   KDL::SegmentMap::const_iterator segment_iter = kdl_tree_.getSegment(link_name);
00249   // go up the tree until we find the root:
00250   while (segment_iter != kdl_tree_.getRootSegment())
00251   {
00252     KDL::Joint::JointType joint_type =  segment_iter->second.segment.getJoint().getType();
00253     if (joint_type != KDL::Joint::None)
00254     {
00255       active_joints.push_back(segment_iter->second.q_nr);
00256       ROS_DEBUG_STREAM("Adding parent " << segment_iter->second.segment.getJoint().getName());
00257     }
00258     segment_iter = segment_iter->second.parent;
00259   }
00260   ROS_DEBUG(" ");
00261   segment_number = fk_solver_->segmentNameToIndex(link_name);
00262 }
00263 
00264 } // namespace chomp
chomp_robot_model.cpp
