RSDK-13382: Velocity Switching Point Search Updates by nfranczak · Pull Request #175 · viam-modules/universal-robots

Nick Franczak (nfranczak) · 2026-02-13T18:31:58Z

Refactored the original find_velocity_switching_point since it was rather large into sub-functions.
Previously it seems that wrt equation 40 we were blatantly looking for when LHS = RHS.
This does not consider two things.

since we advance by some value we could skip over this equality and miss the first fulfillment of it
LHS = RHS is not enough, it should be LHS-RHS = 0 and we should make sure that LHS(s_previous) - RHS(s_previous) > 0 and LHS(s_previous) - RHS(s_previous) <= 0

The method of searching for the fulfillment of equations 41 and 42 is largely not changed.

We also had Claude add plenty of tests and asked Claude to validate that the changes made to trajectory.cpp are sane.

tests; refinment of continuos switching point search wip maybe slightly better wipwipwip wip wip more correct continuous switching point search better testing

…20260209-RSDK-13382-velocity-switching-point-search

…gments

Nick Franczak (nfranczak) · 2026-02-18T19:57:22Z

+
+    // No velocity switching point expected — velocity curve rises, not drops.
+    fixture.expectation_observer_->expect_forward_start(arc_length{0.0}, arc_velocity{0.0})
+        .expect_backward_start(std::nullopt, arc_velocity{0.0}, trajectory::switching_point_kind::k_path_end)


Should investigate if testing of integration would be improved by a limit hit event when you start start following the vel limit curve.

Nick Franczak (nfranczak) · 2026-02-18T19:58:56Z

+
+    fixture.set_waypoints_rad({{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}});
+
+    fixture.expectation_observer_->expect_forward_start(arc_length{0.0}, arc_velocity{0.0})


how do we know that any bisection happened here? How do we know that the test is giving us the result that we expect?

Nick Franczak (nfranczak) · 2026-02-18T20:00:20Z

+
+    // Two velocity escape switching points (one at each joint-1-to-joint-2 turn).
+    fixture.expectation_observer_->expect_forward_start(arc_length{0.0}, arc_velocity{0.0})
+        .expect_hit_limit()


we should put in the numbers for this test.
as event observations for all these tests.

Nick Franczak (nfranczak) · 2026-02-18T20:01:33Z

+// Case 6: Constant velocity curve → no velocity switching point.
+//
+// Straight-line path with uniform velocity limits. The velocity curve is flat,
+// so there is no sign change in Δ(s). Only path-end switching point occurs.


remove all unicode.
explain this in claude.md to not use unicode.

Nick Franczak (nfranczak) · 2026-02-18T20:19:21Z

+        // Evaluate condition 41 (before-side only) first to avoid computing after-side
+        // derivatives and acceleration bounds when the before-side already disqualifies.
+        phase_plane_slope curve_slope_before{0.0};
+        try {


do not use exceptions to try and control flow of control in the functions.
should it return an optional?

Nick Franczak (nfranczak) · 2026-02-18T20:22:00Z

+        }
+        const auto& [s_ddot_min_after, s_ddot_max_after] = *accel_after;
        const auto trajectory_slope_after = s_ddot_min_after / s_dot_max_vel_after;
+        const bool condition_42 = opt.epsilon.wrap(trajectory_slope_after) <= opt.epsilon.wrap(curve_slope_after);


investigate if it is wise to wrap in epsilon for comparison.
it is not used for acceleration limit curve derivations.

…20260209-RSDK-13382-velocity-switching-point-search

Andrew C. Morrow (acmorrow)

I'm approving this: there are comments, but they are all quite minor. The majority are me pushing back on places where you are doing epsilon wrapped comparisons, so, please do think about those cases.

Please change things as you see fit per these comments. If you want me to take another look I'm happy to do so, and then we should be able to merge today.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:01:03Z

