add calc grasp pose

src/brain/CMakeLists.txt

@@ -37,6 +37,7 @@ add_executable(brain_node
   src/cerebellum_hooks.cpp
   src/cerebrum_node.cpp
   src/skill_manager.cpp
+  src/calc_grasp_pose.cpp
 )
 
 target_include_directories(brain_node PUBLIC

src/brain/include/brain/calc_grasp_pose.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Transform.h>
+#include <string>
+#include <vector>
+
+namespace brain {
+
+struct Pose {
+    tf2::Vector3 position;
+    tf2::Quaternion orientation;
+};
+
+struct GraspResult {
+    Pose pre_grasp_pose;
+    Pose grasp_pose;
+};
+
+class GraspPoseCalculator {
+public:
+    static GraspResult calculate(
+        const tf2::Vector3& target_pos,
+        const tf2::Quaternion& target_quat,
+        const std::string& grasp_type = "side",
+        double safety_dist = 0.06,
+        double finger_length = 0.0,
+        const std::string& arm = "right"
+    );
+};
+
+} // namespace brain

src/brain/include/brain/cerebellum_node.hpp

@@ -20,6 +20,7 @@
 // For from_yaml<T>(YAML::Node)
 #include "brain/payload_converters.hpp"
 #include "brain/robot_config.hpp"
+#include "brain/calc_grasp_pose.hpp"
 
 namespace brain
 {
@@ -41,6 +42,11 @@ public:
    */
   explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
+  std::vector<geometry_msgs::msg::Pose> side_pre_grasp_poses_;
+  std::vector<geometry_msgs::msg::Pose> side_grasp_poses_;
+  std::vector<geometry_msgs::msg::Pose> top_pre_grasp_poses_;
+  std::vector<geometry_msgs::msg::Pose> top_grasp_poses_;
+
 private:
   struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
   std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;

src/brain/src/calc_grasp_pose.cpp

@@ -0,0 +1,75 @@
+#include "brain/calc_grasp_pose.hpp"
+#include <cmath>
+#include <iostream>
+
+namespace brain {
+
+tf2::Vector3 normalize_vector(const tf2::Vector3& v) {
+    if (v.length() == 0) return v;
+    return v.normalized();
+}
+
+GraspResult GraspPoseCalculator::calculate(
+    const tf2::Vector3& target_pos,
+    const tf2::Quaternion& target_quat,
+    const std::string& grasp_type,
+    double safety_dist,
+    double finger_length,
+    const std::string& arm
+) {
+    tf2::Transform T_target(target_quat, target_pos);
+    (void) arm;
+    
+    tf2::Vector3 V_world_up(-1.0, 0.0, 0.0);
+    tf2::Vector3 V_world_down = -V_world_up; // [1, 0, 0]
+    
+    tf2::Vector3 P_shoulder(0, 0, 0);
+    tf2::Vector3 P_target = target_pos;
+    tf2::Vector3 V_sb = P_target - P_shoulder;
+    tf2::Transform T_grasp;
+    T_grasp.setIdentity();
+    T_grasp.setOrigin(P_target);
+    
+    tf2::Vector3 z_axis, y_axis, x_axis;
+
+    if (grasp_type == "side") {
+        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
+        z_axis = normalize_vector(V_horiz);
+        x_axis = normalize_vector(V_world_down);
+        y_axis = z_axis.cross(x_axis);
+        x_axis = y_axis.cross(z_axis);
+    } else if (grasp_type == "top") {
+        z_axis = normalize_vector(V_world_down);
+        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
+        x_axis = normalize_vector(V_horiz);
+        y_axis = z_axis.cross(x_axis);
+        x_axis = y_axis.cross(z_axis);
+    } else {
+        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
+        z_axis = normalize_vector(V_horiz);
+        x_axis = normalize_vector(V_world_down);
+        y_axis = z_axis.cross(x_axis);
+        x_axis = y_axis.cross(z_axis);
+    }
+
+    tf2::Matrix3x3 R_grasp(
+        x_axis.x(), y_axis.x(), z_axis.x(),
+        x_axis.y(), y_axis.y(), z_axis.y(),
+        x_axis.z(), y_axis.z(), z_axis.z()
+    );
+    
+    T_grasp.setBasis(R_grasp);
+    T_grasp.setOrigin(T_grasp.getOrigin() - z_axis * finger_length);
+    tf2::Transform T_pre = T_grasp;
+    T_pre.setOrigin(T_pre.getOrigin() - z_axis * safety_dist);
+    
+    GraspResult result;
+    result.grasp_pose.position = T_grasp.getOrigin();
+    result.grasp_pose.orientation = T_grasp.getRotation();
+    result.pre_grasp_pose.position = T_pre.getOrigin();
+    result.pre_grasp_pose.orientation = T_pre.getRotation();
+    
+    return result;
+}
+
+} // namespace brain

src/brain/src/cerebellum_hooks.cpp

@@ -557,6 +557,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
           skill_name.c_str(), resp->success, resp->objects.size(), 
           resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
 
+        node->side_pre_grasp_poses_.clear();
+        node->side_grasp_poses_.clear();
+        node->top_pre_grasp_poses_.clear();
+        node->top_grasp_poses_.clear();
+
         for (size_t i = 0; i < resp->objects.size(); ++i) {
           const auto & obj = resp->objects[i];
           if (node->target_frame_ == obj.class_name) {
@@ -597,20 +602,105 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
               pose_in_arm.pose.orientation.z,
               pose_in_arm.pose.orientation.w);
 
-            geometry_msgs::msg::Pose pose_grasp;
-            pose_grasp.position = pose_in_arm.pose.position;
-            pose_grasp.orientation = node->final_arm_pose_["right_arm"].orientation;
+            tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
+            tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, 
+              pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
 
+            auto result = brain::GraspPoseCalculator::calculate(
+                target_pos, 
+                target_quat, 
+                "side", 
+                0.06, // safety_dist
+                0.0   // finger_length
+            );
+
+            geometry_msgs::msg::Pose pre_grasp_msg;
+            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
+            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
+            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
+            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
+            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
+            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
+            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
+            node->side_pre_grasp_poses_.push_back(pre_grasp_msg);
+
+            geometry_msgs::msg::Pose grasp_msg;
+            grasp_msg.position.x = result.grasp_pose.position.x();
+            grasp_msg.position.y = result.grasp_pose.position.y();
+            grasp_msg.position.z = result.grasp_pose.position.z();
+            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
+            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
+            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
+            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
+            node->side_grasp_poses_.push_back(grasp_msg);
+
+            RCLCPP_INFO(node->get_logger(), "Pre Grasp Pose(side): ");
+            RCLCPP_INFO(node->get_logger(),
+              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+              result.pre_grasp_pose.position.x(),
+              result.pre_grasp_pose.position.y(),
+              result.pre_grasp_pose.position.z(),
+              result.pre_grasp_pose.orientation.x(),
+              result.pre_grasp_pose.orientation.y(),
+              result.pre_grasp_pose.orientation.z(),
+              result.pre_grasp_pose.orientation.w());
             RCLCPP_INFO(node->get_logger(), "Grasp Pose: ");
             RCLCPP_INFO(node->get_logger(),
               "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              pose_grasp.position.x,
-              pose_grasp.position.y,
-              pose_grasp.position.z,
-              pose_grasp.orientation.x,
-              pose_grasp.orientation.y,
-              pose_grasp.orientation.z,
-              pose_grasp.orientation.w);
+              result.grasp_pose.position.x(),
+              result.grasp_pose.position.y(),
+              result.grasp_pose.position.z(),
+              result.grasp_pose.orientation.x(),
+              result.grasp_pose.orientation.y(),
+              result.grasp_pose.orientation.z(),
+              result.grasp_pose.orientation.w());
+
+            result = brain::GraspPoseCalculator::calculate(
+                target_pos, 
+                target_quat, 
+                "top", 
+                0.06, // safety_dist
+                0.0   // finger_length
+            );
+
+            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
+            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
+            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
+            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
+            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
+            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
+            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
+            node->top_pre_grasp_poses_.push_back(pre_grasp_msg);
+
+            grasp_msg.position.x = result.grasp_pose.position.x();
+            grasp_msg.position.y = result.grasp_pose.position.y();
+            grasp_msg.position.z = result.grasp_pose.position.z();
+            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
+            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
+            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
+            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
+            node->top_grasp_poses_.push_back(grasp_msg);
+
+            RCLCPP_INFO(node->get_logger(), "Pre Grasp Pose(top): ");
+            RCLCPP_INFO(node->get_logger(),
+              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+              result.pre_grasp_pose.position.x(),
+              result.pre_grasp_pose.position.y(),
+              result.pre_grasp_pose.position.z(),
+              result.pre_grasp_pose.orientation.x(),
+              result.pre_grasp_pose.orientation.y(),
+              result.pre_grasp_pose.orientation.z(),
+              result.pre_grasp_pose.orientation.w());
+            RCLCPP_INFO(node->get_logger(), "Grasp Pose: ");
+            RCLCPP_INFO(node->get_logger(),
+              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+              result.grasp_pose.position.x(),
+              result.grasp_pose.position.y(),
+              result.grasp_pose.position.z(),
+              result.grasp_pose.orientation.x(),
+              result.grasp_pose.orientation.y(),
+              result.grasp_pose.orientation.z(),
+              result.grasp_pose.orientation.w());
           }
         }
         return true;