[WIP] Add pick-place capability #266
Conversation
There was a problem hiding this comment.
Thanks a lot for this big chunk of work! Guess I will have to get the move_group_interface side of this ready as promised.
I was testing this on our system right now and noticed that the applicability test stage should return true if the object_name of the plan request is empty. That would allow planning pick/place motions for a given set of grasps while ignoring the collision objects in the scene, and who are we to tell the user that they can't do that? Flexibility is good, and it reduces the obstacles until they can start planning.
Apart from that, I went through and left some comments, but I've seen this code before, so it's nothing new.
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2016, Kentaro Wada. |
| @@ -43,7 +45,7 @@ add_compile_options(-fvisibility-inlines-hidden) | |||
| set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) | |||
|
|
|||
| add_subdirectory(src) | |||
| add_subdirectory(test) | |||
| # add_subdirectory(test) | |||
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2017, Bielefeld + Hamburg University |
There was a problem hiding this comment.
Funny how neither of the authors of this file are from those institutions
|
|
||
| /** PickPlaceBase wraps the pipeline to pick or place an object with a given end effector. | ||
| * | ||
| * Picking consist of the following sub stages: |
There was a problem hiding this comment.
| * Picking consist of the following sub stages: | |
| * Picking consists of the following stages: |
| std::string grasp_provider_plugin_name_; | ||
| std::string place_provider_plugin_name_; | ||
|
|
||
| // Pick metrics |
There was a problem hiding this comment.
| // Pick metrics | |
| // Pick parameters |
Same below. Also capitalization further up
|
|
||
| std::transform(hand_joint_names.begin(), hand_joint_names.end(), open_pose.begin(), std::inserter(hand_open_pose, hand_open_pose.end()), std::make_pair<std::string const&, double const&>); | ||
|
|
||
| // TODO(karolyartur): Raise exception if the sizes do not match |
| } | ||
|
|
||
| // ------------------- | ||
| // PlaceProviderDefault |
There was a problem hiding this comment.
Either just leave this as a divider or use the name of the Provider below, I'd say
| cost = std::numeric_limits<double>::infinity(); | ||
|
|
||
| liftSolution(s, cost, comment); | ||
| if (!props.get<bool>("ignore_filter") && !props.get<Predicate>("predicate")(s, comment)) { |
There was a problem hiding this comment.
Was this related to the error we had when all solutions would end with infinite cost?
| auto _current_state = std::make_unique<stages::CurrentState>("current state"); | ||
| _current_state->setTimeout(10); | ||
|
|
||
| // Verify that object is not attached for picking and if object is attached for placing |
There was a problem hiding this comment.
How about wrapping this and either not adding the filter if parameters.object_name is empty, or returning true no matter what?
| @@ -0,0 +1,95 @@ | |||
| ### | |||
There was a problem hiding this comment.
Whoops. I don't think this should be part of the PR, we only used this to complete the PlanPickPlace.action.
|
Hi, I'm wondering what the status is of the PR? |
|
Hi @bryceikeda! Unfortunately, I didn't have the time to follow up on this PR. I have a student currently who would pick this up from here, so there might be some updates in a few months. |
This PR is to continue the work from PR #111
The aim is to provide a Pick-Place planning pipeline based on MTC.
During the design, I tried to keep the key concepts and architecture mentioned in Issue #112