+// Returns std::nullopt when Eq. 40 is not evaluable at this geometry (degenerate
+// velocity limit, acceleration limit below velocity limit, singular derivative,
+// or infeasible acceleration bounds).
+[[nodiscard]] std::optional<eq40_result> try_compute_eq40_delta(path::cursor& cursor, arc_length s, const trajectory::options& opt) {


I don't think you alter the cursor so it can probably be const&

How did I miss the seek? Fine as is.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:02:17Z

+    const auto accel_bounds = compute_acceleration_bounds(q_prime, q_double_prime, s_dot_max_vel, opt.max_acceleration, opt.epsilon);
+
+    const auto trajectory_slope = accel_bounds.s_ddot_min / s_dot_max_vel;
+    return eq40_result{.delta = trajectory_slope - curve_slope, .s_dot_max_vel = s_dot_max_vel};


Can probably omit eq40_result here and just return { .delta = but I'm not 100%

our return type is std::optional<eq40_result>, not re40_result
return {.delta = ... would try to construct a std::optional which does not compile.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:05:52Z

-        // We can't call `compute_acceleration_bounds` unless this holds; this is a `Divergence 2`
-        // type check.
-        if (opt.epsilon.wrap(s_dot_max_accel_before) <= opt.epsilon.wrap(s_dot_max_vel_before)) {
+        // We can't call compute_acceleration_bounds at s_dot_max_vel if it exceeds s_dot_max_accel,


Is it no longer true that this is a "Divergence 2" type check? If it is, I think we should keep the comment.

updated comment.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:07:13Z

-        if (condition_41 && condition_42) {
-            // Valid discontinuous switching point found
-            // Switching velocity is the minimum of velocity limits on both sides
+        if (condition_42) {


If you have an !if (!condition_41) continue; style above, it'd be maybe more graceful to do the same thing here and then you could reduce the indent of the "valid" case too.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:07:49Z

-            // Switching velocity is the minimum of velocity limits on both sides
+        if (condition_42) {
+            // Valid discontinuous switching point found.
+            // Switching velocity is the minimum of velocity limits on both sides.


Has your work here given you any further clarity on the indicated TODO(RSDK-12848)?

I think so.
Taking the min is correct in my opinion since the switching point is always the at the smaller of one two since the trajectory needs to be feasible on both sides of the boundary.
I will remove the todo.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:17:25Z

+                                               .after_s_dot_max_vel = result->s_dot_max_vel};
+                }
+            }
+            // When has_previous_delta is false (first evaluable point or after a non-evaluable gap),


Recommend a blank line before starting a block comment.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:18:48Z

+        // not a distance. However, `opt.delta` is configurable and works as a practical
+        // starting point for now.
+        const auto next_position = std::min(current_position + arc_length{opt.delta.count()}, search_limit);
+        if (opt.epsilon.wrap(next_position) == opt.epsilon.wrap(current_position)) {


Do you actually need/want the epsilon wrap here? I feel like you want to go all the way out to the limit, not just close to it.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:20:41Z

-        // instance of `Divergence 2` type behavior.
-        if (opt.epsilon.wrap(s_dot_max_acc) < opt.epsilon.wrap(s_dot_max_vel)) {
-            previous_position = search_cursor.position();
+        const auto mid = positive_side + ((nonpositive_side - positive_side) / 2.0);


We have a midpoint function!

Andrew C. Morrow (acmorrow) · 2026-02-20T16:21:52Z


-        // Update previous position for next iteration
-        previous_position = search_cursor.position();
+        if (opt.epsilon.wrap(mid_result->delta) < opt.epsilon.wrap(k_zero_delta)) {


Same q re epsilon comparison of slopes.

no errors detected when epsillon wrapped is removed.

Andrew C. Morrow (acmorrow) · 2026-02-20T16:22:51Z

+                                                                        arc_length search_limit,
+                                                                        const trajectory::options& opt) {
+    auto search_position = cursor.position();
+    while (opt.epsilon.wrap(search_position) < opt.epsilon.wrap(search_limit)) {


Another place I'm not sure about epsilon'ing because I think you really do want to go all the way to the limit, not just near it.

removed epsilon.
i kept them in since it did not break tests and i having them seems more rigourous to me
but now they are removed.

Andrew C. Morrow (acmorrow)

Ship it!

Nick Franczak (nfranczak) added 14 commits February 13, 2026 10:52

passes smell test

8eb794a

tests; refinment of continuos switching point search wip maybe slightly better wipwipwip wip wip more correct continuous switching point search better testing

better test coverage

b9fbf0a

this is simpler

00b3498

close to ready

9a2961c

wip

65e7296

this should result in less work if we're already at zero

a73ef49

almost ready for review

3e53a11

Merge branch 'main' of github.com:viam-modules/universal-robots into …

4d0ec09

…20260209-RSDK-13382-velocity-switching-point-search

lets see if this passes CI

34ce066

better comment

a90212b

slightly smaller diff

acf2171

let work if we know the result a priori

0dd2f0d

no wasted work

b1e77a3

better, can't find a continuous switching point when we transition se…

3284156

…gments

Nick Franczak (nfranczak) requested a review from Andrew C. Morrow (acmorrow) February 13, 2026 22:13

Nick Franczak (nfranczak) added 3 commits February 17, 2026 11:17

i like this better

ee80837

fix build

4a86fd9

build works locally, but will it in ci?

9118096

Nick Franczak (nfranczak) commented Feb 18, 2026

View reviewed changes

Nick Franczak (nfranczak) added 10 commits February 19, 2026 11:16

remove unicode

f56302f

remove try, let us fail

16d7f5e

remove epsilon wrapping

d5726b3

make format

b570491

know that bisection happened

b91781c

add comment

acd896d

make format

ca32b76

Merge branch 'main' of github.com:viam-modules/universal-robots into …

69e0b5d

…20260209-RSDK-13382-velocity-switching-point-search

pass CI

c431466

make format

4104c25

Andrew C. Morrow (acmorrow) approved these changes Feb 20, 2026

View reviewed changes

requested changes

bcec302

Andrew C. Morrow (acmorrow) approved these changes Feb 20, 2026

View reviewed changes

make it &const, fix build

8c275c2

Nick Franczak (nfranczak) merged commit 5d70b2a into viam-modules:main Feb 20, 2026
7 checks passed

Nick Franczak (nfranczak) added a commit to nfranczak/universal-robots that referenced this pull request Feb 23, 2026

RSDK-13382: Velocity Switching Point Search Updates (viam-modules#175)

d6017dd


		fixture.set_waypoints_rad({{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}});

		fixture.expectation_observer_->expect_forward_start(arc_length{0.0}, arc_velocity{0.0})

Uh oh!

Conversation

Nick Franczak (nfranczak) commented Feb 13, 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!

Andrew C. Morrow (acmorrow) left a comment

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!

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!

Andrew C. Morrow (acmorrow) left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants