Estimate Robot Geometry using Ellipsoids

.gitignore

@@ -7,3 +7,6 @@ install/
 .venv/
 __pycache__/
 data/
+
+CLAUDE.md
+NOTES.md

potential_fields/config/pfield_config.yaml

@@ -3,12 +3,12 @@ pfield_manager:
     visualize_pf_frequency: 10.0 # [Hz]
     attractive_gain: 3.0 # [Nm]
     rotational_attractive_gain: 0.15 # [Nm/rad]
-    repulsive_gain: 0.6 # [Nm]
+    repulsive_gain: 0.3 # [Nm]
     max_linear_velocity: 0.05 # [m/s]
     max_angular_velocity: 0.4 # [rad/s]
     max_linear_acceleration: 1.0 # [m/s^2]
     max_angular_acceleration: 0.2 # [rad/s^2]
-    influence_distance: 0.2 # Influence distance for obstacle repulsion [m]
+    influence_distance: 0.1 # Influence distance for obstacle repulsion [m]
     visualize_field_vectors: false # Whether to visualize the potential field vectors (Slows down computation if true)
     visualizer_buffer_area: 0.5 # Extra area around obstacles and goal to visualize the PF [m]
     field_resolution: 1.0 # Resolution to show PF velocity arrows in RViz [m]

potential_fields_demos/src/pfield_demo.cpp

@@ -63,7 +63,7 @@ PFDemo::PFDemo() : Node("pfield_demo") {
     rclcpp::sleep_for(std::chrono::milliseconds(100));
   }
   RCLCPP_INFO(this->get_logger(), "Subscribers detected on the /pfield/obstacles topic. Publishing obstacles...");
