$search
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_goal_sampleable_region.h> 00038 00039 namespace ompl_ros_interface 00040 { 00041 bool OmplRosIKSampleableRegion::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 if(!kinematics_loader_.isClassAvailable(kinematics_solver_name)) 00052 { 00053 ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str()); 00054 return false; 00055 } 00056 00057 try 00058 { 00059 kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name); 00060 return false; 00061 } 00062 catch(pluginlib::PluginlibException& ex) //handle the class failing to load 00063 { 00064 ROS_ERROR("The plugin failed to load. Error: %s", ex.what()); 00065 return false; 00066 } 00067 if(!kinematics_solver_->initialize(group_name)) 00068 { 00069 ROS_ERROR("Could not initialize kinematics solver for group %s",group_name.c_str()); 00070 return false; 00071 } 00072 // scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_)); 00073 seed_state_.joint_state.name = kinematics_solver_->getJointNames(); 00074 seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size()); 00075 solution_state_.joint_state.name = kinematics_solver_->getJointNames(); 00076 solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size()); 00077 if(!ompl_ros_interface::getRobotStateToOmplStateMapping(seed_state_,scoped_state_,robot_state_to_ompl_state_mapping_)) 00078 return false; 00079 if(!ompl_ros_interface::getOmplStateToRobotStateMapping(scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_)) 00080 return false; 00081 } 00082 00083 bool OmplRosIKSampleableRegion::configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request, 00084 arm_navigation_msgs::GetMotionPlan::Response &response, 00085 const unsigned int &max_sample_count) 00086 { 00087 max_sample_count_ = max_sample_count; 00088 ik_poses_.clear(); 00089 arm_navigation_msgs::Constraints goal_constraints = request.motion_plan_request.goal_constraints; 00090 00091 if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(), 00092 goal_constraints, 00093 kinematics_solver_->getBaseFrame())) { 00094 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; 00095 return false; 00096 } 00097 if(!arm_navigation_msgs::constraintsToPoseStampedVector(goal_constraints, ik_poses_)) 00098 { 00099 ROS_ERROR("Could not get poses from constraints"); 00100 return false; 00101 } 00102 if(ik_poses_.empty()) 00103 { 00104 ROS_WARN("Could not setup goals for inverse kinematics sampling"); 00105 return false; 00106 } 00107 for(unsigned int i=0; i < ik_poses_.size(); i++) 00108 { 00109 if(ik_poses_[i].header.frame_id != kinematics_solver_->getBaseFrame()) 00110 { 00111 ROS_ERROR("Goals for inverse kinematic sampling in %s frame are not in kinematics frame: %s", 00112 ik_poses_[i].header.frame_id.c_str(), 00113 kinematics_solver_->getBaseFrame().c_str()); 00114 return false; 00115 } 00116 } 00117 return true; 00118 } 00119 00120 unsigned int OmplRosIKSampleableRegion::maxSampleCount(void) const 00121 { 00122 return max_sample_count_; 00123 } 00124 00125 void OmplRosIKSampleableRegion::sampleGoal(ompl::base::State *state) const 00126 00127 { 00128 std::vector<arm_navigation_msgs::RobotState> sampled_states_vector; 00129 sampleGoals(1,sampled_states_vector); 00130 if(!sampled_states_vector.empty()) 00131 { 00132 ompl_ros_interface::robotStateToOmplState(sampled_states_vector.front(), 00133 robot_state_to_ompl_state_mapping_, 00134 state); 00135 } 00136 } 00137 00138 void OmplRosIKSampleableRegion::sampleGoals(const unsigned int &number_goals, 00139 std::vector<arm_navigation_msgs::RobotState> &sampled_states_vector) const 00140 { 00141 arm_navigation_msgs::RobotState seed_state,solution_state; 00142 seed_state = seed_state_; 00143 solution_state = solution_state_; 00144 ompl::base::ScopedState<ompl::base::CompoundStateSpace> scoped_state(state_space_); 00145 unsigned int ik_poses_counter = 0; 00146 for(unsigned int i=0; i < number_goals; i++) 00147 { 00148 //sample a state at random 00149 scoped_state.random(); 00150 ompl_ros_interface::omplStateToRobotState(scoped_state, 00151 ompl_state_to_robot_state_mapping_, 00152 seed_state); 00153 int error_code; 00154 if(kinematics_solver_->getPositionIK(ik_poses_[ik_poses_counter].pose, 00155 seed_state.joint_state.position, 00156 solution_state.joint_state.position, 00157 error_code)) 00158 { 00159 sampled_states_vector.push_back(solution_state); 00160 ik_poses_counter++; 00161 ik_poses_counter = ik_poses_counter%ik_poses_.size(); 00162 } 00163 } 00164 } 00165 }