[RSDK-12891] Use a solve step to get kinematically accurate backward integration by acmorrow · Pull Request #10 · viam-modules/trajex

acmorrow · 2026-04-28T17:57:51Z

@nfranczak - I will leave detailed comments on the review.

acmorrow · 2026-04-28T17:59:15Z


    std::cout << "\n=== Testing " << num_waypoints << " waypoints with " << profile.name << " ===\n";

-    // NOTE: Using very relaxed tolerance (200%) due to known acceleration bound violations


Now that we have the solve step, I've been able to systematically remove or reduce tolerance overrides. I also have more insight in to the remaining violations, so I'm cautiously optimistic it will be possible to drive these lower, and then hopefully even tighten the defaults.

acmorrow · 2026-04-28T18:01:37Z

    // TODO(RSDK-12981): Use a slightly relaxed tolerance in this test since empirically we require it;
    // the expectation is that fixing RSDK-12981 will remove the need for this.
-    fixture.validation_tolerance_percent = 0.5;
+    fixture.validation_tolerance_percent *= 2;


I've changed these to use relative tolerance overrides rather than absolute, consistently, as I think it better expresses the intent.

acmorrow · 2026-04-28T18:04:01Z

+// bounds are well-defined and continuous above the limit curve, so this is safe to call at any
+// phase plane position. Used by the backward integration bisection solve, where evaluation above
+// the limit curve is expected during bracket probing.
+[[gnu::pure]] trajectory::acceleration_bounds compute_acceleration_bounds_unchecked(const xt::xarray<double>& q_prime,


The solve step needs to be able to get the bounds inside the infeasible region. The theory here is that there is nothing algebraically unsound about getting those bounds, they just aren't kinematically meaningful.

Being able to get those results independently of whether we are in the infeasible region makes the solve step a lot easier, since there isn't a forbidden region where we can't get an answer to "what is s_ddot_min" here in the phase plane.

acmorrow · 2026-04-28T18:06:51Z

 struct eq40_result {
    phase_plane_slope delta;
    arc_velocity s_dot_max_vel;
-    arc_acceleration s_ddot_min;


I've consistently gone through and eliminated switching point mandated forward/backward accels everywhere we could, and then ripped out the supporting infrastructure. These were originally added in order to save work: the idea was that since the switching point search had almost certainly already done the relevant computation, it might as well carry the result, and to handle the one case of truly mandated accels (non-diff accel switching points). We still need switching points to carry accel, both forward and back, for that case, but I've removed as much of it as I can. My end goal is to have only non-diff accel sps have set switching points. We aren't quite there yet, and more on that below, but we are closing in. As additional motivation, consider that for backwards integration, we don't really want a mandated accel since we now want to run the solve step.

acmorrow · 2026-04-28T18:07:08Z

-        // Case A: [s_dot_max_acc(s-) < s_dot_max_acc(s+) and (s_ddot_max(s-, s_dot_max_acc(s-))/s_dot_max_acc(s-)) >= d/ds
-        // s_dot_max_acc(s-)] Case B: [s_dot_max_acc(s-) > s_dot_max_acc(s+) and (s_ddot_max(s+, s_dot_max_acc(s+))/s_dot_max_acc(s+)) <=
-        // d/ds s_dot_max_acc(s+)]
+        //  - Case A: [s_dot_max_acc(s-) < s_dot_max_acc(s+) and (s_ddot_max(s-, s_dot_max_acc(s-))/s_dot_max_acc(s-)) >= d/ds


acmorrow · 2026-04-28T18:12:19Z

-                // had a very short subsequent segment, but which contained a more limiting constraint, we might
-                // integrate right over it with a coarse `dt`, and tunnel through the infeasible region.
-                if (const auto segment = *cache.path_cursor; next_point.s > segment.end()) {
+                // Quantize to segment boundaries. If the step crossed or landed within epsilon of


Once source of limit violations was if we ended up with a tiny sliver over which we needed to integrate. The calculation to determine the s_ddot that took us from the current point to the very nearby next point was dividing small numbers by small numbers and getting noisy results. By extending too the segment if we are very close, not just clamping to it if we exceed it, we can ensure that residual segments have length at least epsilon.

acmorrow · 2026-04-28T18:13:16Z


                // Compute the acceleration we should use at this phase point..
-                const auto s_ddot_desired = [&] {
+                const auto [s_ddot_desired, s_ddot_mandated] = [&] {


The only case we get a mandated backward accel is for non-diff.

acmorrow · 2026-04-28T18:13:35Z

                // Backward integration must decrease s.
                if ((next_point.s >= current_point.s)) [[unlikely]] {
-                    throw std::runtime_error{"TOTG algorithm error: backward integration must decrease s and not decrease s_dot"};
+                    throw std::runtime_error{"TOTG algorithm error: backward integration must decrease s"};


Stale exception message from when this was erroneously strict.

acmorrow · 2026-04-28T18:15:02Z

                        "forward trajectory - trajectory is infeasible (would require non-zero initial velocity)"};
                }

+                // The naive Euler step uses s_ddot_min evaluated at the departure point C, but on curved segments the


OK, this is the real meat of the review: the solve step. The idea is that we take our first guess, clamp the s value it takes us to, and bisect in s_dot until we find an s_ddot that yields us a point which has an s_ddot_min equal to what we used to find it, which is kinematically sound.

Claude wrote the code after I iterated with it on the approach. I can share our working doc with you if you would find it interesting.

The way it wrote the code is not how I want this to ship. However, I wanted to get your take on the algorithm before spending time getting the code up to par. If you like, we can sit down and talk through it together.

If we are all happy with it, I'll rework it into a form that aligns better with the standards and practices of the codebase.

acmorrow · 2026-04-28T18:18:13Z

-                    // backward point close enough to the forward trajectory that the splice requires
-                    // infeasible deceleration. Once the backward step uses a fixed-point iteration to
-                    // find the correct acceleration, this check should be re-enabled.
+                    // TODO(RSDK-12981): Disabled because the acceleration required for the simple bridge from the


This is still disabled. The most interesting thing to me is that it seems that we now have far fewer violations of this type now that we are doing the solve step to integrate backwards. However, we aren't down to zero, so I can't enable it. I think that what will be required is searching within the range of acceptable s_ddot's at the last forward point for a point on the backwards segment that is reachable, and adding that as a bridge point. I have code to do it, but I want to land the rest of this first so we can focus on that in isolation. Ideally, after fixing the bridge code, we can tighten tolerances even further, because I think many of the bounds violations are actually at splices.

acmorrow · 2026-04-30T20:57:43Z

+                    // Establish bracket. The naive velocity and the current velocity are our first
+                    // two probes. Keep them ordered (low, high) for consistent bisection.
+                    auto s_dot_low = std::min(next_point.s_dot, current_point.s_dot);
+                    auto s_dot_high = std::max(next_point.s_dot, current_point.s_dot);
+                    auto residual_low = eval_residual(s_dot_low);
+                    auto residual_high = eval_residual(s_dot_high);


Experimentally, this bracket is too small.

acmorrow · 2026-04-30T20:58:04Z

+                    if (!straddles_zero(residual_low, residual_high)) {
+                        if (residual_low > k_zero_acceleration) {
+                            s_dot_high *= 2.0;
+                            residual_high = eval_residual(s_dot_high);
+                        } else {
+                            s_dot_low = arc_velocity{1e-10};
+                            residual_low = eval_residual(s_dot_low);
+                        }
+                    }


Experimentally, this expansion is 100% effective. I dislike it.

acmorrow · 2026-04-30T21:14:10Z

+                        const auto bounds = compute_acceleration_bounds_unchecked(
+                            q_prime, q_double_prime, s_dot, traj.options_.max_acceleration, traj.options_.epsilon);
+                        return required_s_ddot - bounds.s_ddot_min;
+                    };


If the high residual is within epsilon of zero, we should probably just accept the s_ddot_desired as the result.

nfranczak

I've been thinking about the bracket that we construct, the bracket where one end is positive and the other non-positive, [-, +] or [+, -]. Let them be called [range]
By bisecting it we are making the claim that 0 ∈ [range].
But what if this is the range.
[-, +] = [-, + , -, +, -, +].
or
[+, -] = [+, -, +, -, +, -]
Then in either we have three solutions, and that is fine?
I do not see why not?
If so, then the range construction can be simplified to the bottom of the phase plane up to the limit curve we perform bisection on that range dynamically.

acmorrow added 3 commits April 28, 2026 13:51

[RSDK-12981] Use a solve step for non-mandated backward accelerations

7253c52

[RSDK-12981] Fewer mandated accels in switching points

58e3dbb

[RSDK-12981] Tighten test tolerances and tolerance overrides

6e42a22

acmorrow requested a review from nfranczak April 28, 2026 17:57

acmorrow commented Apr 28, 2026

View reviewed changes

[RSDK-12891] Cleanup naming in solve step

e072e22

acmorrow commented Apr 30, 2026

View reviewed changes

nfranczak reviewed May 1, 2026

View reviewed changes

Comment thread src/viam/trajex/totg/trajectory.cpp

Comment thread src/viam/trajex/totg/trajectory.cpp

[RSDK-12891] Solver enhancements

5264115

acmorrow requested a review from nfranczak May 4, 2026 18:34

nfranczak approved these changes May 4, 2026

View reviewed changes

acmorrow merged commit 46576e1 into viam-modules:main May 5, 2026
5 checks passed


		std::cout << "\n=== Testing " << num_waypoints << " waypoints with " << profile.name << " ===\n";

		// NOTE: Using very relaxed tolerance (200%) due to known acceleration bound violations

Conversation

acmorrow commented Apr 28, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

nfranczak left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants