46 const planning_scene::PlanningSceneConstPtr& scene,
47 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix)
48 :
ManipulationStage(
"approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix)
60 const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state,
61 const robot_state::JointModelGroup* group,
const double* joint_group_variable_values)
63 state->setJointGroupPositions(group, joint_group_variable_values);
69 if (grasp_posture->joint_names.size() > 0)
73 for (std::size_t i = 0; i < grasp_posture->points.size(); ++i)
75 state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions);
77 planning_scene->
checkCollision(req, res, *state, *collision_matrix);
85 planning_scene->
checkCollision(req, res, *state, *collision_matrix);
92 bool samplePossibleGoalStates(
const ManipulationPlanPtr& plan,
const robot_state::RobotState& reference_state,
93 double min_distance,
unsigned int attempts)
96 robot_state::RobotStatePtr token_state(
new robot_state::RobotState(reference_state));
97 for (
unsigned int j = 0; j < attempts; ++j)
99 double min_d = std::numeric_limits<double>::infinity();
102 if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
105 for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i)
107 double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_);
111 if (min_d >= min_distance)
113 plan->possible_goal_states_.push_back(token_state);
123 bool executeAttachObject(
const ManipulationPlanSharedDataConstPtr& shared_plan_data,
124 const trajectory_msgs::JointTrajectory& detach_posture,
127 if (shared_plan_data->diff_attached_object_.object.id.empty())
133 ROS_DEBUG_NAMED(
"manipulation",
"Applying attached object diff to maintained planning scene (attaching/detaching " 134 "object to end effector)");
138 moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
141 msg.detach_posture = detach_posture;
142 ok = ps->processAttachedCollisionObjectMsg(msg);
152 void addGripperTrajectory(
const ManipulationPlanPtr& plan,
153 const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
154 const std::string& name)
157 if (!plan->retreat_posture_.joint_names.empty())
159 robot_state::RobotStatePtr ee_closed_state(
160 new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
163 ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
164 ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
167 if (plan->retreat_posture_.points.size() > 0 &&
168 plan->retreat_posture_.points.back().time_from_start >
ros::Duration(0.0))
170 ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
175 <<
" seconds to the grasp closure time. Assign time_from_start to " 176 <<
"your trajectory to avoid this.");
183 et.
effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
185 plan->trajectories_.push_back(et);
189 ROS_WARN_NAMED(
"manipulation",
"No joint states of grasp postures have been defined in the pick place action.");
197 const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_;
201 const double min_distance = 0.01 * jmg->getMaximumExtent();
204 Eigen::Vector3d approach_direction, retreat_direction;
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());
216 if (approach_direction_is_global_frame)
218 planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction;
219 if (retreat_direction_is_global_frame)
221 planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction;
224 robot_state::GroupStateValidityCallbackFn approach_validCallback =
226 &plan->approach_posture_, _1, _2, _3);
227 plan->goal_sampler_->setVerbose(
verbose_);
228 std::size_t attempted_possible_goal_states = 0;
231 for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !
signal_stop_;
232 ++i, ++attempted_possible_goal_states)
235 if (plan->shared_data_->minimize_object_distance_)
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);
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];
249 robot_state::RobotStatePtr first_approach_state(
new robot_state::RobotState(*plan->possible_goal_states_[i]));
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,
255 approach_validCallback);
258 if (d_approach > plan->approach_.min_distance && !
signal_stop_)
260 if (plan->retreat_.desired_distance > 0.0)
263 planning_scene::PlanningScenePtr planning_scene_after_approach =
planning_scene_->diff();
266 planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
269 planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
273 robot_state::GroupStateValidityCallbackFn retreat_validCallback =
275 &plan->retreat_posture_, _1, _2, _3);
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,
284 retreat_validCallback);
288 if (d_retreat > plan->retreat_.min_distance && !
signal_stop_)
291 std::reverse(approach_states.begin(), approach_states.end());
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);
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);
310 plan->trajectories_.push_back(et_approach);
318 plan->trajectories_.push_back(et_retreat);
320 plan->approach_state_ = approach_states.front();
327 plan->approach_state_.swap(first_approach_state);
330 std::reverse(approach_states.begin(), approach_states.end());
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);
342 plan->trajectories_.push_back(et_approach);
347 plan->approach_state_ = approach_states.front();
356 plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
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,...)
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
unsigned int max_goal_count_
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
unsigned int max_goal_count_
boost::function< bool(const ExecutableMotionPlan *)> effect_on_success_
static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION