mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Improve comments for pick-and-place task (#238)
This commit is contained in:
parent
fbc05e4496
commit
702710dec5
@ -144,6 +144,7 @@ void PickPlaceTask::loadParameters() {
|
||||
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
||||
}
|
||||
|
||||
// Initialize the task pipeline, defining individual movement stages
|
||||
bool PickPlaceTask::init() {
|
||||
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
||||
const std::string object = object_name_;
|
||||
@ -153,10 +154,13 @@ bool PickPlaceTask::init() {
|
||||
task_.reset();
|
||||
task_.reset(new moveit::task_constructor::Task());
|
||||
|
||||
// Individual movement stages are collected within the Task object
|
||||
Task& t = *task_;
|
||||
t.stages()->setName(task_name_);
|
||||
t.loadRobotModel();
|
||||
|
||||
/* Create planners used in various stages. Various options are available,
|
||||
namely Cartesian, MoveIt pipeline, and joint interpolation. */
|
||||
// Sampling planner
|
||||
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
|
||||
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
|
||||
@ -201,7 +205,7 @@ bool PickPlaceTask::init() {
|
||||
* *
|
||||
***************************************************/
|
||||
Stage* initial_state_ptr = nullptr;
|
||||
{ // Open Hand
|
||||
{
|
||||
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
||||
stage->setGroup(hand_group_name_);
|
||||
stage->setGoal(hand_open_pose_);
|
||||
@ -214,7 +218,8 @@ bool PickPlaceTask::init() {
|
||||
* Move to Pick *
|
||||
* *
|
||||
***************************************************/
|
||||
{ // Move-to pre-grasp
|
||||
// Connect initial open-hand state with pre-grasp pose defined in the following
|
||||
{
|
||||
auto stage = std::make_unique<stages::Connect>(
|
||||
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
||||
stage->setTimeout(5.0);
|
||||
@ -229,6 +234,7 @@ bool PickPlaceTask::init() {
|
||||
***************************************************/
|
||||
Stage* pick_stage_ptr = nullptr;
|
||||
{
|
||||
// A SerialContainer combines several sub-stages, here for picking the object
|
||||
auto grasp = std::make_unique<SerialContainer>("pick object");
|
||||
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
|
||||
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
|
||||
@ -237,10 +243,11 @@ bool PickPlaceTask::init() {
|
||||
---- * Approach Object *
|
||||
***************************************************/
|
||||
{
|
||||
// Move the eef link forward along its z-axis by an amount within the given min-max range
|
||||
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
|
||||
stage->properties().set("marker_ns", "approach_object");
|
||||
stage->properties().set("link", hand_frame_);
|
||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||
stage->properties().set("link", hand_frame_); // link to perform IK for
|
||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage
|
||||
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
|
||||
|
||||
// Set hand forward direction
|
||||
@ -255,22 +262,23 @@ bool PickPlaceTask::init() {
|
||||
---- * Generate Grasp Pose *
|
||||
***************************************************/
|
||||
{
|
||||
// Sample grasp pose
|
||||
// Sample grasp pose candidates in angle increments around the z-axis of the object
|
||||
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||
stage->properties().configureInitFrom(Stage::PARENT);
|
||||
stage->properties().set("marker_ns", "grasp_pose");
|
||||
stage->setPreGraspPose(hand_open_pose_);
|
||||
stage->setObject(object);
|
||||
stage->setObject(object); // object to sample grasps for
|
||||
stage->setAngleDelta(M_PI / 12);
|
||||
stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
||||
|
||||
// Compute IK
|
||||
// Compute IK for sampled grasp poses
|
||||
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
|
||||
wrapper->setMaxIKSolutions(8);
|
||||
wrapper->setMaxIKSolutions(8); // limit number of solutions
|
||||
wrapper->setMinSolutionDistance(1.0);
|
||||
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
|
||||
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
||||
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
||||
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); // define virtual frame to reach the target_pose
|
||||
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent
|
||||
wrapper->properties().configureInitFrom(Stage::INTERFACE,
|
||||
{ "target_pose" }); // inherit property from child solution
|
||||
grasp->insert(std::move(wrapper));
|
||||
}
|
||||
|
||||
@ -278,6 +286,7 @@ bool PickPlaceTask::init() {
|
||||
---- * Allow Collision (hand object) *
|
||||
***************************************************/
|
||||
{
|
||||
// Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking
|
||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
|
||||
stage->allowCollisions(
|
||||
object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
|
||||
@ -300,7 +309,7 @@ bool PickPlaceTask::init() {
|
||||
***************************************************/
|
||||
{
|
||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
||||
stage->attachObject(object, hand_frame_);
|
||||
stage->attachObject(object, hand_frame_); // attach object to hand_frame_
|
||||
grasp->insert(std::move(stage));
|
||||
}
|
||||
|
||||
@ -352,6 +361,7 @@ bool PickPlaceTask::init() {
|
||||
* *
|
||||
*****************************************************/
|
||||
{
|
||||
// Connect the grasped state to the pre-place state, i.e. realize the object transport
|
||||
auto stage = std::make_unique<stages::Connect>(
|
||||
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
||||
stage->setTimeout(5.0);
|
||||
@ -364,6 +374,7 @@ bool PickPlaceTask::init() {
|
||||
* Place Object *
|
||||
* *
|
||||
*****************************************************/
|
||||
// All placing sub-stages are collected within a serial container again
|
||||
{
|
||||
auto place = std::make_unique<SerialContainer>("place object");
|
||||
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
|
||||
@ -498,13 +509,6 @@ bool PickPlaceTask::execute() {
|
||||
moveit_msgs::MoveItErrorCodes execute_result;
|
||||
|
||||
execute_result = task_->execute(*task_->solutions().front());
|
||||
// // If you want to inspect the goal message, use this instead:
|
||||
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
|
||||
// execute("execute_task_solution", true); execute.waitForServer();
|
||||
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
|
||||
// task_->solutions().front()->toMsg(execute_goal.solution);
|
||||
// execute.sendGoalAndWait(execute_goal);
|
||||
// execute_result = execute.getResult()->error_code;
|
||||
|
||||
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
|
||||
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user