example(sampling-c3) : Use C3's new cost computation helpers

[ebianchi]

Depends on:

• c3#44 (merged)
• c3#53 (merged)

This PR changes the sample cost evaluation implementation to rely on the C3 repo's trajectory evaluation functionality. Due to encountering several cost-related bugs in the existing code, this PR also contains bug fixes.

This migration does the following:

1. Fixes a bug that affected the time-scaling of the sampling C3 controller's C3 mode trajectories (this has potential to change C3 mode aggressiveness; I have not noticed a significant impact).
2. Fixes several cost-related bugs (these have potential to change which sample is selected; I have not noticed a significant impact).
3. Changes the way x_N is defined for cost calculation (this has potential to change which sample is selected; I have not noticed a significant impact).
   • The previous implementation, to me, seems like a minor bug: x_N comes from simulating the LCS for cost from the last planned x_{N-1}, u_{N-1}.
   • The new implementation aligns with how C3 itself implements final state cost: x_N comes from A x_{N-1} + B u_{N-1} + D lambda_{N-1} + d using the LCS for plan.
4. Implements all 6 cost evaluation types, relying on the C3 repo functionality. The new and old implementations were tested to match to floating point accuracy (always < 1e-7 percentage error difference). These tests can be run by checking out this commit and running a sampling C3 demo (e.g. Push Anything).
5. Implements the debugging cost breakdown printouts, relying on the C3 repo functionality. New and old implementations were tested to match (aside from one misleading bug in the old code where non-zero robot-related costs were printed for kSimImpedanceObjectCostOnly even though they did not contribute to the full cost). These tests can be run by checking out this commit and running a sampling C3 demo (e.g. Push Anything).

---

This change is 

[xuanhien070594]

MODULE.bazel line 94 at r1 (raw file):

    module_name = "c3",
    remote = "https://github.com/DAIRLab/c3.git",
    commit = "d95bdf834be3b031ac3d7772dfdacda97723dd5a"

You can update the latest commit since PR 53 has been merged into the main branch of c3 repo.

[ebianchi]

Already done (my commit and your comment were within a couple minutes of each other).

[xuanhien070594]

@xuanhien070594 reviewed 4 files and all commit messages, made 1 comment, and resolved 1 discussion.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on ebianchi).