/
approach_and_translate_stage.cpp
361 lines (319 loc) · 17.9 KB
/
approach_and_translate_stage.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan */
#include <moveit/pick_place/pick_place.h>
#include <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
namespace pick_place
{
ApproachAndTranslateStage::ApproachAndTranslateStage(
const planning_scene::PlanningSceneConstPtr& scene,
const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix)
: ManipulationStage("approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix)
{
max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_;
max_fail_ = GetGlobalPickPlaceParams().max_fail_;
max_step_ = GetGlobalPickPlaceParams().max_step_;
jump_factor_ = GetGlobalPickPlaceParams().jump_factor_;
}
namespace
{
bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
const trajectory_msgs::JointTrajectory* grasp_posture, robot_state::RobotState* state,
const robot_state::JointModelGroup* group, const double* joint_group_variable_values)
{
state->setJointGroupPositions(group, joint_group_variable_values);
collision_detection::CollisionRequest req;
req.verbose = verbose;
req.group_name = group->getName();
if (!grasp_posture->joint_names.empty())
{
// apply the grasp posture for the end effector (we always apply it here since it could be the case the sampler
// changes this posture)
for (std::size_t i = 0; i < grasp_posture->points.size(); ++i)
{
state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions);
collision_detection::CollisionResult res;
planning_scene->checkCollision(req, res, *state, *collision_matrix);
if (res.collision)
return false;
}
}
else
{
collision_detection::CollisionResult res;
planning_scene->checkCollision(req, res, *state, *collision_matrix);
if (res.collision)
return false;
}
return planning_scene->isStateFeasible(*state);
}
bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const robot_state::RobotState& reference_state,
double min_distance, unsigned int attempts)
{
// initialize with scene state
robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state));
for (unsigned int j = 0; j < attempts; ++j)
{
double min_d = std::numeric_limits<double>::infinity();
// Samples given the constraints, populating the joint state group
if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
{
// Check if this new sampled state is closest we've found so far
for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i)
{
double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_);
if (d < min_d)
min_d = d;
}
if (min_d >= min_distance)
{
plan->possible_goal_states_.push_back(token_state);
return true;
}
}
}
return false;
}
// This function is called during trajectory execution, after the gripper is closed, to attach the currently gripped
// object
bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_data,
const trajectory_msgs::JointTrajectory& detach_posture,
const plan_execution::ExecutableMotionPlan* motion_plan)
{
if (shared_plan_data->diff_attached_object_.object.id.empty())
{
// the user did not provide an object to attach, so just return successfully
return true;
}
ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching "
"object to end effector)");
bool ok = false;
{
planning_scene_monitor::LockedPlanningSceneRW ps(motion_plan->planning_scene_monitor_);
moveit_msgs::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_;
// remember the configuration of the gripper before the grasp;
// this configuration will be set again when releasing the object
msg.detach_posture = detach_posture;
ok = ps->processAttachedCollisionObjectMsg(msg);
}
motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent(
(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)(
planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY +
planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE));
return ok;
}
// Add the close end effector trajectory to the overall plan (after the approach trajectory, before the retreat)
void addGripperTrajectory(const ManipulationPlanPtr& plan,
const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix,
const std::string& name)
{
// Check if a "closed" end effector configuration was specified
if (!plan->retreat_posture_.joint_names.empty())
{
robot_state::RobotStatePtr ee_closed_state(
new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
// If user has defined a time for it's gripper movement time, don't add the
// DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
if (!plan->retreat_posture_.points.empty() &&
plan->retreat_posture_.points.back().time_from_start > ros::Duration(0.0))
{
ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0);
}
else
{ // Do what was done before
ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
<< " seconds to the grasp closure time. Assign time_from_start to "
<< "your trajectory to avoid this.");
ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
}
plan_execution::ExecutableTrajectory et(ee_closed_traj, name);
// Add a callback to attach the object to the EE after closing the gripper
et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
et.allowed_collision_matrix_ = collision_matrix;
plan->trajectories_.push_back(et);
}
else
{
ROS_WARN_NAMED("manipulation", "No joint states of grasp postures have been defined in the pick place action.");
}
}
} // namespace
bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
{
const robot_model::JointModelGroup* jmg = plan->shared_data_->planning_group_;
// compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of
// that;
// this is the minimum distance between sampled goal states
const double min_distance = 0.01 * jmg->getMaximumExtent();
// convert approach direction and retreat direction to Eigen structures
Eigen::Vector3d approach_direction, retreat_direction;
tf2::fromMsg(plan->approach_.direction.vector, approach_direction);
tf2::fromMsg(plan->retreat_.direction.vector, retreat_direction);
// if translation vectors are specified in the frame of the ik link name, then we assume the frame is local;
// otherwise, the frame is global
bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(
plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id,
plan->shared_data_->ik_link_->getName());
// transform the input vectors in accordance to frame specified in the header;
if (approach_direction_is_global_frame)
approach_direction =
planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction;
if (retreat_direction_is_global_frame)
retreat_direction =
planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction;
// state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
robot_state::GroupStateValidityCallbackFn approach_valid_callback =
boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
&plan->approach_posture_, _1, _2, _3);
plan->goal_sampler_->setVerbose(verbose_);
std::size_t attempted_possible_goal_states = 0;
do // continously sample possible goal states
{
for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_;
++i, ++attempted_possible_goal_states)
{
// if we are trying to get as close as possible to the goal (maximum one meter)
if (plan->shared_data_->minimize_object_distance_)
{
static const double MAX_CLOSE_UP_DIST = 1.0;
robot_state::RobotStatePtr close_up_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
std::vector<robot_state::RobotStatePtr> close_up_states;
double d_close_up = close_up_state->computeCartesianPath(plan->shared_data_->planning_group_, close_up_states,
plan->shared_data_->ik_link_, approach_direction,
approach_direction_is_global_frame, MAX_CLOSE_UP_DIST,
max_step_, jump_factor_, approach_valid_callback);
// if progress towards the object was made, update the desired goal state
if (d_close_up > 0.0 && close_up_states.size() > 1)
*plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2];
}
// try to compute a straight line path that arrives at the goal using the specified approach direction
robot_state::RobotStatePtr first_approach_state(new robot_state::RobotState(*plan->possible_goal_states_[i]));
std::vector<robot_state::RobotStatePtr> approach_states;
double d_approach = first_approach_state->computeCartesianPath(
plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction,
approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, jump_factor_,
approach_valid_callback);
// if we were able to follow the approach direction for sufficient length, try to compute a retreat direction
if (d_approach > plan->approach_.min_distance && !signal_stop_)
{
if (plan->retreat_.desired_distance > 0.0)
{
// construct a planning scene that is just a diff on top of our actual planning scene
planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff();
// assume the current state of the diff world is the one we plan to reach
planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i];
// apply the difference message to this world that virtually attaches the object we are manipulating
planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_);
// state validity checking during the retreat after the grasp must ensure the gripper posture is that of the
// actual grasp
robot_state::GroupStateValidityCallbackFn retreat_valid_callback =
boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
&plan->retreat_posture_, _1, _2, _3);
// try to compute a straight line path that moves from the goal in a desired direction
robot_state::RobotStatePtr last_retreat_state(
new robot_state::RobotState(planning_scene_after_approach->getCurrentState()));
std::vector<robot_state::RobotStatePtr> retreat_states;
double d_retreat = last_retreat_state->computeCartesianPath(
plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction,
retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_, jump_factor_,
retreat_valid_callback);
// if sufficient progress was made in the desired direction, we have a goal state that we can consider for
// future stages
if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
{
// Create approach trajectory
std::reverse(approach_states.begin(), approach_states.end());
robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
for (std::size_t k = 0; k < approach_states.size(); ++k)
approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
// Create retreat trajectory
robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
for (std::size_t k = 0; k < retreat_states.size(); ++k)
retreat_traj->addSuffixWayPoint(retreat_states[k], 0.0);
// Add timestamps to approach|retreat trajectories
time_param_.computeTimeStamps(*approach_traj);
time_param_.computeTimeStamps(*retreat_traj);
// Convert approach trajectory to an executable trajectory
plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
et_approach.allowed_collision_matrix_ = collision_matrix_;
plan->trajectories_.push_back(et_approach);
// Add gripper close trajectory
addGripperTrajectory(plan, collision_matrix_, "grasp");
// Convert retreat trajectory to an executable trajectory
plan_execution::ExecutableTrajectory et_retreat(retreat_traj, "retreat");
et_retreat.allowed_collision_matrix_ = collision_matrix_;
plan->trajectories_.push_back(et_retreat);
plan->approach_state_ = approach_states.front();
return true;
}
}
else // No retreat was specified, so package up approach and grip trajectories.
{
// Reset the approach_state_ RobotStatePtr so that we can retry computing the cartesian path
plan->approach_state_.swap(first_approach_state);
// Create approach trajectory
std::reverse(approach_states.begin(), approach_states.end());
robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
for (std::size_t k = 0; k < approach_states.size(); ++k)
approach_traj->addSuffixWayPoint(approach_states[k], 0.0);
// Add timestamps to approach trajectories
time_param_.computeTimeStamps(*approach_traj);
// Convert approach trajectory to an executable trajectory
plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach");
et_approach.allowed_collision_matrix_ = collision_matrix_;
plan->trajectories_.push_back(et_approach);
// Add gripper close trajectory
addGripperTrajectory(plan, collision_matrix_, "grasp");
plan->approach_state_ = approach_states.front();
return true;
}
}
}
} while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ &&
samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));
plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
} // namespace pick_place