Class PotentialField

Class Documentation

class PotentialField

Public Types

using ControlPoint = std::pair<Eigen::Vector3d, double>

A world-space point on a robot link surface used for repulsion queries.

point is the world-frame location; surfaceRadius is the local geometry radius at that point (e.g. capsule radius, sphere radius; 0 for box corners). Clearance to an obstacle is computed as sdf(point) - surfaceRadius.

Public Functions

inline PotentialField()
~PotentialField() = default
inline PotentialField(const PotentialField &other)
inline explicit PotentialField(SpatialVector goalPose)

Constructs a PotentialField with the specified goal position. The attractive gain and rotational attractive gain are set to default values.

Parameters:

goalPose – The pose in 3D space that will generate an attractive force.

inline PotentialField(SpatialVector goalPose, double attractiveGain, double repulsiveGain, double rotationalAttractiveGain)
inline PotentialField(double attractiveGain, double repulsiveGain, double rotationalAttractiveGain)
inline PotentialField(double attractiveGain, double repulsiveGain, double rotationalAttractiveGain, double maxLinearVelocity, double maxAngularVelocity, double influenceDistance)
inline PotentialField(double attractiveGain, double repulsiveGain, double rotationalAttractiveGain, double maxLinearVelocity, double maxAngularVelocity, double maxLinearAcceleration, double maxAngularAcceleration, double influenceDistance)
inline PotentialField(SpatialVector goalPose, double attractiveGain, double repulsiveGain, double rotationalAttractiveGain, double maxLinearVelocity, double maxAngularVelocity, double maxLinearAcceleration, double maxAngularAcceleration, double influenceDistance)
inline PotentialField &operator=(const PotentialField &other)
inline bool operator==(const PotentialField &other) const
inline bool operator!=(const PotentialField &other) const
void initializeKinematics(const std::string &urdfFilePath, const std::string &eeLinkName)

Initializes the PFKinematics module with the given URDF file path and end-effector link name.

Note

The end-effector link name must match a link in the URDF model and will be the frame used for planning.

Parameters:

  • urdfFilePath – The file path to the robot’s URDF file.

  • eeLinkName – The name of the end-effector link in the robot model.

inline void setAttractiveGain(double newAttractiveGain)
inline void setRepulsiveGain(double newRepulsiveGain)
inline void setRotationalAttractiveGain(double newRotationalAttractiveGain)
inline void setMaxLinearVelocity(double newMaxLinearVelocity)
inline void setMaxAngularVelocity(double newMaxAngularVelocity)
inline void setMaxLinearAcceleration(double newMaxLinearAcceleration)
inline void setMaxAngularAcceleration(double newMaxAngularAcceleration)
inline void setInfluenceDistance(double newInfluenceDistance)
inline void setGoalPose(SpatialVector newGoalPose)
inline void clearGoalPose()
inline void setIKSolver(std::shared_ptr<IKSolver> ikSolver)
inline void setQuadraticThreshold(double threshold)
inline void enableDynamicQuadraticThreshold(bool enabled)
inline double getAttractiveGain() const
inline double getRepulsiveGain() const
inline double getRotationalAttractiveGain() const
inline double getMaxLinearVelocity() const
inline double getMaxAngularVelocity() const
inline double getMaxLinearAcceleration() const
inline double getMaxAngularAcceleration() const
inline double getInfluenceDistance() const
inline double getRotationalThreshold() const
inline double getQuadraticThreshold() const
inline size_t getNumJoints() const
inline size_t getNumLinks() const
inline bool isUsingDynamicQuadraticThreshold() const
inline bool isGoalSet() const
inline SpatialVector getGoalPose() const
inline std::vector<PotentialFieldObstacle> getEnvObstacles() const
inline std::vector<PotentialFieldObstacle> getRobotObstacles() const
std::pair<SpatialVector, TaskSpaceTwist> stepIntegration(const SpatialVector &currentPose, const TaskSpaceTwist &prevTwist, const double dt, const std::string &integrationMethod, std::vector<double> &jointAngles, const std::vector<double> &jointVelocities = {})

Integrates the current pose forward in time based on the potential field.

