Go to the documentation of this file.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
00035
00036
00037 #include <moveit/pick_place/reachable_valid_pose_filter.h>
00038 #include <moveit/kinematic_constraints/utils.h>
00039 #include <eigen_conversions/eigen_msg.h>
00040 #include <boost/bind.hpp>
00041 #include <ros/console.h>
00042
00043 pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter(
00044 const planning_scene::PlanningSceneConstPtr& scene,
00045 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
00046 const constraint_samplers::ConstraintSamplerManagerPtr& constraints_sampler_manager)
00047 : ManipulationStage("reachable & valid pose filter")
00048 , planning_scene_(scene)
00049 , collision_matrix_(collision_matrix)
00050 , constraints_sampler_manager_(constraints_sampler_manager)
00051 {
00052 }
00053
00054 namespace
00055 {
00056 bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
00057 const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
00058 const pick_place::ManipulationPlan* manipulation_plan, robot_state::RobotState* state,
00059 const robot_model::JointModelGroup* group, const double* joint_group_variable_values)
00060 {
00061 collision_detection::CollisionRequest req;
00062 req.verbose = verbose;
00063 req.group_name = manipulation_plan->shared_data_->planning_group_->getName();
00064
00065 state->setJointGroupPositions(group, joint_group_variable_values);
00066
00067 if (manipulation_plan->approach_posture_.points.empty())
00068 {
00069 state->update();
00070 collision_detection::CollisionResult res;
00071 planning_scene->checkCollision(req, res, *state, *collision_matrix);
00072 if (res.collision)
00073 return false;
00074 }
00075 else
00076
00077
00078 for (std::size_t i = 0; i < manipulation_plan->approach_posture_.points.size(); ++i)
00079 {
00080 state->setVariablePositions(manipulation_plan->approach_posture_.joint_names,
00081 manipulation_plan->approach_posture_.points[i].positions);
00082 state->update();
00083 collision_detection::CollisionResult res;
00084 planning_scene->checkCollision(req, res, *state, *collision_matrix);
00085 if (res.collision)
00086 return false;
00087 }
00088 return planning_scene->isStateFeasible(*state);
00089 }
00090 }
00091
00092 bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const ManipulationPlanPtr& plan,
00093 robot_state::RobotState& token_state) const
00094 {
00095 tf::poseMsgToEigen(plan->goal_pose_.pose, plan->transformed_goal_pose_);
00096 plan->transformed_goal_pose_ =
00097 planning_scene_->getFrameTransform(token_state, plan->goal_pose_.header.frame_id) * plan->transformed_goal_pose_;
00098 token_state.updateStateWithLinkAt(plan->shared_data_->ik_link_, plan->transformed_goal_pose_);
00099 collision_detection::CollisionRequest req;
00100 req.verbose = verbose_;
00101 collision_detection::CollisionResult res;
00102 req.group_name = plan->shared_data_->end_effector_group_->getName();
00103 planning_scene_->checkCollision(req, res, token_state, *collision_matrix_);
00104 return res.collision == false;
00105 }
00106
00107 bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const
00108 {
00109
00110 robot_state::RobotStatePtr token_state(new robot_state::RobotState(planning_scene_->getCurrentState()));
00111 if (isEndEffectorFree(plan, *token_state))
00112 {
00113
00114
00115
00116
00117 if (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id))
00118 {
00119 tf::poseEigenToMsg(plan->transformed_goal_pose_, plan->goal_pose_.pose);
00120 plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame();
00121 }
00122
00123
00124 plan->goal_constraints_ =
00125 kinematic_constraints::constructGoalConstraints(plan->shared_data_->ik_link_->getName(), plan->goal_pose_);
00126
00127 const std::string& planning_group = plan->shared_data_->planning_group_->getName();
00128
00129
00130
00131 plan->goal_sampler_ =
00132 constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_);
00133 if (plan->goal_sampler_)
00134 {
00135 plan->goal_sampler_->setGroupStateValidityCallback(boost::bind(
00136 &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3));
00137 plan->goal_sampler_->setVerbose(verbose_);
00138 if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
00139 {
00140 plan->possible_goal_states_.push_back(token_state);
00141 return true;
00142 }
00143 else if (verbose_)
00144 ROS_INFO_NAMED("manipulation", "Sampler failed to produce a state");
00145 }
00146 else
00147 ROS_ERROR_THROTTLE_NAMED(1, "manipulation", "No sampler was constructed");
00148 }
00149 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION;
00150 return false;
00151 }