bool Action::graspPlan(MetaBlock *block, const std::string surface_name) //computePlanButtonClicked
{
bool success(false);
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Planning " << block->name << " at pose " << block->start_pose);
move_group_->setGoalTolerance(0.1);//0.05 //TODO
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
if (!move_group_)
return false;
//move_group_->setApproximateJointValueTargets(target, move_group_->getEndEffectorLink().c_str());
move_group_->setPoseTargets(configureForPlanning(generateGrasps(block)), move_group_->getEndEffectorLink().c_str());
current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
success = move_group_->plan(*current_plan_);
if (!success)
current_plan_.reset();
if (verbose_ && success)
ROS_INFO_STREAM_NAMED("pick_place","Grasp plannin success! \n\n");
return success;
}
bool Action::pickAction(MetaBlock *block,
const std::string surface_name,
int attempts_nbr,
double planning_time,
double tolerance_min)
{
bool success(false);
//if (verbose_)
ROS_INFO_STREAM("Pick at pose " << block->start_pose.position.x << " " << block->start_pose.position.y << " " << block->start_pose.position.z);
if (attempts_nbr == 0)
attempts_nbr = attempts_max_;
if (planning_time == 0.0)
planning_time = planning_time_;
if (tolerance_min == 0.0)
tolerance_min = tolerance_min_;
std::vector<moveit_msgs::Grasp> grasps = generateGrasps(block);
if (grasps.size() > 0)
{
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
move_group_->setPlanningTime(planning_time); //30.0);
//move_group_->setPlannerId("RRTConnectkConfigDefault");
//move_group_->setPlannerId("RRTConnect");
double tolerance_min = 0.01;
double tolerance_max = 0.5;
double tolerance = tolerance_min;
int attempts = 0;
//find a planning solution while increasing tolerance
while (!success && (attempts < attempts_max_))
{
move_group_->setGoalTolerance(tolerance);//0.05 //TODO to check
success = move_group_->pick(block->name, grasps);
if (!success)
{
tolerance_min = tolerance;
tolerance = (tolerance_max-tolerance_min) / 2.0;
}
++attempts;
}
if (verbose_ & success)
ROS_INFO_STREAM("Pick success with tolerance " << tolerance << "\n\n");
}
return success;
}
void Action::filterGrasps(MetaBlock *block)
{
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Picking " << block->name << " at pose " << block->start_pose);
std::vector<moveit_msgs::Grasp> grasps = generateGrasps(block);
if (grasps.size() > 0)
{
grasp_filter_.filterGrasps(grasps);
}
}
bool Action::graspPlanAllPossible(MetaBlock *block, const std::string surface_name) //computePlanButtonClicked
{
bool success(false);
if (verbose_)
ROS_INFO_STREAM("Planning all possible grasps to " << block->start_pose);
move_group_->setGoalTolerance(0.1);
//move_group_->setPoseReferenceFrame("base_link"); //"LWristYaw_link"
//move_group_->setStartState(*move_group_->getCurrentState());
//move_group_->setPlanningTime(30); //in GUI=5
//move_group_->setNumPlanningAttempts(10); //in GUI=10
//move_group_->setMaxVelocityScalingFactor(1.0); //in GUI = 1.0
//move_group_->setPlannerId("RRTConnectkConfigDefault");
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
std::vector<geometry_msgs::Pose> targets = configureForPlanning(generateGrasps(block));
moveit::planning_interface::MoveGroup::Plan plan;
if (targets.size() > 0)
{
int counts = 0;
for (std::vector<geometry_msgs::Pose>::iterator it=targets.begin(); it!=targets.end();++it)
{
//move_group_->setPoseTarget(*it, move_group_->getEndEffectorLink().c_str());
//move_group_->setPositionTarget(it->position.x, it->position.y, it->position.z, move_group_->getEndEffectorLink().c_str());
success = move_group_->setApproximateJointValueTarget(*it, move_group_->getEndEffectorLink().c_str());
success = move_group_->plan(plan);
if (success)
++counts;
}
if (verbose_)
ROS_INFO_STREAM( "Planning success for " << counts << " generated poses! \n\n");
}
return success;
}