Parameters:

  • currentPose – The current pose of the robot/query point.

  • prevTwist – The previous twist (for acceleration limiting).

  • dt – The time step [s].

  • integrationMethod – The method to use: “task_space” or “whole_body”.

  • jointAngles – Current joint angles (required for “whole_body”, updated via IK for “task_space”).

  • jointVelocities – Previous joint velocities (required for “whole_body”).

Returns:

A pair containing the new SpatialVector and the TaskSpaceTwist applied.

void updateObstaclesFromKinematics(const std::vector<double> &jointAngles)

Given a new set of joint angles, updates the internal obstacles based on the robot kinematics.

Note

This function requires that PFKinematics has been initialized using initializeKinematics prior to calling this function.

Parameters:

jointAngles – The new joint angles to compute the updated obstacle poses [rad]

void addObstacle(PotentialFieldObstacle obstacle)

Adds a new obstacle to the potential field.

Parameters:

obstacle – The obstacle to be added.

void addObstacles(const std::vector<PotentialFieldObstacle> &obstacles)

Adds multiple obstacles to the potential field.

Parameters:

obstacles – The obstacles to be added.

bool removeObstacle(const std::string &obstacleFrameID)

Attempts to remove an obstacle by ID.

Note

If the obstacle is not found, no action is taken.

Parameters:

obstacleFrameID – the frame ID of the obstacle obtained on obstacle creation.

Returns:

true if the obstacle was removed successfully.

void clearObstacles()

Clears all obstacles from the potential field.

PotentialFieldObstacle getObstacleByID(const std::string &obstacleFrameID) const

Gets an obstacle by its frame ID.

Parameters:

obstacleFrameID – the frame ID of the obstacle to retrieve.

Returns:

PotentialFieldObstacle The obstacle with the specified frame ID.

std::vector<PotentialFieldObstacle> getObstaclesByGroup(ObstacleGroup group) const

Gets all of the obstacles belonging to a specific group.

Note

Obstacle groups are [STATIC, DYNAMIC, ROBOT]

Parameters:

group – The group to filter obstacles by.

Returns:

std::vector<PotentialFieldObstacle> A vector of obstacles belonging to the specified group.

bool isPointInsideObstacle(Eigen::Vector3d worldPoint) const

Determines if the given point (in the world frame) is inside any obstacle.

Parameters:

worldPoint – The point in world coordinates to check.

Returns:

true If the point is inside any obstacle.

bool isPointWithinInfluenceZone(Eigen::Vector3d worldPoint) const

Determines if the given point (in the world frame) is within the influence zone of any obstacle.

Parameters:

worldPoint – The point in world coordinates to check.

Returns:

true If the point is within the influence zone of any obstacle.

PFLimits computeFieldBounds(const SpatialVector &queryPose, double bufferArea) const

Computes the spatial limits (bounding box) of the potential field for visualization or analysis. The bounds encompass all obstacles, the goal, and the query pose, expanded by a buffer area.

Parameters:

  • queryPose – The current query pose to include in the bounds

  • bufferArea – The extra margin to add around the computed limits [m]

Returns:

PFLimits The computed spatial limits

Eigen::VectorXd evaluateWholeBodyJointTorquesAtConfiguration(const std::vector<double> &jointAngles, const std::vector<double> &prevJointVelocities, const SpatialVector &eePose, const double dt, std::vector<Eigen::Vector3d> *outLinkForces = nullptr, std::vector<double> *outLinkClearances = nullptr, Eigen::Vector3d *outAttractionForce = nullptr)

Computes the whole-body joint velocities resulting from the combination of the attractive and repulsive forces at the given configuration and joint velocities.

Parameters:

  • jointAngles – [in] The current joint angles of the robot [rad]

  • prevJointVelocities – [in] The previous joint velocities of the robot [rad/s]

  • eePose – [in] The current end-effector pose

  • dt – [in] The time step over which to integrate acceleration [s]

  • outLinkForces – [out] A vector of linear forces acting on each link [N]

  • outLinkClearances – [out] A vector of minimum clearances between each link and obstacles [m]

  • outAttractionForce – [out] The linear force vector acting on the end-effector [N]

