Investigate conical attraction force causing oscillations

[Sharwin24]

The D* Threshold that determines when to switch between conical and quadratic attraction force seems to produce weird behavior so there must be a mistake in the implementation.

potential_fields/potential_fields_library/src/pfield/pfield.cpp

Lines 522 to 540 in 8d03c37

	Eigen::Vector3d PotentialField::computeAttractiveForceLinear(const SpatialVector& queryPose) const {
	if (!this->goalSet) return Eigen::Vector3d::Zero();
	// Choset Attractive Potential: F = zeta * (q - q_goal)
	const Eigen::Vector3d direction = queryPose.getPosition() - this->goalPose.getPosition();
	const double magnitude = direction.norm();
	// TODO(Sharwin24): Investigate why Conical block has problems with oscillations
	return -this->attractiveGain * direction;
	// if (magnitude <= dStar) {
	// // Quadratic Attraction Region
	// return -this->attractiveGain * direction;
	// }
	// else {
	// // Conical Attraction Region
	// // The Choset attractive potential defines a threshold for switching to quadratic behavior from conical
	// const double dStar = this->dynamicQuadraticThresholdEnabled ? this->computeDynamicQuadraticThreshold(queryPose)
	// : this->dStarThreshold;
	// return (dStar * (-this->attractiveGain * direction)) / magnitude;
	// }
	}

Another thing to investigate is the dynamic threshold based on line of sight from obstacles to the goal. This feature sounds good in theory but in practice it may not actually have the desired effect on robot arms so we should consider just removing it.

We have the infrastructure in PotentialField to set this value and enable the dynamic threshold but it's currently unreachable by the ROS node (pfield_manager) on nominal use.

potential_fields/potential_fields/src/ros/pfield_manager.cpp

Lines 109 to 110 in 8d03c37

	this->pField->enableDynamicQuadraticThreshold(false);
	this->pField->setQuadraticThreshold(1.0);