add angle delta for generate grasp poses

This commit is contained in:
v4hn 2017-03-02 17:21:36 +01:00
parent 19c351b27d
commit 798f9c0839
3 changed files with 13 additions and 1 deletions

View File

@ -30,6 +30,8 @@ public:
void setTimeout(double timeout); void setTimeout(double timeout);
void setAngleDelta(double delta);
protected: protected:
std::string group_; std::string group_;
@ -39,6 +41,8 @@ protected:
double timeout_; double timeout_;
double angle_delta_;
/* temp values */ /* temp values */
double current_angle_; double current_angle_;

View File

@ -18,6 +18,7 @@
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name) moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
: moveit::task_constructor::SubTask::SubTask(name), : moveit::task_constructor::SubTask::SubTask(name),
timeout_(0.1), timeout_(0.1),
angle_delta_(0.1),
current_angle_(0.0), current_angle_(0.0),
remaining_time_(timeout_), remaining_time_(timeout_),
tried_current_state_as_seed_(false) tried_current_state_as_seed_(false)
@ -48,6 +49,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setTimeout(double timeout
remaining_time_= timeout; remaining_time_= timeout;
} }
void
moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delta){
angle_delta_= delta;
}
bool bool
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
@ -100,7 +106,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
while(current_angle_ < 2*M_PI){ while(current_angle_ < 2*M_PI){
if( remaining_time_ <= 0.0 ){ if( remaining_time_ <= 0.0 ){
current_angle_+= 0.2; current_angle_+= angle_delta_;
remaining_time_= timeout_; remaining_time_= timeout_;
tried_current_state_as_seed_= false; tried_current_state_as_seed_= false;
previous_solutions_.clear(); previous_solutions_.clear();

View File

@ -41,6 +41,8 @@ int main(int argc, char** argv){
st->setEndEffector("s_model_tool0"); st->setEndEffector("s_model_tool0");
st->setGroup("arm"); st->setGroup("arm");
st->setObject("object"); st->setObject("object");
st->setTimeout(0.5);
st->setAngleDelta(0.1);
t.addStart(st); t.addStart(st);