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_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 ROS_DEBUG("Trying to initialize solver %s",kinematics_solver_name.c_str());
00052 if(!kinematics_loader_.isClassAvailable(kinematics_solver_name))
00053 {
00054 ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str());
00055 return false;
00056 }
00057 ROS_DEBUG("Found solver %s",kinematics_solver_name.c_str());
00058
00059 try
00060 {
00061 kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
00062 }
00063 catch(pluginlib::PluginlibException& ex)
00064 {
00065 ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
00066 return false;
00067 }
00068 ROS_DEBUG("Loaded solver %s",kinematics_solver_name.c_str());
00069 if(!kinematics_solver_->initialize(group_name))
00070 {
00071 ROS_ERROR("Could not initialize kinematics solver for group %s",group_name.c_str());
00072 return false;
00073 }
00074 ROS_DEBUG("Initialized solver %s",kinematics_solver_name.c_str());
00075 scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_));
00076 seed_state_.joint_state.name = kinematics_solver_->getJointNames();
00077 seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00078
00079 solution_state_.joint_state.name = kinematics_solver_->getJointNames();
00080 solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
00081 if(!ompl_ros_interface::getRobotStateToOmplStateMapping(seed_state_,*scoped_state_,robot_state_to_ompl_state_mapping_))
00082 {
00083 ROS_ERROR("Could not get mapping between robot state and ompl state");
00084 return false;
00085 }
00086 if(!ompl_ros_interface::getOmplStateToRobotStateMapping(*scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_))
00087 {
00088 ROS_ERROR("Could not get mapping between ompl state and robot state");
00089 return false;
00090 }
00091 else
00092 {
00093 ROS_DEBUG("Real vector index: %d",ompl_state_to_robot_state_mapping_.real_vector_index);
00094 for(unsigned int i=0; i < ompl_state_to_robot_state_mapping_.real_vector_mapping.size(); i++)
00095 ROS_DEBUG("mapping: %d %d",i,ompl_state_to_robot_state_mapping_.real_vector_mapping[i]);
00096 }
00097 ROS_DEBUG("Initialized Ompl Ros IK Sampler");
00098 return true;
00099 }
00100
00101 bool OmplRosIKSampler::configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request,
00102 arm_navigation_msgs::GetMotionPlan::Response &response,
00103 const unsigned int &max_sample_count)
00104 {
00105 ik_poses_counter_ = 0;
00106 max_sample_count_ = max_sample_count;
00107 num_samples_ = 0;
00108 ik_poses_.clear();
00109 arm_navigation_msgs::Constraints goal_constraints = request.motion_plan_request.goal_constraints;
00110
00111 if(!collision_models_interface_->convertConstraintsGivenNewWorldTransform(*collision_models_interface_->getPlanningSceneState(),
00112 goal_constraints,
00113 kinematics_solver_->getBaseFrame())) {
00114 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00115 return false;
00116 }
00117
00118 if(!arm_navigation_msgs::constraintsToPoseStampedVector(goal_constraints, ik_poses_))
00119 {
00120 ROS_ERROR("Could not get poses from constraints");
00121 return false;
00122 }
00123 if(ik_poses_.empty())
00124 {
00125 ROS_WARN("Could not setup goals for inverse kinematics sampling");
00126 return false;
00127 }
00128 else
00129 ROS_DEBUG("Setup %d poses for IK sampler",(int)ik_poses_.size());
00130 for(unsigned int i=0; i < ik_poses_.size(); i++)
00131 {
00132 if(ik_poses_[i].header.frame_id != kinematics_solver_->getBaseFrame())
00133 {
00134 ROS_ERROR("Goals for inverse kinematic sampling in %s frame are not in kinematics frame: %s",
00135 ik_poses_[i].header.frame_id.c_str(),
00136 kinematics_solver_->getBaseFrame().c_str());
00137 return false;
00138 }
00139 }
00140 return true;
00141 }
00142
00143 bool OmplRosIKSampler::sampleGoal(const ompl::base::GoalLazySamples *gls, ompl::base::State *state)
00144 {
00145 arm_navigation_msgs::RobotState seed_state,solution_state;
00146 seed_state = seed_state_;
00147 solution_state = solution_state_;
00148 ompl::base::ScopedState<ompl::base::CompoundStateSpace> scoped_state(state_space_);
00149
00150 scoped_state.random();
00151
00152 ompl_ros_interface::omplStateToRobotState(scoped_state,
00153 ompl_state_to_robot_state_mapping_,
00154 seed_state);
00155 int error_code;
00156 if(kinematics_solver_->getPositionIK(ik_poses_[ik_poses_counter_].pose,
00157 seed_state.joint_state.position,
00158 solution_state.joint_state.position,
00159 error_code))
00160 {
00161
00162 ompl_ros_interface::robotStateToOmplState(solution_state,
00163 robot_state_to_ompl_state_mapping_,
00164 state);
00165 ik_poses_counter_++;
00166 ik_poses_counter_ = ik_poses_counter_%ik_poses_.size();
00167 return true;
00168 }
00169 return false;
00170 }
00171
00172 bool OmplRosIKSampler::sampleGoals(const ompl::base::GoalLazySamples *gls, ompl::base::State *state)
00173 {
00174 bool continue_sampling = sampleGoal(gls,state);
00175 if(continue_sampling)
00176 num_samples_++;
00177
00178
00179 if(num_samples_ > max_sample_count_ || gls->isAchieved())
00180 return false;
00181 else
00182 return true;
00183 }
00184
00185 }
