00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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::PlanningMonitor *planning_monitor)
00046 {
00047 planning_monitor_ = planning_monitor;
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)
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
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 motion_planning_msgs::GetMotionPlan::Request &request,
00084 motion_planning_msgs::GetMotionPlan::Response &response,
00085 const unsigned int &max_sample_count)
00086 {
00087 max_sample_count_ = max_sample_count;
00088 ik_poses_.clear();
00089 motion_planning_msgs::Constraints goal_constraints = request.motion_plan_request.goal_constraints;
00090
00091 if(!planning_monitor_->transformConstraintsToFrame(goal_constraints,
00092 kinematics_solver_->getBaseFrame(),
00093 response.error_code))
00094 return false;
00095
00096 if(!motion_planning_msgs::constraintsToPoseStampedVector(goal_constraints, ik_poses_))
00097 {
00098 ROS_ERROR("Could not get poses from constraints");
00099 return false;
00100 }
00101 if(ik_poses_.empty())
00102 {
00103 ROS_INFO("Could not setup goals for inverse kinematics sampling");
00104 return false;
00105 }
00106 for(unsigned int i=0; i < ik_poses_.size(); i++)
00107 {
00108 if(ik_poses_[i].header.frame_id != kinematics_solver_->getBaseFrame())
00109 {
00110 ROS_ERROR("Goals for inverse kinematic sampling in %s frame are not in kinematics frame: %s",
00111 ik_poses_[i].header.frame_id.c_str(),
00112 kinematics_solver_->getBaseFrame().c_str());
00113 return false;
00114 }
00115 }
00116 return true;
00117 }
00118
00119 unsigned int OmplRosIKSampleableRegion::maxSampleCount(void) const
00120 {
00121 return max_sample_count_;
00122 }
00123
00124 void OmplRosIKSampleableRegion::sampleGoal(ompl::base::State *state) const
00125
00126 {
00127 std::vector<motion_planning_msgs::RobotState> sampled_states_vector;
00128 sampleGoals(1,sampled_states_vector);
00129 if(!sampled_states_vector.empty())
00130 {
00131 ompl_ros_interface::robotStateToOmplState(sampled_states_vector.front(),
00132 robot_state_to_ompl_state_mapping_,
00133 state);
00134 }
00135 }
00136
00137 void OmplRosIKSampleableRegion::sampleGoals(const unsigned int &number_goals,
00138 std::vector<motion_planning_msgs::RobotState> &sampled_states_vector) const
00139 {
00140 motion_planning_msgs::RobotState seed_state,solution_state;
00141 seed_state = seed_state_;
00142 solution_state = solution_state_;
00143 ompl::base::ScopedState<ompl::base::CompoundStateSpace> scoped_state(state_space_);
00144 unsigned int ik_poses_counter = 0;
00145 for(unsigned int i=0; i < number_goals; i++)
00146 {
00147
00148 scoped_state.random();
00149 ompl_ros_interface::omplStateToRobotState(scoped_state,
00150 ompl_state_to_robot_state_mapping_,
00151 seed_state);
00152 int error_code;
00153 if(kinematics_solver_->getPositionIK(ik_poses_[ik_poses_counter].pose,
00154 seed_state.joint_state.position,
00155 solution_state.joint_state.position,
00156 error_code))
00157 {
00158 sampled_states_vector.push_back(solution_state);
00159 ik_poses_counter++;
00160 ik_poses_counter = ik_poses_counter%ik_poses_.size();
00161 }
00162 }
00163 }
00164 }