reachable_valid_pose_filter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <boost/bind.hpp>
41 #include <ros/console.h>
42 
44  const planning_scene::PlanningSceneConstPtr& scene,
45  const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
46  const constraint_samplers::ConstraintSamplerManagerPtr& constraints_sampler_manager)
47  : ManipulationStage("reachable & valid pose filter")
48  , planning_scene_(scene)
49  , collision_matrix_(collision_matrix)
50  , constraints_sampler_manager_(constraints_sampler_manager)
51 {
52 }
53 
54 namespace
55 {
56 bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
57  const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
58  const pick_place::ManipulationPlan* manipulation_plan, robot_state::RobotState* state,
59  const robot_model::JointModelGroup* group, const double* joint_group_variable_values)
60 {
62  req.verbose = verbose;
63  req.group_name = manipulation_plan->shared_data_->planning_group_->getName();
64 
65  state->setJointGroupPositions(group, joint_group_variable_values);
66 
67  if (manipulation_plan->approach_posture_.points.empty())
68  {
69  state->update();
71  planning_scene->checkCollision(req, res, *state, *collision_matrix);
72  if (res.collision)
73  return false;
74  }
75  else
76  // apply approach posture for the end effector (we always apply it here since it could be the case the sampler
77  // changes this posture)
78  for (std::size_t i = 0; i < manipulation_plan->approach_posture_.points.size(); ++i)
79  {
80  state->setVariablePositions(manipulation_plan->approach_posture_.joint_names,
81  manipulation_plan->approach_posture_.points[i].positions);
82  state->update();
84  planning_scene->checkCollision(req, res, *state, *collision_matrix);
85  if (res.collision)
86  return false;
87  }
88  return planning_scene->isStateFeasible(*state);
89 }
90 }
91 
93  robot_state::RobotState& token_state) const
94 {
95  tf::poseMsgToEigen(plan->goal_pose_.pose, plan->transformed_goal_pose_);
96  plan->transformed_goal_pose_ =
97  planning_scene_->getFrameTransform(token_state, plan->goal_pose_.header.frame_id) * plan->transformed_goal_pose_;
98  token_state.updateStateWithLinkAt(plan->shared_data_->ik_link_, plan->transformed_goal_pose_);
100  req.verbose = verbose_;
102  req.group_name = plan->shared_data_->end_effector_group_->getName();
103  planning_scene_->checkCollision(req, res, token_state, *collision_matrix_);
104  return res.collision == false;
105 }
106 
107 bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const
108 {
109  // initialize with scene state
110  robot_state::RobotStatePtr token_state(new robot_state::RobotState(planning_scene_->getCurrentState()));
111  if (isEndEffectorFree(plan, *token_state))
112  {
113  // update the goal pose message if anything has changed; this is because the name of the frame in the input goal
114  // pose
115  // can be that of objects in the collision world but most components are unaware of those transforms,
116  // so we convert to a frame that is certainly known
117  if (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id))
118  {
119  tf::poseEigenToMsg(plan->transformed_goal_pose_, plan->goal_pose_.pose);
120  plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame();
121  }
122 
123  // convert the pose we want to reach to a set of constraints
124  plan->goal_constraints_ =
125  kinematic_constraints::constructGoalConstraints(plan->shared_data_->ik_link_->getName(), plan->goal_pose_);
126 
127  const std::string& planning_group = plan->shared_data_->planning_group_->getName();
128 
129  // construct a sampler for the specified constraints; this can end up calling just IK, but it is more general
130  // and allows for robot-specific samplers, producing samples that also change the base position if needed, etc
131  plan->goal_sampler_ =
132  constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_);
133  if (plan->goal_sampler_)
134  {
135  plan->goal_sampler_->setGroupStateValidityCallback(boost::bind(
136  &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3));
137  plan->goal_sampler_->setVerbose(verbose_);
138  if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
139  {
140  plan->possible_goal_states_.push_back(token_state);
141  return true;
142  }
143  else if (verbose_)
144  ROS_INFO_NAMED("manipulation", "Sampler failed to produce a state");
145  }
146  else
147  ROS_ERROR_THROTTLE_NAMED(1, "manipulation", "No sampler was constructed");
148  }
149  plan->error_code_.val = moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION;
150  return false;
151 }
planning_scene::PlanningSceneConstPtr planning_scene_
#define ROS_INFO_NAMED(name,...)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
planning_group
bool verbose
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
trajectory_msgs::JointTrajectory approach_posture_
#define ROS_ERROR_THROTTLE_NAMED(rate, name,...)
bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose=false) const
bool isEndEffectorFree(const ManipulationPlanPtr &plan, robot_state::RobotState &token_state) const
ManipulationPlanSharedDataConstPtr shared_data_
ReachableAndValidPoseFilter(const planning_scene::PlanningSceneConstPtr &scene, const collision_detection::AllowedCollisionMatrixConstPtr &collision_matrix, const constraint_samplers::ConstraintSamplerManagerPtr &constraints_sampler_manager)
constraint_samplers::ConstraintSamplerManagerPtr constraints_sampler_manager_
collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_
virtual bool evaluate(const ManipulationPlanPtr &plan) const


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 18 2018 02:49:29