mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ggp: add max_ik_solutions parameter
With 6DOF planning groups the user often knows an upper limit on how many ik solutions there are. Specifying the limit avoids looking for additional IK solutions until timeout.
This commit is contained in:
parent
8501bb8ae1
commit
f1039b8c14
@ -38,6 +38,8 @@ public:
|
|||||||
|
|
||||||
void setAngleDelta(double delta);
|
void setAngleDelta(double delta);
|
||||||
|
|
||||||
|
void setMaxIKSolutions(uint32_t n);
|
||||||
|
|
||||||
void ignoreCollisions(bool flag);
|
void ignoreCollisions(bool flag);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
@ -49,6 +51,8 @@ protected:
|
|||||||
|
|
||||||
double grasp_offset_;
|
double grasp_offset_;
|
||||||
|
|
||||||
|
uint32_t max_ik_solutions_;
|
||||||
|
|
||||||
bool ignore_collisions_;
|
bool ignore_collisions_;
|
||||||
|
|
||||||
std::string gripper_grasp_pose_;
|
std::string gripper_grasp_pose_;
|
||||||
|
|||||||
@ -19,6 +19,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::st
|
|||||||
: moveit::task_constructor::SubTask::SubTask(name),
|
: moveit::task_constructor::SubTask::SubTask(name),
|
||||||
timeout_(0.1),
|
timeout_(0.1),
|
||||||
angle_delta_(0.1),
|
angle_delta_(0.1),
|
||||||
|
max_ik_solutions_(0),
|
||||||
current_angle_(0.0),
|
current_angle_(0.0),
|
||||||
grasp_offset_(0.0),
|
grasp_offset_(0.0),
|
||||||
ignore_collisions_(false),
|
ignore_collisions_(false),
|
||||||
@ -71,6 +72,10 @@ moveit::task_constructor::subtasks::GenerateGraspPose::setAngleDelta(double delt
|
|||||||
angle_delta_= delta;
|
angle_delta_= delta;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
moveit::task_constructor::subtasks::GenerateGraspPose::setMaxIKSolutions(uint32_t n){
|
||||||
|
max_ik_solutions_= n;
|
||||||
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
||||||
@ -144,7 +149,7 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
tf::poseEigenToMsg(object_pose_eigen, object_pose);
|
tf::poseEigenToMsg(object_pose_eigen, object_pose);
|
||||||
|
|
||||||
while( canCompute() ){
|
while( canCompute() ){
|
||||||
if( remaining_time_ <= 0.0 ){
|
if( remaining_time_ <= 0.0 || (max_ik_solutions_ != 0 && previous_solutions_.size() >= max_ik_solutions_)){
|
||||||
std::cout << "computed angle " << current_angle_
|
std::cout << "computed angle " << current_angle_
|
||||||
<< " with " << previous_solutions_.size()
|
<< " with " << previous_solutions_.size()
|
||||||
<< " cached ik solutions"
|
<< " cached ik solutions"
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user