diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp
index 8e2aafc..755fbe2 100644
--- a/rbs_skill_servers/src/pick_place_pose_loader.cpp
+++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp
@@ -67,26 +67,29 @@ GetGraspPlacePoseServer::collect_pose(
     Eigen::Vector3d posedir;
     Eigen::Affine3d pose_ = pose;
     tf2::fromMsg(direction, posedir);
-    std::cout << "\nPoseDir: =" << posedir << std::endl;
-    Eigen::Matrix3d matIx = posedir * posedir.transpose();
-    Eigen::Matrix3d matIZ = Eigen::Vector3d::UnitZ() * Eigen::Vector3d::UnitZ().transpose();
+    Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
+    Ixy.diagonal() = posedir;
+    Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
+    Iz.diagonal() = Eigen::Vector3d::UnitZ();
+
     if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
     {
         std::cout << "\n TBD" << std::endl;
     } 
     else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
     {
-        pose_.pretranslate(-(matIx * scale_vec));
+        // Gp -- grasp point frame
+        pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
         pose_v_.push_back(tf2::toMsg(pose_));
-        pose_.pretranslate(matIZ * scale_vec);
+        pose_.pretranslate(Iz * scale_vec);     // Gp-y + z
         pose_v_.push_back(tf2::toMsg(pose_));
-        pose_.pretranslate(matIx * scale_vec);
+        pose_.pretranslate(Ixy * scale_vec);    // Gp+z
         pose_v_.push_back(tf2::toMsg(pose_));
     } 
     else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
     {
-        std::cout << "\npreplace = \n" << pose_.pretranslate(matIx * scale_vec).translation() << std::endl;
-        pose_v_.push_back(tf2::toMsg(pose_));
+        pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
+        pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
     } 
     else 
     {