New grasp pose generator stage #196

bostoncleek · 2020-08-11T19:00:22Z

This PR introduces a new stage called GraspProvider. This stage uses grasping_msgs::GraspPlanningAction to communicate with an action server external to MTC. The action server is used to provide grasp candidates to MTC. For example the action server can use machine learning pipelines to sample grasps given a point cloud or depth image. This feature removes complex dependencies from MTC that would otherwise be required to generate grasp candidates.

The GraspProvider stage sends a request for grasps. If a time out is reached or no grasps are found then planning fails. Otherwise the provided grasps will be used by MTC.

Closes #188

codecov · 2020-08-11T20:16:55Z

Codecov Report

Merging #196 (7c47532) into master (e923fbc) will decrease coverage by 0.77%.
The diff coverage is 1.74%.

@@            Coverage Diff             @@
##           master     #196      +/-   ##
==========================================
- Coverage   54.63%   53.86%   -0.76%     
==========================================
  Files         102      106       +4     
  Lines        7852     7967     +115     
==========================================
+ Hits         4289     4291       +2     
- Misses       3563     3676     +113

Impacted Files	Coverage Δ
...clude/moveit/task_constructor/stages/action_base.h	`0.00% <0.00%> (ø)`
...de/moveit/task_constructor/stages/grasp_provider.h	`0.00% <0.00%> (ø)`
core/src/stages/grasp_provider.cpp	`1.00% <1.00%> (ø)`
core/src/stages/action_base.cpp	`7.70% <7.70%> (ø)`

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update e923fbc...7c47532. Read the comment docs.

mamoll · 2020-08-12T23:31:34Z

The intent is to add a base class that can be used to implement support for GPD and Dex-Net. These grasp synthesis pipelines have a whole slew of dependencies, so the GPD/Dex-Net support would likely live in a separate packages (say, moveit_task_constructor_gpd, and moveit_task_constructor_dexnet).

mamoll · 2020-08-17T20:02:03Z

@rhaschke @v4hn perhaps it's helpful to see how this code can be used. See https://github.com/PickNikRobotics/deep_grasp_demo/blob/master/src/gpd_pick_place_task.cpp for an example.

core/include/moveit/task_constructor/stages/action_base.h

JStech

Looks good. I mentioned a few minor things.

I see what you mean about the reformatting. If all that code does need to be reformatted to pass some new requirement, it seems that it should be done in a different PR.

core/include/moveit/task_constructor/stages/action_base.h

core/include/moveit/task_constructor/stages/deep_grasp_pose.h

bostoncleek · 2020-08-27T21:05:29Z

It might be useful to check out the implementation and documentations for the demos.

bostoncleek · 2020-09-03T16:20:57Z

Can I get a review please? @v4hn @rhaschke

lianghongzhuo · 2020-09-24T13:41:49Z

Looks good for me when I test this with our ur5 robot.

JStech · 2021-01-24T03:38:29Z

I'm using this for a project and I was surprised to see it's still not merged. @v4hn and @rhaschke could you please take a look at this PR?

JStech · 2021-01-25T18:23:07Z

I had to rebase this to get it to build on Noetic (it was a very easy rebase). Here is my branch.

rhaschke · 2021-01-25T19:36:11Z

Thanks @JStech for providing the rebased branch link. Currently, there is so much other work, that I cannot catch up with all PR reviews. As this one is a larger one, it will probably take some more time... Please be patient (and continue using your own branch meanwhile).

lianghongzhuo · 2021-02-07T08:12:50Z

@JStech 's rebase link works for me, (ubuntu 18, ros melodic with moveit master branch)

davetcoleman · 2021-03-10T15:58:18Z

@bostoncleek think this could get merged now?

bostoncleek · 2021-03-10T16:04:52Z

@bostoncleek think this could get merged now?

@davetcoleman This was ready to be merged back on August 11th. It is a matter of the maintainers pressing the merge button!

Looks like it does need a rebase though. I will squash and rebase today.

v4hn · 2021-03-10T17:03:12Z

It's not just merge, but also review (and possibly fixup). I'm sorry for not getting around to do it. There is always more pressing issues and often two crying kids in the background since Corona homeoffice. Thanks for rebasing in advance!

davetcoleman

I did a very quick skim, lgtm

bostoncleek · 2021-03-10T17:24:16Z

It's not just merge, but also review (and possibly fixup). I'm sorry for not getting around to do it. There is always more pressing issues and often two crying kids in the background since Corona homeoffice. Thanks for rebasing in advance!

Any change requests will be fixed so please make a comment on what you would like.

v4hn · 2021-03-10T17:30:36Z