Returns:

Eigen::VectorXd The resulting whole-body joint velocities [rad/s]

TaskSpaceTwist evaluateWholeBodyTaskSpaceTwistAtConfiguration(const std::vector<double> &jointAngles, const std::vector<double> &prevJointVelocities, const SpatialVector &eePose, const double dt)

Computes the task-space twist resulting from whole-body joint velocities.

Parameters:

  • jointAngles – The current joint angles of the robot [rad]

  • prevJointVelocities – The previous joint velocities of the robot [rad/s]

  • dt – The time step over which to integrate acceleration [s]

  • eePose – The current end-effector pose

Returns:

TaskSpaceTwist The resulting task-space twist

TaskSpaceWrench evaluateWrenchAtPose(const SpatialVector &queryPose) const

Given a 3D position, computes the task-space wrench by combining attractive and repulsive forces.

Parameters:

queryPose – The pose in 3D space to compute the wrench.

Returns:

TaskSpaceWrench The resultant task-space wrench [N, Nm]

TaskSpaceTwist evaluateVelocityAtPose(const SpatialVector &queryPose) const

Given a 3D position, computes the task-space twist (velocity) by combining attractive and repulsive forces and converting to velocity vectors.

Parameters:

queryPose – The pose in 3D space to compute the velocity

Returns:

TaskSpaceTwist The resultant task-space twist [m/s, rad/s]

TaskSpaceTwist evaluateLimitedVelocityAtPose(const SpatialVector &queryPose, const TaskSpaceTwist &prevTwist = TaskSpaceTwist(), const double dt = 0.0) const

Performs the same operation as evaluateVelocityAtPose but applies velocity limits to the resulting twist.

Note

This function allows you to provide the previous twist and time step for acceleration limiting, but can be called with default parameters to only apply velocity limits

Eigen::Vector3d removeOpposingForce(const Eigen::Vector3d &attractionForce, const Eigen::Vector3d &repulsiveForce) const

Given attraction and repulsive force vectors, mitigate local minima near broad surfaces by removing opposing components of the repulsive force.

Removes the component of the repulsive force that directly opposes the attractive force, while preserving tangential components that encourage sliding around obstacles. Formally, if u_att = F_att/||F_att|| and dot = F_rep·u_att < 0, return F_rep - dot·u_att; otherwise return F_rep. When ||F_att|| is near zero, the repulsive force is returned unchanged.

Note

This is typically applied only in planning (not in evaluateWrenchAtPose) to avoid altering the visualized field and unit-test semantics.

Parameters:

  • attractionForce – Attractive linear force vector, typically calculated from computeAttractiveForceLinear.

  • repulsiveForce – Repulsive linear force vector, typically calculated from computeRepulsiveForceLinear.

Returns:

Eigen::Vector3d Resultant force vector with opposing components removed

TaskSpaceWrench evaluateWrenchAtPoseWithOpposingForceRemoval(const SpatialVector &queryPose) const

Given a 3D position, computes the task-space wrench (force/torque) by combining attractive and repulsive forces and removing any repulsive components that oppose the attractive force to help escape local minima.

Note

This function is primarily intended for use during path planning and simply coordinates the use of computeAttractiveForceLinear, computeRepulsiveForceLinear, and removeOpposingForce to produce the final wrench.

Parameters:

queryPose – The pose in 3D space to compute the wrench.

Returns:

TaskSpaceWrench The resultant task-space wrench [N, Nm]

TaskSpaceTwist wrenchToTwist(const TaskSpaceWrench &wrench) const

Converts a task-space wrench to a task-space twist (velocity) using gain parameters converting force to velocity [m/s per N] and [rad/s per Nm].

Note

This function assumes a massless system where linearGain [(m/s)/N] and angularGain [(rad/s)/Nm] are both 1.0

Parameters:

wrench – The task-space wrench to convert.

Returns:

TaskSpaceTwist The resultant task-space twist [m/s, rad/s].

TaskSpaceTwist applyMotionConstraints(const TaskSpaceTwist &twist, const TaskSpaceTwist &prevTwist, const double dt) const

Applies velocity and acceleration limits to a task-space twist.

Parameters:

  • twist – The input task-space twist to be limited

  • prevTwist – The previous task-space twist for acceleration limiting, can use zero if not available

  • dt – The time step over which to apply acceleration limits [s]

Returns:

TaskSpaceTwist The limited task-space twist

SpatialVector interpolateNextPose(const SpatialVector &currentPose, const TaskSpaceTwist &prevTwist, const double dt)

Given a current pose and a delta time, computes the next pose after the time step.

Note

This function uses both integrateLinearVelocity and integrateAngularVelocity and combines their results to compute the next pose.

Parameters:

  • currentPose – The starting pose as a SpatialVector.

  • prevTwist – The previous task-space twist for acceleration limiting.

  • dt – The time step over which to compute the next pose [s].

Returns:

SpatialVector The resulting pose after applying the velocity field for the time step.

Eigen::Vector3d integrateLinearVelocity(const Eigen::Vector3d &currentPosition, const Eigen::Vector3d &linearVelocity, double deltaTime)

Given a current position, an instantaneous linear velocity vector, and a delta time, integrates the linear velocity to compute the new position after the time step.

Parameters:

  • currentPosition – The starting position as a 3D vector.

  • linearVelocity – The instantaneous linear velocity vector in meters per second.

  • deltaTime – The time step over which to integrate the linear velocity [s].

Returns:

Eigen::Vector3d The resulting position after integrating the linear velocity.

Eigen::Quaterniond integrateAngularVelocity(const Eigen::Quaterniond &currentOrientation, const Eigen::Vector3d &angularVelocity, double deltaTime)

Given a current orientation and a delta time, computes the angular velocity to compute the new orientation after the time step.

Parameters:

  • currentOrientation – The starting orientation as a quaternion.

  • angularVelocity – The instantaneous angular velocity vector in radians per second.

  • deltaTime – The time step over which to integrate the angular velocity [s].

Returns:

Eigen::Quaterniond The resulting orientation after integrating the angular velocity.

Eigen::VectorXd convertJointTorquesToJointVelocities(const Eigen::VectorXd &jointTorques, const std::vector<double> &jointAngles, const std::vector<double> &prevJointVelocities, const double dt) const

Converts joint torques to joint velocities utilizing the robot dynamics equation or a simple proportional gain.

The full robot dynamics equation, where joint accelerations (q̈) are integrated to get joint velocities (q̇): M(q)·q̈ = τ − C(q,q̇)·q̇ − G(q) − D·q̇ If the robot dynamics equation is disabled, it uses a simple proportional gain: q̇ = torqueToVelocityGain · τ

Note

