ompl_ros_interface: ompl_ros_joint_planner.cpp Source 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/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_,planning_monitor_))
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(motion_planning_msgs::GetMotionPlan::Request &request,
00076                                           motion_planning_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 
00085   for (unsigned int i = 0 ; i < request.motion_plan_request.goal_constraints.position_constraints.size() ; ++i)
00086   {
00087     if (!planning_monitor_->getTransformListener()->frameExists(request.motion_plan_request.goal_constraints.position_constraints[i].header.frame_id))
00088     {
00089       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;
00090       ROS_ERROR("Frame '%s' is not defined for goal position constraint message %u", request.motion_plan_request.goal_constraints.position_constraints[i].header.frame_id.c_str(), i);
00091       return false;
00092     }
00093     if (!(request.motion_plan_request.goal_constraints.position_constraints[i].link_name == end_effector_name_))
00094     {
00095       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::INVALID_LINK_NAME;
00096       ROS_ERROR("Cartesian goals for link %s are the only ones that can be processed", end_effector_name_.c_str());
00097       return false;      
00098     }
00099   }
00100 
00101   for (unsigned int i = 0 ; i < request.motion_plan_request.goal_constraints.orientation_constraints.size() ; ++i)
00102   {
00103     if (!planning_monitor_->getTransformListener()->frameExists(request.motion_plan_request.goal_constraints.orientation_constraints[i].header.frame_id))
00104     { 
00105       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;
00106       ROS_ERROR("Frame '%s' is not defined for goal pose constraint message %u", request.motion_plan_request.goal_constraints.orientation_constraints[i].header.frame_id.c_str(), i);
00107       return false;
00108     }
00109     if (!(request.motion_plan_request.goal_constraints.orientation_constraints[i].link_name == end_effector_name_))
00110     {
00111       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::INVALID_LINK_NAME;
00112       ROS_ERROR("Cartesian goals for link %s are the only ones that can be processed", end_effector_name_.c_str());
00113       return false;      
00114     }
00115   }
00116 
00117   for (unsigned int i = 0 ; i < request.motion_plan_request.path_constraints.position_constraints.size() ; ++i)
00118     if (!planning_monitor_->getTransformListener()->frameExists(request.motion_plan_request.path_constraints.position_constraints[i].header.frame_id))
00119     {
00120       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;      
00121       ROS_ERROR("Frame '%s' is not defined for path pose constraint message %u", request.motion_plan_request.path_constraints.position_constraints[i].header.frame_id.c_str(), i);
00122       return false;
00123     }
00124 
00125   for (unsigned int i = 0 ; i < request.motion_plan_request.path_constraints.orientation_constraints.size() ; ++i)
00126     if (!planning_monitor_->getTransformListener()->frameExists(request.motion_plan_request.path_constraints.orientation_constraints[i].header.frame_id))
00127     {
00128       response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::FRAME_TRANSFORM_FAILURE;
00129       ROS_ERROR("Frame '%s' is not defined for path pose constraint message %u", request.motion_plan_request.path_constraints.orientation_constraints[i].header.frame_id.c_str(), i);
00130       return false;
00131     }
00132 
00133   if(request.motion_plan_request.allowed_planning_time.toSec() <= 0.0)
00134   {
00135     response.error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::INVALID_TIMEOUT;
00136     ROS_ERROR("Request does not specify correct allowed planning time %f",request.motion_plan_request.allowed_planning_time.toSec());
00137     return false;
00138   }
00139   return true;
00140 }
00141 
00142 bool OmplRosJointPlanner::setGoal(motion_planning_msgs::GetMotionPlan::Request &request,
00143                                    motion_planning_msgs::GetMotionPlan::Response &response)
00144 {
00145 
00146   if(!request.motion_plan_request.goal_constraints.joint_constraints.empty() 
00147      && request.motion_plan_request.goal_constraints.position_constraints.empty() 
00148      && request.motion_plan_request.goal_constraints.orientation_constraints.empty())
00149   {
00150     ROS_DEBUG("Joint space goal");
00151     return setJointGoal(request,response);
00152   }
00153   else if(!request.motion_plan_request.goal_constraints.position_constraints.empty()
00154           && !request.motion_plan_request.goal_constraints.orientation_constraints.empty())
00155   {
00156     ROS_DEBUG("Pose goal");
00157     return setPoseGoal(request,response);
00158   }
00159   else 
00160   {
00161     ROS_ERROR("Cannot handle request");
00162     return false;
00163   }
00164 }
00165 
00166 bool OmplRosJointPlanner::setStart(motion_planning_msgs::GetMotionPlan::Request &request,
00167                                     motion_planning_msgs::GetMotionPlan::Response &response)
00168 {
00169   ompl::base::ScopedState<ompl::base::CompoundStateSpace> start(state_space_);
00170   ROS_DEBUG("Start");
00171   if(!ompl_ros_interface::kinematicStateGroupToOmplState(physical_joint_state_group_,
00172                                                          kinematic_state_to_ompl_state_mapping_,
00173                                                          start))
00174   {
00175     ROS_ERROR("Could not set start state");
00176     return false;
00177   }
00178 
00179   if(!ompl_ros_interface::robotStateToOmplState(request.motion_plan_request.start_state,start,false))
00180   {
00181     ROS_ERROR("Could not set start state");
00182     return false;
00183   }
00184 
00185   ompl_ros_interface::OmplRosJointStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosJointStateValidityChecker*>(state_validity_checker_.get());  
00186   if(!my_checker->isStateValid(start.get()))
00187   {
00188     response.error_code = my_checker->getLastErrorCode();
00189     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00190       response.error_code.val = response.error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00191     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00192       response.error_code.val = response.error_code.START_STATE_IN_COLLISION;
00193     ROS_ERROR("Start state is invalid. Reason: %s",motion_planning_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00194     return false;
00195   }
00196   planner_->getProblemDefinition()->clearStartStates(); 
00197   planner_->addStartState(start);
00198   return true;
00199 }
00200 
00201 bool OmplRosJointPlanner::setJointGoal(motion_planning_msgs::GetMotionPlan::Request &request,
00202                                         motion_planning_msgs::GetMotionPlan::Response &response)
00203 {
00204   ompl::base::ScopedState<ompl::base::CompoundStateSpace> goal(state_space_);
00205   ompl::base::GoalPtr goal_states(new ompl::base::GoalStates(planner_->getSpaceInformation()));
00206   unsigned int dimension = physical_joint_state_group_->getDimension();
00207   unsigned int num_goals = request.motion_plan_request.goal_constraints.joint_constraints.size()/dimension;
00208   if(!(num_goals*dimension == request.motion_plan_request.goal_constraints.joint_constraints.size()))
00209   {
00210     if(request.motion_plan_request.goal_constraints.joint_constraints.size() < dimension)
00211     {
00212       response.error_code.val = response.error_code.PLANNING_FAILED;
00213       ROS_ERROR("Joint space goal specification did not specify goal for all joints in group"); 
00214       return false;
00215     }
00216     else
00217       num_goals = 1;
00218   }
00219   for(unsigned int i=0; i < num_goals; i++)
00220   {
00221     std::vector<motion_planning_msgs::JointConstraint> joint_constraints;
00222     for(unsigned int j=0; j < dimension; j++)
00223       joint_constraints.push_back(request.motion_plan_request.goal_constraints.joint_constraints[i*dimension+j]);
00224     if(!ompl_ros_interface::jointConstraintsToOmplState(joint_constraints,goal))
00225     {
00226       response.error_code.val = response.error_code.PLANNING_FAILED;
00227       ROS_ERROR("Could not convert joint space constraints to ompl state");     
00228       return false;
00229     }
00230     goal_states->as<ompl::base::GoalStates>()->addState(goal.get());
00231   }
00232   ompl_ros_interface::OmplRosJointStateValidityChecker *my_checker = dynamic_cast<ompl_ros_interface::OmplRosJointStateValidityChecker*>(state_validity_checker_.get());  
00233   if(num_goals == 1 && !my_checker->isStateValid(goal.get()))
00234   {
00235     response.error_code = my_checker->getLastErrorCode();
00236     if(response.error_code.val == response.error_code.PATH_CONSTRAINTS_VIOLATED)
00237       response.error_code.val = response.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00238     else if(response.error_code.val == response.error_code.COLLISION_CONSTRAINTS_VIOLATED)
00239       response.error_code.val = response.error_code.GOAL_IN_COLLISION;
00240     ROS_ERROR("Joint space goal is invalid. Reason: %s",motion_planning_msgs::armNavigationErrorCodeToString(response.error_code).c_str());
00241     return false;
00242   }  
00243   planner_->setGoal(goal_states);    
00244   return true;
00245 }
00246 
00247 bool OmplRosJointPlanner::setPoseGoal(motion_planning_msgs::GetMotionPlan::Request &request,
00248                                        motion_planning_msgs::GetMotionPlan::Response &response)
00249 {
00250   if(!ik_sampler_available_)
00251   {
00252     ROS_ERROR("Cannot solve for pose goals since an ik sampler has not been defined");
00253     response.error_code.val = response.error_code.PLANNING_FAILED;
00254     return false;
00255   }
00256   ik_sampler_.configureOnRequest(request,response,100);
00257   ompl::base::GoalPtr goal;
00258   goal.reset(new ompl::base::GoalLazySamples(planner_->getSpaceInformation(),boost::bind(&OmplRosIKSampler::sampleGoals,&ik_sampler_,_1,_2)));
00259   planner_->setGoal(goal);
00260   return true;
00261 }
00262 
00263 bool OmplRosJointPlanner::initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker)
00264 {
00265   state_validity_checker.reset(new ompl_ros_interface::OmplRosJointStateValidityChecker(planner_->getSpaceInformation().get(),
00266                                                                                         planning_monitor_,
00267                                                                                         ompl_state_to_kinematic_state_mapping_));
00268     return true;
00269 }
00270 
00271 motion_planning_msgs::RobotTrajectory OmplRosJointPlanner::getSolutionPath()
00272 {
00273   motion_planning_msgs::RobotTrajectory robot_trajectory;
00274   ompl::geometric::PathGeometric solution = planner_->getSolutionPath();
00275   solution.interpolate();
00276   omplPathGeometricToRobotTrajectory(solution,robot_trajectory);
00277   return robot_trajectory;
00278 }
00279 
00280 
00281 }