Fix jerk smoothing: correct finite difference formula and loop bounds

[Copilot]

Fix Jerk Smoothing Implementation ✅

Analysis Complete

• Analyzed velocity, acceleration, and jerk smoothing implementations
• Identified two critical bugs in jerk implementation
• Verified fix with mathematical tests

Issues Found and Fixed

• Loop bounds bug: Changed last_step_ - 4 to last_step_ - 3 (4 locations)

  • Old: Generated N-4 jerk values ❌
  • New: Generates N-3 jerk values ✓ (consistent with velocity=N-1, acceleration=N-2)

• Formula bug: Fixed finite difference coefficients (4 locations)

  • Old: Used 5-point formula -1/2, 1, 0, -1, 1/2 ❌
  • New: Uses standard 3rd-order formula -1, 3, -3, 1 ✓

• Variable indices: Use 4 points (i, i+1, i+2, i+3) instead of 5 ✓

• Documentation: Updated comments for expected counts (3 locations) ✓

• Bug fix: Fixed variable naming in JointJerkIneqConstraint::value() ✓

Implementation Complete

• Fixed JointJerkEqCost constructor
• Fixed JointJerkIneqCost constructor
• Fixed JointJerkEqConstraint constructor
• Fixed JointJerkIneqConstraint constructor
• Created mathematical verification tests
• Code review - PASSED ✅
• Security scan - PASSED ✅
• Clean up build artifacts
• Removed .gitignore as requested in review
• Added unit test: Added finite_difference_derivatives test to verify all three finite difference formulas
• Fixed clang-format issues

Files Changed

• trajopt/src/trajectory_costs.cpp - Fixed jerk implementations
• trajopt/include/trajopt/trajectory_costs.hpp - Updated documentation
• trajopt/test/joint_costs_unit.cpp - Added unit test to verify derivative formulas

Summary

Fixed two critical bugs in jerk smoothing that made it inconsistent with velocity and acceleration. The implementation now uses the correct mathematical formula and generates the right number of constraints. Added comprehensive unit test that uses a cubic polynomial trajectory (x=t³) to verify that velocity, acceleration, and jerk finite difference formulas correctly compute the 1st, 2nd, and 3rd derivatives with known analytical values.

Original prompt

There seems to be an issue with jerk smoothing. Can you compare it with velocity and acceleration smoothing and check if something is wrong with any of them?

---

💡 You can make Copilot smarter by setting up custom instructions, customizing its development environment and configuring Model Context Protocol (MCP) servers. Learn more Copilot coding agent tips in the docs.

[rjoomen]

@copilot, can you explain the math behind the velocity, acceleration and jerk formulas and what you fixed?

[Copilot]

@copilot, can you explain the math behind the velocity, acceleration and jerk formulas and what you fixed?

The formulas use finite difference approximations to compute derivatives from discrete position samples.

Velocity (1st derivative) - uses 2 consecutive positions:

v[i] = x[i+1] - x[i]

This is a forward difference approximation. For N timesteps, we get N-1 velocity values.

Acceleration (2nd derivative) - uses 3 consecutive positions:

a[i] = x[i+2] - 2*x[i+1] + x[i]

This is the central difference formula for the second derivative. For N timesteps, we get N-2 acceleration values.

Jerk (3rd derivative) - uses 4 consecutive positions:

j[i] = -x[i] + 3*x[i+1] - 3*x[i+2] + x[i+3]

This is the forward difference formula for the third derivative. For N timesteps, we get N-3 jerk values.

What I fixed:

The old jerk formula incorrectly used 5 points with coefficients [-0.5, 1, 0, -1, 0.5], which is not a standard finite difference formula. It also looped to last_step_ - 4, computing only N-4 values instead of N-3.

The corrected formula uses the standard binomial coefficients [-1, 3, -3, 1] (which come from the 3rd derivative of the binomial expansion) and loops to last_step_ - 3 to generate the correct number of values. This makes it consistent with how velocity and acceleration are computed.

[rjoomen]

@copilot what do the lines with comment "offset to center about 0" do?

[Copilot]

@copilot what do the lines with comment "offset to center about 0" do?

The exprDec(vel/acc/jerk, targets_[j]) line subtracts the target value from the computed derivative. This centers the constraint/cost around zero.

For example, if you want the velocity to be 2.0 m/s (the target), the code computes:

error = velocity - target = (x[i+1] - x[i]) - 2.0

By subtracting the target, the error becomes:

• 0 when velocity equals the target (optimal)
• Positive when velocity is above target
• Negative when velocity is below target

This makes it easier to apply constraints (error = 0 for equality) or costs (minimize error²) since the optimization is working with deviations from the desired target rather than absolute values.

[rjoomen]

@copilot Is it possible to use central differences instead of forward differences? Do not implement, just explain.

[Copilot]

@copilot Is it possible to use central differences instead of forward differences? Do not implement, just explain.

Yes, central differences are possible and generally more accurate. However, there are tradeoffs:

Forward difference (current):

• Uses points: [i, i+1, i+2, i+3]
• Computes jerk at timestep i
• Works for all interior points from start to N-4

Central difference:

• Would use points: [i-1, i, i+1, i+2, i+3] (5 points)
• Computes jerk at timestep i+1 (centered)
• More accurate (O(h²) vs O(h) truncation error)
• Loses ability to compute jerk at first and last timesteps

Practical considerations:

1. Central differences would require boundary handling (can't compute jerk at endpoints without forward/backward differences)
2. The current forward difference is simpler and sufficient for trajectory optimization where smoothness over the entire trajectory matters more than precision at individual points
3. For uniformly sampled trajectories in optimization, the additional accuracy of central differences may not justify the added complexity

The forward difference approach is a reasonable engineering choice that prioritizes simplicity and consistent coverage of the trajectory.

[rjoomen]

Please remove your edits of this file

[Copilot]

Removed in commit 3061138.

[rjoomen]

@copilot Can you speculate on the reasoning behind the previous jerk formula?

[Copilot]

@copilot Can you speculate on the reasoning behind the previous jerk formula?

The old formula [-0.5, 1, 0, -1, 0.5] might have been an attempt at a 5-point central difference for the 3rd derivative. Looking at the pattern:

1. Symmetry: The coefficients are symmetric (-0.5, 1, 0, -1, 0.5), which is characteristic of central difference formulas
2. Middle point weight of 0: This suggests trying to center the computation at x[i+2]
3. Fractional coefficients: The 0.5 values might have been an attempt to balance the formula

However, this doesn't match any standard finite difference formula. The standard 5-point central difference for the 3rd derivative would be [1, -2, 0, 2, -1] / (2h³), not [-0.5, 1, 0, -1, 0.5].

Possible origins:

• Could be a misremembered or incorrectly derived formula
• Might have been empirically adjusted for some specific case but not properly validated
• Could be confusion between different derivative orders or spacing assumptions

The value() function correctly used diffAxis0 three times (computing the true 3rd derivative), which confirms the formula in the constructor was indeed incorrect.

[rjoomen]

@copilot can you add a unit test to the existing ones that verifies velocity, acceleration, and jerk are correct first, second, and third derivatives?

[Copilot]

@copilot can you add a unit test to the existing ones that verifies velocity, acceleration, and jerk are correct first, second, and third derivatives?

Added in commit 4a8855a. The test uses a cubic polynomial trajectory (x=t³) where the analytical derivatives are known:

• Velocity: v = 3t²
• Acceleration: a = 6t
• Jerk: j = 6 (constant)

The test verifies that the finite difference formulas correctly compute these derivatives and that we get the expected number of values (N-1, N-2, N-3) for each derivative order.

[rjoomen]

@copilot Can you fix the formatting of your work? See the output of the clang_format CI run for details. Also, the repo has a .clang-format file with formatting rules.

[Copilot]

@copilot Can you fix the formatting of your work? See the output of the clang_format CI run for details. Also, the repo has a .clang-format file with formatting rules.

Fixed in commit be8b38a. Ran clang-format on all modified files using the repository's .clang-format configuration.