-  // this->createAndPublishObstacles();
+  this->createAndPublishObstacles();
 
   // Initialize demo service
   this->runPlanPathDemoService = this->create_service<std_srvs::srv::Empty>(
@@ -122,7 +122,7 @@ void PFDemo::handleRunPlanPathDemo(
   pathPlanRequest->delta_time = 0.02; // 20 ms between waypoints
   pathPlanRequest->goal_tolerance = 0.01; // 10 mm tolerance
   pathPlanRequest->max_iterations = 25000; // Max iterations for planning
-  pathPlanRequest->planning_method = PlanPath::Request::PLANNING_METHOD_TASK_SPACE; // "task_space" or "whole_body"
+  pathPlanRequest->planning_method = PlanPath::Request::PLANNING_METHOD_WHOLE_BODY; // "task_space" or "whole_body"
   const double dt = pathPlanRequest->delta_time;
 
   // Publish the goal pose
@@ -205,14 +205,30 @@ void PFDemo::createAndPublishObstacles() {
   // box.height = 0.6;
   // allObstacles.obstacles.push_back(box);
 
-  // Real Cylinder in the way
+  // // Real Cylinder in the way
+  // potential_fields_interfaces::msg::Obstacle cyl;
+  // cyl.frame_id = "pitcher";
+  // cyl.type = "Cylinder";
+  // cyl.group = "Static";
+  // cyl.radius = 0.18 / 2.0;
+  // cyl.height = 0.3;
+  // cyl.pose.position.x = 0.62;
+  // cyl.pose.position.y = 0.00;
+  // cyl.pose.position.z = (cyl.height / 2.0);
+  // cyl.pose.orientation.x = 0.0;
+  // cyl.pose.orientation.y = 0.0;
+  // cyl.pose.orientation.z = 0.0;
+  // cyl.pose.orientation.w = 1.0;
+  // allObstacles.obstacles.push_back(cyl);
+
+  // Fake Cylinder
   potential_fields_interfaces::msg::Obstacle cyl;
   cyl.frame_id = "pitcher";
   cyl.type = "Cylinder";
   cyl.group = "Static";
   cyl.radius = 0.18 / 2.0;
   cyl.height = 0.3;
-  cyl.pose.position.x = 0.62;
+  cyl.pose.position.x = 0.51;
   cyl.pose.position.y = 0.00;
   cyl.pose.position.z = (cyl.height / 2.0);
   cyl.pose.orientation.x = 0.0;

potential_fields_library/include/pfield/pf_obstacle.hpp

@@ -44,7 +44,9 @@ namespace pfield {
     SPHERE, // [center, radius]
     BOX, // [center, length, width, height]
     CYLINDER, // [center, radius, height]
-    MESH // [center, scale_x, scale_y, scale_z]
+    MESH, // [center, scale_x, scale_y, scale_z]
+    ELLIPSOID, // [center, semi_x, semi_y, semi_z] — smooth analytic SDF, used for robot link approximation
+    CAPSULE // [center, radius, height] — cylinder shaft (height) with hemispherical endcaps (radius); total length = height + 2*radius
   };
 
   enum class ObstacleGroup {
@@ -63,6 +65,10 @@ namespace pfield {
       return "Cylinder";
     case ObstacleType::MESH:
       return "Mesh";
+    case ObstacleType::ELLIPSOID:
+      return "Ellipsoid";
+    case ObstacleType::CAPSULE:
+      return "Capsule";
     default:
       throw std::invalid_argument("Unknown obstacle type");
     }
@@ -81,6 +87,12 @@ namespace pfield {
     else if (typeStr == "Mesh") {
       return ObstacleType::MESH;
     }
+    else if (typeStr == "Ellipsoid") {
+      return ObstacleType::ELLIPSOID;
+    }
+    else if (typeStr == "Capsule") {
+      return ObstacleType::CAPSULE;
+    }
     else {
       throw std::invalid_argument("Unknown obstacle type string: " + typeStr);
     }
@@ -119,14 +131,38 @@ namespace pfield {
     double length = 0.0; // Length for box, unused for sphere and cylinder
     double width = 0.0; // Width for box, unused for sphere and cylinder
     double height = 0.0; // Height for cylinder and box, unused for sphere
+    double semi_x = 0.0; // Semi-axis along X for ellipsoid, unused for other types
+    double semi_y = 0.0; // Semi-axis along Y for ellipsoid, unused for other types
+    double semi_z = 0.0; // Semi-axis along Z for ellipsoid, unused for other types
+    // Rotation from obstacle frame to ellipsoid principal-axis frame (from PCA).
+    // Identity for axis-aligned ellipsoids; non-identity when PCA axes differ from mesh origin axes.
+    Eigen::Matrix3d ellipsoid_axes = Eigen::Matrix3d::Identity();
+    // Offset from the collision-origin frame to the ellipsoid center (mesh centroid in mesh-local frame).
+    Eigen::Vector3d centroid_offset = Eigen::Vector3d::Zero();
+    // Original mesh URI and scale for ELLIPSOID obstacles fitted from a mesh, used for ghost visualization.
+    // Empty for non-mesh-derived ellipsoids.
+    std::string source_mesh_resource;
+    Eigen::Vector3d source_mesh_scale = Eigen::Vector3d::Ones();
 
     ObstacleGeometry() = default;
+    // Existing 4-arg constructor — unchanged, semi-axes default to 0
     ObstacleGeometry(double radius, double length, double width, double height)
       : radius(radius), length(length), width(width), height(height) {}
+    // Ellipsoid constructor (axis-aligned, centered at origin)
+    ObstacleGeometry(double semi_x, double semi_y, double semi_z, bool /*ellipsoid_tag*/)
+      : semi_x(semi_x), semi_y(semi_y), semi_z(semi_z) {}
+    // Ellipsoid constructor with PCA axes and centroid offset
+    ObstacleGeometry(double semi_x, double semi_y, double semi_z,
+      const Eigen::Matrix3d& axes, const Eigen::Vector3d& centroid)
+      : semi_x(semi_x), semi_y(semi_y), semi_z(semi_z),
+        ellipsoid_axes(axes), centroid_offset(centroid) {}
 
     bool operator==(const ObstacleGeometry& other) const {
       return radius == other.radius && length == other.length &&
-        width == other.width && height == other.height;
+        width == other.width && height == other.height &&
+        semi_x == other.semi_x && semi_y == other.semi_y && semi_z == other.semi_z &&
+        centroid_offset == other.centroid_offset &&
+        ellipsoid_axes == other.ellipsoid_axes;
     }
     bool operator!=(const ObstacleGeometry& other) const {
       return !(*this == other);
@@ -142,6 +178,10 @@ namespace pfield {
         return {radius, height};
       case ObstacleType::MESH:
         return {length, width, height};
+      case ObstacleType::ELLIPSOID:
+        return {semi_x, semi_y, semi_z};
+      case ObstacleType::CAPSULE:
+        return {radius, height};
       default:
         throw std::invalid_argument("Unknown obstacle type");
       }

potential_fields_library/src/pfield/pf_kinematics.cpp

@@ -14,11 +14,13 @@
 
 #include "pfield/pf_kinematics.hpp"
 #include "pfield/pfield_common.hpp"
+#include "pfield/mesh_collision.hpp"
+#include <coal/BV/OBB.h>
 #include <pinocchio/fwd.hpp>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
-#include <Eigen/Geometry>
+#include <Eigen/Dense>
 
 namespace pfield {
 
@@ -210,8 +212,6 @@ namespace pfield {
     double length = 0.0;
     double width = 0.0;
     double height = 0.0;
-    bool isMesh = false;
-    std::string meshResource;
 
     auto* geometry = collisionObject.geometry.get();
     if (urdf::Box* b = dynamic_cast<urdf::Box*>(geometry)) {
@@ -230,26 +230,104 @@ namespace pfield {
       height = c->length;
     }
     else if (urdf::Mesh* m = dynamic_cast<urdf::Mesh*>(geometry)) {
-      obstacleType = ObstacleType::MESH;
-      length = m->scale.x;
-      width = m->scale.y;
-      height = m->scale.z;
-      isMesh = true;
-      meshResource = m->filename;
+      // Fit a minimum-volume oriented bounding box (OBB) to the mesh using PCA:
+      //   Pass 1 — PCA on all scaled vertices to find the principal-axis orientation.
+      //   Pass 2 — Refit full extents as 2 * max |projection| onto each PCA axis.
+      // ellipsoid_axes stores the OBB rotation; centroid_offset stores the OBB center.
+      // For SDF and Coal, we use a standard BOX (length/width/height = full extents).
+      const double sx = m->scale.x > 0.0 ? m->scale.x : 1.0;
+      const double sy = m->scale.y > 0.0 ? m->scale.y : 1.0;
+      const double sz = m->scale.z > 0.0 ? m->scale.z : 1.0;
+      const Eigen::Vector3d scale(sx, sy, sz);
+
+      double half_x = 0.0, half_y = 0.0, half_z = 0.0;
+      Eigen::Matrix3d obbAxes = Eigen::Matrix3d::Identity();
+      Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
+      bool fitted = false;
+
+      try {
+        auto meshData = loadMesh(m->filename);
+        if (meshData && !meshData->vertices.empty()) {
+          const size_t nVerts = meshData->vertices.size();
+
+          // Pass 1a: centroid of scaled vertices
+          for (const auto& v : meshData->vertices) {
+            centroid += v.cwiseProduct(scale);
+          }
+          centroid /= static_cast<double>(nVerts);
+
+          // Pass 1b: covariance matrix → PCA axes (dominant axis first)
+          Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
+          for (const auto& v : meshData->vertices) {
+            const Eigen::Vector3d vc = v.cwiseProduct(scale) - centroid;
+            cov += vc * vc.transpose();
+          }
+          cov /= static_cast<double>(nVerts);
+          Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
+          obbAxes = solver.eigenvectors().rowwise().reverse();
+
+          // Pass 2: half-extents = max |projection| onto each PCA axis
+          for (const auto& v : meshData->vertices) {
+            const Eigen::Vector3d proj = obbAxes.transpose() * (v.cwiseProduct(scale) - centroid);
+            half_x = std::max(half_x, std::abs(proj.x()));
+            half_y = std::max(half_y, std::abs(proj.y()));
+            half_z = std::max(half_z, std::abs(proj.z()));
+          }
+          fitted = true;
+        }
+      }
+      catch (const std::exception&) {
+        // Mesh load failed; fall through to scale-based fallback
+      }
+
+      if (!fitted) {
+        half_x = sx / 2.0;
+        half_y = sy / 2.0;
+        half_z = sz / 2.0;
+      }
+
+      const double minHalf = 1e-3;
+      half_x = std::max(half_x, minHalf);
+      half_y = std::max(half_y, minHalf);
+      half_z = std::max(half_z, minHalf);
+
+      // Capsule fitting:
+      //   PCA axis 0 (half_x) = dominant axis → becomes capsule Z axis.
+      //   radius = max of the two transverse semi-extents (half_y, half_z).
+      //   shaft length = max(0, 2*half_x - 2*radius) so endcaps don't overshoot.
+      // The rotation matrix obbAxes maps capsule-Z (axis 0) to the mesh principal axis.
+      // We need capsule-Z = column 0 of obbAxes, so we build a rotation that maps
+      // world-Z to obbAxes[:,0] — which is just using obbAxes as-is since column 0
+      // already IS the dominant axis direction, and our SDF uses the Z axis in capsule frame.
+      // We swap columns so that column 2 (capsule Z) = the dominant PCA axis.
+      Eigen::Matrix3d capsuleAxes;
+      capsuleAxes.col(0) = obbAxes.col(1);  // minor axis → capsule X
+      capsuleAxes.col(1) = obbAxes.col(2);  // minor axis → capsule Y
+      capsuleAxes.col(2) = obbAxes.col(0);  // dominant axis → capsule Z (SDF axis)
+      // Ensure right-handed
+      if (capsuleAxes.determinant() < 0.0) { capsuleAxes.col(0) = -capsuleAxes.col(0); }
+
+      const double capsuleRadius = std::max(half_y, half_z);
+      const double capsuleShaft = std::max(0.0, 2.0 * half_x - 2.0 * capsuleRadius);
+
+      // 4-arg ctor: (radius, length, width, height) — use radius + height for capsule
+      ObstacleGeometry capsuleGeom(capsuleRadius, 0.0 /*length unused*/, 0.0 /*width unused*/, capsuleShaft);
+      capsuleGeom.ellipsoid_axes = capsuleAxes;
+      capsuleGeom.centroid_offset = centroid;
+      capsuleGeom.source_mesh_resource = m->filename;
+      capsuleGeom.source_mesh_scale = scale;
+      return PotentialFieldObstacle(
+        frameID, position, orientation, ObstacleType::CAPSULE, ObstacleGroup::ROBOT,
+        capsuleGeom
+      );
     }
     else {
       throw std::runtime_error("obstacleFromCollisionObject: Unsupported geometry type in collision object");
     }
     ObstacleGeometry obstacleGeom(radius, length, width, height);
-
-    // For mesh obstacles, pass mesh resource and scale to constructor
-    PotentialFieldObstacle obstacle(
-      frameID, position, orientation, obstacleType, ObstacleGroup::ROBOT,
-      obstacleGeom,
-      isMesh ? meshResource : std::string(),
-      isMesh ? Eigen::Vector3d(length, width, height) : Eigen::Vector3d::Ones()
+    return PotentialFieldObstacle(
+      frameID, position, orientation, obstacleType, ObstacleGroup::ROBOT, obstacleGeom
     );
-    return obstacle;
   }
 
   std::vector<PotentialFieldObstacle> PFKinematics::updateObstaclesFromJointAngles(const std::vector<double>& jointAngles) {