approach_and_translate_stage.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 
41 #include <ros/console.h>
42 
43 namespace pick_place
44 {
46  const planning_scene::PlanningSceneConstPtr& scene,
47  const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix)
48  : ManipulationStage("approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix)
49 {
54 }
55 
56 namespace
57 {
58 bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
59  const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
60  const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state,
61  const robot_state::JointModelGroup* group, const double* joint_group_variable_values)
62 {
63  state->setJointGroupPositions(group, joint_group_variable_values);
64 
66  req.verbose = verbose;
67  req.group_name = group->getName();
68 
69  if (grasp_posture->joint_names.size() > 0)
70  {
71  // apply the grasp posture for the end effector (we always apply it here since it could be the case the sampler
72  // changes this posture)
73  for (std::size_t i = 0; i < grasp_posture->points.size(); ++i)
74  {
75  state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions);
77  planning_scene->checkCollision(req, res, *state, *collision_matrix);
78  if (res.collision)
79  return false;
80  }
81  }
82  else
83  {
85  planning_scene->checkCollision(req, res, *state, *collision_matrix);
86  if (res.collision)
87  return false;
88  }
89  return planning_scene->isStateFeasible(*state);
90 }
91 
92 bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const robot_state::RobotState& reference_state,
93  double min_distance, unsigned int attempts)
94 {
95  // initialize with scene state
96  robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state));
97  for (unsigned int j = 0; j < attempts; ++j)
98  {
99  double min_d = std::numeric_limits<double>::infinity();
100 
101  // Samples given the constraints, populating the joint state group
102  if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
103  {
104  // Check if this new sampled state is closest we've found so far
105  for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i)
106  {
107  double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_);
108  if (d < min_d)
109  min_d = d;
110  }
111  if (min_d >= min_distance)
112  {
113  plan->possible_goal_states_.push_back(token_state);
114  return true;
115  }
116  }
117  }
118  return false;
119 }
120 
121 // This function is called during trajectory execution, after the gripper is closed, to attach the currently gripped
122 // object
123 bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_data,
124  const trajectory_msgs::JointTrajectory& detach_posture,
125  const plan_execution::ExecutableMotionPlan* motion_plan)
126 {
127  if (shared_plan_data->diff_attached_object_.object.id.empty())
128  {
129  // the user did not provide an object to attach, so just return successfully
130  return true;
131  }
132 
133  ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching "
134  "object to end effector)");
135  bool ok = false;
136  {
138  moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
139  // remember the configuration of the gripper before the grasp;
140  // this configuration will be set again when releasing the object
141  msg.detach_posture = detach_posture;
142  ok = ps->processAttachedCollisionObjectMsg(msg);
143  }
144  motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent(
148  return ok;
149 }
150 
151 // Add the close end effector trajectory to the overall plan (after the approach trajectory, before the retreat)
152 void addGripperTrajectory(const ManipulationPlanPtr& plan,
153  const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
154  const std::string& name)
155 {
156  // Check if a "closed" end effector configuration was specified
157  if (!plan->retreat_posture_.joint_names.empty())
158  {
159  robot_state::RobotStatePtr ee_closed_state(
160  new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
161 
162  robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
163  ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
164  ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
165  // If user has defined a time for it's gripper movement time, don't add the
166  // DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
167  if (plan->retreat_posture_.points.size() > 0 &&
168  plan->retreat_posture_.points.back().time_from_start > ros::Duration(0.0))
169  {
170  ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
171  }
172  else
173  { // Do what was done before
175  << " seconds to the grasp closure time. Assign time_from_start to "
176  << "your trajectory to avoid this.");
177  ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
178  }
179 
180  plan_execution::ExecutableTrajectory et(ee_closed_traj, name);
181 
182  // Add a callback to attach the object to the EE after closing the gripper
183  et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
184  et.allowed_collision_matrix_ = collision_matrix;
185  plan->trajectories_.push_back(et);
186  }
187  else
188  {
189  ROS_WARN_NAMED("manipulation", "No joint states of grasp postures have been defined in the pick place action.");
190  }
191 }
192 
193 } // annonymous namespace
194 
195 bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
196 {
197  const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_;
198  // compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of
199  // that;
200  // this is the minimum distance between sampled goal states
201  const double min_distance = 0.01 * jmg->getMaximumExtent();
202 
203  // convert approach direction and retreat direction to Eigen structures
204  Eigen::Vector3d approach_direction, retreat_direction;
205  tf::vectorMsgToEigen(plan->approach_.direction.vector, approach_direction);
206  tf::vectorMsgToEigen(plan->retreat_.direction.vector, retreat_direction);
207 
208  // if translation vectors are specified in the frame of the ik link name, then we assume the frame is local;
209  // otherwise, the frame is global
210  bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(
211  plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
212  bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id,
213  plan->shared_data_->ik_link_->getName());
214 
215  // transform the input vectors in accordance to frame specified in the header;
216  if (approach_direction_is_global_frame)
217  approach_direction =
218  planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction;
219  if (retreat_direction_is_global_frame)
220  retreat_direction =
221  planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction;
222 
223  // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
224  robot_state::GroupStateValidityCallbackFn approach_validCallback =
225  boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
226  &plan->approach_posture_, _1, _2, _3);
227  plan->goal_sampler_->setVerbose(verbose_);
228  std::size_t attempted_possible_goal_states = 0;
229  do // continously sample possible goal states
230  {
231  for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_;
232  ++i, ++attempted_possible_goal_states)
233  {
234  // if we are trying to get as close as possible to the goal (maximum one meter)
235  if (plan->shared_data_->minimize_object_distance_)
236  {
237  static const double MAX_CLOSE_UP_DIST = 1.0;
238  robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
239  std::vector<robot_state::RobotStatePtr> close_up_states;
240  double d_close_up = close_up_state->computeCartesianPath(
241  plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction,
242  approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_, approach_validCallback);
243  // if progress towards the object was made, update the desired goal state
244  if (d_close_up > 0.0 && close_up_states.size() > 1)
245  *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2];
246  }
247 
248  // try to compute a straight line path that arrives at the goal using the specified approach direction
249  robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
250 
251  std::vector<robot_state::RobotStatePtr> approach_states;
252  double d_approach = first_approach_state->computeCartesianPath(
253  plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction,
254  approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, jump_factor_,
255  approach_validCallback);
256 
257  // if we were able to follow the approach direction for sufficient length, try to compute a retreat direction
258  if (d_approach > plan->approach_.min_distance && !signal_stop_)
259  {
260  if (plan->retreat_.desired_distance > 0.0)
261  {
262  // construct a planning scene that is just a diff on top of our actual planning scene
263  planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff();
264 
265  // assume the current state of the diff world is the one we plan to reach
266  planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
267 
268  // apply the difference message to this world that virtually attaches the object we are manipulating
269  planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
270 
271  // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the
272  // actual grasp
273  robot_state::GroupStateValidityCallbackFn retreat_validCallback =
274  boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
275  &plan->retreat_posture_, _1, _2, _3);
276 
277  // try to compute a straight line path that moves from the goal in a desired direction
278  robot_state::RobotStatePtr last_retreat_state(
279  new robot_state::RobotState(planning_scene_after_approach->getCurrentState()));
280  std::vector<robot_state::RobotStatePtr> retreat_states;
281  double d_retreat = last_retreat_state->computeCartesianPath(
282  plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction,
283  retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_, jump_factor_,
284  retreat_validCallback);
285 
286  // if sufficient progress was made in the desired direction, we have a goal state that we can consider for
287  // future stages
288  if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
289  {
290  // Create approach trajectory
291  std::reverse(approach_states.begin(), approach_states.end());
292  robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
293  planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
294  for (std::size_t k = 0; k < approach_states.size(); ++k)
295  approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
296 
297  // Create retreat trajectory
298  robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
299  planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
300  for (std::size_t k = 0; k < retreat_states.size(); ++k)
301  retreat_traj->addSuffixWayPoint(retreat_states[k], 0.0);
302 
303  // Add timestamps to approach|retreat trajectories
304  time_param_.computeTimeStamps(*approach_traj);
305  time_param_.computeTimeStamps(*retreat_traj);
306 
307  // Convert approach trajectory to an executable trajectory
308  plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
310  plan->trajectories_.push_back(et_approach);
311 
312  // Add gripper close trajectory
313  addGripperTrajectory(plan, collision_matrix_, "grasp");
314 
315  // Convert retreat trajectory to an executable trajectory
316  plan_execution::ExecutableTrajectory et_retreat(retreat_traj, "retreat");
318  plan->trajectories_.push_back(et_retreat);
319 
320  plan->approach_state_ = approach_states.front();
321  return true;
322  }
323  }
324  else // No retreat was specified, so package up approach and grip trajectories.
325  {
326  // Reset the approach_state_ RobotStatePtr so that we can retry computing the cartesian path
327  plan->approach_state_.swap(first_approach_state);
328 
329  // Create approach trajectory
330  std::reverse(approach_states.begin(), approach_states.end());
331  robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
332  planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
333  for (std::size_t k = 0; k < approach_states.size(); ++k)
334  approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
335 
336  // Add timestamps to approach trajectories
337  time_param_.computeTimeStamps(*approach_traj);
338 
339  // Convert approach trajectory to an executable trajectory
340  plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
342  plan->trajectories_.push_back(et_approach);
343 
344  // Add gripper close trajectory
345  addGripperTrajectory(plan, collision_matrix_, "grasp");
346 
347  plan->approach_state_ = approach_states.front();
348 
349  return true;
350  }
351  }
352  }
353  } while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ &&
354  samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));
355 
356  plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
357 
358  return false;
359 }
360 }
d
collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix_
#define ROS_WARN_NAMED(name,...)
collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_
trajectory_processing::IterativeParabolicTimeParameterization time_param_
const PickPlaceParams & GetGlobalPickPlaceParams()
ApproachAndTranslateStage(const planning_scene::PlanningSceneConstPtr &scene, const collision_detection::AllowedCollisionMatrixConstPtr &collision_matrix)
virtual bool evaluate(const ManipulationPlanPtr &plan) const
#define ROS_DEBUG_NAMED(name,...)
bool verbose
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose=false) const
#define ROS_INFO_STREAM(args)
planning_scene::PlanningSceneConstPtr planning_scene_
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
boost::function< bool(const ExecutableMotionPlan *)> effect_on_success_
static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
Definition: pick_place.h:116


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 15 2018 03:47:02