ompl_ros_ik_sampler.cpp
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, Inc. 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 <ompl_ros_interface/ik/ompl_ros_ik_sampler.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 bool OmplRosIKSampler::initialize(const ompl::base::StateSpacePtr &state_space,
00042                                   const std::string &kinematics_solver_name,
00043                                   const std::string &group_name,
00044                                   const std::string &end_effector_name,
00045                                   const planning_environment::CollisionModelsInterface* cmi)
00046 {
00047   collision_models_interface_ = cmi;
00048   state_space_ = state_space;
00049   group_name_ = group_name;
00050   end_effector_name_ = end_effector_name;
00051 
00052   ros::NodeHandle node_handle("~");
00053 
00054   ROS_DEBUG("Trying to initialize solver %s",kinematics_solver_name.c_str());
00055   if(!kinematics_loader_.isClassAvailable(kinematics_solver_name))
00056   {
00057     ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str());
00058     return false;
00059   }
00060   ROS_DEBUG("Found solver %s",kinematics_solver_name.c_str());
00061   
00062   try
00063   {
00064     kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
00065   }
00066   catch(pluginlib::PluginlibException& ex)    //handle the class failing to load
00067   {
00068     ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
00069     return false;
00070   }
00071   ROS_DEBUG("Loaded solver %s",kinematics_solver_name.c_str());
00072 
00073   std::string base_name, tip_name;
00074   if(!node_handle.hasParam(group_name_+"/root_name"))
00075   {
00076     ROS_ERROR_STREAM("Kinematics solver has no root name " << base_name << " in param ns " << node_handle.getNamespace());
00077     throw new OMPLROSException();
00078   }
00079   node_handle.getParam(group_name_+"/root_name",base_name);
00080 
00081   if(!node_handle.hasParam(group_name_+"/tip_name"))
00082   {
00083     ROS_ERROR_STREAM("Kinematics solver has no root name " << tip_name << " in param ns " << node_handle.getNamespace());
00084     throw new OMPLROSException();
00085   }
00086   node_handle.getParam(group_name_+"/tip_name",tip_name);
00087 
00088   if(!kinematics_solver_->initialize(group_name,
00089                                      base_name,
00090                                      tip_name,
00091                                      .01)) {
00092     ROS_ERROR("Could not initialize kinematics solver for group %s",group_name.c_str());
00093     return false;
00094   }
00095   ROS_DEBUG("Initialized solver %s",kinematics_solver_name.c_str());
00096   scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_));
00097   seed_state_.joint_state.name = kinematics_solver_->getJointNames();
00098   seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00099   //  arm_navigation_msgs::printJointState(seed_state_.joint_state);
00100   solution_state_.joint_state.name = kinematics_solver_->getJointNames();
00101   solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00102   if(!ompl_ros_interface::getRobotStateToOmplStateMapping(seed_state_,*scoped_state_,robot_state_to_ompl_state_mapping_))
00103   {
00104     ROS_ERROR("Could not get mapping between robot state and ompl state");
00105     return false;
00106   }
00107   if(!ompl_ros_interface::getOmplStateToRobotStateMapping(*scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_))
00108   {
00109     ROS_ERROR("Could not get mapping between ompl state and robot state");
00110     return false;
00111   }
00112   else
00113   {
00114     ROS_DEBUG("Real vector index: %d",ompl_state_to_robot_state_mapping_.real_vector_index);
00115     for(unsigned int i=0; i < ompl_state_to_robot_state_mapping_.real_vector_mapping.size(); i++)
00116       ROS_DEBUG("mapping: %d %d",i,ompl_state_to_robot_state_mapping_.real_vector_mapping[i]);
00117   }
00118   ROS_DEBUG("Initialized Ompl Ros IK Sampler");
00119   return true;
00120 }  
00121 
00122 bool OmplRosIKSampler::configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request,
00123                                           arm_navigation_msgs::GetMotionPlan::Response &response,
00124                                           const unsigned int &max_sample_count)
00125 {
00126   ik_poses_counter_ = 0;
00127   max_sample_count_ = max_sample_count;
00128   num_samples_ = 0;
00129   ik_poses_.clear();
00130   arm_navigation_msgs::Constraints goal_constraints = request.motion_plan_request.goal_constraints;
00131 
00132   if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00133                                                                             goal_constraints,
00134                                                                             kinematics_solver_->getBaseName())) {
00135     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00136     return false;
00137   }
00138   
00139   if(!arm_navigation_msgs::constraintsToPoseStampedVector(goal_constraints, ik_poses_))
00140   {
00141     ROS_ERROR("Could not get poses from constraints");
00142     return false;
00143   }       
00144   if(ik_poses_.empty())
00145   {
00146     ROS_WARN("Could not setup goals for inverse kinematics sampling");
00147     return false;
00148   }
00149   else
00150     ROS_DEBUG("Setup %d poses for IK sampler",(int)ik_poses_.size());
00151   for(unsigned int i=0; i < ik_poses_.size(); i++)
00152   {
00153     if(ik_poses_[i].header.frame_id != kinematics_solver_->getBaseName())
00154     {
00155       ROS_ERROR("Goals for inverse kinematic sampling in %s frame are not in kinematics frame: %s",
00156                 ik_poses_[i].header.frame_id.c_str(),
00157                 kinematics_solver_->getBaseName().c_str());
00158       return false;
00159     }
00160   }
00161   return true;
00162 }
00163 
00164 bool OmplRosIKSampler::sampleGoal(const ompl::base::GoalLazySamples *gls, ompl::base::State *state)
00165 {
00166   arm_navigation_msgs::RobotState seed_state,solution_state;
00167   seed_state = seed_state_;
00168   solution_state = solution_state_; 
00169   ompl::base::ScopedState<ompl::base::CompoundStateSpace> scoped_state(state_space_);
00170   //sample a state at random
00171   scoped_state.random();
00172 
00173   ompl_ros_interface::omplStateToRobotState(scoped_state,
00174                                             ompl_state_to_robot_state_mapping_,
00175                                             seed_state);    
00176   int error_code;
00177   if(kinematics_solver_->getPositionIK(ik_poses_[ik_poses_counter_].pose,
00178                                        seed_state.joint_state.position,
00179                                        solution_state.joint_state.position,
00180                                        error_code))
00181   {
00182     // arm_navigation_msgs::printJointState(solution_state.joint_state);
00183     ompl_ros_interface::robotStateToOmplState(solution_state,
00184                                               robot_state_to_ompl_state_mapping_,
00185                                               state);    
00186     ik_poses_counter_++;
00187     ik_poses_counter_ = ik_poses_counter_%ik_poses_.size();
00188     return true;
00189   }
00190   return false;
00191 }
00192 
00193 bool OmplRosIKSampler::sampleGoals(const ompl::base::GoalLazySamples *gls, ompl::base::State *state)
00194 {
00195   bool continue_sampling = sampleGoal(gls,state);
00196   if(continue_sampling)
00197     num_samples_++;
00198   //  return continue_sampling && gls->maxSampleCount() < max_sample_count_ && !gls->isAchieved();
00199 
00200   if(num_samples_ > max_sample_count_)
00201     return false;
00202   else
00203     return true;
00204 }
00205 
00206 }


ompl_ros_interface
Author(s): Sachin Chitta
autogenerated on Thu Dec 12 2013 11:10:58