From 3c6dfe28d8c1d7bd306cea059319139e9df387a8 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 14 Apr 2020 19:53:17 +0200 Subject: [PATCH] Improve 'pregrasp' parameter 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 --- .../stages/generate_grasp_pose.h | 2 +- core/src/stages/generate_grasp_pose.cpp | 55 +++++++++++++++---- 2 files changed, 44 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index b6d34c0dd..b0cd65d1b 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/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); } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 6d3d11070..c9f92146e 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -54,7 +54,7 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(nam p.declare("object"); p.declare("angle_delta", 0.1, "angular steps (rad)"); - p.declare("pregrasp", std::string(""), "pregrasp posture"); + p.declare("pregrasp", "pregrasp posture"); p.declare("grasp", "grasp posture"); } @@ -78,15 +78,40 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) { const std::string& eef = props.get("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("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(pregrasp_prop); std::map 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(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 +150,16 @@ void GenerateGraspPose::compute() { const std::string& eef = props.get("eef"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - // Apply pregrasp pose if defined - if (!props.get("pregrasp").empty()) { - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); - robot_state.setToDefaultValues(jmg, props.get("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(pregrasp_prop)); + } else if (pregrasp_prop.type() == typeid(sensor_msgs::JointState)) { + const auto& pregrasp_state = boost::any_cast(pregrasp_prop); + current_state.setVariablePositions(pregrasp_state.name, pregrasp_state.position); + } } geometry_msgs::PoseStamped target_pose_msg;