Apply Smoothing to WBV output Joint Velocities

[Sharwin24]

Problem

In #42, the control point approach to multi-rigid body from the Principles of Robot motion textbook was implemented. Essentially, after fitting ellipsoids, capsules, or spheres to the rigid-body links of the robot (depending on which shape fits the best), we define a set of control points for each link (see below). These control points are used to calculate the repulsion force from obstacle surfaces onto each independent robot link, where the force applied to a control point can be converted into the corresponding joint torque using the robot's Jacobian.

The problem with the planned paths using this approach is still a spiky joint velocity trajectory.

Proposed Solution(s)

Some path smoothing as a post-processing step would be helpful here. Some options could be:

• Low-Pass Filtering
• Interpolating the same trajectory with a higher timestep

Relevant Code

The path planning method planPathFromWholeBodyJointVelocities in pfield.cpp would be the place to call this helper function after the path is created and before the metadata finalizing step (line 1055 in pfield.cpp) which may look something like this:

PlannedPath applyPathSmoothing(const PlannedPath& path) {
    // Create a copy of path, smooth it, and return it
}