From 9c5c87fc580b5d03689a956aa1e054b3dbad1f82 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Mar 2017 19:23:56 +0100 Subject: [PATCH] generate_grasp_pose: add grasp offset This (as probably known from many other implementations...) allows to grasp the object with the end effector link being further away from it. --- .../subtasks/generate_grasp_pose.h | 4 ++++ src/subtasks/generate_grasp_pose.cpp | 22 ++++++++++++++----- src/test/test_plan_generate_grasp_pose.cpp | 1 + 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index fe54c089..30d2065c 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -28,6 +28,8 @@ public: void setObject(std::string object); + void setGraspOffset(double grasp_offset); + void setTimeout(double timeout); void setAngleDelta(double delta); @@ -37,6 +39,8 @@ protected: std::string eef_; + double grasp_offset_; + std::string object_; double timeout_; diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index d9bea388..b3b8ea33 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -20,6 +20,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st timeout_(0.1), angle_delta_(0.1), current_angle_(0.0), + grasp_offset_(0.0), remaining_time_(timeout_), tried_current_state_as_seed_(false) { @@ -43,6 +44,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setObject(std::string obj object_= object; } +void +moveit::task_constructor::subtasks::GenerateGraspPose::setGraspOffset(double offset){ + grasp_offset_= offset; +} + void moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout){ timeout_= timeout; @@ -102,12 +108,12 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ std::placeholders::_2, std::placeholders::_3); - geometry_msgs::Pose pose; - const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_); - if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + geometry_msgs::Pose object_pose, grasp_pose; + const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_); + if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); - tf::poseEigenToMsg(object_pose, pose); + tf::poseEigenToMsg(object_pose_eigen, object_pose); while(current_angle_ < 2*M_PI){ if( remaining_time_ <= 0.0 ){ @@ -119,14 +125,18 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ continue; } - pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); + grasp_pose= object_pose; + + grasp_pose.position.x-= grasp_offset_*cos(current_angle_); + grasp_pose.position.y-= grasp_offset_*sin(current_angle_); + grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); if(tried_current_state_as_seed_) grasp_state.setToRandomPositions(jmg); tried_current_state_as_seed_= true; auto now= std::chrono::steady_clock::now(); - bool succeeded= grasp_state.setFromIK(jmg, pose, eef_, 1, remaining_time_, is_valid); + bool succeeded= grasp_state.setFromIK(jmg, grasp_pose, eef_, 1, remaining_time_, is_valid); remaining_time_-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); if(succeeded){ diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index 61bc81e8..22c32268 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -43,6 +43,7 @@ int main(int argc, char** argv){ st->setObject("object"); st->setTimeout(0.5); st->setAngleDelta(0.1); + st->setGraspOffset(0.03); t.addStart(st);