From 988886d57c15562f739f4a18d7984b300494f970 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 13 Sep 2017 12:00:45 +0200 Subject: [PATCH] GGP: allow for negative deltas --- src/subtasks/generate_grasp_pose.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index 81f523aa..170054e4 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -74,7 +74,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delt bool moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ - return current_angle_ > 0; + return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; } namespace { @@ -143,7 +143,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ tf::poseEigenToMsg(object_pose_eigen, object_pose); - while(current_angle_ < 2*M_PI){ + while( canCompute() ){ if( remaining_time_ <= 0.0 ){ std::cout << "computed angle " << current_angle_ << " with " << previous_solutions_.size()