ompl_ros_interface: ompl_ros_interface: ompl_ros_joint

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/planners/ompl_ros_joint_planner.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 
00042 bool OmplRosJointPlanner::initializePlanningStateSpace(ompl::base::StateSpacePtr &state_space)
00043 {
00044   //Setup the corresponding ompl state space
00045   state_space = ompl_ros_interface::jointGroupToOmplStateSpacePtr(physical_joint_group_,
00046                                                                         ompl_state_to_kinematic_state_mapping_,
00047                                                                         kinematic_state_to_ompl_state_mapping_);
00048   if(!state_space)
00049   {
00050     ROS_ERROR("Could not set up the ompl state space from group %s",group_name_.c_str());
00051     return false;
00052   }
00053   std::string physical_group_name = physical_joint_group_->getName();
00054   if(node_handle_.hasParam(physical_group_name+"/tip_name"))
00055   {
00056     node_handle_.getParam(physical_group_name+"/tip_name",end_effector_name_);
00057     ROS_DEBUG("Group: %s, End effector: %s",physical_group_name.c_str(),end_effector_name_.c_str());
00058     if(node_handle_.hasParam(physical_group_name+"/kinematics_solver"))
00059     {
00060       node_handle_.getParam(physical_group_name+"/kinematics_solver",kinematics_solver_name_);
00061       ROS_DEBUG("Kinematics solver: %s",kinematics_solver_name_.c_str());
00062       ROS_DEBUG("Created new ik sampler: %s",kinematics_solver_name_.c_str());
00063       if(!ik_sampler_.initialize(state_space_,kinematics_solver_name_,physical_group_name,end_effector_name_,collision_models_interface_))
00064       {
00065         ROS_ERROR("Could not set IK sampler for pose goal");
00066       }
00067       else
00068         ik_sampler_available_ = true;
00069     }
00070   }
00071 
00072   return true;
00073 }
00074 
00075 bool OmplRosJointPlanner::isRequestValid(arm_navigation_msgs::GetMotionPlan::Request &request,
00076                                          arm_navigation_msgs::GetMotionPlan::Response &response)
00077 {
00078   if(request.motion_plan_request.group_name != group_name_)
00079   {
00080     ROS_ERROR("Invalid group name: %s",request.motion_plan_request.group_name.c_str());
00081     response.error_code.val = response.error_code.INVALID_GROUP_NAME;
00082     return false;
00083   }
00084   for (unsigned int i = 0 ; i < request.motion_plan_request.goal_constraints.position_constraints.size() ; ++i)
00085   {
00086     if (!(request.motion_plan_request.goal_constraints.position_constraints[i].link_name == end_effector_name_))
00087     {
00088       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_LINK_NAME;
00089       ROS_ERROR("Cartesian goals for link %s are the only ones that can be processed", end_effector_name_.c_str());
00090       return false;      
00091     }
00092   }
00093   for (unsigned int i = 0 ; i < request.motion_plan_request.goal_constraints.orientation_constraints.size() ; ++i)
00094   {
00095     if (!(request.motion_plan_request.goal_constraints.orientation_constraints[i].link_name == end_effector_name_))
00096     {
00097       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_LINK_NAME;
00098       ROS_ERROR("Cartesian goals for link %s are the only ones that can be processed", end_effector_name_.c_str());
00099       return false;      
00100     }
00101   }
00102   if(!request.motion_plan_request.goal_constraints.position_constraints.empty()
00103      && !request.motion_plan_request.goal_constraints.orientation_constraints.empty())
00104   {
00105     if(request.motion_plan_request.goal_constraints.position_constraints.size() != request.motion_plan_request.goal_constraints.orientation_constraints.size())
00106     {
00107       ROS_ERROR("Can only deal with requests that have the same number of position and orientation constraints");
00108       response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_GOAL_POSITION_CONSTRAINTS;
00109       return false;
00110     }
00111   }
00112   if(request.motion_plan_request.allowed_planning_time.toSec() <= 0.0)
00113   {
00114     response.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_TIMEOUT;
00115     ROS_ERROR("Request does not specify correct allowed planning time %f",request.motion_plan_request.allowed_planning_time.toSec());
00116     return false;
00117   }
00118   return true;
00119 }
00120 
00121 bool OmplRosJointPlanner::setGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
00122                                   arm_navigation_msgs::GetMotionPlan::Response &response)
00123 {
00124 
00125   if(!request.motion_plan_request.goal_constraints.joint_constraints.empty() 
00126      && request.motion_plan_request.goal_constraints.position_constraints.empty() 
00127      && request.motion_plan_request.goal_constraints.orientation_constraints.empty())
00128   {
00129     ROS_DEBUG("Joint space goal");
00130     return setJointGoal(request,response);
00131   }
00132   else if(!request.motion_plan_request.goal_constraints.position_constraints.empty()
00133           && !request.motion_plan_request.goal_constraints.orientation_constraints.empty())
00134   {
00135     ROS_DEBUG("Pose goal");
00136     return setPoseGoal(request,response);
00137   }
00138   else 
00139   {
00140     ROS_ERROR("Cannot handle request since its not a joint goal or fully specified pose goal (with position and orientation constraints");
00141     return false;
00142   }
00143 }
00144 
00145 bool OmplRosJointPlanner::setStart(arm_navigation_msgs::GetMotionPlan::Request &request,
00146                                     arm_navigation_msgs::GetMotionPlan::Response &response)
00147 {
00148   ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_);
00149   ROS_DEBUG("Start");
00150   if(!ompl_ros_interface::kinematicStateGroupToOmplState(physical_joint_state_group_,
00151                                                          kinematic_state_to_ompl_state_mapping_,
00152                                                          start))
00153   {
00154     ROS_ERROR("Could not set start state");
00155     return false;
00156   }
00157 
00158   ompl_ros_interface::OmplRosJointStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosJointStateValidityChecker*>(state_validity_checker_.get());  
00159   if(!my_checker->isStateValid(start.get()))
00160   {
00161     response.error_code = my_checker->getLastErrorCode();
00162     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00163       response.error_code.val = response.error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00164     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00165       response.error_code.val = response.error_code.START_STATE_IN_COLLISION;
00166     ROS_ERROR("Start state is invalid. Reason: %s",arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00167     return false;
00168   }
00169   planner_->getProblemDefinition()->clearStartStates(); 
00170   planner_->addStartState(start);
00171   return true;
00172 }
00173 
00174 bool OmplRosJointPlanner::setJointGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
00175                                         arm_navigation_msgs::GetMotionPlan::Response &response)
00176 {
00177   ompl::base::ScopedState<ompl::base::CompoundStateSpace> goal(state_space_);
00178   ompl::base::GoalPtr goal_states(new ompl::base::GoalStates(planner_->getSpaceInformation()));
00179   unsigned int dimension = physical_joint_state_group_->getDimension();
00180   unsigned int num_goals = request.motion_plan_request.goal_constraints.joint_constraints.size()/dimension;
00181   if(!(num_goals*dimension == request.motion_plan_request.goal_constraints.joint_constraints.size()))
00182   {
00183     if(request.motion_plan_request.goal_constraints.joint_constraints.size() < dimension)
00184     {
00185       response.error_code.val = response.error_code.PLANNING_FAILED;
00186       ROS_ERROR_STREAM("Joint space goal specification did not specify goal for all joints in group expected " << dimension <<" but got " << request.motion_plan_request.goal_constraints.joint_constraints.size() );   
00187       return false;
00188     }
00189     else
00190       num_goals = 1;
00191   }
00192   for(unsigned int i=0; i < num_goals; i++)
00193   {
00194     std::vector<arm_navigation_msgs::JointConstraint> joint_constraints;
00195     for(unsigned int j=0; j < dimension; j++)
00196       joint_constraints.push_back(request.motion_plan_request.goal_constraints.joint_constraints[i*dimension+j]);
00197     if(!ompl_ros_interface::jointConstraintsToOmplState(joint_constraints,goal))
00198     {
00199       response.error_code.val = response.error_code.PLANNING_FAILED;
00200       ROS_ERROR("Could not convert joint space constraints to ompl state");     
00201       return false;
00202     }
00203     goal_states->as<ompl::base::GoalStates>()->addState(goal.get());
00204   }
00205   ompl_ros_interface::OmplRosJointStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosJointStateValidityChecker*>(state_validity_checker_.get());  
00206   if(num_goals == 1 && !my_checker->isStateValid(goal.get()))
00207   {
00208     response.error_code = my_checker->getLastErrorCode();
00209     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00210       response.error_code.val = response.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00211     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00212       response.error_code.val = response.error_code.GOAL_IN_COLLISION;
00213     ROS_ERROR("Joint space goal is invalid. Reason: %s",arm_navigation_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00214     return false;
00215   }  
00216   ROS_INFO_STREAM("Setting " << num_goals << " goals");
00217   planner_->setGoal(goal_states);    
00218   return true;
00219 }
00220 
00221 bool OmplRosJointPlanner::setPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &request,
00222                                        arm_navigation_msgs::GetMotionPlan::Response &response)
00223 {
00224   if(!ik_sampler_available_)
00225   {
00226     ROS_ERROR("Cannot solve for pose goals since an ik sampler has not been defined");
00227     response.error_code.val = response.error_code.PLANNING_FAILED;
00228     return false;
00229   }
00230   ik_sampler_.configureOnRequest(request,response,100);
00231   ompl::base::GoalPtr goal;
00232   goal.reset(new ompl::base::GoalLazySamples(planner_->getSpaceInformation(),boost::bind(&OmplRosIKSampler::sampleGoals,&ik_sampler_,_1,_2)));
00233   planner_->setGoal(goal);
00234   return true;
00235 }
00236 
00237 bool OmplRosJointPlanner::initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker)
00238 {
00239   state_validity_checker.reset(new ompl_ros_interface::OmplRosJointStateValidityChecker(planner_->getSpaceInformation().get(),
00240                                                                                         collision_models_interface_,
00241                                                                                         ompl_state_to_kinematic_state_mapping_));
00242     return true;
00243 }
00244 
00245 arm_navigation_msgs::RobotTrajectory OmplRosJointPlanner::getSolutionPath()
00246 {
00247   arm_navigation_msgs::RobotTrajectory robot_trajectory;
00248   ompl::geometric::PathGeometric solution = planner_->getSolutionPath();
00249   solution.interpolate();
00250   omplPathGeometricToRobotTrajectory(solution,robot_trajectory);
00251   return robot_trajectory;
00252 }
00253 
00254 
00255 }
ompl_ros_joint_planner.cpp
