diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index 7c22a676..fe54c089 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -30,6 +30,8 @@ public: void setTimeout(double timeout); + void setAngleDelta(double delta); + protected: std::string group_; @@ -39,6 +41,8 @@ protected: double timeout_; + double angle_delta_; + /* temp values */ double current_angle_; diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index cb134d12..eebdc185 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -18,6 +18,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name) : moveit::task_constructor::SubTask::SubTask(name), timeout_(0.1), + angle_delta_(0.1), current_angle_(0.0), remaining_time_(timeout_), tried_current_state_as_seed_(false) @@ -48,6 +49,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout remaining_time_= timeout; } +void +moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delta){ + angle_delta_= delta; +} + bool moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ @@ -100,7 +106,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){ while(current_angle_ < 2*M_PI){ if( remaining_time_ <= 0.0 ){ - current_angle_+= 0.2; + current_angle_+= angle_delta_; remaining_time_= timeout_; tried_current_state_as_seed_= false; previous_solutions_.clear(); diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index a3e95b97..b84315a8 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -41,6 +41,8 @@ int main(int argc, char** argv){ st->setEndEffector("s_model_tool0"); st->setGroup("arm"); st->setObject("object"); + st->setTimeout(0.5); + st->setAngleDelta(0.1); t.addStart(st);