As I said, I did not have the time to look through this in detail, and I'm sorry for that.

My biggest concern, as I can recall from discussion with @henningkayser and @lianghongzhuo was that the use-case to receive candidate poses in an any-time fashion through feedback and spawn solutions based on them continuously should be well supported, even if you did not use that approach in your project.
Such an approach might be valuable for projects like this too.

bostoncleek · 2021-03-10T17:39:56Z

As I said, I did not have the time to look through this in detail, and I'm sorry for that.

My biggest concern, as I can recall from discussion with @henningkayser and @lianghongzhuo was that the use-case to receive candidate poses in an any-time fashion through feedback and spawn solutions based on them continuously should be well supported, even if you did not use that approach in your project.
Such an approach might be valuable for projects like this too.

Don't have the time? ... its World MoveIt Day! of course you have the time. Grow the project a little don't be afraid to let others make a contribution towards your work.

If it doesn't happen today I will see you on World MoveIt Day 2022!

JStech · 2021-03-10T17:43:54Z

the use-case to receive candidate poses in an any-time fashion through feedback and spawn solutions based on them continuously should be well supported, even if you did not use that approach in your project.

This sounds more like a request for an enhancement. Why block merging useful code because we can imagine even more useful code?

davetcoleman · 2021-03-11T15:13:11Z

@bostoncleek could you fix the conflict?
@JafarAbdi would be a good candidate to review and merge this, given his extensive work with MTC

rhaschke · 2021-03-11T16:25:07Z

I shortly looked into this PR and I was surprised by the many unrelated commits. @bostoncleek, it would be great if you could rebase and cleanup the history.

bostoncleek · 2021-04-30T16:21:32Z

@v4hn I have addressed @rhaschke requests. Please let me know if there are other patches that need to be made.

lianghongzhuo · 2021-05-22T07:31:31Z

any updates?

lianghongzhuo · 2021-06-01T14:14:21Z

@bostoncleek Could you rebase this PR? There is some upstream change that makes this PR unable to compile.
Like add this 437cc55

v4hn · 2021-06-02T09:08:24Z

There is some upstream change that makes this PR unable to compile. Like add this 437cc55

Hongzhuo, I stated above that I will fix this up and merge it, and so I will do that. If I wouldn't spend some of my time fixing other bugs you cannot find yourself in our setups, I also would have more time for things like this. (And there are numerous other current issues/patches active in MoveIt/MTC). In the meanwhile feel free to use the PR branch if you need the stage.

bostoncleek · 2021-06-04T16:37:37Z

@lianghongzhuo yes I will rebase

bostoncleek · 2021-06-24T14:42:26Z

This is failing because of clang-tidy.

error: unknown warning option '-Wno-unused-but-set-parameter'; did you mean '-Wno-unused-parameter'? [clang-diagnostic-unknown-warning-option]

I don't see where add_compile_options() are set using a call to -Wno.

Can a maintainer provide help?

v4hn · 2021-06-25T12:59:44Z

I rebased.

#274 (which was not included here yet) recently made the CI configuration quite a bit stricter and addressed this particular error.
Let's see whether new ones pop up now. :-)

qyp-robot · 2021-07-15T03:08:13Z

感谢您清理此 PR。现在，审查要容易得多。我喜欢这里的一些想法，即：

异步接收结果

使用结合 GeneratePose 和 ActionBase 的模块化类结构

但是，当前的实现也有几个设计缺陷：

您应该求助于grasping_msgs::GraspPlanningAction由多个现有抓取提供程序使用的现有。这样，新阶段会自动推广到所有这些。

因此，应该命名该阶段GraspProvider（或类似名称）。这个阶段没有关于深度学习的具体内容。

该阶段通过鸭子类型取决于具体的消息类型，例如在准备目标或接收结果时。因此，实际上，我没有看到对类进行模板化的优势。请仅为grasping_msgs::GraspPlanningAction.

这样做，还将允许合并其他特定于抓取的参数，例如动作服务器提供的预抓取和抓取姿势。请在此处查看我自己的 GraspProvider 阶段实现。结合这两种方法将产生一个很好的解决方案。

您还没有处理多个反馈消息和线程安全。

Hello! @rhaschke
After making the following 4 changes:

Solved the problem of missing header files: a298899
Modification ofgrasp_provider.h:
19fb72c

3.Update the demo to use grasping_msgs::GraspPlanningAction, all related .cpp and .h files have been modified, of course, here I refer to @marcpujol-leitat:
https://github.com/PickNikRobotics/deep_grasp_demo/pull/11/files

