Skip to content

Commit 77347f4

Browse files
rhaschkerhaschke
committed
Repeated pick-and-place
1 parent 5120ca5 commit 77347f4

File tree

2 files changed

+198
-93
lines changed

2 files changed

+198
-93
lines changed

‎demo/include/moveit_task_constructor_demo/pick_place_task.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,10 @@ class PickPlaceTask
8080

8181
private:
8282
void loadParameters();
83+
std::unique_ptr<SerialContainer> container(moveit::task_constructor::Task& t,
84+
const solvers::PlannerInterfacePtr& sampling_planner,
85+
const solvers::PlannerInterfacePtr& cartesian_planner,
86+
const geometry_msgs::PoseStamped& place_pose);
8387

8488
static constexpr char LOGNAME[]{ "pick_place_task" };
8589

@@ -117,5 +121,8 @@ class PickPlaceTask
117121
// Place metrics
118122
geometry_msgs::Pose place_pose_;
119123
double place_surface_offset_;
124+
125+
std::mutex stats_mutex_;
126+
std::vector<std::chrono::steady_clock::time_point> callback_times_;
120127
};
121128
} // namespace moveit_task_constructor_demo

‎demo/src/pick_place_task.cpp‎

Lines changed: 191 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,11 @@
3535
*/
3636

3737
#include <moveit_task_constructor_demo/pick_place_task.h>
38+
#include <moveit/task_constructor/stages/noop.h>
3839
#include <rosparam_shortcuts/rosparam_shortcuts.h>
3940

41+
#define PICK_PLACE_REPETITIONS 4
42+
4043
namespace moveit_task_constructor_demo {
4144

4245
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
@@ -100,103 +103,33 @@ void setupDemoScene(ros::NodeHandle& pnh) {
100103
spawnObject(psi, createObject(pnh));
101104
}
102105

103-
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
104-
: pnh_(pnh), task_name_(task_name) {
105-
loadParameters();
106-
}
107-
108-
void PickPlaceTask::loadParameters() {
109-
/****************************************************
110-
* *
111-
* Load Parameters *
112-
* *
113-
***************************************************/
114-
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
115-
116-
// Planning group properties
117-
size_t errors = 0;
118-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
119-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
120-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
121-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
122-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
123-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
124-
125-
// Predefined pose targets
126-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
127-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
128-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
129-
130-
// Target object
131-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
132-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
133-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
134-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
135-
support_surfaces_ = { surface_link_ };
136-
137-
// Pick/Place metrics
138-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_);
139-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_);
140-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_);
141-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_);
142-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
143-
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
144-
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
145-
}
146-
147-
// Initialize the task pipeline, defining individual movement stages
148-
bool PickPlaceTask::init() {
149-
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
106+
std::unique_ptr<SerialContainer> PickPlaceTask::container(moveit::task_constructor::Task& t,
107+
const solvers::PlannerInterfacePtr& sampling_planner,
108+
const solvers::PlannerInterfacePtr& cartesian_planner,
109+
const geometry_msgs::PoseStamped& place_pose) {
150110
const std::string object = object_name_;
151-
152-
// Reset ROS introspection before constructing the new object
153-
// TODO(v4hn): global storage for Introspection services to enable one-liner
154-
task_.reset();
155-
task_.reset(new moveit::task_constructor::Task());
156-
157-
// Individual movement stages are collected within the Task object
158-
Task& t = *task_;
159-
t.stages()->setName(task_name_);
160-
t.loadRobotModel();
161-
162-
/* Create planners used in various stages. Various options are available,
163-
namely Cartesian, MoveIt pipeline, and joint interpolation. */
164-
// Sampling planner
165-
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
166-
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
167-
168-
// Cartesian planner
169-
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
170-
cartesian_planner->setMaxVelocityScalingFactor(1.0);
171-
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
172-
cartesian_planner->setStepSize(.01);
173-
174-
// Set task properties
175-
t.setProperty("group", arm_group_name_);
176-
t.setProperty("eef", eef_name_);
177-
t.setProperty("hand", hand_group_name_);
178-
t.setProperty("hand_grasping_frame", hand_frame_);
179-
t.setProperty("ik_frame", hand_frame_);
111+
auto serial_container = std::make_unique<SerialContainer>("pick/place");
112+
t.properties().exposeTo(serial_container->properties(), { "eef", "hand", "group", "ik_frame" });
113+
serial_container->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
180114

181115
/****************************************************
182116
* *
183117
* Current State *
184118
* *
185119
***************************************************/
186120
{
187-
auto current_state = std::make_unique<stages::CurrentState>("current state");
121+
auto noop = std::make_unique<stages::NoOp>("NoOp state");
188122

189123
// Verify that object is not attached
190-
auto applicability_filter =
191-
std::make_unique<stages::PredicateFilter>("applicability test", std::move(current_state));
124+
auto applicability_filter = std::make_unique<stages::PredicateFilter>("applicability test", std::move(noop));
192125
applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
193126
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
194127
comment = "object with id '" + object + "' is already attached and cannot be picked";
195128
return false;
196129
}
197130
return true;
198131
});
199-
t.add(std::move(applicability_filter));
132+
serial_container->insert(std::move(applicability_filter));
200133
}
201134

