Make pregrasp pose optional in GenerateGraspPose by henningkayser · Pull Request #157 · moveit/moveit_task_constructor

henningkayser · 2020-04-10T18:43:21Z

The pregrasp pose is now set as an empty string by default so that no pregrasp pose is applied. Instead, the stage will simply forward the current joint state of the gripper. There are several scenarios where it makes sense to let the pregrasp pose unspecified by default and to simply use the current state instead. For instance, a gripper could have different types of "open", depending on the monitored stage or could even use dynamically computed pregrasp poses matching to the size of the object. Also, an optional pregrasp pose enables to use this stage for suction grippers that don't have any predefined poses configured at all, so that it's easy to set up the same grasping pipeline for finger and suction grippers.

rhaschke

Generally, I approve. However, I suggest keeping the unset default value,
requiring a user to explicitly set the pregrasp posture to an empty string if he wants to have the new behavior.

rhaschke · 2020-04-11T09:58:46Z

core/src/stages/generate_grasp_pose.cpp

 	p.declare<double>("angle_delta", 0.1, "angular steps (rad)");

-	p.declare<boost::any>("pregrasp", "pregrasp posture");
+	p.declare<boost::any>("pregrasp", std::string(""), "pregrasp posture");


Keep "unset" as default here?

Out of curiosity, is there a reason to declare the pregrasp property as boost::any rather than std::string .? I see the only way it's being used is as std::string

Very good point. I think, the idea was to enable some message alternatively. But you are right, currently, we only use strings.

see comment below

henningkayser · 2020-04-13T13:42:09Z

Generally, I approve. However, I suggest keeping the unset default value,
requiring a user to explicitly set the pregrasp posture to an empty string if he wants to have the new behavior.

@rhaschke I also thought about this, but I actually think that setting an empty string smells like a weird workaround and not like well-defined default behavior. I would suggest we don't check for an empty string, but only if the property is specified at all (checking the value of boost_any). The pregrasp pose property can actually be set to a moveit_msgs::RobotState, but this is not implemented and would result in a bad_any_cast exception if specified. Right now I'm fixing this by adding conditions that check for the property type.
What is your proposed semantics for using the RobotState message as pregrasp pose? I would expect it to override the full robot state including attached collision objects, otherwise I would use a JointState message for this. Also, should we filter by eef joints or just apply the joint state values for the full robot? The same semantics needs to be discussed for the "grasp" property, but since I don't see it being used anywhere it doesn't cause any trouble.
@v4hn any thoughts about this?

rhaschke · 2020-04-13T19:01:46Z

Hi @henningkayser,
Using propertry().defined() you can check for definition of a property. However, I'm not sure we should have a default at all. Usually, we follow the concept that settings need to be explicitly decided for. On the other hand, if no posture is desired, doing nothing is a reasonable approach.

rhaschke · 2020-04-13T19:06:27Z

Going for a JointState message is fine. However, only the gripper joints should be considered.

v4hn · 2020-04-14T16:10:44Z

Wow, Easter vacations, and suddenly there is patches turning up. 😄

The pregrasp pose property can actually be set to a moveit_msgs::RobotState, but this is not implemented and would result in a bad_any_cast exception if specified. Right now I'm fixing this by adding conditions that check for the property type.
What is your proposed semantics for using the RobotState message as pregrasp pose? I would expect it to override the full robot state including attached collision objects, otherwise I would use a JointState message for this.

RobotState also supports MultiDOFJointState and up to now there was no reason to exclude those. As you implied, specifying attached objects here pretty much breaks a lot later on (probably in Connect stages), but sure, your attached objects might be tweezers, so we might as well apply them. shrug

At the same time, if you add JointState and we remove RobotState until someone actually needs it, that's fine by me.

I also thought about this, but I actually think that setting an empty string smells like a weird workaround and not like well-defined default behavior. I would suggest we don't check for an empty string, but only if the property is specified at all (checking the value of boost_any).

I agree with Robert in that explicit is better than implicit in MTC.
Defaults might make some things look easier, but it's also easier to forget about them when you would need to change them for your task (a problem the old MoveIt code base shows a lot...).
But actually, this stage functionally requires neither pregrasp, nor grasp. Even more interesting, grasp does not have a default value, but it works just fine not to specify it. Robert's SimpleGrasp container requires both properties to be set (and probably to be of type string or RobotState?), but it's not the only user of this stage.
So I would argue:

either the pregrasp/grasp property support could be implemented in a wrapper altogether (conceptually cleaner, but quite ugly to look at...)
or the Stage can be refactored and a derived class could add the properties
or, indeed, we explicitly make both optional, but also do not pass them on in the state properties if they are the default.

I'd support the second and third approach.

Also, should we filter by eef joints or just apply the joint state values for the full robot?

This stage seeds the IK solver, so you conceptually don't know the full joint state at this point and shouldn't touch it. Filtering for joints of the eef is reasonable, possibly complaining if anything unexpected was found.

The same semantics needs to be discussed for the "grasp" property, but since I don't see it being used anywhere it doesn't cause any trouble.

It is exposed in the interface states and read by SimpleGrasp too.

All in all it is rather ridiculous how much discussion there is around this trivial stage that is meant as an example...

henningkayser · 2020-04-14T18:13:42Z

either the pregrasp/grasp property support could be implemented in a wrapper altogether (conceptually cleaner, but quite ugly to look at...)

Still on the menu, right now this stage really works fine for basic use cases, though

or the Stage can be refactored and a derived class could add the properties

Hmm, sounds a little bit like too much overhead for this feature

or, indeed, we explicitly make both optional, but also do not pass them on in the state properties if they are the default.

I don't really get why default behavior (that doesn't imply/change things based on some arbitrary assumptions but simply skips a feature) needs to be explicit. To me, 'pregrasp' and 'grasp' properties seem like optional features with certain use cases based on first pick pipeline designs. Couldn't we add the concept of optional properties that just don't do anything if not specified? These could also be checked on a property level without having to look into default values.

Also, I just added support for JointState messages. If you both think setting an empty string is the way to go to disable the pregrasp pose, I can add that as well.

* Make parameter optional (skip pregrasp if left empty) * Allow setting by JointState, filtered by eef joints * Check parameter validity + improved error messages

v4hn

Still on the menu, right now this stage really works fine for basic use cases, though

I agree and I don't really use or need the properties myself at the moment.
Still, they are required to fulfill the interface of SimpleGrasp.

or, indeed, we explicitly make both optional, but also do not pass them on in the state properties if they are the default.

I don't really get why default behavior (that doesn't imply/change things based on some arbitrary assumptions but simply skips a feature) needs to be explicit. To me, 'pregrasp' and 'grasp' properties seem like optional features with certain use cases based on first pick pipeline designs. Couldn't we add the concept of optional properties that just don't do anything if not specified? These could also be checked on a property level without having to look into default values.

That's not quite what I wrote, but pretty much what I meant. I like your current patch, aside from the inline comments. I would only force "don't do anything" to include "not being passed on to other stages via InterfaceState properties", thus the exposeTo should be run conditionally too.

Could you please adjust handling the grasp property similarly?

Also, I'm not sure the SimpleGrasp container can handle JointStates in the properties, but that's not overly important here as you provide a new feature.

v4hn · 2020-04-14T19:36:54Z

core/src/stages/generate_grasp_pose.cpp

+			if (!eef_jmg->getVariableDefaultPositions(pregrasp_name, m))
+				errors.push_back(*this, "pregrasp is set to unknown end effector pose: " + pregrasp_name);
+		} else if (pregrasp_prop.type() == typeid(sensor_msgs::JointState)) {
+			// check if the specified pregrasp pose is a valid named target


The comment does not make sense.

v4hn · 2020-04-14T19:37:22Z

core/src/stages/generate_grasp_pose.cpp

+			const auto& pregrasp_state = boost::any_cast<sensor_msgs::JointState>(pregrasp_prop);
+			if (pregrasp_state.name.size() == pregrasp_state.position.size() &&
+			    pregrasp_state.name.size() == pregrasp_state.velocity.size() &&
+			    pregrasp_state.name.size() == pregrasp_state.effort.size()) {


Why would you require velocity/effort to be set?
Positions suffice, though dynamics might be specified to define the dynamic grasp behavior.

Currently, the code only uses positions. Dynamic grasp behaviour comes into play if a trajectory would be specified. That's not the case.

v4hn · 2020-04-14T19:37:57Z

core/src/stages/generate_grasp_pose.cpp

+			if (pregrasp_state.name.size() == pregrasp_state.position.size() &&
+			    pregrasp_state.name.size() == pregrasp_state.velocity.size() &&
+			    pregrasp_state.name.size() == pregrasp_state.effort.size()) {
+				// We only apply the joint state for for joints of the end effector


Suggested change

// We only apply the joint state for for joints of the end effector

// We only apply the joint state for joints of the end effector

v4hn · 2020-04-14T19:40:11Z

core/src/stages/generate_grasp_pose.cpp

+				if (eef_state.name.empty())
+					errors.push_back(*this, "pregrasp JointState doesn't contain joint values for end effector group");
+				else
+					properties().set("pregrasp_state", eef_state);  // Override property with filtered joint state


I'll leave it to Robert to comment on whether changing properties dynamically is a good idea.
I would consider adding a private member variable instead.

Fine with me.

v4hn · 2020-04-14T19:41:22Z

core/src/stages/generate_grasp_pose.cpp

+			const auto& pregrasp_name = boost::any_cast<std::string>(pregrasp_prop);
+			std::map<std::string, double> m;
+			if (!eef_jmg->getVariableDefaultPositions(pregrasp_name, m))
+				errors.push_back(*this, "pregrasp is set to unknown end effector pose: " + pregrasp_name);


If you lookup the named target anyway, you might as well save the result for the compute call to avoid branching on the any again there.

rhaschke · 2020-08-11T12:27:53Z

@henningkayser: What's the status of this PR?

Make pregrasp pose optional in GenerateGraspPose

990ec26

henningkayser requested a review from v4hn April 10, 2020 18:53

rhaschke reviewed Apr 11, 2020

View reviewed changes

henningkayser force-pushed the pr-optional_pregrasp branch from 7bc28f2 to 3143fd0 Compare April 14, 2020 17:59

Improve 'pregrasp' parameter handling in GenerateGraspPose

3c6dfe2

* Make parameter optional (skip pregrasp if left empty) * Allow setting by JointState, filtered by eef joints * Check parameter validity + improved error messages

henningkayser force-pushed the pr-optional_pregrasp branch from 3143fd0 to 3c6dfe2 Compare April 14, 2020 18:24

v4hn reviewed Apr 14, 2020

View reviewed changes

	// We only apply the joint state for for joints of the end effector
	// We only apply the joint state for joints of the end effector

Conversation

henningkayser commented Apr 10, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rhaschke left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

JafarAbdi Apr 11, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

henningkayser commented Apr 13, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rhaschke commented Apr 13, 2020

Uh oh!

rhaschke commented Apr 13, 2020

Uh oh!

v4hn commented Apr 14, 2020

Uh oh!

henningkayser commented Apr 14, 2020

Uh oh!

v4hn left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

rhaschke commented Aug 11, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

henningkayser commented Apr 10, 2020 •

edited

Loading

JafarAbdi Apr 11, 2020 •

edited

Loading

henningkayser commented Apr 13, 2020 •

edited

Loading