Now the compilation can pass, but when I run roslaunch moveit_task_constructor_gpd gpd_demo.launch, the terminal has been stuck in the following stages, and the terminal information prompt is as follows:

[INFO] [1626318339.260666630]: Loading task parameters
[INFO] [1626318339.270120710]: Initializing task pipeline
[INFO] [1626318339.278097396]: Loading robot model'panda'...

I don't know how to solve it. Is there any solution?
Looking forward to your reply, thank you very much!

core/include/moveit/task_constructor/stages/grasp_provider.h

bostoncleek · 2022-06-02T13:52:19Z

WMD 2022!

I made some changes and ready for a review. I would really like to get this PR merged. @rhaschke @v4hn @JafarAbdi

The demo in this PR PickNikRobotics/deep_grasp_demo#14 has been updated to show the changed message type works.

davetcoleman · 2022-06-02T18:19:12Z

Thanks for revitalizing this!

Possible reviewers: @henningkayser @mamoll @vatanaksoytezer

bostoncleek · 2022-06-02T18:41:05Z

Thanks for revitalizing this!

Possible reviewers: @henningkayser @mamoll @vatanaksoytezer

If this can get in I will update the tutorial as well.

core/src/stages/action_base.cpp

msgs/CMakeLists.txt

core/src/stages/grasp_provider.cpp

JafarAbdi · 2022-06-02T18:49:06Z

core/src/stages/grasp_provider.cpp

+void GraspProvider::feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) {
+	found_candidates_ = true;
+
+	// Protect grasp candidate incase feedback is sent asynchronously
+	const std::lock_guard<std::mutex> lock(grasp_mutex_);
+	grasp_candidates_ = feedback->grasps;
+}
+
+void GraspProvider::doneCallback(const actionlib::SimpleClientGoalState& state,
+                                 const grasping_msgs::GraspPlanningResultConstPtr& result) {
+	if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
+		ROS_DEBUG_STREAM_NAMED(LOGNAME, "Found grasp candidates");
+	} else {
+		ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found (state): %s", clientPtr_->getState().toString().c_str());
+	}
+}


This only use the first feedback message? Why not moving found_candidates_ = true; to doneCallback and use the grasps from the goal result? We could remove the mutex if we do so.

The reason the mutex is here is to handle multiple feedback messages. After discussing with @rhaschke he was concerned there could be many feedback messages containing grasp samples. The flag is used to indicate that at least one message was received.

