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(); } }