diff --git a/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst b/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst index 8db5aa8246..0738577e92 100644 --- a/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst +++ b/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst @@ -166,6 +166,7 @@ Open ``mtc_node.cpp`` in your editor of choice, and paste in the following code. geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; + pose.orientation.w = 1.0; object.pose = pose; moveit::planning_interface::PlanningSceneInterface psi; @@ -826,6 +827,7 @@ We then create a ``ComputeIK`` stage and pass it our ``GeneratePlacePose`` stage geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; + target_pose_msg.pose.orientation.w = 1.0; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage diff --git a/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp b/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp index d168f73a75..8d0e2e76e3 100644 --- a/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp +++ b/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp @@ -58,6 +58,7 @@ void MTCTaskNode::setupPlanningScene() geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; + pose.orientation.w = 1.0; object.pose = pose; moveit::planning_interface::PlanningSceneInterface psi; @@ -287,6 +288,7 @@ mtc::Task MTCTaskNode::createTask() geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; + target_pose_msg.pose.orientation.w = 1.0; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage