Adding attraction and repulsion forces to PlannedPath and CSV

[Sharwin24]

Adding these fields to the struct containing data from the result of running the planPath methods and propagating those through the functions, CSV file, and plotting code:

std::vector<std::vector<double>> linkObstacleClearances; // Min Clearances between links and obstacles [m]
std::vector<Eigen::Vector3d> attractionForces; // Attractive Force vectors applied to the EE
std::vector<std::vector<Eigen::Vector3d>> repulsiveForces; // Repulsive Force vectors applied to each link

---

WBV Planning with Obstacle

Same Demo without Obstacle

[Copilot]

Pull request overview

This PR adds three new fields to the PlannedPath struct to capture and export attraction and repulsion forces during path planning: linkObstacleClearances (minimum clearances between links and obstacles), attractionForces (attractive force vectors on the end-effector), and repulsiveForces (repulsive force vectors on each link). These fields are populated during whole-body path planning and exported to CSV files for analysis and visualization.

Key Changes

• Added three new vector fields to PlannedPath struct for storing force and clearance data at each path point
• Modified evaluateWholeBodyJointTorquesAtConfiguration and computeWholeBodyRepulsionJointTorques to accept optional output parameters for capturing forces and clearances
• Updated CSV export to include new columns for attraction forces, joint torques, link clearances, and link repulsive forces with appropriate headers

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 9 comments.

File	Description
pfield_library/include/pfield/pfield.hpp	Added new fields to PlannedPath struct, updated recordPathPoint signature to accept force/clearance parameters, and updated function documentation with [in]/[out] parameter tags
pfield_library/src/pfield/pfield.cpp	Implemented force/clearance capture in repulsion computation functions, populated new fields during whole-body planning, and extended CSV export with new columns and header generation functions

Comments suppressed due to low confidence (1)

pfield_library/src/pfield/pfield.cpp:1180

• Missing leading comma causes CSV column misalignment when jointAngles data is absent. When i >= path.jointAngles.size(), the if block is skipped entirely, leaving no comma to separate the attraction force columns from subsequent columns. This will cause the CSV to have inconsistent column structure across rows. Consider writing an empty value or proper number of commas when data is missing.

      // Write: joint angles
      if (i < path.jointAngles.size()) {
        csvFile << ",";
        for (unsigned int j = 0; j < numJoints; ++j) {
          csvFile << path.jointAngles[i][j];
          if (j < numJoints - 1) {
            csvFile << ",";
          }
        }
      }

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The new linkObstacleClearances, attractionForces, and repulsiveForces fields in the PlannedPath struct lack test coverage. These fields are added to the structure but there are no tests verifying that they are correctly populated or that they contain expected values during path planning. Consider adding tests that verify these fields are populated when using planPathFromWholeBodyJointVelocities.