202135
/****************************************************
@@ -210,7 +143,7 @@ bool PickPlaceTask::init() {
210143
stage->setGroup(hand_group_name_);
211144
stage->setGoal(hand_open_pose_);
212145
initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator
213-
t.add(std::move(stage));
146+
serial_container->insert(std::move(stage));
214147
}
215148

216149
/****************************************************
@@ -225,7 +158,7 @@ bool PickPlaceTask::init() {
225158
auto stage = std::make_unique<stages::Connect>("move to pick", planners);
226159
stage->setTimeout(5.0);
227160
stage->properties().configureInitFrom(Stage::PARENT);
228-
t.add(std::move(stage));
161+
serial_container->insert(std::move(stage));
229162
}
230163

231164
/****************************************************
@@ -353,7 +286,7 @@ bool PickPlaceTask::init() {
353286
pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator
354287

355288
// Add grasp container to task
356-
t.add(std::move(grasp));
289+
serial_container->insert(std::move(grasp));
357290
}
358291

359292
/******************************************************
@@ -367,7 +300,7 @@ bool PickPlaceTask::init() {
367300
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
368301
stage->setTimeout(5.0);
369302
stage->properties().configureInitFrom(Stage::PARENT);
370-
t.add(std::move(stage));
303+
serial_container->insert(std::move(stage));
371304
}
372305

373306
/******************************************************
@@ -410,11 +343,7 @@ bool PickPlaceTask::init() {
410343
stage->setObject(object);
411344

412345
// Set target pose
413-
geometry_msgs::PoseStamped p;
414-
p.header.frame_id = object_reference_frame_;
415-
p.pose = place_pose_;
416-
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
417-
stage->setPose(p);
346+
stage->setPose(place_pose);
418347
stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions
419348

420349
// Compute IK
@@ -471,7 +400,7 @@ bool PickPlaceTask::init() {
471400
}
472401

473402
// Add place container to task
474-
t.add(std::move(place));
403+
serial_container->insert(std::move(place));
475404
}
476405

477406
/******************************************************
@@ -484,12 +413,139 @@ bool PickPlaceTask::init() {
484413
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
485414
stage->setGoal(arm_home_pose_);
486415
stage->restrictDirection(stages::MoveTo::FORWARD);
487-
t.add(std::move(stage));
416+
serial_container->insert(std::move(stage));
417+
}
418+
419+
return serial_container;
420+
}
421+
422+
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
423+
: pnh_(pnh), task_name_(task_name) {
424+
loadParameters();
425+
}
426+
427+
void PickPlaceTask::loadParameters() {
428+
/****************************************************
429+
* *
430+
* Load Parameters *
431+
* *
432+
***************************************************/
433+
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
434+
435+
// Planning group properties
436+
size_t errors = 0;
437+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
438+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
439+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
440+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
441+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
442+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
443+
444+
// Predefined pose targets
445+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
446+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
447+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
448+
449+
// Target object
450+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
451+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
452+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
453+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
454+
support_surfaces_ = { surface_link_ };
455+
456+
// Pick/Place metrics
457+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_);
458+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_);
459+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_);
460+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_);
461+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
462+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
463+
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
464+
}
465+
466+
// Initialize the task pipeline, defining individual movement stages
467+
bool PickPlaceTask::init() {
468+
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
469+
470+
// Reset ROS introspection before constructing the new object
471+
// TODO(v4hn): global storage for Introspection services to enable one-liner
472+
task_.reset();
473+
task_.reset(new moveit::task_constructor::Task());
474+
475+
// Individual movement stages are collected within the Task object
476+
Task& t = *task_;
477+
t.stages()->setName(task_name_);
478+
t.loadRobotModel();
479+
480+
t.setPruning(true);
481+
482+
/* Create planners used in various stages. Various options are available,
483+
namely Cartesian, MoveIt pipeline, and joint interpolation. */
484+
// Sampling planner
485+
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
486+
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
487+
488+
// Cartesian planner
489+
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
490+
cartesian_planner->setMaxVelocityScalingFactor(1.0);
491+
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
492+
cartesian_planner->setStepSize(.01);
493+
494+
// Set task properties
495+
t.setProperty("group", arm_group_name_);
496+
t.setProperty("eef", eef_name_);
497+
t.setProperty("hand", hand_group_name_);
498+
t.setProperty("hand_grasping_frame", hand_frame_);
499+
t.setProperty("ik_frame", hand_frame_);
500+
501+
/****************************************************
502+
* *
503+
* Current State *
504+
* *
505+
***************************************************/
506+
{
507+
auto current_state = std::make_unique<stages::CurrentState>("current state");
508+
t.add(std::move(current_state));
509+
}
510+
511+
double delta_angle = 360 / PICK_PLACE_REPETITIONS;
512+
513+
// Repeat pick and place N times, placing the object around world Z axis doing a complete circle.
514+
for (int i = 0; i < PICK_PLACE_REPETITIONS; ++i) {
515+
geometry_msgs::PoseStamped place_pose;
516+
517+
place_pose.header.frame_id = world_frame_;
518+
place_pose.pose = place_pose_;
519+
place_pose.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
520+
521+
// Rotate the XY position around world Z by delta_angle
522+
double angle = (i * delta_angle) * M_PI / 180.0; // radians
523+
double c = std::cos(angle);
524+
double s = std::sin(angle);
525+
double x = place_pose.pose.position.x;
526+
double y = place_pose.pose.position.y;
527+
place_pose.pose.position.x = c * x - s * y;
528+
place_pose.pose.position.y = s * x + c * y;
529+
530+
t.add(std::move(container(t, sampling_planner, cartesian_planner, place_pose)));
488531
}
489532

490-
// prepare Task structure for planning
491533
try {
492534
t.init();
535+
536+
// attach solution callbacks to all stages to collect timestamps
537+
t.stages()->traverseRecursively([this](const Stage& stage, unsigned int) {
538+
Stage& s = const_cast<Stage&>(stage);
539+
std::string name = s.name();
540+
541+
s.addSolutionCallback([this, name](const SolutionBase& sol) {
542+
(void)sol;
543+
auto now = std::chrono::steady_clock::now();
544+
std::lock_guard<std::mutex> lock(this->stats_mutex_);
545+
this->callback_times_.push_back(now);
546+
});
547+
return true;
548+
});
493549
} catch (InitStageException& e) {
494550
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
495551
return false;
@@ -502,7 +558,49 @@ bool PickPlaceTask::plan() {
502558
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
503559
int max_solutions = pnh_.param<int>("max_solutions", 10);
504560

505-
return static_cast<bool>(task_->plan(max_solutions));
561+
{
562+
std::lock_guard<std::mutex> lock(stats_mutex_);
563+
callback_times_.clear();
564+
}
565+
auto start_time = std::chrono::steady_clock::now();
566+
bool ok = static_cast<bool>(task_->plan(max_solutions));
567+
auto end_time = std::chrono::steady_clock::now();
568+
569+
ROS_INFO_STREAM_NAMED(LOGNAME, "Task: \n" << *task_);
570+
571+
std::vector<std::chrono::steady_clock::time_point> times_copy;
572+
{
573+
std::lock_guard<std::mutex> lock(stats_mutex_);
574+
times_copy = callback_times_;
575+
}
576+
577+
if (!times_copy.empty()) {
578+
// aggregate per-second counts relative to start_time
579+
std::map<long, int> counts_per_second;
580+
for (const auto& tp : times_copy) {
581+
long s = std::chrono::duration_cast<std::chrono::seconds>(tp - start_time).count();
582+
if (s < 0)
583+
s = 0;
584+
counts_per_second[s]++;
585+
}
586+
587+
long total_seconds = std::max<long>(
588+
1, static_cast<long>(std::chrono::duration_cast<std::chrono::seconds>(end_time - start_time).count()));
589+
590+
ROS_INFO_NAMED(LOGNAME, "Solution-callbacks per second (relative to planning start):");
591+
for (long s = 0; s <= total_seconds; ++s) {
592+
int c = 0;
593+
auto it = counts_per_second.find(s);
594+
if (it != counts_per_second.end())
595+
c = it->second;
596+
ROS_INFO_STREAM_NAMED(LOGNAME, " " << s << "s: " << c);
597+
}
598+
599+
} else {
600+
ROS_INFO_NAMED(LOGNAME, "No solution-callbacks recorded during planning.");
601+
}
602+
603+
return ok;
506604
}
507605

508606
bool PickPlaceTask::execute() {

0 commit comments

Comments
 (0)