But as soon as we enter the if statement if (monitorGoal()) { in void GraspProvider::compute() { we will not update the grasp_candidates_ since the mutex is locked, and it will get unlocked just before we exit the function which mean next compute will send a new goal, and we only get the chance to handle the first feedback, right?

I see that would simply this by removing the mutex. Yeah this does only handle the first feedback message if compute().

I would like it to be able to handle multiple feedback messages. Do you see a way that could work?

What do you think about storing all the grasps from the feedback and results fields into a container. Then after the done callback flags success the compute() function operates over all those grasp candidates?

Do you see any issues with thread safety?

See bostoncleek#2 for a proposal 😬

This stage uses action messages to communicate with an action server external to MTC. The action server can use machine learning pipelines to sample grasps given a point cloud/depth image. This removes complex dependencies from MTC. The stage sends a request for grasps. If a time out is reached or no grasps are found then planning fails. This stage uses a template parameter which is the action message and inherits from an action base class. This allows other stages to inherit these properties and use action messages.

The action base class can only be used with the action msg GraspPlanning.action from grasping_msgs.

The grasp detection flag is an atomic bool. The scope of the grasp candidate's mutex has been moved.

mamoll requested review from rhaschke and v4hn August 12, 2020 23:31

davetcoleman reviewed Aug 18, 2020

View reviewed changes

core/include/moveit/task_constructor/stages/action_base.h Outdated Show resolved Hide resolved

JStech approved these changes Aug 24, 2020

View reviewed changes

bostoncleek force-pushed the pr-deep_grasp_stage branch 2 times, most recently from e7e0d89 to f8ccde7 Compare August 26, 2020 16:27

bostoncleek mentioned this pull request Sep 1, 2020

MoveIt deep grasping tutorial moveit/moveit_tutorials#521

Merged

2 tasks

v4hn mentioned this pull request Mar 3, 2021

"Best way" for a stage to accept grasp poses from external source at runtime #239

Closed

davetcoleman approved these changes Mar 10, 2021

View reviewed changes

bostoncleek force-pushed the pr-deep_grasp_stage branch 2 times, most recently from bdb2552 to 7717231 Compare March 11, 2021 19:32

bostoncleek force-pushed the pr-deep_grasp_stage branch from a8ae45f to ae17d5c Compare April 30, 2021 15:40

bostoncleek force-pushed the pr-deep_grasp_stage branch 2 times, most recently from 03e6f24 to ef0082a Compare June 5, 2021 00:52

v4hn force-pushed the pr-deep_grasp_stage branch from ef0082a to ae376b6 Compare June 25, 2021 12:55

rhaschke mentioned this pull request Jul 12, 2021

Missing moveit_task_constructor_msgs/SampleGraspPosesAction.h #281

Open

bostoncleek force-pushed the pr-deep_grasp_stage branch 3 times, most recently from 9cd3fe0 to 3c03a32 Compare January 10, 2022 22:04

bostoncleek commented Jan 11, 2022

View reviewed changes

core/include/moveit/task_constructor/stages/grasp_provider.h Show resolved Hide resolved

bostoncleek mentioned this pull request Jan 25, 2022

Update Demo to use GraspProvider interface and grasping_msgs::GraspPlanningAction PickNikRobotics/deep_grasp_demo#14

Open

bostoncleek requested a review from rhaschke February 18, 2022 18:48

JafarAbdi reviewed Jun 2, 2022

View reviewed changes

bostoncleek requested a review from JafarAbdi June 2, 2022 20:51

bostoncleek and others added 5 commits June 17, 2022 13:35

Rename stage to Grasp Provider

8220806

Remove template paramete to action base class

e671e06

The action base class can only be used with the action msg GraspPlanning.action from grasping_msgs.

Handle asynchronous feedback

5145e0c

The grasp detection flag is an atomic bool. The scope of the grasp candidate's mutex has been moved.

Fix Jafar's change requests

7c47532

bostoncleek force-pushed the pr-deep_grasp_stage branch from 1667ea7 to 7c47532 Compare June 17, 2022 17:38

New grasp pose generator stage #196

Are you sure you want to change the base?

New grasp pose generator stage #196

Conversation

bostoncleek commented Aug 11, 2020 • edited Loading

codecov bot commented Aug 11, 2020 • edited Loading

Codecov Report

mamoll commented Aug 12, 2020

mamoll commented Aug 17, 2020

JStech left a comment

Choose a reason for hiding this comment

bostoncleek commented Aug 27, 2020

bostoncleek commented Sep 3, 2020

lianghongzhuo commented Sep 24, 2020

JStech commented Jan 24, 2021

JStech commented Jan 25, 2021

rhaschke commented Jan 25, 2021

lianghongzhuo commented Feb 7, 2021

davetcoleman commented Mar 10, 2021

bostoncleek commented Mar 10, 2021 • edited Loading

v4hn commented Mar 10, 2021 via email

davetcoleman left a comment

Choose a reason for hiding this comment

bostoncleek commented Mar 10, 2021

v4hn commented Mar 10, 2021

bostoncleek commented Mar 10, 2021

JStech commented Mar 10, 2021

davetcoleman commented Mar 11, 2021

rhaschke commented Mar 11, 2021

bostoncleek commented Apr 30, 2021

lianghongzhuo commented May 22, 2021

lianghongzhuo commented Jun 1, 2021

v4hn commented Jun 2, 2021 via email

bostoncleek commented Jun 4, 2021

bostoncleek commented Jun 24, 2021

v4hn commented Jun 25, 2021 • edited Loading

qyp-robot commented Jul 15, 2021

bostoncleek commented Jun 2, 2022 • edited Loading

davetcoleman commented Jun 2, 2022

bostoncleek commented Jun 2, 2022

JafarAbdi Jun 2, 2022

Choose a reason for hiding this comment

bostoncleek Jun 2, 2022 • edited Loading

Choose a reason for hiding this comment

JafarAbdi Jun 3, 2022

Choose a reason for hiding this comment

bostoncleek Jun 3, 2022

Choose a reason for hiding this comment

bostoncleek Jun 3, 2022

Choose a reason for hiding this comment

bostoncleek Jun 3, 2022

Choose a reason for hiding this comment

bostoncleek Jun 3, 2022

Choose a reason for hiding this comment

JafarAbdi Jun 12, 2022

Choose a reason for hiding this comment

bostoncleek commented Aug 11, 2020 •

edited

Loading

codecov bot commented Aug 11, 2020 •

edited

Loading

bostoncleek commented Mar 10, 2021 •

edited

Loading

v4hn commented Jun 25, 2021 •

edited

Loading

bostoncleek commented Jun 2, 2022 •

edited

Loading

bostoncleek Jun 2, 2022 •

edited

Loading