This function currently always disables the Robot Dynamics Equation due to bugs (see Issue #23).

Parameters:

  • jointTorques – Generalized joint torques [Nm].

  • jointAngles – Current joint configuration [rad].

  • prevJointVelocities – Previous joint velocities for Coriolis and damping [rad/s].

  • dt – Integration timestep [s].

Returns:

Eigen::VectorXd Joint velocities [rad/s].

Eigen::Vector3d computeAttractiveForceLinear(const SpatialVector &queryPose) const

Computes the attractive force towards the goal position.

Note

Equation: F = -attractiveGain * (distance * direction)

Parameters:

queryPose – The pose in 3D space to compute the force from

Returns:

Eigen::Vector3d The attractive force vector [N]

Eigen::Vector3d computeAttractiveMoment(const SpatialVector &queryPose) const

Computes the attractive moment (torque) to align the query pose’s orientation with the goal pose’s orientation.

Note

Equation: M = rotationalAttractiveGain * (angle * axis)

Parameters:

queryPose – The pose in 3D space to compute the moment from

Returns:

Eigen::Vector3d The attractive moment vector [Nm]

Eigen::Vector3d computeRepulsiveForceLinear(const SpatialVector &queryPose) const

Computes the repulsive linear force from all obstacles.

Note

Equation: F = repulsiveGain * (1/distance - 1/influence) * (1 / distance^2) * direction

Parameters:

queryPose – The position in 3D space to compute the force from

Returns:

Eigen::Vector3d The repulsive force vector [N]

Eigen::VectorXd computeEndEffectorAttractionJointTorques(const SpatialVector &eePose, const std::vector<double> &jointAngles) const

Computes joint torques for end-effector attraction to the goal pose.

Note

Uses the evaluateWrenchAtPose method to compute task-space forces, then maps them to joint space via the end-effector Jacobian transpose.

Parameters:

  • eePose – The current end-effector pose

  • jointAngles – The current joint configuration [rad]

Returns:

Eigen::VectorXd Joint torques for end-effector attraction [Nm]

Eigen::VectorXd computeWholeBodyRepulsionJointTorques(const std::vector<double> &jointAngles, std::vector<Eigen::Vector3d> *outLinkForces = nullptr, std::vector<double> *outLinkClearances = nullptr) const

Computes joint torques for whole-body obstacle repulsion.

Note

Iterates over all robot links and environment obstacles, computing repulsive forces at the nearest points and mapping them to joint space via link Jacobian transposes. Uses COAL distance queries and the Khatib repulsive potential formulation.

Parameters:

  • jointAngles – [in] The current joint configuration [rad]

  • outLinkForces – [out] A pointer to a vector to store the computed forces on each link [N]

  • outLinkClearances – [out] A pointer to a vector to store the minimum clearances between each link and obstacles [m]

Returns:

Eigen::VectorXd Joint torques for whole-body obstacle avoidance [Nm]

TaskSpaceTwist constrainedTwistAtPose(const SpatialVector &pose, const TaskSpaceTwist &prevTwist, const double dt)

Computes the constrained twist at a given pose using the previous twist and the time step for velocity and acceleration limiting.

Parameters:

  • pose – The current pose to compute the constrained twist for

  • prevTwist – The previous twist to use for limiting

  • dt – The time step for the integration

Returns:

TaskSpaceTwist The computed constrained twist

std::pair<SpatialVector, TaskSpaceTwist> rungeKuttaStep(const SpatialVector &currentPose, const TaskSpaceTwist &prevTwist, const double dt)

Computes the next pose and twist using the Runge-Kutta 4-step (RK4) integration method.

Parameters:

  • currentPose – The current pose to compute the next pose from

  • prevTwist – The previous twist to use for the integration

  • dt – The time step for the integration

Returns:

std::pair<SpatialVector, TaskSpaceTwist> The computed next pose and twist

double computeDynamicQuadraticThreshold(const SpatialVector &queryPose) const

Compute a per-step switch distance d* that blends goal/path clearance and stopping distance.

Note

When dynamic thresholding is disabled, the fixed dStarThreshold is used.

Parameters:

queryPose – The current pose to compute the dynamic threshold for

Returns:

double The computed dynamic quadratic threshold distance d* [m]

double minObstacleClearanceAt(const Eigen::Vector3d &point) const

Computes the minimum obstacle clearance at a given point.

Note

Returns +infinity if no obstacles are present as std::numeric_limits<double>::infinity().

Parameters:

point – The point in 3D space to compute the clearance for

Returns:

double The minimum clearance distance to the nearest obstacle [m]

double minClearanceAlongSegment(const Eigen::Vector3d &from, const Eigen::Vector3d &to, int samples = 7) const

Computes the minimum obstacle clearance along a line segment.

Note

Returns +infinity if no obstacles are present as std::numeric_limits<double>::infinity().

Parameters:

  • from – The starting point of the line segment

  • to – The ending point of the line segment

  • samples – The number of samples to take along the segment (default is 7)

Returns:

double The minimum clearance distance along the segment [m]

bool isRobotInCollisionWithEnvironment(double clearanceThreshold = 0.0) const

Checks if the robot is within a certain clearance distance from any environment obstacles.

Parameters:

clearanceThreshold – The clearance distance to check against [m]

Returns:

true If the robot is in collision with an environment obstacle

Returns:

false If the robot is not in collision with any environment obstacles

PlannedPath planPathFromTaskSpaceWrench(const SpatialVector &startPose, const std::vector<double> &startJointAngles, const double dt, const double goalTolerance, const size_t maxIters = 30000)

Plans a path from the start pose to the goal pose using the potential field.

Note

The path is planned by iteratively evaluating the velocity field at the current pose, applying motion constraints, and integrating (RK4) to find the next pose until the goal is reached. The IK solver is used to translate end-effector poses to joint angles at each step.

Parameters:

  • startPose – The starting pose as a SpatialVector.

  • startJointAngles – The starting joint angles for the robot [rad].

  • dt – The time step for each iteration of the path planning [s].

  • goalTolerance – The tolerance for reaching the goal pose [m].

  • maxIters – The maximum number of iterations to perform for path planning, defaults to 30000.

Returns:

PlannedPath The planned path containing poses, twists, joint angles, and timestamps.

PlannedPath planPathFromWholeBodyJointVelocities(const std::vector<double> &startJointAngles, const double dt, const double goalTolerance, const size_t maxIters = 30000)

Plans a path from the starting configuration to the internal goal pose using whole-body joint velocities.

Note

This method computes joint velocities by considering both end-effector attraction to the goal and whole-body obstacle repulsion. Unlike planPathFromTaskSpaceWrench, this method directly integrates joint velocities rather than task-space velocities, providing better whole-body collision avoidance. Forward kinematics is used to compute the end-effector pose at each step from the integrated joint configuration.

Parameters:

  • startJointAngles – The starting joint angles for the robot [rad].

  • dt – The time step for each iteration of the path planning [s].

  • goalTolerance – The tolerance for reaching the goal pose [m].

  • maxIters – The maximum number of iterations to perform for path planning, defaults to 30000.

Returns:

PlannedPath The planned path containing poses, twists, joint angles, and timestamps.

std::vector<double> computeInverseKinematics(const SpatialVector &targetPose, const std::vector<double> &seedJointAngles) const

Runs the IKSolver to compute joint angles for a desired end-effector pose.

Parameters:

  • targetPose – The desired end-effector pose as a SpatialVector.

  • seedJointAngles – The initial guess for joint angles [rad].

Returns:

std::vector<double> The computed joint angles [rad].

bool createPlannedPathCSV(const PlannedPath &path, const std::string &filePath) const

Creates a CSV file from a planned path with the following columns: Time [s], Position X [m], Position Y [m], Position Z [m], Orientation Qx, Orientation Qy, Orientation Qz, Orientation Qw, Linear Velocity X [m/s], Linear Velocity Y [m/s], Linear Velocity Z [m/s], Angular Velocity X [rad/s], Angular Velocity Y [rad/s], Angular Velocity Z [rad/s], Minimum Obstacle Clearance [m], Number of Joints Joint 1 [rad], Joint 2 [rad], …, Joint N [rad].

Note

The real header looks like: time_s, pos_x_m, pos_y_m, pos_z_m, q_x, q_y, q_z, q_w, vel_x_m_s, vel_y_m_s, vel_z_m_s, ang_vel_x_rad_s, ang_vel_y_rad_s, ang_vel_z_rad_s, min_obstacle_clearance_m, num_joints, joint_1_rad, joint_2_rad, …, joint_N_rad

Parameters:

  • path – The planned path to convert to CSV

  • filePath – The file path to save the CSV

Returns:

true If the CSV was created successfully

Returns:

false If there was an error creating the CSV

Public Static Functions

static std::vector<ControlPoint> buildControlPoints(const PotentialFieldObstacle &link)

Computes the set of control points for a robot link obstacle.

Control points are world-space representative points on the link surface used for floating-control-point repulsion. The set depends on geometry type:

  • CAPSULE: two endcap centers + shaft midpoint; surfaceRadius = capsule radius.

  • BOX: 8 OBB corners; surfaceRadius = 0.

  • SPHERE: link center; surfaceRadius = sphere radius.

  • CYLINDER: two endcap centers + shaft midpoint; surfaceRadius = cylinder radius.

  • default: link center; surfaceRadius = halfDimensions().norm().

Parameters:

link – The robot link obstacle.

Returns:

Vector of (world point, surface radius) pairs.