From 19c351b27db19c955feb185fab63b6b1eb771410 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Mar 2017 17:07:42 +0100 Subject: [PATCH] generate grasp poses: retrieve multiple IK solutions & check collisions --- .../subtasks/generate_grasp_pose.h | 4 ++ src/subtasks/generate_grasp_pose.cpp | 59 ++++++++++++++++--- src/test/test_plan_generate_grasp_pose.cpp | 2 +- 3 files changed, 55 insertions(+), 10 deletions(-) diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index 38a6f43c..7c22a676 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -45,6 +45,10 @@ protected: double remaining_time_; + bool tried_current_state_as_seed_; + + std::vector< std::vector > previous_solutions_; + ros::Publisher pub; }; diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index f720cacc..cb134d12 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include @@ -12,12 +13,14 @@ #include #include +#include moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name) : moveit::task_constructor::SubTask::SubTask(name), timeout_(0.1), current_angle_(0.0), - remaining_time_(timeout_) + remaining_time_(timeout_), + tried_current_state_as_seed_(false) { ros::NodeHandle nh; pub= nh.advertise("display_robot_state", 50); @@ -51,6 +54,23 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ return current_angle_ < 2*M_PI; } +namespace { + bool isValid(planning_scene::PlanningSceneConstPtr scene, + std::vector< std::vector >& old_solutions, + robot_state::RobotState* state, + const robot_model::JointModelGroup* jmg, + const double* joint_positions){ + for( std::vector sol : old_solutions ){ + if( jmg->distance(joint_positions, sol.data()) < 0.1 ){ + return false; + } + } + state->setJointGroupPositions(jmg, joint_positions); + state->update(); + return scene->isStateColliding(*state, jmg->getName()); + } +} + bool moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); @@ -61,6 +81,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ const moveit::core::JointModelGroup* jmg= grasp_state.getJointModelGroup(group_); + const moveit::core::GroupStateValidityCallbackFn is_valid= + std::bind( + &isValid, + scene_, + previous_solutions_, + std::placeholders::_1, + 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()) @@ -68,22 +98,33 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ tf::poseEigenToMsg(object_pose, pose); - for(; current_angle_ < 2*M_PI; current_angle_+= 0.2, remaining_time_= timeout_){ - if( remaining_time_ <= 0.0 ) + while(current_angle_ < 2*M_PI){ + if( remaining_time_ <= 0.0 ){ + current_angle_+= 0.2; + remaining_time_= timeout_; + tried_current_state_as_seed_= false; + previous_solutions_.clear(); continue; + } pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); - std::cout << "trying " << current_angle_ << std::endl; + std::cout << "trying " << current_angle_ << " with remaining time " << remaining_time_ << std::endl; + + 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_, remaining_time_/*,validState*/); + bool succeeded= grasp_state.setFromIK(jmg, pose, eef_, 1, remaining_time_, is_valid); remaining_time_-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); - moveit_msgs::DisplayRobotState drs; - robot_state::robotStateToRobotStateMsg(grasp_state, drs.state); - pub.publish(drs); - if(succeeded){ + moveit_msgs::DisplayRobotState drs; + robot_state::robotStateToRobotStateMsg(grasp_state, drs.state); + pub.publish(drs); + + previous_solutions_.emplace_back(); + grasp_state.copyJointGroupPositions(jmg, previous_solutions_.back()); moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); sendBothWays(trajectory, grasp_scene); return true; diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index fe50dc24..a3e95b97 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -22,7 +22,7 @@ void spawnObject(){ o.primitive_poses[0].orientation.w= 1.0; o.primitives.resize(1); o.primitives[0].type= shape_msgs::SolidPrimitive::SPHERE; - o.primitives[0].dimensions.resize(1, 0.07); + o.primitives[0].dimensions.resize(1, 0.03); psi.applyCollisionObject(o); }