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 cf45286a90..a4e6613d7c 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 @@ -696,7 +696,7 @@ Before we compute inverse kinematics for the poses generated above, we first nee .. code-block:: c++ - Eigen::Isometry3d grasp_frame_transform; + Eigen::Isometry3d grasp_frame_transform = Eigen::Isometry3d::Identity(); Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()); 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 f16374e97d..89aee13201 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 @@ -188,7 +188,7 @@ mtc::Task MTCTaskNode::createTask() stage->setMonitoredStage(current_state_ptr); // Hook into current state // This is the transform from the object frame to the end-effector frame - Eigen::Isometry3d grasp_frame_transform; + Eigen::Isometry3d grasp_frame_transform = Eigen::Isometry3d::Identity(); Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());