Improve 'pregrasp' paremeter handling in GenerateGraspPose

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

Author: henningkayser

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

@@ -57,7 +57,7 @@ class GenerateGraspPose : public GeneratePose
 	void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
 
 	void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
-	void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
+	void setPreGraspPose(const sensor_msgs::JointState& pregrasp) { properties().set("pregrasp", pregrasp); }
 	void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
 	void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
 

core/src/stages/generate_grasp_pose.cpp

@@ -54,7 +54,7 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(nam
 	p.declare<std::string>("object");
 	p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
 
-	p.declare<boost::any>("pregrasp", std::string(""), "pregrasp posture");
+	p.declare<boost::any>("pregrasp", "pregrasp posture");
 	p.declare<boost::any>("grasp", "grasp posture");
 }
 
@@ -78,15 +78,41 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
 	const std::string& eef = props.get<std::string>("eef");
 	if (!robot_model->hasEndEffector(eef))
 		errors.push_back(*this, "unknown end effector: " + eef);
-	else {
+	else if (props.property("pregrasp").defined()) {
 		// if pregrasp pose is defined, check if it's valid
-		const std::string& pregrasp_name = props.get<std::string>("pregrasp");
-		if (!pregrasp_name.empty()) {
-			// check availability of eef pose
+		const moveit::core::JointModelGroup* eef_jmg = robot_model->getEndEffector(eef);
+		const boost::any& pregrasp_prop = props.get("pregrasp");
+		if (pregrasp_prop.type() == typeid(std::string)) {
+			// check if the specified pregrasp pose is a valid named target
+			const auto& pregrasp_name = boost::any_cast<std::string>(pregrasp_prop);
 			std::map<std::string, double> m;
-			const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
-			if (!jmg->getVariableDefaultPositions(pregrasp_name, m))
-				errors.push_back(*this, "unknown end effector pose: " + pregrasp_name);
+			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
+			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()) {
+				// We only apply the joint state for for joints of the end effector
+				sensor_msgs::JointState eef_state;
+				eef_state.header = pregrasp_state.header;
+				for (size_t i = 0; i < pregrasp_state.name.size(); ++i) {
+					if (eef_jmg->hasJointModel(pregrasp_state.name[i])) {
+						eef_state.name.push_back(pregrasp_state.name[i]);
+						eef_state.position.push_back(pregrasp_state.position[i]);
+						eef_state.velocity.push_back(pregrasp_state.velocity[i]);
+						eef_state.effort.push_back(pregrasp_state.effort[i]);
+					}
+				}
+				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
+			} else {
+				errors.push_back(*this, "pregrasp JointState is malformed - size mismatch between joint names and values");
+			}
 		}
 	}
 
@@ -125,10 +151,16 @@ void GenerateGraspPose::compute() {
 	const std::string& eef = props.get<std::string>("eef");
 	const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
 
-	// Apply pregrasp pose if defined
-	if (!props.get<std::string>("pregrasp").empty()) {
-		robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
-		robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
+	// Apply pregrasp target or joint state if defined
+	const boost::any& pregrasp_prop = props.get("pregrasp");
+	if (!pregrasp_prop.empty()) {
+		robot_state::RobotState& current_state = scene->getCurrentStateNonConst();
+		if (pregrasp_prop.type() == typeid(std::string)) {
+			current_state.setToDefaultValues(jmg, boost::any_cast<std::string>(pregrasp_prop));
+		} else if (pregrasp_prop.type() == typeid(sensor_msgs::JointState)) {
+			const auto& pregrasp_state = boost::any_cast<sensor_msgs::JointState>(pregrasp_prop);
+			current_state.setVariablePositions(pregrasp_state.name, pregrasp_state.position);
+		}
 	}
 
 	geometry_msgs::PoseStamped target_pose_msg;