void MotionPlanningFrame::computePlanButtonClicked()
{
if (!move_group_)
return;
// Clear status
ui_->result_label->setText("Planning...");
configureForPlanning();
current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
if (move_group_->plan(*current_plan_))
{
ui_->execute_button->setEnabled(true);
// Success
ui_->result_label->setText(QString("Time: ").append(
QString::number(current_plan_->planning_time_,'f',3)));
}
else
{
current_plan_.reset();
// Failure
ui_->result_label->setText("Failed");
}
}
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;
}
void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
{
if (!move_group_)
return;
configureForPlanning();
move_group_->move();
ui_->plan_and_execute_button->setEnabled(true);
}
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;
}
void MotionPlanningFrame::computePlanGoalsButtonClicked()
{
if (!move_group_)
return;
// Clear status
ui_->result_label->setText("Planning...");
// Reset the current plan.
current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
// The primitive plan is used for actual execution on the robot. HACK
// We reset it here.
primitive_plan_.reset(new apc_msgs::PrimitivePlan);
// Get the list of goals (waypoints) to follow.
QListWidget* goals_list = ui_->active_goals_list;
// Get the current start state.
robot_state::RobotState start_state = *planning_display_->getQueryStartState();
// The target goal state will be initialized to the start state.
robot_state::RobotState goal_state = start_state;
// For each item in the active goals list, configure for planning and then
// append to the plan.
for (int i = 0; i < goals_list->count(); i++)
{
// First get the plan represented by the item.
apc_msgs::PrimitivePlan plan =
getMessageFromUserData<apc_msgs::PrimitivePlan>(goals_list->item(i)->data(Qt::UserRole));
// Loop through the actions in the plan.
for (int j = 0; j < plan.actions.size(); j++)
{
// Get the last action from the user data because it is the goal state.
apc_msgs::PrimitiveAction action = plan.actions.back();
// Get the goal robot state from user data.
getStateFromAction(goal_state, action);
// HACK Reset move group so that I can plan with a different group... SMH.
changePlanningGroupHelper(action.group_name);
planning_display_->waitForAllMainLoopJobs(); // I hope there are no cyclic main job loops.
// Set move group variables, like start and goal states, etc.
configureForPlanning(start_state, goal_state);
// Compute plan specific to the current primitive action.
moveit::planning_interface::MoveGroup::Plan move_group_plan;
// Make a planning service call. This will append any plans to the input.
if (!move_group_->plan(move_group_plan))
{
ui_->result_label->setText("Failed");
current_plan_.reset();
return;
}
// Copy plan over to primitive action.
action.joint_trajectory = move_group_plan.trajectory_.joint_trajectory;
action.duration = move_group_plan.planning_time_;
// TODO Eef trajectory support.
// TODO Dense trajectory support.
// HACK If we are planning for the hands, overwrite plan with linear interpolation.
if (action.group_name == "crichton_left_hand" ||
action.group_name == "crichton_right_hand")
{
computeLinearInterpPlan(start_state, action);
move_group_plan.trajectory_.joint_trajectory = action.joint_trajectory;
}
// Append plan onto the current plan.
move_group_->appendPlan(*current_plan_, move_group_plan);
// Append action onto primitive plan.
primitive_plan_->actions.push_back(action);
// Start the next plan from this goal.
start_state = goal_state;
}
}
// Success
ui_->execute_button->setEnabled(true);
ui_->result_label->setText(QString("Time: ").append(
QString::number(current_plan_->planning_time_,'f',3)));
// Copy trajectory over to display.
{
// Get a robot model.
const robot_model::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
// Construct a new robot trajectory.
robot_trajectory::RobotTrajectoryPtr display_trajectory(new robot_trajectory::RobotTrajectory(robot_model, ""));
// Copy current plan over to robot trajectory.
display_trajectory->setRobotTrajectoryMsg(planning_display_->getPlanningSceneRO()->getCurrentState(),
current_plan_->start_state_,
current_plan_->trajectory_);
// Swap the plan trajectory into our planning display.
planning_display_->setTrajectoryToDisplay(display_trajectory);
// Display trail. FIXME This doesn't accomplish anything actually.
previewButtonClicked();
}
}
