Fix "Extentinator"

[Sharwin24]

Extentinator (Extent Estimator)

The "Extentinator" is the extent estimator. The purpose of this function is to compute an optimal influenceDistance to use for obstacle avoidance.

Currently the estimateRobotExtentRadius function calculates a conservative bounding sphere radius for the robot to be used as a default influence distance for the potential field following these steps:

• Iterating through all collision geometries defined in the robot's URDF.
• Computing the world position of each collision object assuming the robot is in its "zero" configuration (all joint angles at 0).
• Calculating a local bounding radius for each geometry type (Box, Sphere, Cylinder). For Meshes, it currently uses a heuristic based on scale and a hardcoded fallback radius (0.3m) without loading the actual mesh data.
• Determining the maximum extent by finding the farthest point on any collision object from the robot's base origin (distance to object center + object radius)

Potential (pun intended) improvements:

• Mesh Loading: Actually load the mesh files to compute precise bounding box/sphere dimensions instead of using a heuristic.
• Reachability/Manipulability: Consider the robot's workspace or reachability map rather than just a static zero-pose extent, as the robot might extend further in other configurations.
  Center of Workspace: Instead of the base origin, consider computing the radius relative to a "workspace center" where the robot will likely spend most of it's time

Usage

potential_fields/potential_fields_library/src/pfield/pfield.cpp

Lines 70 to 78 in 6e7f1b3

	void PotentialField::initializeKinematics(const std::string& urdfFilePath, const std::string& eeLinkName) {
	this->urdfFileName = urdfFilePath;
	this->eeLinkName = eeLinkName;
	this->pfKinematics = std::make_unique<PFKinematics>(this->urdfFileName);
	// Assign influence distance using maximum robot extent or the default, whichever is more generous
	// TODO(Sharwin24): Uncomment this once the estimateRobotExtenRadius function is fixed and verified to be accurate
	// const double maxExtent = this->pfKinematics->estimateRobotExtentRadius();
	// this->influenceDistance = std::max(this->influenceDistance, maxExtent);
	}

Current Implementation

potential_fields/potential_fields_library/src/pfield/pf_kinematics.cpp

Lines 259 to 327 in 6e7f1b3

	double PFKinematics::estimateRobotExtentRadius() {
	// Require a valid URDF model
	if (!this->robotModel || this->robotModel->links_.empty()) { return 0.0; }
	// Prepare transforms for links that have collision geometry. Use nominal zero joint values.
	std::vector<double> q_zero;
	if (!this->jointNamesCache.empty()) {
	q_zero.assign(this->jointNamesCache.size(), 0.0);
	}
	else {
	// If caches are not initialized, we can still rely on Pinocchio model dimensions
	// but computeLinkTransforms requires caches. In that edge case, return 0.0 gracefully.
	return 0.0;
	}
	std::vector<Eigen::Affine3d> world_T_link;
	try {
	world_T_link = this->computeLinkTransforms(q_zero);
	}
	catch (...) {
	return 0.0;
	}
	// The collision catalog and collisionLinkNames align with the templates/order used
	const size_t N = std::min(world_T_link.size(), this->collisionCatalog.size());
	double max_extent = 0.0;
	for (size_t i = 0; i < N; ++i) {
	const auto& entry = this->collisionCatalog[i];
	if (!entry.col || !entry.col->geometry) { continue; }
	// Link pose in world/base frame and collision origin in link frame
	const Eigen::Affine3d& T_world_link = world_T_link[i];
	const Eigen::Affine3d T_link_col = urdfPoseToEigen(entry.col->origin);
	const Eigen::Affine3d T_world_col = T_world_link * T_link_col;
	const Eigen::Vector3d p_world = T_world_col.translation();
	// Compute local bounding-sphere radius r_local from geometry
	double r_local = 0.0;
	if (auto box = std::dynamic_pointer_cast<urdf::Box>(entry.col->geometry)) {
	const double l = box->dim.x, w = box->dim.y, h = box->dim.z;
	r_local = 0.5 * std::sqrt(l * l + w * w + h * h);
	}
	else if (auto sph = std::dynamic_pointer_cast<urdf::Sphere>(entry.col->geometry)) {
	r_local = sph->radius;
	}
	else if (auto cyl = std::dynamic_pointer_cast<urdf::Cylinder>(entry.col->geometry)) {
	r_local = std::sqrt(cyl->radius * cyl->radius + 0.25 * cyl->length * cyl->length);
	}
	else if (auto mesh = std::dynamic_pointer_cast<urdf::Mesh>(entry.col->geometry)) {
	const double sx = mesh->scale.x > 0.0 ? mesh->scale.x : 1.0;
	const double sy = mesh->scale.y > 0.0 ? mesh->scale.y : 1.0;
	const double sz = mesh->scale.z > 0.0 ? mesh->scale.z : 1.0;
	const double smax = std::max(sx, std::max(sy, sz));
	// TODO(Sharwin24): Load mesh and compute real extents if possible
	const double meshRadius = 0.30; // default fallback radius in meters
	r_local = meshRadius * smax; // conservative heuristic without loading mesh
	}
	else {
	// Unsupported type; skip
	continue;
	}
	const double extent = p_world.norm() + r_local;
	if (extent > max_extent) { max_extent = extent; }
	}
	return max_extent; // 0.0 if nothing found
	}