diff --git a/CMakeLists.txt b/CMakeLists.txt index 43162ae..95be67a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -262,6 +262,7 @@ target_sources(viam-trajex-totg src/viam/trajex/trajex.cpp src/viam/trajex/totg/observers.cpp src/viam/trajex/totg/path.cpp + src/viam/trajex/totg/streaming/session.cpp src/viam/trajex/totg/trajectory.cpp src/viam/trajex/totg/uniform_sampler.cpp src/viam/trajex/totg/waypoint_accumulator.cpp @@ -515,6 +516,55 @@ if (VIAM_TRAJEX_BUILD_TESTS) ) + # Separate binary for streaming session tests so they can be looped fast + # without paying the integration suite's runtime. + add_executable(viam-trajex-totg-streaming-test) + + target_sources(viam-trajex-totg-streaming-test + PRIVATE + src/viam/trajex/totg/streaming/test/main.cpp + src/viam/trajex/totg/streaming/test/session.cpp + ) + + target_link_libraries(viam-trajex-totg-streaming-test + PRIVATE + viam::trajex::totg + Boost::boost + ) + + target_compile_definitions(viam-trajex-totg-streaming-test PRIVATE + VIAM_TRAJEX_STREAMING_TEST_DATA_DIR="${CMAKE_CURRENT_SOURCE_DIR}/src/viam/trajex/totg/streaming/test/data" + ) + + apply_wall_werror(viam-trajex-totg-streaming-test) + + add_test( + NAME viam-trajex-totg-streaming-test + COMMAND viam-trajex-totg-streaming-test + ) + + + # Streaming-session simulator. Standalone binary, not wired into ctest -- + # see orbsanding-streaming-session-simulation-plan.md. + add_executable(viam-trajex-totg-streaming-sim-exe) + + target_sources(viam-trajex-totg-streaming-sim-exe + PRIVATE + src/viam/trajex/totg/streaming/test/sim/streaming_sim_main.cpp + ) + + set_target_properties(viam-trajex-totg-streaming-sim-exe PROPERTIES + OUTPUT_NAME viam-trajex-totg-streaming-sim + ) + + target_link_libraries(viam-trajex-totg-streaming-sim-exe + PRIVATE + viam::trajex::totg::tools + ) + + apply_wall_werror(viam-trajex-totg-streaming-sim-exe) + + add_executable(viam-trajex-totg-tools-test) target_sources(viam-trajex-totg-tools-test diff --git a/go/totg/rdk/service.go b/go/totg/rdk/service.go index 4f9c8fd..1f6f281 100644 --- a/go/totg/rdk/service.go +++ b/go/totg/rdk/service.go @@ -114,7 +114,7 @@ func (s *Service) Metadata(_ context.Context) (mlmodel.MLMetadata, error) { { Name: totg.KeyTrajectorySamplingFreqHz, Description: "Output sample rate, in Hz [scalar]", - DataType: "int64", + DataType: "float64", Shape: []int{1}, }, }, diff --git a/go/totg/rdk/service_test.go b/go/totg/rdk/service_test.go index e5b42b3..adf635d 100644 --- a/go/totg/rdk/service_test.go +++ b/go/totg/rdk/service_test.go @@ -46,7 +46,7 @@ func buildInputs() ml.Tensors { ), totg.KeyTrajectorySamplingFreqHz: tensor.New( tensor.WithShape(1), - tensor.WithBacking([]int64{100}), + tensor.WithBacking([]float64{100.0}), ), } } @@ -170,7 +170,7 @@ func TestMinimalTrajectoryLatency(t *testing.T) { ), totg.KeyTrajectorySamplingFreqHz: tensor.New( tensor.WithShape(1), - tensor.WithBacking([]int64{100}), + tensor.WithBacking([]float64{100.0}), ), } @@ -283,7 +283,7 @@ func TestHypercubeTortureLatency(t *testing.T) { ), totg.KeyTrajectorySamplingFreqHz: tensor.New( tensor.WithShape(1), - tensor.WithBacking([]int64{10}), + tensor.WithBacking([]float64{10.0}), ), } @@ -346,7 +346,7 @@ func TestPhase4Gate(t *testing.T) { totg.KeyVelocityLimitsRadsPerSec: tensor.New(tensor.WithShape(nDOF), tensor.WithBacking(velLimits)), totg.KeyAccelerationLimitsRadsPerSec2: tensor.New(tensor.WithShape(nDOF), tensor.WithBacking(accLimits)), totg.KeyPathToleranceDeltaRads: tensor.New(tensor.WithShape(1), tensor.WithBacking([]float64{0.05})), - totg.KeyTrajectorySamplingFreqHz: tensor.New(tensor.WithShape(1), tensor.WithBacking([]int64{200})), + totg.KeyTrajectorySamplingFreqHz: tensor.New(tensor.WithShape(1), tensor.WithBacking([]float64{200.0})), } s := newTestService(t) diff --git a/go/totg/streaming/session.go b/go/totg/streaming/session.go new file mode 100644 index 0000000..4a1b490 --- /dev/null +++ b/go/totg/streaming/session.go @@ -0,0 +1,185 @@ +//go:build !windows && !no_cgo + +// Package streaming exposes the trajex TOTG streaming session as a Go API, +// layered on top of github.com/viam-modules/trajex's TensorMap. Sessions are +// stateful: callers construct a session with a fixed configuration, extend it +// with waypoint batches over time, and pull samples incrementally. +// +// The schema-key constants (KeyWaypointsRads, KeyVelocityLimitsRadsPerSec, +// etc.) are re-exported from the parent totg package so callers using both +// stateless Generate and streaming sessions need to import only one set of +// key names. +package streaming + +/* +#cgo CFLAGS: -I${SRCDIR}/../../../src/viam/trajex/capi + +#include "capi.h" +*/ +import "C" + +import ( + "context" + "time" + + "github.com/pkg/errors" + + trajex "github.com/viam-modules/trajex/go" + "github.com/viam-modules/trajex/go/totg" +) + +// Re-export the shared schema-key constants from totg so streaming callers +// have a single source of truth. The values are identical to totg.Key* by +// construction (both initialized from the same C externs at package init). +var ( + KeyVelocityLimitsRadsPerSec = totg.KeyVelocityLimitsRadsPerSec //nolint:revive + KeyAccelerationLimitsRadsPerSec2 = totg.KeyAccelerationLimitsRadsPerSec2 //nolint:revive + KeyPathToleranceDeltaRads = totg.KeyPathToleranceDeltaRads //nolint:revive + KeyPathColinearizationRatio = totg.KeyPathColinearizationRatio //nolint:revive + KeyTrajectorySamplingFreqHz = totg.KeyTrajectorySamplingFreqHz //nolint:revive + KeyWaypointsRads = totg.KeyWaypointsRads //nolint:revive + KeySampleTimesSec = totg.KeySampleTimesSec //nolint:revive + KeyConfigurationsRads = totg.KeyConfigurationsRads //nolint:revive + KeyVelocitiesRadsPerSec = totg.KeyVelocitiesRadsPerSec //nolint:revive + KeyAccelerationsRadsPerSec2 = totg.KeyAccelerationsRadsPerSec2 //nolint:revive +) + +// Session is a Go-owned handle to a CAPI streaming session. Close must be +// called (typically via defer) to release the underlying C resource. Session +// is not safe for concurrent use; concurrent operations on the same handle +// are the caller's responsibility, matching the C ABI's contract. +type Session struct { + handle *C.viam_trajex_totg_streaming_session_t +} + +// New constructs a streaming session from a configuration tensor map. The +// options map carries the velocity / acceleration limits, path tolerance, +// sample rate, and optional path colinearization ratio; see the C ABI header +// for the full schema. The options map is read-only during construction and +// may be closed by the caller as soon as New returns. +func New(options *trajex.TensorMap) (*Session, error) { + optHandle := (*C.viam_trajex_tensor_map_t)(options.UnsafeHandle()) + var errOut *C.char + h := C.viam_trajex_totg_streaming_session_create(optHandle, &errOut) + if h == nil { + msg := C.GoString(errOut) + C.viam_trajex_string_destroy(errOut) + return nil, errors.Errorf("trajex/totg/streaming: New failed: %s", msg) + } + return &Session{handle: h}, nil +} + +// Close releases the underlying C session. Safe to call on a nil receiver +// and idempotent: subsequent calls are no-ops. +func (s *Session) Close() { + if s == nil || s.handle == nil { + return + } + C.viam_trajex_totg_streaming_session_destroy(s.handle) + s.handle = nil +} + +// Extend appends a waypoint batch to the session. The batch must contain a +// waypoints_rads tensor of shape [n_waypoints, n_dof]; on calls after the +// first, batch[0] must compare bit-exactly equal to the session's most +// recently stored waypoint (the seam contract). +// +// Extend honors ctx at entry and exit: if ctx is already cancelled when +// Extend is called, it returns ctx.Err() without invoking the C ABI; if ctx +// is cancelled while the C call is in flight (which itself cannot be +// interrupted), the result is discarded and ctx.Err() is returned. +func (s *Session) Extend(ctx context.Context, batch *trajex.TensorMap) error { + if err := ctx.Err(); err != nil { + return err + } + batchHandle := (*C.viam_trajex_tensor_map_t)(batch.UnsafeHandle()) + var errOut *C.char + rc := C.viam_trajex_totg_streaming_session_extend(s.handle, batchHandle, &errOut) + if rc != 0 { + msg := C.GoString(errOut) + C.viam_trajex_string_destroy(errOut) + return errors.Errorf("trajex/totg/streaming: Extend failed: %s", msg) + } + if err := ctx.Err(); err != nil { + return err + } + return nil +} + +// SampleNext pulls up to n samples into outputs. The output map's prior +// contents are replaced. If the session is exhausted, outputs carries +// zero-length sample tensors. +// +// Honors ctx like Extend. +func (s *Session) SampleNext(ctx context.Context, n int, outputs *trajex.TensorMap) error { + if err := ctx.Err(); err != nil { + return err + } + outHandle := (*C.viam_trajex_tensor_map_t)(outputs.UnsafeHandle()) + var errOut *C.char + rc := C.viam_trajex_totg_streaming_session_sample_next(s.handle, C.size_t(n), outHandle, &errOut) + if rc != 0 { + msg := C.GoString(errOut) + C.viam_trajex_string_destroy(errOut) + return errors.Errorf("trajex/totg/streaming: SampleNext failed: %s", msg) + } + if err := ctx.Err(); err != nil { + return err + } + return nil +} + +// SampleAtLeast pulls samples until the most recent sample's time is at least +// CurrentTime + horizon, writing them into outputs. +// +// Honors ctx like Extend. +func (s *Session) SampleAtLeast(ctx context.Context, horizon time.Duration, outputs *trajex.TensorMap) error { + if err := ctx.Err(); err != nil { + return err + } + outHandle := (*C.viam_trajex_tensor_map_t)(outputs.UnsafeHandle()) + var errOut *C.char + rc := C.viam_trajex_totg_streaming_session_sample_at_least(s.handle, C.double(horizon.Seconds()), outHandle, &errOut) + if rc != 0 { + msg := C.GoString(errOut) + C.viam_trajex_string_destroy(errOut) + return errors.Errorf("trajex/totg/streaming: SampleAtLeast failed: %s", msg) + } + if err := ctx.Err(); err != nil { + return err + } + return nil +} + +// CurrentTime returns the global time of the most recently emitted sample, +// or zero if no samples have been emitted yet. +func (s *Session) CurrentTime() time.Duration { + var out C.double + C.viam_trajex_totg_streaming_session_current_time_sec(s.handle, &out) + return time.Duration(float64(out) * float64(time.Second)) +} + +// GenerationCount returns the cumulative number of trajectories the session +// has installed as active (first build + each pivot + each rebase). Zero for +// a fresh session before the first Extend. +func (s *Session) GenerationCount() int64 { + var out C.int64_t + C.viam_trajex_totg_streaming_session_generation_count(s.handle, &out) + return int64(out) +} + +// HasActiveTrajectory reports whether the session has an active trajectory. +// False iff fresh (no successful Extend has occurred yet). +func (s *Session) HasActiveTrajectory() bool { + var out C.int + C.viam_trajex_totg_streaming_session_has_active_trajectory(s.handle, &out) + return out != 0 +} + +// ActiveDuration returns the duration of the active trajectory. Returns zero +// when no active trajectory is present. +func (s *Session) ActiveDuration() time.Duration { + var out C.double + C.viam_trajex_totg_streaming_session_active_duration_sec(s.handle, &out) + return time.Duration(float64(out) * float64(time.Second)) +} diff --git a/go/totg/streaming/session_test.go b/go/totg/streaming/session_test.go new file mode 100644 index 0000000..ba1fa1d --- /dev/null +++ b/go/totg/streaming/session_test.go @@ -0,0 +1,171 @@ +//go:build !windows && !no_cgo + +package streaming_test + +import ( + "context" + "testing" + "time" + + "go.viam.com/test" + + trajex "github.com/viam-modules/trajex/go" + "github.com/viam-modules/trajex/go/totg/streaming" +) + +// buildOptions constructs a session configuration tensor map: 2 DOF, +// conservative limits, 100 Hz sample rate. Caller must Close it. +func buildOptions(t *testing.T) *trajex.TensorMap { + t.Helper() + opts, err := trajex.NewTensorMap() + test.That(t, err, test.ShouldBeNil) + + test.That(t, opts.InsertFloat64s(streaming.KeyVelocityLimitsRadsPerSec, []uint64{2}, []float64{1.0, 1.0}), test.ShouldBeNil) + test.That(t, opts.InsertFloat64s(streaming.KeyAccelerationLimitsRadsPerSec2, []uint64{2}, []float64{1.0, 1.0}), test.ShouldBeNil) + test.That(t, opts.InsertScalarFloat64(streaming.KeyPathToleranceDeltaRads, 0.1), test.ShouldBeNil) + test.That(t, opts.InsertScalarFloat64(streaming.KeyTrajectorySamplingFreqHz, 100.0), test.ShouldBeNil) + + return opts +} + +// buildBatch constructs a waypoint batch tensor map carrying the supplied +// flat waypoint coordinates as [n, 2] (2 DOF). Caller must Close it. +func buildBatch(t *testing.T, waypoints []float64) *trajex.TensorMap { + t.Helper() + const dof = 2 + if len(waypoints)%dof != 0 { + t.Fatalf("buildBatch: waypoints length %d not divisible by dof %d", len(waypoints), dof) + } + n := uint64(len(waypoints) / dof) + + batch, err := trajex.NewTensorMap() + test.That(t, err, test.ShouldBeNil) + test.That(t, batch.InsertFloat64s(streaming.KeyWaypointsRads, []uint64{n, dof}, waypoints), test.ShouldBeNil) + return batch +} + +func TestSessionLifecycle(t *testing.T) { + opts := buildOptions(t) + defer opts.Close() + + sess, err := streaming.New(opts) + test.That(t, err, test.ShouldBeNil) + defer sess.Close() + + // Fresh session: no active trajectory, nothing sampled yet. + test.That(t, sess.HasActiveTrajectory(), test.ShouldBeFalse) + test.That(t, sess.GenerationCount(), test.ShouldEqual, int64(0)) + test.That(t, sess.CurrentTime(), test.ShouldEqual, time.Duration(0)) + test.That(t, sess.ActiveDuration(), test.ShouldEqual, time.Duration(0)) +} + +func TestSessionExtendAndSample(t *testing.T) { + opts := buildOptions(t) + defer opts.Close() + sess, err := streaming.New(opts) + test.That(t, err, test.ShouldBeNil) + defer sess.Close() + + // Bootstrap with three 2-DOF waypoints. + batch := buildBatch(t, []float64{ + 0.0, 0.0, + 1.0, 0.0, + 1.0, 1.0, + }) + defer batch.Close() + + test.That(t, sess.Extend(context.Background(), batch), test.ShouldBeNil) + + // Active trajectory should now exist. + test.That(t, sess.HasActiveTrajectory(), test.ShouldBeTrue) + test.That(t, sess.GenerationCount(), test.ShouldEqual, int64(1)) + test.That(t, sess.ActiveDuration(), test.ShouldBeGreaterThan, time.Duration(0)) + + // Pull 10 samples. The output map's contents are populated with the + // sample tensors. + out, err := trajex.NewTensorMap() + test.That(t, err, test.ShouldBeNil) + defer out.Close() + + test.That(t, sess.SampleNext(context.Background(), 10, out), test.ShouldBeNil) + + timesShape, timesData, ok, err := out.ViewFloat64s(streaming.KeySampleTimesSec) + test.That(t, err, test.ShouldBeNil) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, len(timesShape), test.ShouldEqual, 1) + test.That(t, timesShape[0], test.ShouldEqual, uint64(10)) + test.That(t, len(timesData), test.ShouldEqual, 10) + + cfgShape, cfgData, ok, err := out.ViewFloat64s(streaming.KeyConfigurationsRads) + test.That(t, err, test.ShouldBeNil) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, cfgShape, test.ShouldResemble, []uint64{10, 2}) + test.That(t, len(cfgData), test.ShouldEqual, 20) + + // Timestamps strictly monotonic and CurrentTime tracks the last sample. + for i := 1; i < len(timesData); i++ { + test.That(t, timesData[i], test.ShouldBeGreaterThan, timesData[i-1]) + } + expectedCurrent := time.Duration(timesData[len(timesData)-1] * float64(time.Second)) + // Allow ~1 ns slack for the float-seconds round-trip. + delta := sess.CurrentTime() - expectedCurrent + if delta < 0 { + delta = -delta + } + test.That(t, delta < time.Microsecond, test.ShouldBeTrue) +} + +func TestSessionSampleAtLeast(t *testing.T) { + opts := buildOptions(t) + defer opts.Close() + sess, err := streaming.New(opts) + test.That(t, err, test.ShouldBeNil) + defer sess.Close() + + batch := buildBatch(t, []float64{ + 0.0, 0.0, + 1.0, 0.0, + 1.0, 1.0, + }) + defer batch.Close() + test.That(t, sess.Extend(context.Background(), batch), test.ShouldBeNil) + + out, err := trajex.NewTensorMap() + test.That(t, err, test.ShouldBeNil) + defer out.Close() + + horizon := 100 * time.Millisecond + test.That(t, sess.SampleAtLeast(context.Background(), horizon, out), test.ShouldBeNil) + + test.That(t, sess.CurrentTime(), test.ShouldBeGreaterThanOrEqualTo, horizon) +} + +func TestSessionExtendCtxCancel(t *testing.T) { + opts := buildOptions(t) + defer opts.Close() + sess, err := streaming.New(opts) + test.That(t, err, test.ShouldBeNil) + defer sess.Close() + + batch := buildBatch(t, []float64{ + 0.0, 0.0, + 1.0, 1.0, + }) + defer batch.Close() + + ctx, cancel := context.WithCancel(context.Background()) + cancel() + err = sess.Extend(ctx, batch) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, "context") +} + +func TestSessionCloseIdempotent(t *testing.T) { + opts := buildOptions(t) + defer opts.Close() + sess, err := streaming.New(opts) + test.That(t, err, test.ShouldBeNil) + + sess.Close() + sess.Close() +} diff --git a/go/totg/totg_test.go b/go/totg/totg_test.go index 2062f9f..15f05e0 100644 --- a/go/totg/totg_test.go +++ b/go/totg/totg_test.go @@ -54,7 +54,7 @@ func buildMinimalInputs(t *testing.T) *trajex.TensorMap { test.That(t, in.InsertFloat64s(totg.KeyVelocityLimitsRadsPerSec, []uint64{2}, []float64{1.0, 1.0}), test.ShouldBeNil) test.That(t, in.InsertFloat64s(totg.KeyAccelerationLimitsRadsPerSec2, []uint64{2}, []float64{1.0, 1.0}), test.ShouldBeNil) test.That(t, in.InsertScalarFloat64(totg.KeyPathToleranceDeltaRads, 0.1), test.ShouldBeNil) - test.That(t, in.InsertScalarInt64(totg.KeyTrajectorySamplingFreqHz, 100), test.ShouldBeNil) + test.That(t, in.InsertScalarFloat64(totg.KeyTrajectorySamplingFreqHz, 100.0), test.ShouldBeNil) return in } diff --git a/src/viam/trajex/capi/capi.cpp b/src/viam/trajex/capi/capi.cpp index 84c6351..a849fde 100644 --- a/src/viam/trajex/capi/capi.cpp +++ b/src/viam/trajex/capi/capi.cpp @@ -25,6 +25,7 @@ #endif #include +#include #include #include #include @@ -61,13 +62,28 @@ struct viam_trajex_tensor_map { std::unordered_map> tensors; }; +// The opaque streaming session type. Wraps an in-place +// `viam::trajex::totg::streaming::session` plus a captured DOF count so empty-sample +// outputs (shape-zero tensors) still carry the correct second-dimension extent for +// the configuration / velocity / acceleration arrays. +struct viam_trajex_totg_streaming_session { + viam::trajex::totg::streaming::session sess; + std::size_t n_dof; + + viam_trajex_totg_streaming_session(viam::trajex::totg::path::options popts, + viam::trajex::totg::trajectory::options topts, + viam::trajex::types::hertz sample_rate, + std::size_t dof) + : sess(std::move(popts), std::move(topts), sample_rate), n_dof(dof) {} +}; + namespace { // Default values for optional inputs to viam_trajex_totg_generate, per the // schema documented in capi.h. constexpr double k_default_colinearization_ratio = 0.0; constexpr double k_default_dedup_tolerance = 1e-5; -constexpr std::int64_t k_default_sampling_freq_hz = 100; +constexpr double k_default_sampling_freq_hz = 100.0; bool dtype_is_valid(viam_trajex_dtype_t dtype) noexcept { switch (dtype) { @@ -198,12 +214,11 @@ viam_trajex_tensor_map totg_generate_impl(const viam_trajex_tensor_map& inputs) dedup_tolerance = (*xa)(0); } - auto sampling_freq_hz = k_default_sampling_freq_hz; - if (const auto* xa = find_xarray(inputs, viam_trajex_totg_key_trajectory_sampling_freq_hz)) { + auto sampling_freq = k_default_sampling_freq_hz; + if (const auto* xa = find_xarray(inputs, viam_trajex_totg_key_trajectory_sampling_freq_hz)) { require_scalar(*xa, viam_trajex_totg_key_trajectory_sampling_freq_hz); - sampling_freq_hz = (*xa)(0); + sampling_freq = (*xa)(0); } - const auto sampling_freq = static_cast(sampling_freq_hz); // Hand the waypoints xarray directly to the accumulator: it borrows // (the input map owns and outlives this function), no copy. @@ -263,6 +278,75 @@ viam_trajex_tensor_map totg_generate_impl(const viam_trajex_tensor_map& inputs) return out; } +// Build a streaming session from a config tensor map. Throws on missing required keys, +// dtype mismatches, or shape mismatches; the extern "C" wrapper converts exceptions to +// NULL with diagnostic. +std::unique_ptr build_streaming_session(const viam_trajex_tensor_map& options) { + namespace totg = viam::trajex::totg; + + const auto& velocity_limits = require_xarray(options, viam_trajex_totg_key_velocity_limits_rads_per_sec); + if (velocity_limits.dimension() != 1) { + throw std::invalid_argument("velocity_limits_rads_per_sec must be 1D [n_dof]"); + } + const auto n_dof = velocity_limits.shape(0); + + const auto& acceleration_limits = require_xarray(options, viam_trajex_totg_key_acceleration_limits_rads_per_sec2); + require_shape_1d(acceleration_limits, viam_trajex_totg_key_acceleration_limits_rads_per_sec2, n_dof); + + const auto& path_tolerance_xa = require_xarray(options, viam_trajex_totg_key_path_tolerance_delta_rads); + require_scalar(path_tolerance_xa, viam_trajex_totg_key_path_tolerance_delta_rads); + const auto path_tolerance = path_tolerance_xa(0); + + const auto& sample_rate_xa = require_xarray(options, viam_trajex_totg_key_trajectory_sampling_freq_hz); + require_scalar(sample_rate_xa, viam_trajex_totg_key_trajectory_sampling_freq_hz); + const auto sample_rate_hz = sample_rate_xa(0); + + auto colinearization_ratio = k_default_colinearization_ratio; + if (const auto* xa = find_xarray(options, viam_trajex_totg_key_path_colinearization_ratio)) { + require_scalar(*xa, viam_trajex_totg_key_path_colinearization_ratio); + colinearization_ratio = (*xa)(0); + } + + auto path_opts = totg::path::options{}.set_max_blend_deviation(path_tolerance); + if (colinearization_ratio > 0.0) { + path_opts.set_max_linear_deviation(path_tolerance * colinearization_ratio); + } + + totg::trajectory::options trajectory_opts; + trajectory_opts.max_velocity = velocity_limits; + trajectory_opts.max_acceleration = acceleration_limits; + + return std::make_unique( + std::move(path_opts), std::move(trajectory_opts), viam::trajex::types::hertz{sample_rate_hz}, n_dof); +} + +// Materialize a vector of trajectory samples into the four output tensors of the +// streaming sample schema. The shape-zero case (no samples) still emits all four keys +// with the right DOF-dimension extent, so the caller can read the shape uniformly. +viam_trajex_tensor_map materialize_samples(const std::vector& samples, std::size_t n_dof) { + const std::size_t n = samples.size(); + + using shape_t = typename xt::xarray::shape_type; + xt::xarray times = xt::zeros(shape_t{n}); + xt::xarray configurations = xt::zeros(shape_t{n, n_dof}); + xt::xarray velocities = xt::zeros(shape_t{n, n_dof}); + xt::xarray accelerations = xt::zeros(shape_t{n, n_dof}); + + for (std::size_t i = 0; i < n; ++i) { + times(i) = samples[i].time.count(); + xt::view(configurations, i, xt::all()) = samples[i].configuration; + xt::view(velocities, i, xt::all()) = samples[i].velocity; + xt::view(accelerations, i, xt::all()) = samples[i].acceleration; + } + + viam_trajex_tensor_map out; + out.tensors.emplace(viam_trajex_totg_key_sample_times_sec, std::move(times)); + out.tensors.emplace(viam_trajex_totg_key_configurations_rads, std::move(configurations)); + out.tensors.emplace(viam_trajex_totg_key_velocities_rads_per_sec, std::move(velocities)); + out.tensors.emplace(viam_trajex_totg_key_accelerations_rads_per_sec2, std::move(accelerations)); + return out; +} + // Allocate a NUL-terminated copy of `msg` via new[]. Bad-alloc inside a // boundary catch handler is intentional fail-fast (per the design doc): // cascading bad_alloc during exception handling triggers std::terminate @@ -455,4 +539,152 @@ int viam_trajex_totg_generate(const viam_trajex_tensor_map_t* inputs, viam_traje } } +viam_trajex_totg_streaming_session_t* viam_trajex_totg_streaming_session_create(const viam_trajex_tensor_map_t* options, + const char** error_out) { + if (error_out) { + *error_out = nullptr; + } + try { + if (!options) { + throw std::invalid_argument("options is null"); + } + return build_streaming_session(*options).release(); + } catch (const std::exception& e) { + if (error_out) { + *error_out = duplicate_error_string(e.what()); + } + return nullptr; + } catch (...) { + if (error_out) { + *error_out = duplicate_error_string("unknown exception"); + } + return nullptr; + } +} + +void viam_trajex_totg_streaming_session_destroy(viam_trajex_totg_streaming_session_t* session) { + try { + delete session; + } catch (...) { // NOLINT(bugprone-empty-catch) + // Defensive boundary; session destructor should be noexcept. + } +} + +int viam_trajex_totg_streaming_session_extend(viam_trajex_totg_streaming_session_t* session, + const viam_trajex_tensor_map_t* batch, + const char** error_out) { + if (error_out) { + *error_out = nullptr; + } + try { + if (!session) { + throw std::invalid_argument("session is null"); + } + if (!batch) { + throw std::invalid_argument("batch is null"); + } + const auto& waypoints = require_xarray(*batch, viam_trajex_totg_key_waypoints_rads); + if (waypoints.dimension() != 2) { + throw std::invalid_argument("waypoints_rads must be 2D [n_waypoints, n_dof]"); + } + if (waypoints.shape(1) != session->n_dof) { + throw std::invalid_argument("waypoints_rads DOF does not match session DOF"); + } + // Accumulator borrows from `waypoints` (owned by the input map, which the caller + // guarantees outlives this call). The session's extend internally copies any + // waypoints it retains, so the accumulator's lifetime ending at function exit is + // fine. + const viam::trajex::totg::waypoint_accumulator acc(waypoints); + session->sess.extend(acc); + return 0; + } catch (const std::exception& e) { + if (error_out) { + *error_out = duplicate_error_string(e.what()); + } + return -1; + } catch (...) { + if (error_out) { + *error_out = duplicate_error_string("unknown exception"); + } + return -1; + } +} + +int viam_trajex_totg_streaming_session_sample_next(viam_trajex_totg_streaming_session_t* session, + std::size_t n, + viam_trajex_tensor_map_t* outputs, + const char** error_out) { + if (error_out) { + *error_out = nullptr; + } + try { + if (!session) { + throw std::invalid_argument("session is null"); + } + if (!outputs) { + throw std::invalid_argument("outputs is null"); + } + auto samples = session->sess.sample_next(n); + *outputs = materialize_samples(samples, session->n_dof); + return 0; + } catch (const std::exception& e) { + if (error_out) { + *error_out = duplicate_error_string(e.what()); + } + return -1; + } catch (...) { + if (error_out) { + *error_out = duplicate_error_string("unknown exception"); + } + return -1; + } +} + +int viam_trajex_totg_streaming_session_sample_at_least(viam_trajex_totg_streaming_session_t* session, + double horizon_sec, + viam_trajex_tensor_map_t* outputs, + const char** error_out) { + if (error_out) { + *error_out = nullptr; + } + try { + if (!session) { + throw std::invalid_argument("session is null"); + } + if (!outputs) { + throw std::invalid_argument("outputs is null"); + } + auto samples = session->sess.sample_at_least(viam::trajex::totg::trajectory::seconds{horizon_sec}); + *outputs = materialize_samples(samples, session->n_dof); + return 0; + } catch (const std::exception& e) { + if (error_out) { + *error_out = duplicate_error_string(e.what()); + } + return -1; + } catch (...) { + if (error_out) { + *error_out = duplicate_error_string("unknown exception"); + } + return -1; + } +} + +void viam_trajex_totg_streaming_session_current_time_sec(const viam_trajex_totg_streaming_session_t* session, double* out) { + *out = session->sess.current_time().count(); +} + +void viam_trajex_totg_streaming_session_generation_count(const viam_trajex_totg_streaming_session_t* session, std::int64_t* out) { + *out = static_cast(session->sess.trajectory_generation_count()); +} + +void viam_trajex_totg_streaming_session_has_active_trajectory(const viam_trajex_totg_streaming_session_t* session, int* out) { + *out = session->sess.active_trajectory() != nullptr ? 1 : 0; +} + +void viam_trajex_totg_streaming_session_active_duration_sec(const viam_trajex_totg_streaming_session_t* session, double* out) { + const auto* active = session->sess.active_trajectory(); + *out = active ? active->duration().count() : 0.0; +} + } // extern "C" diff --git a/src/viam/trajex/capi/capi.h b/src/viam/trajex/capi/capi.h index 7dcd2e3..07f1115 100644 --- a/src/viam/trajex/capi/capi.h +++ b/src/viam/trajex/capi/capi.h @@ -317,7 +317,7 @@ extern const char viam_trajex_totg_key_waypoints_rads[]; /// |-----|-------|-------|---------|---------| /// | `path_colinearization_ratio` | F64 | `[1]` | `0.0` (off) | Colinearization aggressiveness | /// | `waypoint_deduplication_tolerance_rads` | F64 | `[1]` | `1e-5` | Waypoints closer than this are merged | -/// | `trajectory_sampling_freq_hz` | I64 | `[1]` | `100` | Output sample rate in Hz | +/// | `trajectory_sampling_freq_hz` | F64 | `[1]` | `100.0` | Output sample rate in Hz | /// /// ## Outputs /// @@ -354,6 +354,196 @@ int viam_trajex_totg_generate(const viam_trajex_tensor_map_t* VIAM_TRAJEX_RESTRI /// @} +/// +/// @defgroup viam_trajex_totg_streaming TOTG: streaming session +/// +/// Stateful, incremental trajectory generation. Waypoint batches arrive over time via +/// `extend`; samples are pulled via `sample_next` / `sample_at_least`. The session +/// internally manages pivots (smooth re-plan onto a new trajectory when admissible) and +/// rebases (start a fresh chain when the prior active drains while batches are queued). +/// +/// All entry points operate on an opaque session handle constructed by +/// `viam_trajex_totg_streaming_session_create` and released by +/// `viam_trajex_totg_streaming_session_destroy`. The library carries no global state; +/// independent sessions on different threads need no synchronization, but concurrent +/// operations on the same session are the caller's responsibility. +/// +/// Streaming reuses the schema keys from `viam_trajex_totg_generate` for waypoint and +/// sample-output tensors. The only streaming-specific addition is a tighter contract +/// on the sample rate (required at session construction rather than optional). +/// +/// @{ +/// + +/// +/// Opaque streaming session handle. Construct via +/// `viam_trajex_totg_streaming_session_create`; release via +/// `viam_trajex_totg_streaming_session_destroy`. +/// +typedef struct viam_trajex_totg_streaming_session viam_trajex_totg_streaming_session_t; + +/// +/// Construct a streaming session from a tensor map carrying the session's fixed +/// configuration. Inputs map is consumed read-only and may be freed by the caller as +/// soon as this entry point returns. +/// +/// ## Required inputs +/// +/// | Key | Dtype | Shape | Meaning | +/// |-----|-------|-------|---------| +/// | `velocity_limits_rads_per_sec` | F64 | `[n_dof]` | Per-joint maximum velocity; defines DOF | +/// | `acceleration_limits_rads_per_sec2` | F64 | `[n_dof]` | Per-joint maximum acceleration | +/// | `path_tolerance_delta_rads` | F64 | `[1]` | Path-blending tolerance | +/// | `trajectory_sampling_freq_hz` | F64 | `[1]` | Sample rate in Hz (required for streaming) | +/// +/// ## Optional inputs +/// +/// | Key | Dtype | Shape | Default | Meaning | +/// |-----|-------|-------|---------|---------| +/// | `path_colinearization_ratio` | F64 | `[1]` | `0.0` (off) | Colinearization aggressiveness | +/// +/// @param options Tensor map carrying the configuration keys above. Must not be NULL. +/// +/// @param error_out On NULL return, receives a newly-allocated NUL-terminated diagnostic +/// string the caller releases via `viam_trajex_string_destroy`. May be +/// NULL if the caller does not need a diagnostic. +/// +/// @return Newly-allocated session handle, or NULL on bad arguments, allocation failure, +/// or a caught exception during construction. +/// +viam_trajex_totg_streaming_session_t* viam_trajex_totg_streaming_session_create(const viam_trajex_tensor_map_t* options, + const char** error_out); + +/// +/// Destroy a streaming session and release all owned resources. +/// +/// @param session Session to destroy. NULL is a no-op. +/// +void viam_trajex_totg_streaming_session_destroy(viam_trajex_totg_streaming_session_t* session); + +/// +/// Add a waypoint batch to the session. The batch may either pivot the active trajectory +/// (smooth re-plan onto a new chain) or stage for later absorption into the next rebase; +/// the choice is made internally based on whether the candidate trajectory's branch point +/// sits ahead of the consumer's watermark. The decision is invisible to the caller. +/// +/// On the first call, the batch bootstraps the initial trajectory. On subsequent calls, +/// `batch[0]` must compare bit-exactly equal to the session's most recently stored +/// waypoint (the "seam" requirement); the seam is dropped before the remainder is +/// absorbed. Callers are responsible for deduplicating adjacent waypoints in the batch. +/// +/// ## Required inputs +/// +/// | Key | Dtype | Shape | Meaning | +/// |-----|-------|-------|---------| +/// | `waypoints_rads` | F64 | `[n_waypoints, n_dof]` | Waypoint batch including seam | +/// +/// @param session Session handle. Must not be NULL. +/// +/// @param batch Tensor map carrying the waypoints batch. Must not be NULL and must not +/// alias any tensor map the caller is using elsewhere on this thread. +/// +/// @param error_out On `-1` return, receives a newly-allocated diagnostic string the +/// caller releases via `viam_trajex_string_destroy`. May be NULL. +/// +/// @return 0 on success, -1 on bad arguments or any caught exception (seam mismatch, +/// DOF mismatch, trajectory generation failure, etc.). +/// +int viam_trajex_totg_streaming_session_extend(viam_trajex_totg_streaming_session_t* VIAM_TRAJEX_RESTRICT session, + const viam_trajex_tensor_map_t* VIAM_TRAJEX_RESTRICT batch, + const char** error_out); + +/// +/// Pull up to `n` samples from the session, writing them into `outputs`. Any prior +/// contents of `outputs` are replaced. If the session is exhausted (no more samples +/// available), `outputs` will carry zero-length sample tensors. +/// +/// ## Outputs +/// +/// | Key | Dtype | Shape | Meaning | +/// |-----|-------|-------|---------| +/// | `sample_times_sec` | F64 | `[k]` | Sample timestamps in global time (`k <= n`) | +/// | `configurations_rads` | F64 | `[k, n_dof]` | Joint positions at each sample | +/// | `velocities_rads_per_sec` | F64 | `[k, n_dof]` | Joint velocities at each sample | +/// | `accelerations_rads_per_sec2` | F64 | `[k, n_dof]` | Joint accelerations at each sample | +/// +/// @param session Session handle. Must not be NULL. +/// +/// @param n Maximum number of samples to produce. +/// +/// @param outputs Caller-allocated output tensor map. Contents replaced on success; left +/// unchanged on failure. Must not be NULL and must be distinct from any +/// input map. +/// +/// @param error_out On `-1` return, receives a newly-allocated diagnostic string the +/// caller releases via `viam_trajex_string_destroy`. May be NULL. +/// +/// @return 0 on success, -1 on bad arguments or caught exception. +/// +int viam_trajex_totg_streaming_session_sample_next(viam_trajex_totg_streaming_session_t* VIAM_TRAJEX_RESTRICT session, + size_t n, + viam_trajex_tensor_map_t* VIAM_TRAJEX_RESTRICT outputs, + const char** error_out); + +/// +/// Pull samples until the most recent sample's time is at least +/// `current_time + horizon_sec`, writing them into `outputs`. Returns fewer samples if +/// the session is exhausted. See `viam_trajex_totg_streaming_session_sample_next` for +/// the output schema. +/// +/// @param session Session handle. Must not be NULL. +/// +/// @param horizon_sec Minimum amount of trajectory time to advance before stopping. +/// +/// @param outputs Caller-allocated output tensor map. Contents replaced on success. +/// +/// @param error_out See `sample_next`. +/// +/// @return 0 on success, -1 on bad arguments or caught exception. +/// +int viam_trajex_totg_streaming_session_sample_at_least(viam_trajex_totg_streaming_session_t* VIAM_TRAJEX_RESTRICT session, + double horizon_sec, + viam_trajex_tensor_map_t* VIAM_TRAJEX_RESTRICT outputs, + const char** error_out); + +/// +/// @name Session status accessors +/// +/// Scalar getters for session state. These cannot fail given a non-NULL handle; the +/// session is either valid or its destruction has been called and using the handle +/// is undefined behavior. Passing NULL for `session` or for an output pointer is +/// undefined. +/// +/// @{ +/// + +/// +/// Global time of the most recently emitted sample, or zero if no samples have been +/// emitted yet. +/// +void viam_trajex_totg_streaming_session_current_time_sec(const viam_trajex_totg_streaming_session_t* session, double* out); + +/// +/// Cumulative count of trajectories the session has installed as active (first build + +/// each pivot + each rebase). Zero for a fresh session before the first `extend`. +/// +void viam_trajex_totg_streaming_session_generation_count(const viam_trajex_totg_streaming_session_t* session, int64_t* out); + +/// +/// Nonzero iff the session has an active trajectory (any successful `extend` has +/// occurred). Zero iff fresh. +/// +void viam_trajex_totg_streaming_session_has_active_trajectory(const viam_trajex_totg_streaming_session_t* session, int* out); + +/// +/// Duration of the active trajectory in seconds. Returns zero if no active trajectory. +/// +void viam_trajex_totg_streaming_session_active_duration_sec(const viam_trajex_totg_streaming_session_t* session, double* out); + +/// @} + +/// @} + #ifdef __cplusplus } // extern "C" #endif diff --git a/src/viam/trajex/capi/test/capi_compile_smoke.c b/src/viam/trajex/capi/test/capi_compile_smoke.c index 0b301b6..514b5fd 100644 --- a/src/viam/trajex/capi/test/capi_compile_smoke.c +++ b/src/viam/trajex/capi/test/capi_compile_smoke.c @@ -37,6 +37,17 @@ int main(void) { (void)viam_trajex_string_destroy; (void)viam_trajex_totg_generate; + // Streaming session entry points. + (void)viam_trajex_totg_streaming_session_create; + (void)viam_trajex_totg_streaming_session_destroy; + (void)viam_trajex_totg_streaming_session_extend; + (void)viam_trajex_totg_streaming_session_sample_next; + (void)viam_trajex_totg_streaming_session_sample_at_least; + (void)viam_trajex_totg_streaming_session_current_time_sec; + (void)viam_trajex_totg_streaming_session_generation_count; + (void)viam_trajex_totg_streaming_session_has_active_trajectory; + (void)viam_trajex_totg_streaming_session_active_duration_sec; + // Force-resolve every exported key constant. (void)viam_trajex_totg_key_waypoints_rads; (void)viam_trajex_totg_key_velocity_limits_rads_per_sec; diff --git a/src/viam/trajex/service/mlmodel.cpp b/src/viam/trajex/service/mlmodel.cpp index 1cf44c6..ccd1917 100644 --- a/src/viam/trajex/service/mlmodel.cpp +++ b/src/viam/trajex/service/mlmodel.cpp @@ -202,6 +202,8 @@ std::shared_ptr mlmodel::infer(const named_tensor_v .acceleration_limits = std::move(acceleration_limits), .path_blend_tolerance = path_tolerance, .colinearization_ratio = colinearization_ratio, + .min_blend_curvature = std::nullopt, + .max_blend_curvature = std::nullopt, .segment_totg = config_.segment_for_trajex, }); diff --git a/src/viam/trajex/totg/scripts/streaming_sim_plot.py b/src/viam/trajex/totg/scripts/streaming_sim_plot.py new file mode 100755 index 0000000..de21c81 --- /dev/null +++ b/src/viam/trajex/totg/scripts/streaming_sim_plot.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +""" +Plot a streaming session simulation result CSV. + +Reads the CSV emitted by viam-trajex-totg-streaming-sim. Renders a single +figure: (W_c x W_r) grid with a viridis colormap over rebase count, a bold +contour at rebases == 0 (the rebase-free frontier), and starved cells +overlaid in red and annotated with the waypoint index where starvation hit. + +Numpy + matplotlib only. + +Usage: + streaming_sim_plot.py +""" + +import csv +import sys + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.colors import ListedColormap + + +def load_csv(path): + """Return (metadata_dict, list-of-rows). + + Header is `#`-prefixed `key: value` lines preceding the column header. + Rows are dicts with `commit_window`, `replan_budget`, `rebases`, and + `starved_at_waypoint` (None if completed). + """ + metadata = {} + rows = [] + with open(path, newline='') as f: + # Pull leading `#` lines for metadata; everything after goes to csv.reader. + body_lines = [] + for line in f: + if line.startswith('#'): + stripped = line[1:].strip() + if ':' in stripped: + key, _, value = stripped.partition(':') + metadata[key.strip()] = value.strip() + else: + body_lines.append(line) + reader = csv.DictReader(body_lines) + for row in reader: + starved = row['starved_at_waypoint'] + rows.append({ + 'commit_window': float(row['commit_window']), + 'replan_budget': float(row['replan_budget']), + 'rebases': int(row['rebases']), + 'starved_at_waypoint': int(starved) if starved else None, + }) + return metadata, rows + + +def assemble_grid(rows): + """Pivot the cell rows into 2D arrays indexed by (W_r, W_c). + + Returns: + w_c_axis (1D), w_r_axis (1D), rebases (2D shape (n_r, n_c)), + starved (2D shape (n_r, n_c), np.nan for not-starved). + """ + w_c_vals = sorted({r['commit_window'] for r in rows}) + w_r_vals = sorted({r['replan_budget'] for r in rows}) + w_c_index = {v: i for i, v in enumerate(w_c_vals)} + w_r_index = {v: i for i, v in enumerate(w_r_vals)} + + rebases = np.full((len(w_r_vals), len(w_c_vals)), np.nan) + starved = np.full((len(w_r_vals), len(w_c_vals)), np.nan) + for r in rows: + i = w_r_index[r['replan_budget']] + j = w_c_index[r['commit_window']] + rebases[i, j] = r['rebases'] + if r['starved_at_waypoint'] is not None: + starved[i, j] = r['starved_at_waypoint'] + + return np.array(w_c_vals), np.array(w_r_vals), rebases, starved + + +def cell_edges(centers): + """Compute pcolormesh edge coordinates from cell-center values on a + geometric grid. Edges sit at the geometric midpoints between centers; + the outer edges extend by the same log-step the centers use. + """ + log = np.log(centers) + mids = (log[:-1] + log[1:]) / 2.0 + first = log[0] - (log[1] - log[0]) / 2.0 + last = log[-1] + (log[-1] - log[-2]) / 2.0 + return np.exp(np.concatenate([[first], mids, [last]])) + + +def plot(metadata, w_c_axis, w_r_axis, rebases, starved, output_path): + fig, ax = plt.subplots(figsize=(10, 7)) + + w_c_edges = cell_edges(w_c_axis) + w_r_edges = cell_edges(w_r_axis) + + # Background: rebase count. Use vmin=0 so zero-rebase cells sit at the + # darkest end of viridis; starved cells get overlaid separately. + rebases_for_plot = np.where(np.isnan(starved), rebases, np.nan) + vmax = max(1, int(np.nanmax(rebases)) if np.any(~np.isnan(rebases)) else 1) + mesh = ax.pcolormesh( + w_c_edges, w_r_edges, rebases_for_plot, + cmap='viridis', vmin=0, vmax=vmax, shading='flat', + ) + + # Starved cells: solid red overlay with the starve waypoint annotated. + starved_mask = ~np.isnan(starved) + if np.any(starved_mask): + red_cmap = ListedColormap([(0.85, 0.15, 0.15, 1.0)]) + ax.pcolormesh( + w_c_edges, w_r_edges, + np.where(starved_mask, 1.0, np.nan), + cmap=red_cmap, shading='flat', + ) + for i in range(starved.shape[0]): + for j in range(starved.shape[1]): + if starved_mask[i, j]: + ax.text( + w_c_axis[j], w_r_axis[i], + f'{int(starved[i, j])}', + ha='center', va='center', + color='white', fontsize=7, fontweight='bold', + ) + + # Bold contour at rebases == 0 (the rebase-free frontier). pcolormesh + # uses edges, but contour wants centers; draw it on the centers grid. + # Mask out starved cells so the contour isn't tugged by red region values. + rebase_field = np.where(starved_mask, np.nan, rebases) + if np.any(rebase_field == 0) and np.any(rebase_field > 0): + ax.contour( + w_c_axis, w_r_axis, rebase_field, + levels=[0.5], colors='white', linewidths=2.5, + ) + + ax.set_xscale('log') + ax.set_yscale('log') + ax.set_xlabel('commit window W_c (seconds)') + ax.set_ylabel('replan budget W_r (seconds)') + + title_bits = [metadata.get('workload', 'unknown workload')] + if 'n_waypoints' in metadata: + title_bits.append(f"N={metadata['n_waypoints']}") + if 'batch_size' in metadata: + title_bits.append(f"batch={metadata['batch_size']}") + if 'speed_factor' in metadata: + title_bits.append(f"speed={metadata['speed_factor']}") + if 'sample_rate_hz' in metadata: + title_bits.append(f"{metadata['sample_rate_hz']}Hz") + ax.set_title(' '.join(title_bits)) + + cbar = fig.colorbar(mesh, ax=ax) + cbar.set_label('rebases (red = starved, label = starve waypoint)') + + fig.tight_layout() + fig.savefig(output_path, dpi=150) + print(f'wrote {output_path}', file=sys.stderr) + + +def main(): + if len(sys.argv) != 3: + print('usage: streaming_sim_plot.py ', file=sys.stderr) + sys.exit(2) + input_path = sys.argv[1] + output_path = sys.argv[2] + + metadata, rows = load_csv(input_path) + if not rows: + print('error: no rows in input CSV', file=sys.stderr) + sys.exit(1) + w_c_axis, w_r_axis, rebases, starved = assemble_grid(rows) + plot(metadata, w_c_axis, w_r_axis, rebases, starved, output_path) + + +if __name__ == '__main__': + main() diff --git a/src/viam/trajex/totg/scripts/waypoints_to_replay.py b/src/viam/trajex/totg/scripts/waypoints_to_replay.py new file mode 100755 index 0000000..385465b --- /dev/null +++ b/src/viam/trajex/totg/scripts/waypoints_to_replay.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 +""" +Convert a raw joint-space waypoint dump into a canonical TOTG replay record. + +Input is a JSON array of arrays: N rows, each row a DOF-length list of joint +positions in radians. The output conforms to the schema consumed by +viam::trajex::totg::replay_planner::create (see src/viam/trajex/totg/tools/replay.cpp). + +The converter does NOT deduplicate waypoints. Duplicate-row handling is left to +the consumer of the replay record (e.g., the test that drives a streaming +session). + +Velocity, acceleration, and path-tolerance limits are accepted on the command +line in degree units for readability and converted to radians internally to +match the schema. + +Typical usage: + + waypoints_to_replay.py \\ + --max-vel-deg-per-sec 20 \\ + --max-accel-deg-per-sec2 20 \\ + --output path/to/output.trajex-totg-replay.json \\ + path/to/raw_waypoints.json + +Streams default to stdin/stdout when the positional input or `--output` is +omitted (or set to '-'), enabling pipeline usage: + + cat raw.json | waypoints_to_replay.py --max-vel-deg-per-sec 20 \\ + --max-accel-deg-per-sec2 20 > out.json +""" + +import argparse +import datetime +import json +import math +import sys +from pathlib import Path + + +SCHEMA_VERSION = 1 + +# The converter intentionally holds no opinions on the optional trajex +# parameters (blend curvature bounds, colinearization ratio, path tolerance). +# When a CLI flag is not provided, the corresponding JSON field is omitted from +# the record, and the C++ replay deserializer falls back to whichever trajex +# default applies (path::options defaults for the curvature bounds, nullopt for +# colinearization_ratio, and planner_base::config's 0.0 for path tolerance). +# Use of this tool with incomplete overrides is the user's responsibility: the +# point is to merge waypoints with externally-supplied parameters, not to guess +# the parameters. + + +def load_waypoints(stream, source_label): + """Read a raw waypoint dump from `stream` and return (waypoints, dof). + + `source_label` is used purely for error messages so callers can convey the + origin (a path or "") without the loader caring which. + + Validates that the input is a non-empty rectangular array of arrays with at + least two rows. Raises ValueError with a descriptive message on any + structural problem. + """ + data = json.load(stream) + + if not isinstance(data, list) or not data: + raise ValueError(f"{source_label}: expected a non-empty JSON array of waypoints") + if len(data) < 2: + raise ValueError(f"{source_label}: at least two waypoints required, got {len(data)}") + + dof = None + for i, row in enumerate(data): + if not isinstance(row, list): + raise ValueError(f"{source_label}: waypoint {i} is not an array") + if dof is None: + dof = len(row) + if dof == 0: + raise ValueError(f"{source_label}: waypoint 0 has zero joints") + elif len(row) != dof: + raise ValueError( + f"{source_label}: waypoint {i} has {len(row)} joints, " + f"expected {dof} (matching waypoint 0)" + ) + for j, v in enumerate(row): + if not isinstance(v, (int, float)): + raise ValueError(f"{source_label}: waypoint {i}, joint {j} is not numeric") + + return data, dof + + +def build_record(waypoints, dof, args): + """Assemble the replay record as a dict matching the canonical schema. + + Only the mandatory fields (vel/accel limits, waypoints) and the optional + fields the user explicitly supplied on the command line are emitted. + Omission is meaningful: the deserializer treats absent optional fields as + "use the trajex default" rather than "default to whatever the script picks." + """ + deg_to_rad = math.pi / 180.0 + vel_rad_per_sec = args.max_vel_deg_per_sec * deg_to_rad + accel_rad_per_sec2 = args.max_accel_deg_per_sec2 * deg_to_rad + + # The replay deserializer keys on field names; ordering does not matter for + # parsing. json.dumps(sort_keys=True) downstream renders the file with the + # same alphabetical ordering used by the C++ jsoncpp StyledWriter for the + # existing replay records, so converted files visually match precedent. + record = { + "max_acceleration_vec_rads_per_sec2": [accel_rad_per_sec2] * dof, + "max_velocity_vec_rads_per_sec": [vel_rad_per_sec] * dof, + "schema_version": SCHEMA_VERSION, + "timestamp": datetime.datetime.now(datetime.timezone.utc).isoformat().replace("+00:00", "Z"), + "waypoints_rads": waypoints, + } + if args.max_blend_curvature is not None: + record["max_blend_curvature"] = args.max_blend_curvature + if args.min_blend_curvature is not None: + record["min_blend_curvature"] = args.min_blend_curvature + if args.path_colinearization_ratio is not None: + record["path_colinearization_ratio"] = args.path_colinearization_ratio + if args.path_tolerance_deg is not None: + record["path_tolerance_delta_rads"] = args.path_tolerance_deg * deg_to_rad + return record + + +def parse_args(argv): + parser = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + parser.add_argument( + "input", + nargs="?", + default="-", + help=( + "Path to the raw waypoint dump (JSON array of arrays, radians). " + "Use '-' or omit to read from stdin." + ), + ) + parser.add_argument( + "--output", + "-o", + default="-", + help=( + "Path to write the replay record to. Use '-' or omit to write to stdout." + ), + ) + parser.add_argument( + "--max-vel-deg-per-sec", + type=float, + required=True, + help="Per-joint velocity limit, applied uniformly across all DOF.", + ) + parser.add_argument( + "--max-accel-deg-per-sec2", + type=float, + required=True, + help="Per-joint acceleration limit, applied uniformly across all DOF.", + ) + parser.add_argument( + "--max-blend-curvature", + type=float, + default=None, + help="Upper bound on blend curvature. Omitted from output if unset.", + ) + parser.add_argument( + "--min-blend-curvature", + type=float, + default=None, + help="Lower bound on blend curvature. Omitted from output if unset.", + ) + parser.add_argument( + "--path-colinearization-ratio", + type=float, + default=None, + help="Path colinearization ratio. Omitted from output if unset.", + ) + parser.add_argument( + "--path-tolerance-deg", + type=float, + default=None, + help="Path blend tolerance, in degrees. Omitted from output if unset.", + ) + return parser.parse_args(argv) + + +def main(argv=None): + args = parse_args(argv) + + try: + if args.input == "-": + waypoints, dof = load_waypoints(sys.stdin, "") + else: + input_path = Path(args.input) + with open(input_path, "r") as f: + waypoints, dof = load_waypoints(f, str(input_path)) + except (OSError, ValueError, json.JSONDecodeError) as e: + print(f"error: {e}", file=sys.stderr) + return 1 + + record = build_record(waypoints, dof, args) + + if args.output == "-": + json.dump(record, sys.stdout, indent=1, sort_keys=True) + sys.stdout.write("\n") + else: + output_path = Path(args.output) + output_path.parent.mkdir(parents=True, exist_ok=True) + with open(output_path, "w") as f: + json.dump(record, f, indent=1, sort_keys=True) + f.write("\n") + print( + f"wrote {output_path} (dof={dof}, waypoints={len(waypoints)})", + file=sys.stderr, + ) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/viam/trajex/totg/streaming/session.cpp b/src/viam/trajex/totg/streaming/session.cpp new file mode 100644 index 0000000..8726660 --- /dev/null +++ b/src/viam/trajex/totg/streaming/session.cpp @@ -0,0 +1,343 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace viam::trajex::totg::streaming { + +namespace { + +// These helpers materialize accumulator row-views and 2D-xarray slices into owned xarrays. +// The session needs stable storage independent of caller-provided accumulators because +// waypoint_accumulator holds row-views into source arrays it does not own. + +xt::xarray view_to_xarray_(const waypoint_accumulator::value_type& row) { + const std::size_t dof = row.shape(0); + xt::xarray result = xt::zeros(std::vector{dof}); + for (std::size_t j = 0; j < dof; ++j) { + result(j) = row(j); + } + return result; +} + +xt::xarray row_to_xarray_(const xt::xarray& arr, std::size_t row) { + const std::size_t dof = arr.shape(1); + xt::xarray result = xt::zeros(std::vector{dof}); + for (std::size_t j = 0; j < dof; ++j) { + result(j) = arr(row, j); + } + return result; +} + +bool rows_bit_exact_(const waypoint_accumulator::value_type& a, const xt::xarray& b) { + if (a.shape(0) != b.shape(0)) { + return false; + } + for (std::size_t i = 0; i < b.shape(0); ++i) { + if (a(i) != b(i)) { + return false; + } + } + return true; +} + +xt::xarray accumulator_to_xarray_(const waypoint_accumulator& batch) { + const std::size_t count = batch.size(); + const std::size_t dof = batch.dof(); + xt::xarray result = xt::zeros(std::vector{count, dof}); + for (std::size_t i = 0; i < count; ++i) { + const auto& row = batch.at(i); + for (std::size_t j = 0; j < dof; ++j) { + result(i, j) = row(j); + } + } + return result; +} + +// Caller must ensure batch.size() > from. +xt::xarray accumulator_tail_to_xarray_(const waypoint_accumulator& batch, std::size_t from) { + const std::size_t count = batch.size() - from; + const std::size_t dof = batch.dof(); + xt::xarray result = xt::zeros(std::vector{count, dof}); + for (std::size_t i = 0; i < count; ++i) { + const auto& row = batch.at(from + i); + for (std::size_t j = 0; j < dof; ++j) { + result(i, j) = row(j); + } + } + return result; +} + +xt::xarray concat_active_with_batch_tail_(const xt::xarray& base, + const waypoint_accumulator& batch, + std::size_t batch_from) { + const std::size_t n_base = base.shape(0); + const std::size_t n_add = batch.size() - batch_from; + const std::size_t dof = base.shape(1); + xt::xarray result = xt::zeros(std::vector{n_base + n_add, dof}); + for (std::size_t i = 0; i < n_base; ++i) { + for (std::size_t j = 0; j < dof; ++j) { + result(i, j) = base(i, j); + } + } + for (std::size_t i = 0; i < n_add; ++i) { + const auto& row = batch.at(batch_from + i); + for (std::size_t j = 0; j < dof; ++j) { + result(n_base + i, j) = row(j); + } + } + return result; +} + +xt::xarray stack_anchor_and_staged_(const xt::xarray& anchor, const std::vector>& staged) { + const std::size_t dof = anchor.shape(0); + std::size_t total_rows = 1; + for (const auto& s : staged) { + total_rows += s.shape(0); + } + xt::xarray result = xt::zeros(std::vector{total_rows, dof}); + for (std::size_t j = 0; j < dof; ++j) { + result(0, j) = anchor(j); + } + std::size_t row = 1; + for (const auto& s : staged) { + for (std::size_t i = 0; i < s.shape(0); ++i) { + for (std::size_t j = 0; j < dof; ++j) { + result(row, j) = s(i, j); + } + ++row; + } + } + return result; +} + +// Returns the local time of the first divergence between `active`'s integration points +// and `candidate`'s integration points, walking them in lockstep. If `active`'s entire +// integration-point sequence is a prefix of `candidate`'s, returns the active's duration +// (the branch effectively sits at the end of active). +trajectory::seconds find_branch_local_time_(const trajectory& active, const trajectory& candidate) { + const auto& active_pts = active.get_integration_points(); + const auto& candidate_pts = candidate.get_integration_points(); + const auto result = std::ranges::mismatch(active_pts, candidate_pts); + if (result.in1 == active_pts.end()) { + return active.duration(); + } + return result.in1->time; +} + +} // namespace + +namespace { + +trajectory::seconds validate_sample_rate_and_compute_period_(types::hertz sample_rate) { + if (!std::isfinite(sample_rate.value) || sample_rate.value <= 0.0) { + throw std::invalid_argument("streaming::session: sample_rate must be positive and finite"); + } + return trajectory::seconds{1.0 / sample_rate.value}; +} + +} // namespace + +session::session(path::options path_options, trajectory::options trajectory_options, types::hertz sample_rate) + : path_options_(std::move(path_options)), + trajectory_options_(std::move(trajectory_options)), + sample_rate_(sample_rate), + sample_period_(validate_sample_rate_and_compute_period_(sample_rate)) {} + +trajectory session::build_trajectory_from_(const xt::xarray& waypoints) const { + const waypoint_accumulator acc(waypoints); + path p = path::create(acc, path_options_); + return trajectory::create(std::move(p), trajectory_options_); +} + +void session::extend(const waypoint_accumulator& batch) { + if (batch.empty()) { + throw std::invalid_argument("streaming::session::extend: batch is empty"); + } + + // First extend: build the initial trajectory directly from the batch. + if (!active_) { + auto new_waypoints = accumulator_to_xarray_(batch); + auto new_active = build_trajectory_from_(new_waypoints); // throws on validation failure + + // Build the sampler for the new active before committing any moves so the throw + // contract (state unchanged on failure) is preserved. + uniform_sampler new_sampler = uniform_sampler::quantized_for_trajectory(new_active, sample_rate_, trajectory::seconds{0.0}); + + last_waypoint_ = row_to_xarray_(new_waypoints, new_waypoints.shape(0) - 1); + active_waypoints_ = std::move(new_waypoints); + active_ = std::move(new_active); + cursor_.emplace(active_->create_cursor()); + sampler_.emplace(std::move(new_sampler)); + generation_count_ = 1; + return; + } + + // Subsequent extends: validate DOF and seam before touching any state. + if (batch.dof() != active_waypoints_.shape(1)) { + throw std::invalid_argument("streaming::session::extend: DOF mismatch"); + } + if (!rows_bit_exact_(batch.at(0), last_waypoint_)) { + throw std::invalid_argument("streaming::session::extend: seam mismatch"); + } + + const std::size_t post_seam_count = batch.size() - 1; + + // Already locked-out: skip the candidate build, just record the new waypoints in staging. + if (!staged_batches_.empty()) { + if (post_seam_count > 0) { + staged_batches_.push_back(accumulator_tail_to_xarray_(batch, 1)); + } + last_waypoint_ = view_to_xarray_(batch.at(batch.size() - 1)); + return; + } + + // Seam-only batch with no new waypoints: nothing to do. + if (post_seam_count == 0) { + return; + } + + // Build a candidate trajectory over (active waypoints) + (batch sans seam) and decide + // whether the pivot is admissible by branch-detecting against the current active. + auto new_waypoints = concat_active_with_batch_tail_(active_waypoints_, batch, 1); + auto candidate = build_trajectory_from_(new_waypoints); // throws on validation failure + + const auto branch_local = find_branch_local_time_(*active_, candidate); + const auto branch_global = epoch_ + branch_local; + + const bool can_pivot = (emitted_sample_count_ == 0) || (branch_global > current_time_); + if (can_pivot) { + // Compute where the new sampler should start. If no samples have been emitted, the + // new sampler picks up at local time zero (same as the first build). Otherwise it + // picks up one sample period past the last emitted sample's local time, so the + // sample grid stays approximately uniform across the pivot. + const auto starting_local_time = + (emitted_sample_count_ == 0) ? trajectory::seconds{0.0} : (current_time_ - epoch_) + sample_period_; + uniform_sampler new_sampler = uniform_sampler::quantized_for_trajectory(candidate, sample_rate_, starting_local_time); + + last_waypoint_ = row_to_xarray_(new_waypoints, new_waypoints.shape(0) - 1); + active_waypoints_ = std::move(new_waypoints); + active_ = std::move(candidate); + cursor_.emplace(active_->create_cursor()); + sampler_.emplace(std::move(new_sampler)); + ++generation_count_; + } else { + staged_batches_.push_back(accumulator_tail_to_xarray_(batch, 1)); + last_waypoint_ = view_to_xarray_(batch.at(batch.size() - 1)); + } +} + +void session::rebase_() { + // Preconditions: active_ holds, staged_batches_ non-empty. + // + // The new chain's first waypoint is the active's last waypoint (the literal end of + // the prior chain's waypoint sequence), not the sampled terminal pose. Sampling the + // trajectory at its duration would produce a value that's mathematically equal to + // the last waypoint for a rest-to-rest trajectory but may differ by float drift, + // which trajex's path-coalescing tolerances can interact pathologically with. Keep + // the streaming layer in the waypoint domain. + const auto old_duration = active_->duration(); + auto anchor = row_to_xarray_(active_waypoints_, active_waypoints_.shape(0) - 1); + + auto new_waypoints = stack_anchor_and_staged_(anchor, staged_batches_); + auto new_active = build_trajectory_from_(new_waypoints); + + // The previous chain's terminal was emitted as its last sample at global time + // (epoch_ + old_duration). Start the new sampler one nominal sample period past that + // so the seam shows no duplicate sample and the inter-trajectory gap is exactly + // sample_period_. + uniform_sampler new_sampler = uniform_sampler::quantized_for_trajectory(new_active, sample_rate_, sample_period_); + + active_waypoints_ = std::move(new_waypoints); + active_ = std::move(new_active); + cursor_.emplace(active_->create_cursor()); + sampler_.emplace(std::move(new_sampler)); + epoch_ = epoch_ + old_duration; + staged_batches_.clear(); + ++generation_count_; +} + +std::optional session::sample_one_() { + if (!sampler_ || !cursor_) { + return std::nullopt; + } + + auto local_sample = sampler_->next(*cursor_); + if (!local_sample) { + if (staged_batches_.empty()) { + return std::nullopt; + } + rebase_(); + local_sample = sampler_->next(*cursor_); + if (!local_sample) { + // Defensive: the freshly-built sampler should always have at least one sample + // to emit, but if a degenerate trajectory somehow has none, treat as drained + // rather than infinite-looping. + return std::nullopt; + } + } + + auto sample = std::move(*local_sample); + sample.time = sample.time + epoch_; + ++emitted_sample_count_; + current_time_ = sample.time; + return sample; +} + +std::vector session::sample_next(std::size_t n) { + std::vector result; + result.reserve(n); + for (std::size_t i = 0; i < n; ++i) { + auto opt = sample_one_(); + if (!opt) { + break; + } + result.push_back(std::move(*opt)); + } + return result; +} + +std::vector session::sample_at_least(trajectory::seconds horizon) { + const auto target = current_time_ + horizon; + std::vector result; + while (true) { + auto opt = sample_one_(); + if (!opt) { + break; + } + result.push_back(std::move(*opt)); + if (current_time_ >= target) { + break; + } + } + return result; +} + +trajectory::seconds session::current_time() const noexcept { + return current_time_; +} + +const trajectory* session::active_trajectory() const noexcept { + return active_ ? &(*active_) : nullptr; +} + +trajectory::seconds session::active_epoch() const noexcept { + return epoch_; +} + +std::size_t session::trajectory_generation_count() const noexcept { + return generation_count_; +} + +} // namespace viam::trajex::totg::streaming diff --git a/src/viam/trajex/totg/streaming/session.hpp b/src/viam/trajex/totg/streaming/session.hpp new file mode 100644 index 0000000..781e3c7 --- /dev/null +++ b/src/viam/trajex/totg/streaming/session.hpp @@ -0,0 +1,232 @@ +#pragma once + +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +#include +#include +#include +#include +#include + +namespace viam::trajex::totg::streaming { + +/// +/// Streaming-input, streaming-output trajectory execution session. +/// +/// Holds an active trajectory that grows as new waypoint batches arrive while +/// sampling proceeds. Each `extend()` call may either pivot the active trajectory +/// to a new one that incorporates the additional waypoints, or stage the batch +/// for later if the time of divergence between the old and new trajectories falls +/// behind the latest emitted sample. Staged batches are absorbed into a new +/// trajectory built from the current trajectory's terminal pose once that +/// trajectory has been sampled through. +/// +/// Sampling is forward-only and stateful: each call to `sample_next()` or +/// `sample_at_least()` advances an internal cursor that gates the admissibility +/// of subsequent extends. The session assumes single-threaded ownership; sampling +/// and extending from different threads is unsupported. +/// +class session { + public: + /// + /// Constructs a session with the parameters used to build each trajectory and the + /// sample rate at which samples will be emitted. + /// + /// No trajectory exists until the first call to `extend()`. + /// + /// @param path_options Path-construction options (used for every trajectory built by the session) + /// @param trajectory_options Trajectory-construction options (used for every trajectory built by the session) + /// @param sample_rate Nominal sample rate. Each underlying trajectory's sampler is + /// quantized to land its last sample exactly on the trajectory's + /// duration, so per-sample spacing approximates 1 / sample_rate + /// with small per-trajectory drift. The parameter shape may + /// change when sampler injection lands. + /// + session(path::options path_options, trajectory::options trajectory_options, types::hertz sample_rate); + + /// + /// Adds a batch of waypoints to the session. + /// + /// If no active trajectory exists, builds the initial one from `batch`. + /// Otherwise, requires `batch`'s first waypoint to compare bit-exactly equal to the + /// session's most recently stored waypoint, then absorbs the remainder of `batch` and + /// attempts to build a trajectory incorporating it. The result is either swapped in + /// (pivot) or held aside for later (stage). The choice is invisible to the caller + /// and reversible only via subsequent extensions. + /// + /// Waypoints in `batch` are assumed to have been deduplicated by the caller. The + /// bit-exact seam requirement means the merged sequence retains the dedup invariant + /// after the seam point is dropped. + /// + /// @param batch Waypoints to append + /// @throws std::invalid_argument if `batch`'s DOF disagrees with the session's existing + /// waypoint DOF, or if its first waypoint does not equal the session's last + /// @throws Any exception raised by trajectory construction if computing the updated + /// trajectory fails. Session state is unchanged in that case. + /// + void extend(const waypoint_accumulator& batch); + + /// + /// Returns the global time of the most recently emitted sample, or zero if no samples + /// have been emitted yet. + /// + /// "Global time" is measured from the start of the session and runs continuously across + /// pivots and rebases. + /// + /// @return Time of the most recently emitted sample + /// + trajectory::seconds current_time() const noexcept; + + /// + /// Pulls the next `n` samples from the session, advancing the sampling cursor. + /// + /// What "next" means is sampler-defined; for the current uniform sampler, samples are + /// spaced according to the session's sample rate. Returns fewer than `n` samples if the + /// session is exhausted (active trajectory ran out and no staged batches were available + /// to rebase onto). + /// + /// @param n Number of samples to attempt to produce. Defaults to 1. + /// @return Vector of up to `n` samples + /// + std::vector sample_next(std::size_t n = 1); + + /// + /// Pulls samples until the most recent sample's time is at least + /// `current_time() + horizon`, advancing the sampling cursor accordingly. + /// + /// Returns fewer (possibly zero) samples than that target if the session is exhausted. + /// The `at_least` qualifier reflects that non-uniform samplers may overshoot the + /// requested horizon by a bounded amount; the session does not split a sample-period. + /// + /// @param horizon Minimum amount of time to advance before stopping + /// @return Vector of samples covering at least `horizon`, or fewer on exhaustion + /// + std::vector sample_at_least(trajectory::seconds horizon); + + /// + /// Returns a pointer to the active trajectory, or null if none has been built yet. + /// + /// @note This is an internal implementation detail exposed for testing. Production + /// callers should drive the session through `extend()` and the sampling + /// methods; reaching past those to the underlying trajectory is not part of + /// the supported usage pattern. + /// @warning The returned pointer is invalidated by any mutating call on the session, + /// including `extend()`, `sample_next()`, and `sample_at_least()`, because + /// any of those may pivot or rebase the active trajectory. Do not hold the + /// pointer across any such call. + /// @return Pointer to the active trajectory, or null if no trajectory has been built + /// + const trajectory* active_trajectory() const noexcept; + + /// + /// Returns the global time at which the active trajectory's local t=0 sits. + /// + /// Pivots preserve the epoch; rebases advance it by the prior active trajectory's + /// duration. Returns zero when no active trajectory exists. + /// + /// @note This is an internal implementation detail exposed for testing. Production + /// callers should not need to translate between local and global time; + /// sampling methods deliver samples in global time directly. + /// @warning The returned value is invalidated by any mutating call on the session + /// (see `active_trajectory()`). + /// @return Global time corresponding to the active trajectory's local origin + /// + trajectory::seconds active_epoch() const noexcept; + + /// + /// Returns the cumulative number of trajectories the session has produced. + /// + /// Increments by one each time a new trajectory becomes active: at the first + /// successful `extend()`, on each pivot, and on each rebase. Stays unchanged on + /// stage (no new active is produced), on failed extends, and on sampling calls + /// that do not cross a chain boundary. Returns zero for a fresh session. + /// + /// @note This is an internal implementation detail exposed for testing. The + /// counter exists so tests can witness pivot and rebase transitions + /// without relying on object-address comparisons of `active_trajectory()`, + /// which need not change across a transition. + /// @return Number of trajectories the session has built + /// + std::size_t trajectory_generation_count() const noexcept; + + private: + // Construction-time configuration. Reused for every trajectory the session builds. + path::options path_options_; + trajectory::options trajectory_options_; + types::hertz sample_rate_; + + // Nominal sample period, derived once from sample_rate_. Used to compute the + // per-trajectory starting offset at pivot and rebase transitions. + trajectory::seconds sample_period_; + + // The waypoint set that built `active_`. Stable owned storage required because + // waypoint_accumulator stores row-views into the array it was constructed over. + // Empty (shape (0,)) until the first successful extend; thereafter shape (N, dof). + xt::xarray active_waypoints_; + + // The currently active trajectory, or nullopt before the first successful extend. + // Storage in std::optional is in-place, so `&*active_` is a stable address across + // pivot and rebase (which both proceed by move-assigning a freshly-built trajectory + // into this optional). Tests must use trajectory_generation_count() to witness + // transitions instead of comparing pointers. + std::optional active_; + + // Per-trajectory uniform sampler and cursor. Reconstructed at every transition + // (first build, pivot, rebase) so each new active is sampled on a fresh grid + // aligned to its own duration. Both reference active_; reconstruction order is + // always (assign active_) -> (emplace sampler_/cursor_) so the cursor points at + // the freshly-installed trajectory. + std::optional sampler_; + std::optional cursor_; + + // Global time at which active_'s local t=0 sits. Pivots leave this unchanged; rebases + // advance it by the prior active's duration. + trajectory::seconds epoch_{0.0}; + + // Global time of the most recently emitted sample, or zero if no sample has been + // emitted yet. Cached for the current_time() accessor. + trajectory::seconds current_time_{0.0}; + + // Cumulative count of samples emitted. Used at pivot time to distinguish "no + // samples yet" (start new sampler at offset 0) from "samples emitted" (start at + // current local time + one sample period). + std::size_t emitted_sample_count_{0}; + + // Batches received while locked-out, each pre-stripped of its seam point. Drained + // into the new active during the next rebase. + std::vector> staged_batches_; + + // The most recently received waypoint, against which the next extend's seam is + // bit-exactly validated. Empty (shape (0,)) before the first extend. + xt::xarray last_waypoint_; + + // Cumulative count of trajectories the session has installed as active. Increments + // on first build, on each pivot, and on each rebase. + std::size_t generation_count_{0}; + + // Builds a trajectory from the given waypoints xarray, threading through path::options + // and trajectory::options. Throws on validation failure inside path::create or + // trajectory::create; the session's state is unaffected because this is called before + // any member is mutated. + trajectory build_trajectory_from_(const xt::xarray& waypoints) const; + + // Emits a single sample, advancing the cursor. Triggers a rebase if the active is + // exhausted at the next-sample index and staging is non-empty. Returns nullopt when + // the session is fully drained. + std::optional sample_one_(); + + // Rebuilds the active trajectory from {terminal_pose, ...staged_batches}, advances + // the epoch by the prior active's duration, clears staging, and increments the + // generation count. Preconditions: active_ holds a value, staged_batches_ is non-empty. + void rebase_(); +}; + +} // namespace viam::trajex::totg::streaming diff --git a/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_70b8daa458acb982d2aa37c5197ed569.trajex-totg-replay.json b/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_70b8daa458acb982d2aa37c5197ed569.trajex-totg-replay.json new file mode 100644 index 0000000..531e150 --- /dev/null +++ b/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_70b8daa458acb982d2aa37c5197ed569.trajex-totg-replay.json @@ -0,0 +1,3330 @@ +{ + "max_acceleration_vec_rads_per_sec2": [ + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659 + ], + "max_blend_curvature": 100000.0, + "max_velocity_vec_rads_per_sec": [ + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659 + ], + "min_blend_curvature": 1e-05, + "path_colinearization_ratio": 0.5, + "path_tolerance_delta_rads": 0.008726646259971648, + "schema_version": 1, + "timestamp": "2026-06-04T01:20:27.922342Z", + "waypoints_rads": [ + [ + -2.136748790740967, + -0.7803743481636047, + -1.7049429416656494, + 5.754814083047677e-06, + -0.8599405288696289, + -5.052760124206542 + ], + [ + -1.5501105567951856, + -1.5729290057568917, + -2.3689453141446473, + 0.07095849311028786, + -0.7768625860952549, + -0.02889760864505653 + ], + [ + -1.5501105567951856, + -1.5729290057568917, + -2.3689453141446473, + 0.07095849311028786, + -0.7768625860952549, + -0.02889760864505653 + ], + [ + -1.5501109829373323, + -1.573559901945905, + -2.3695668862260524, + 0.07096036879712828, + -0.7768478707695416, + -0.028900919623528 + ], + [ + -1.5501118760365795, + -1.5742069438301247, + -2.37021949918514, + 0.07096316469263564, + -0.7768540207820871, + -0.028901466342084417 + ], + [ + -1.550111165574964, + -1.5748537318663867, + -2.3708718471255263, + 0.07096036005700024, + -0.776859794860129, + -0.028899015538913074 + ], + [ + -1.5501111666787737, + -1.5755008454784813, + -2.3715247550410807, + 0.07095993510984899, + -0.7768655770169642, + -0.0288984257387587 + ], + [ + -1.5501111678658313, + -1.5761488706165294, + -2.372179441041085, + 0.07095944788341865, + -0.7768722239090688, + -0.028897748556658012 + ], + [ + -1.5501110923866819, + -1.576798539974241, + -2.3728374239578134, + 0.07095851692308124, + -0.7768808601507299, + -0.028897155531266303 + ], + [ + -1.5501111702296186, + -1.5774476702769127, + -2.373494175061045, + 0.07095828494010557, + -0.7768881238258994, + -0.02889612993441024 + ], + [ + -1.5501108687768073, + -1.578096010662133, + -2.374149567570129, + 0.07095662931957329, + -0.7768944890322287, + -0.02889644353029508 + ], + [ + -1.550111087925181, + -1.578750889660515, + -2.3748176215077907, + 0.07095645062204209, + -0.7769086497800888, + -0.028894301965737 + ], + [ + -1.5501110950126327, + -1.5794035418175039, + -2.3754813260651733, + 0.07095567361703584, + -0.7769196772321688, + -0.028893201260010798 + ], + [ + -1.5501110964642986, + -1.5800571245286732, + -2.376146847165148, + 0.07095480665354943, + -0.7769315895648395, + -0.02889199138133617 + ], + [ + -1.5501110975813532, + -1.5807116469672424, + -2.3768141996107035, + 0.0709538739514655, + -0.7769443910187162, + -0.028890690278496327 + ], + [ + -1.5501110986263766, + -1.581367115648881, + -2.377483395622188, + 0.0709528762727083, + -0.7769580879776897, + -0.02888929823743864 + ], + [ + -1.550111184907357, + -1.5820231994457257, + -2.378153737385299, + 0.07095222539188037, + -0.7769721712802115, + -0.02888748577508318 + ], + [ + -1.5501108628367342, + -1.5826775490562002, + -2.378820837272717, + 0.07095012134439337, + -0.7769839272177735, + -0.028887334020180643 + ], + [ + -1.5501109098882149, + -1.5833358610073918, + -2.379495565748785, + 0.07094906405121215, + -0.7770003289514661, + -0.028885900290942713 + ], + [ + -1.5501110986926994, + -1.5839985306044526, + -2.380178763255256, + 0.07094821404253894, + -0.7770218976490746, + -0.028882800478792325 + ], + [ + -1.5501111038336393, + -1.5846587953822848, + -2.3808572971441233, + 0.07094690855876858, + -0.7770401229650723, + -0.02888096486428665 + ], + [ + -1.55011119022294, + -1.5853196691814007, + -2.3815369628230743, + 0.07094593367235942, + -0.7770587138400427, + -0.028878703491266575 + ], + [ + -1.5501108644693222, + -1.5859787759940494, + -2.382213326084503, + 0.07094349421435918, + -0.7770749399818028, + -0.028878087997781778 + ], + [ + -1.550111187032116, + -1.586644675998726, + -2.3829027076687153, + 0.07094293732380788, + -0.7770991585618662, + -0.02887469278453882 + ], + [ + -1.5501108695901764, + -1.5873061249033606, + -2.3835836670142636, + 0.07094037378262548, + -0.7771178192753662, + -0.028873735083497828 + ], + [ + -1.5501111895068693, + -1.5879740308922599, + -2.384276952614737, + 0.0709396881655136, + -0.7771439419630194, + -0.02887014818833689 + ], + [ + -1.5501108702295463, + -1.588637426785623, + -2.384961699279462, + 0.07093698661143656, + -0.7771644238649588, + -0.028869003760634792 + ], + [ + -1.550111196009695, + -1.589307844372868, + -2.385659909242192, + 0.07093617054048493, + -0.7771931944036689, + -0.028865066270842024 + ], + [ + -1.5501111966940921, + -1.589975935853877, + -2.38635367026085, + 0.07093431245570157, + -0.7772187613839998, + -0.02886247012291255 + ], + [ + -1.550111194258372, + -1.5906447452514467, + -2.3870487883086917, + 0.0709323733213818, + -0.7772448209294202, + -0.028859913415201405 + ], + [ + -1.5501108722537638, + -1.5913120963969096, + -2.3877412249267875, + 0.07092939542132326, + -0.7772690005924983, + -0.02885839446119373 + ], + [ + -1.5501111968272996, + -1.5919861875451389, + -2.388446535151287, + 0.07092830344312069, + -0.7773009905364158, + -0.02885421693821709 + ], + [ + -1.5501108736810163, + -1.592655546915034, + -2.38914287435995, + 0.07092518662691191, + -0.7773270460890551, + -0.02885250928741144 + ], + [ + -1.5501111992809182, + -1.5933317403646625, + -2.3898522691042614, + 0.07092395465791762, + -0.7773610267424459, + -0.028848129087696326 + ], + [ + -1.5501112004745863, + -1.5940060714943538, + -2.390558156233066, + 0.07092167490404462, + -0.7773925065316467, + -0.028844937319180867 + ], + [ + -1.5501108757688498, + -1.5946784822737574, + -2.3912604228421013, + 0.07091834773437332, + -0.777421410580955, + -0.028842944477105757 + ], + [ + -1.5501112029708375, + -1.595357870865308, + -2.3919760228964555, + 0.07091690355882038, + -0.7774584142001224, + -0.028838256061623983 + ], + [ + -1.5501108770528358, + -1.5960323420605307, + -2.392682290244333, + 0.07091343406732818, + -0.7774892399097946, + -0.02883606997799335 + ], + [ + -1.5501111199555515, + -1.596714694612739, + -2.393403749495515, + 0.07091141105524178, + -0.7775295208689254, + -0.0288313101755626 + ], + [ + -1.5501111244933476, + -1.5973943021261183, + -2.394119879295839, + 0.07090879114519885, + -0.7775659532110385, + -0.028827631359886058 + ], + [ + -1.5501111258478049, + -1.5980749823734288, + -2.3948380924007253, + 0.0709060828744059, + -0.7776033963785601, + -0.028823837820633493 + ], + [ + -1.5501111269546461, + -1.5987567389936987, + -2.395558394072876, + 0.07090330097217795, + -0.7776418490529154, + -0.028819941617752254 + ], + [ + -1.5501112104130643, + -1.5994387624238702, + -2.3962791080089327, + 0.07090086831561077, + -0.7776800695892704, + -0.028815795060354973 + ], + [ + -1.5501112104130643, + -1.5994387624238702, + -2.3962791080089327, + 0.07090086831561077, + -0.7776800695892704, + -0.028815795060354973 + ], + [ + -1.5501112434870445, + -1.594009996800937, + -2.3905658101033693, + 0.07092138428824486, + -0.777398059163347, + -0.028844692594241758 + ], + [ + -1.5501106296106517, + -1.5886437994438602, + -2.3849731425534033, + 0.07093800484476592, + -0.7771721778511562, + -0.028866474584707067 + ], + [ + -1.550111160588361, + -1.5833407157233927, + -2.379504895638638, + 0.07094949186962264, + -0.7770066424736063, + -0.028884504479740114 + ], + [ + -1.5501119019091743, + -1.578100189713386, + -2.374157152821455, + 0.0709600669732353, + -0.7768980832647167, + -0.028892912528716258 + ], + [ + -1.550113066631037, + -1.5729095166248277, + -2.3689074272336765, + 0.07096810036373467, + -0.7768360763147308, + -0.028906951703950967 + ], + [ + -1.5501130819320679, + -1.5729613304138184, + -2.368959903717041, + 0.07096801698207855, + -0.7768366932868958, + -0.02890681102871895 + ], + [ + -1.3560022845541804, + -1.6600991955712128, + -2.5561471951503156, + 0.043479978729829595, + -0.8607942596910934, + 0.18794246637738152 + ], + [ + -1.3560022845541804, + -1.6600991955712128, + -2.5561471951503156, + 0.043479978729829595, + -0.8607942596910934, + 0.18794246637738152 + ], + [ + -1.3560019421428504, + -1.6609642621559517, + -2.5572140603810416, + 0.043471660620740406, + -0.8609954110098134, + 0.18795483814745761 + ], + [ + -1.3560019426696206, + -1.6618343392551118, + -2.558290372436109, + 0.04346395149976015, + -0.8612014506951268, + 0.1879666570899771 + ], + [ + -1.3560019439331454, + -1.6627094088046777, + -2.5593761835885664, + 0.043456115386347256, + -0.8614124361119648, + 0.18797876628102053 + ], + [ + -1.3560020075489227, + -1.663585811031559, + -2.5604643867540897, + 0.0434484577240685, + -0.8616237194681337, + 0.18799104537911857 + ], + [ + -1.3560020038147933, + -1.6644658187470915, + -2.561559437644572, + 0.043440428819447874, + -0.86183857410206, + 0.1880033482868704 + ], + [ + -1.356002002616214, + -1.6653492142175133, + -2.5626608756599785, + 0.04343229741127387, + -0.8620564247587743, + 0.18801582123969107 + ], + [ + -1.356002002510631, + -1.666236042537154, + -2.5637687861203977, + 0.04342405925657389, + -0.862277312347158, + 0.18802846490775804 + ], + [ + -1.3560019466747344, + -1.667126781423636, + -2.564884114478424, + 0.043415365231207294, + -0.8625021175756642, + 0.18804094775336436 + ], + [ + -1.3560019473165068, + -1.6680191965797893, + -2.566002477897989, + 0.043406997807653044, + -0.8627274653927154, + 0.18805405518864865 + ], + [ + -1.3560019480304755, + -1.668918093673048, + -2.567133218810006, + 0.04339833126441567, + -0.8629595197436742, + 0.1880671082639307 + ], + [ + -1.3560019537416619, + -1.6698190484205913, + -2.5682677454172724, + 0.04338974865971899, + -0.8631928096742167, + 0.1880806552153097 + ], + [ + -1.3560020192870432, + -1.6707234132742776, + -2.5694086660068605, + 0.04338122349215981, + -0.8634288059054277, + 0.1880943241939315 + ], + [ + -1.3560020145899228, + -1.671631875134616, + -2.5705573522773535, + 0.0433723015791987, + -0.8636688198071315, + 0.18810802410924876 + ], + [ + -1.3560020127522423, + -1.672544138001269, + -2.5717131964613515, + 0.04336326592890067, + -0.8639121874607603, + 0.18812191313259186 + ], + [ + -1.3560019539782375, + -1.6734608923471845, + -2.5728775493823086, + 0.04335374325889831, + -0.864160029435208, + 0.1881356642305927 + ], + [ + -1.3560019593258053, + -1.6743788464157983, + -2.574044012642403, + 0.0433446050982673, + -0.8644077706016465, + 0.18815000635998108 + ], + [ + -1.35600195533342, + -1.6753050010813695, + -2.575226115725389, + 0.04333506496303871, + -0.8646640594984549, + 0.18816440810329982 + ], + [ + -1.3560019617457237, + -1.6762308883653625, + -2.5764075138538156, + 0.04332568706748466, + -0.8649187831603811, + 0.1881791344222573 + ], + [ + -1.3560019573666613, + -1.6771652549707325, + -2.577605077971576, + 0.04331589074650898, + -0.8651823286146533, + 0.18819393584023508 + ], + [ + -1.356001965029272, + -1.6781013881534397, + -2.578805845852124, + 0.04330622555684096, + -0.8654466348710957, + 0.18820919116436555 + ], + [ + -1.356002033259807, + -1.6790413431375368, + -2.5800137796822282, + 0.04329661391337884, + -0.8657139826060868, + 0.1882246119025898 + ], + [ + -1.3560019667554781, + -1.6799846671215355, + -2.5812280522731887, + 0.043286320633410987, + -0.8659846047280075, + 0.18823980478598445 + ], + [ + -1.3560019614117569, + -1.680936311784433, + -2.5824581338497365, + 0.043275987063309665, + -0.8662634141354042, + 0.1882554447940346 + ], + [ + -1.3560019690840908, + -1.6818875692800515, + -2.5836872662439743, + 0.04326583827877936, + -0.8665404400199411, + 0.18827140098032322 + ], + [ + -1.356001963402525, + -1.682848248665346, + -2.584934342825142, + 0.043255226464762264, + -0.8668272117567007, + 0.18828747723854636 + ], + [ + -1.3560019715664944, + -1.6838085355766605, + -2.5861804544465667, + 0.043244805461855974, + -0.8671121645385037, + 0.18830386901224008 + ], + [ + -1.3560019728531485, + -1.6847759907775992, + -2.5874401582792625, + 0.04323407718793616, + -0.8674041568778696, + 0.18832043889440572 + ], + [ + -1.3560019663219973, + -1.6857508828515817, + -2.588713962365586, + 0.04322302769016296, + -0.8677034609220402, + 0.18833720061112785 + ], + [ + -1.35600197530175, + -1.6867253168978003, + -2.5899866673619774, + 0.04321218230390412, + -0.8680008245505209, + 0.18835427284674228 + ], + [ + -1.3560019682701439, + -1.6877100985888265, + -2.5912790623684794, + 0.043200830171216065, + -0.8683088383570124, + 0.18837150966903443 + ], + [ + -1.3560019691927583, + -1.6886971872157865, + -2.5925756700757443, + 0.04318950571168291, + -0.8686180936747866, + 0.18838902504075597 + ], + [ + -1.356001979025242, + -1.6896865692328873, + -2.593876462877067, + 0.04317821402105843, + -0.8689285641870302, + 0.18840681189874892 + ], + [ + -1.3560019803092545, + -1.6906839522356063, + -2.595192411306242, + 0.04316658148873793, + -0.8692468508825417, + 0.1884248177675015 + ], + [ + -1.3560019720577539, + -1.6916896916291992, + -2.5965241882393095, + 0.04315459039757153, + -0.8695733164908337, + 0.18844305862350053 + ], + [ + -1.3560019729228077, + -1.6926978804934711, + -2.5978604343419334, + 0.043142631651429855, + -0.8699010956703772, + 0.18846158288553133 + ], + [ + -1.3560019788983997, + -1.6937094152348517, + -2.59920285355924, + 0.04313065557527489, + -0.8702311776311532, + 0.1884804516880329 + ], + [ + -1.3560019949263757, + -1.6947314938447937, + -2.600565271911368, + 0.04311835988318179, + -0.8705717106342165, + 0.18849967760974026 + ], + [ + -1.3560019762837459, + -1.69575617889353, + -2.601932515947257, + 0.04310574935741952, + -0.8709140390536656, + 0.18851875482767538 + ], + [ + -1.3560019764769526, + -1.6967872158662136, + -2.603311661769178, + 0.04309310588379385, + -0.8712618610271406, + 0.1885383660596842 + ], + [ + -1.3560019891517308, + -1.6978208405172333, + -2.6046955198025175, + 0.04308051010525626, + -0.8716110371830494, + 0.18855825856879357 + ], + [ + -1.3560019891517308, + -1.6978208405172333, + -2.6046955198025175, + 0.04308051010525626, + -0.8716110371830494, + 0.18855825856879357 + ], + [ + -1.3560022960075881, + -1.6896990425984784, + -2.59390051064749, + 0.04317856427673743, + -0.8689439054188933, + 0.18840678757291793 + ], + [ + -1.3560021950037884, + -1.6819077109438796, + -2.5837259162797612, + 0.04326586154337132, + -0.8665648070443985, + 0.18827332596387808 + ], + [ + -1.356002134251802, + -1.6744033306886035, + -2.574090914408674, + 0.04334427708805995, + -0.8644370335261663, + 0.18815222503965778 + ], + [ + -1.356001966101563, + -1.6671277021867872, + -2.5648858395167116, + 0.04341540296397233, + -0.8625031728181815, + 0.18804108522487334 + ], + [ + -1.3560022213577168, + -1.660103842854586, + -2.556155698124416, + 0.04347924849718988, + -0.8607994057840337, + 0.18794388732682285 + ], + [ + -1.3560022115707395, + -1.6601459980010986, + -2.5562081336975098, + 0.04347886517643928, + -0.8608096241950989, + 0.1879444718360901 + ], + [ + -1.3628306829569914, + -1.6088746983114157, + -2.454648969344269, + 0.07984169393387167, + -0.8470503996778008, + 0.15490467073684186 + ], + [ + -1.3628306829569914, + -1.6088746983114157, + -2.454648969344269, + 0.07984169393387167, + -0.8470503996778008, + 0.15490467073684186 + ], + [ + -1.3628306002098927, + -1.6095879035016052, + -2.4554288577455265, + 0.07983654251028605, + -0.8471188296450748, + 0.1549133421107328 + ], + [ + -1.3628305256212163, + -1.610294031790056, + -2.4561952316117677, + 0.07983222543896973, + -0.8471773028021437, + 0.1549187971780239 + ], + [ + -1.3628304915780265, + -1.6110072969308342, + -2.4569754911380914, + 0.07982730051640538, + -0.847245135620124, + 0.15492543378316206 + ], + [ + -1.3628300505084203, + -1.6117133853849837, + -2.457742168235451, + 0.07982123208128371, + -0.8473039160376913, + 0.15493270401660453 + ], + [ + -1.3628305307667747, + -1.612431655164431, + -2.4585312330030353, + 0.07981824022900251, + -0.8473749627791294, + 0.1549399023495255 + ], + [ + -1.3628305304887904, + -1.6131476585717672, + -2.459316520843473, + 0.07981334549678668, + -0.8474441316605185, + 0.15494728433456947 + ], + [ + -1.3628305338195417, + -1.6138646810876431, + -2.4601037718493086, + 0.07980840792643984, + -0.8475141468052888, + 0.15495474057268074 + ], + [ + -1.3628305352936039, + -1.6145831512535038, + -2.4608937961972095, + 0.07980336609235994, + -0.8475854786677324, + 0.15496235099864852 + ], + [ + -1.3628305371235816, + -1.6153030939285886, + -2.4616866409524754, + 0.07979823243749208, + -0.8476581550095338, + 0.1549701014332148 + ], + [ + -1.3628305389067927, + -1.6160245171408083, + -2.462482321101104, + 0.07979300418704947, + -0.8477321819315627, + 0.15497799530486658 + ], + [ + -1.3628305407140267, + -1.6167474315675194, + -2.463280856643015, + 0.07978768114056146, + -0.8478075687915799, + 0.1549860330986946 + ], + [ + -1.3628305483246768, + -1.6174711384652427, + -2.464080903247787, + 0.07978232282015643, + -0.8478834460259774, + 0.15499428215914274 + ], + [ + -1.362830541215051, + -1.618197163759043, + -2.4648854039387205, + 0.07977676629226571, + -0.8479617359039874, + 0.15500256637365675 + ], + [ + -1.3628305448857474, + -1.6189252207128808, + -2.465693780083559, + 0.07977113041925031, + -0.8480419712687418, + 0.1550110273312928 + ], + [ + -1.3628305419969522, + -1.6196541381220733, + -2.4665038074008105, + 0.07976539887254055, + -0.8481228116211448, + 0.1550196762108977 + ], + [ + -1.3628305432592107, + -1.620384563165544, + -2.4673167196599164, + 0.07975959611703365, + -0.8482050179030549, + 0.15502844189258735 + ], + [ + -1.3628305502263585, + -1.6211161720734601, + -2.4681318891958606, + 0.07975374124975976, + -0.8482881669467817, + 0.15503739774594016 + ], + [ + -1.3628305456252203, + -1.621850267575427, + -2.4689518270980293, + 0.07974768314790263, + -0.8483738843339889, + 0.15504644007232082 + ], + [ + -1.3628305527723497, + -1.6225850656504535, + -2.4697730953502854, + 0.07974162742474065, + -0.8484599432605727, + 0.15505570270081956 + ], + [ + -1.3628305510732048, + -1.6233219318059247, + -2.4705983283331796, + 0.07973540628491035, + -0.8485480563176843, + 0.15506506665882322 + ], + [ + -1.362830549178339, + -1.6240606078126842, + -2.4714270241070904, + 0.07972906983722144, + -0.8486378803349942, + 0.15507456894868454 + ], + [ + -1.362830550346622, + -1.624800614201843, + -2.4722582574529897, + 0.07972266211333029, + -0.8487288134767712, + 0.15508425455690597 + ], + [ + -1.3628306080333472, + -1.6255600870321258, + -2.4731265511567844, + 0.07971425725101305, + -0.8488441554719905, + 0.15509566404366518 + ], + [ + -1.3628305497059972, + -1.6262860134304578, + -2.4739310083451156, + 0.07970948849045893, + -0.8489156598816336, + 0.1551041647604489 + ], + [ + -1.3628305021950078, + -1.6270349783436142, + -2.4747790608190305, + 0.07970179585590846, + -0.8490162897313172, + 0.1551146552116366 + ], + [ + -1.3628305544034292, + -1.6277775967874957, + -2.4756155859641615, + 0.07969596197923837, + -0.8491080551382317, + 0.15512462349205974 + ], + [ + -1.3628305027394398, + -1.62852986406867, + -2.476469943017361, + 0.07968804403010436, + -0.8492116429083121, + 0.1551354596219367 + ], + [ + -1.3628305571360158, + -1.6292759230521157, + -2.4773130404750243, + 0.07968200037479357, + -0.8493065628508585, + 0.15514574157228414 + ], + [ + -1.3628304977848835, + -1.630031317868812, + -2.4781733658310565, + 0.07967387091523451, + -0.8494128766339187, + 0.15515690143991026 + ], + [ + -1.3628311768985053, + -1.6307801671966105, + -2.479022963516577, + 0.07966916840326166, + -0.8495128174438522, + 0.15517077730127138 + ], + [ + -1.3628300488498024, + -1.631541330279783, + -2.479893120079719, + 0.0796577435033403, + -0.8496223914752751, + 0.15518056239773811 + ], + [ + -1.3628305652819004, + -1.6322931944297014, + -2.480747314749676, + 0.07965280552322697, + -0.849722274655833, + 0.15518992296553427 + ], + [ + -1.362830494909423, + -1.6330550837748086, + -2.4816200301286737, + 0.07964424132001063, + -0.8498342874941051, + 0.15520175319493282 + ], + [ + -1.362831152323018, + -1.633811464011004, + -2.4824839790854436, + 0.07963899753123302, + -0.8499411420867277, + 0.15521613502788847 + ], + [ + -1.3628305283321867, + -1.6345770074400323, + -2.483363038949988, + 0.07962933515341945, + -0.8500542471580156, + 0.15522464065768513 + ], + [ + -1.362830532922941, + -1.6353437795939527, + -2.4842448746276102, + 0.0796207768171923, + -0.8501700713308226, + 0.15523727509688728 + ], + [ + -1.3628311864854192, + -1.6361035470937597, + -2.485115406804267, + 0.07961531106110568, + -0.8502793707458274, + 0.1552519468086789 + ], + [ + -1.3628305722740717, + -1.6368723246782924, + -2.4860002766173768, + 0.07960567476662184, + -0.8503940371954138, + 0.15526128351423663 + ], + [ + -1.362830491582005, + -1.6376442958004884, + -2.4868922079355036, + 0.07959647446561607, + -0.8505148677278321, + 0.1552741021486516 + ], + [ + -1.362831111209224, + -1.6384124642013023, + -2.487778588229267, + 0.07959038236738199, + -0.8506325118168401, + 0.1552892982805287 + ], + [ + -1.362831111209224, + -1.6384124642013023, + -2.487778588229267, + 0.07959038236738199, + -0.8506325118168401, + 0.1552892982805287 + ], + [ + -1.3628310366311505, + -1.6323121973694006, + -2.4807847572223896, + 0.07965215456727338, + -0.8497485579214301, + 0.15519395670408698 + ], + [ + -1.3628299750685278, + -1.6262820224852086, + -2.473924778739407, + 0.07970701439076451, + -0.8489139590115844, + 0.15510139187212285 + ], + [ + -1.3628307730238065, + -1.620398500176992, + -2.4673440005768104, + 0.07975881612951177, + -0.8482240805152768, + 0.15503088408947285 + ], + [ + -1.36282983526915, + -1.6145780825528508, + -2.460884949573364, + 0.07980090185825604, + -0.8475811716926175, + 0.15495904241533592 + ], + [ + -1.3628299835123927, + -1.608872254979744, + -2.45464473124139, + 0.07983926670566388, + -0.8470485076578748, + 0.15490282420181953 + ], + [ + -1.3628299236297607, + -1.6089200973510742, + -2.454697132110596, + 0.07983894646167754, + -0.8470529913902283, + 0.1549032926559448 + ], + [ + -1.5605061876285207, + -1.5143292947136267, + -2.2551799416003098, + 0.047935975714123995, + -0.7446530184708235, + -0.02510577432815721 + ], + [ + -1.5605061876285207, + -1.5143292947136267, + -2.2551799416003098, + 0.047935975714123995, + -0.7446530184708235, + -0.02510577432815721 + ], + [ + -1.5605055462351032, + -1.51490846307949, + -2.2556969096448136, + 0.04793704313310001, + -0.744588897978047, + -0.025105523556352614 + ], + [ + -1.5605052567610005, + -1.5154896954802022, + -2.2562189553726286, + 0.04793902605933935, + -0.7445299782485568, + -0.025110612495789122 + ], + [ + -1.560505255633917, + -1.5160718134224727, + -2.256742510905016, + 0.04794205469974213, + -0.7444714847353261, + -0.025114736846202457 + ], + [ + -1.560505256790354, + -1.5166544949657796, + -2.2572671976412435, + 0.04794506562847586, + -0.744413557972409, + -0.025118835072582543 + ], + [ + -1.5605052578944394, + -1.5172377434797726, + -2.257793022135036, + 0.04794804713415052, + -0.7443562013090532, + -0.025122893137964335 + ], + [ + -1.5605053261451765, + -1.5178215349892343, + -2.2583199074074995, + 0.047951251049462434, + -0.7442992814480918, + -0.025126798240977057 + ], + [ + -1.560505260008099, + -1.518405947971778, + -2.2588480968969145, + 0.04795392177981639, + -0.7442432043096928, + -0.025130888713676067 + ], + [ + -1.5605054015077624, + -1.5189918036531875, + -2.259379093745703, + 0.04795715956468886, + -0.7441890977870294, + -0.025133476717130704 + ], + [ + -1.560505264561419, + -1.519576442070338, + -2.259907764609689, + 0.04795969018333455, + -0.7441325094920905, + -0.025138736706629253 + ], + [ + -1.5605054026288532, + -1.520163440191483, + -2.260441054388041, + 0.04796285433330825, + -0.7440795478264014, + -0.025141239866810175 + ], + [ + -1.5605052667802453, + -1.5207492353540837, + -2.2609720435040765, + 0.04796532883833798, + -0.7440241227357803, + -0.025146409822123947 + ], + [ + -1.5605052656209728, + -1.5213364973400296, + -2.261505918283525, + 0.04796809263284832, + -0.7439707970373527, + -0.02515017270551594 + ], + [ + -1.5605052667830464, + -1.521924341209965, + -2.2620409594030804, + 0.04797083755705629, + -0.7439180564419967, + -0.025153907667682295 + ], + [ + -1.5605053347471918, + -1.5225127408537833, + -2.2625770849796605, + 0.04797380442310121, + -0.743865762794075, + -0.02515749531429967 + ], + [ + -1.560505268898141, + -1.5231017763796413, + -2.2631145441894764, + 0.047976236220765515, + -0.7438143283164556, + -0.025161253453952873 + ], + [ + -1.5605052701300175, + -1.5236913714663933, + -2.263653095346149, + 0.04797889106767105, + -0.7437633446520634, + -0.025164865363797365 + ], + [ + -1.5605052712436527, + -1.5242815534696224, + -2.264192822463171, + 0.047981515039899836, + -0.7437129492654772, + -0.025168435368166094 + ], + [ + -1.5605052723665693, + -1.5248723243426903, + -2.2647337293999072, + 0.047984108625069084, + -0.7436631441630029, + -0.025171963926605853 + ], + [ + -1.560505406031506, + -1.5254645838482395, + -2.265277565214647, + 0.04798699609661335, + -0.743615461026418, + -0.025174144050901306 + ], + [ + -1.5605052767493952, + -1.5260556413192101, + -2.2658190991803497, + 0.04798921429105044, + -0.7435653139001068, + -0.02517890792670129 + ], + [ + -1.5605054072631337, + -1.5266490852588055, + -2.2663653086917335, + 0.0479920270679349, + -0.7435188167176533, + -0.025181001730564322 + ], + [ + -1.5605052789785758, + -1.5272413334041988, + -2.266909225959195, + 0.04799418683665167, + -0.7434698615617572, + -0.02518567248054641 + ], + [ + -1.5605052779322435, + -1.5278350735344866, + -2.2674560798343886, + 0.04799661650695512, + -0.7434230294646752, + -0.025188979641717194 + ], + [ + -1.5605052791063598, + -1.5284294148650495, + -2.2680041370822894, + 0.04799902570948675, + -0.7433768001569607, + -0.025192256856650913 + ], + [ + -1.56050528022473, + -1.5290243576519893, + -2.268553398277634, + 0.04800140353601203, + -0.7433311726894792, + -0.02519549136269378 + ], + [ + -1.5605054089183905, + -1.529620807361317, + -2.2691056251539234, + 0.04800406173267721, + -0.7432876852507989, + -0.02519740333162543 + ], + [ + -1.5605052845498404, + -1.5302160562500982, + -2.2696555489809085, + 0.04800607530425173, + -0.7432417321835177, + -0.02520184420299056 + ], + [ + -1.5605052835795317, + -1.5308128126174156, + -2.2702084397289766, + 0.04800834923446945, + -0.7431979172935017, + -0.02520493917109217 + ], + [ + -1.5605052847469476, + -1.5314101794655226, + -2.27076255161754, + 0.04801060179992542, + -0.7431547136861452, + -0.02520800305428615 + ], + [ + -1.5605052858804795, + -1.5320081570218333, + -2.2713178851624063, + 0.048012822555573854, + -0.7431121203319312, + -0.025211023673603488 + ], + [ + -1.5605052870131984, + -1.532606747115031, + -2.2718744438597422, + 0.04801501158197876, + -0.7430701388544155, + -0.025214001100909034 + ], + [ + -1.5605052881434456, + -1.5332059517811143, + -2.272432231610594, + 0.0480171687594754, + -0.7430287711899402, + -0.025216935181495655 + ], + [ + -1.5605053556009072, + -1.5338057463380261, + -2.272991166986331, + 0.04801954812426488, + -0.7429878783273037, + -0.0252197368000198 + ], + [ + -1.5605052902574643, + -1.5344062122101048, + -2.2735515082649433, + 0.04802138649211372, + -0.742947883721737, + -0.02522267208888208 + ], + [ + -1.5605052915525537, + -1.535007272191872, + -2.2741130054569503, + 0.048023448299352095, + -0.7429083681539681, + -0.025225476160667015 + ], + [ + -1.5605052926848086, + -1.53560895432182, + -2.274675746075209, + 0.0480254770882937, + -0.742869473080776, + -0.025228235549899707 + ], + [ + -1.5605052938271176, + -1.536211260700219, + -2.2752397342425343, + 0.04802747355619089, + -0.7428312005842862, + -0.02523095097138559 + ], + [ + -1.560505294965776, + -1.5368141933556472, + -2.275804973837468, + 0.048029437542140195, + -0.7427935525331573, + -0.02523362222136728 + ], + [ + -1.5605052961124644, + -1.537417754245637, + -2.2763714685973304, + 0.04803136898401811, + -0.742756530678854, + -0.025236249202078197 + ], + [ + -1.560505363333279, + -1.53802191901914, + -2.2769391369540357, + 0.048033522407726216, + -0.7427199955527792, + -0.0252387494773831 + ], + [ + -1.560505363333279, + -1.53802191901914, + -2.2769391369540357, + 0.048033522407726216, + -0.7427199955527792, + -0.0252387494773831 + ], + [ + -1.5605056206992733, + -1.5332096691894779, + -2.272439521570637, + 0.048017858788091616, + -0.7430346020886376, + -0.025216892686219357 + ], + [ + -1.5605072194819438, + -1.5284327691207085, + -2.2680104656706295, + 0.048005652644668044, + -0.7433813857719072, + -0.025198473963087654 + ], + [ + -1.5605054045481452, + -1.5236944364254659, + -2.2636590818628326, + 0.04797906111620255, + -0.7437680499373919, + -0.025164986691008962 + ], + [ + -1.5605053300242069, + -1.5189878422923926, + -2.259371502957689, + 0.04795647484434619, + -0.7441831842392485, + -0.025137246726765157 + ], + [ + -1.5605055862628612, + -1.5143306629413738, + -2.2551827267241076, + 0.0479336789452813, + -0.7446563236771915, + -0.025109506645146194 + ], + [ + -1.5605056285858154, + -1.51438307762146, + -2.2552297115325928, + 0.04793393611907959, + -0.7446510195732117, + -0.02510981820523739 + ], + [ + -1.5649478232739937, + -1.453711387685113, + -2.13603539739469, + 0.0658136287043245, + -0.6999604388109281, + -0.04517745690485368 + ], + [ + -1.5649478232739937, + -1.453711387685113, + -2.13603539739469, + 0.0658136287043245, + -0.6999604388109281, + -0.04517745690485368 + ], + [ + -1.5649480719521178, + -1.4542524766400884, + -2.136471381950762, + 0.06582244274011573, + -0.6998555698297946, + -0.04518891428383535 + ], + [ + -1.5649480760396763, + -1.454794597874883, + -2.136909610186497, + 0.06583057071607254, + -0.6997519040664208, + -0.045199540365938096 + ], + [ + -1.5649480774717064, + -1.455337113975702, + -2.137348658246925, + 0.06583865316069919, + -0.6996486610842398, + -0.0452101079816546 + ], + [ + -1.5649480789425347, + -1.4558800251396786, + -2.13778852677215, + 0.06584670533229631, + -0.6995458426608194, + -0.04522063507542552 + ], + [ + -1.5649480804251097, + -1.4564233322229605, + -2.138229217461614, + 0.06585472688474708, + -0.6994434496926736, + -0.045231121249991124 + ], + [ + -1.564948048911016, + -1.4569672017214252, + -2.138671709571285, + 0.06586267190109439, + -0.6993428841914712, + -0.04524290662312797 + ], + [ + -1.5649480804784075, + -1.4575111375468008, + -2.139113071390748, + 0.06587065970215732, + -0.6992399410414053, + -0.045251949442953535 + ], + [ + -1.5649480849268558, + -1.4580556357548382, + -2.1395562360576217, + 0.06587860718970016, + -0.6991388298538455, + -0.045262333438312034 + ], + [ + -1.5649480863353387, + -1.458600533568231, + -2.140000229474617, + 0.06588650479599815, + -0.6990381453827974, + -0.0452726541753548 + ], + [ + -1.5649480878153308, + -1.459145830949577, + -2.140445052357969, + 0.06589437176542022, + -0.6989378900062381, + -0.04528293395310226 + ], + [ + -1.5649480892896663, + -1.4596915286187817, + -2.14089070612315, + 0.06590220752239821, + -0.6988380643345916, + -0.04529317209632742 + ], + [ + -1.5649480907313205, + -1.4602376274339328, + -2.1413371924593316, + 0.06591001173891796, + -0.6987386692272758, + -0.045303368231174965 + ], + [ + -1.5649480919198706, + -1.460784128114179, + -2.141784512654369, + 0.06591778267567376, + -0.6986397049188222, + -0.045313520325492275 + ], + [ + -1.564948177348846, + -1.4613310364169712, + -2.1422326098827216, + 0.06592579503677627, + -0.6985409867035466, + -0.04532333071772734 + ], + [ + -1.5649480943412102, + -1.4618783384298855, + -2.142681661598694, + 0.0659332303666811, + -0.6984430735117242, + -0.04533369903717967 + ], + [ + -1.5649482862922681, + -1.4624264476771331, + -2.143132255215695, + 0.06594138415541789, + -0.6983462144269909, + -0.04534213812902821 + ], + [ + -1.5649510689282955, + -1.4629750643344288, + -2.1435831941287655, + 0.0659579615660374, + -0.6982495665073731, + -0.04536678546914901 + ], + [ + -1.5649480951564279, + -1.4635226197594589, + -2.144033553368441, + 0.06595614377457526, + -0.6981512947243005, + -0.04536362911615079 + ], + [ + -1.5649480702841254, + -1.4640717918157924, + -2.144487023392338, + 0.06596369725308597, + -0.6980564189286668, + -0.04537487109912117 + ], + [ + -1.5649480988181808, + -1.4646209489475652, + -2.1449392329863115, + 0.06597128235483016, + -0.6979590917941075, + -0.045383392748256084 + ], + [ + -1.5649481015058735, + -1.4651706922270114, + -2.1453932809527507, + 0.06597880909555744, + -0.6978636066996452, + -0.045393217894304866 + ], + [ + -1.5649481026966237, + -1.4657208442160363, + -2.145848175969134, + 0.06598629326410643, + -0.6977685578081315, + -0.045402988405546345 + ], + [ + -1.564948103904163, + -1.466271405989552, + -2.1463039207443253, + 0.06599374520870122, + -0.6976739480705624, + -0.04541271608946627 + ], + [ + -1.5649481898220676, + -1.466822382285256, + -2.146760453774354, + 0.06600144007979146, + -0.6975795845081633, + -0.045422101179048574 + ], + [ + -1.5649481063615494, + -1.4673737620288347, + -2.1472179656078363, + 0.06600855216895292, + -0.6974860488198198, + -0.04543204269321103 + ], + [ + -1.564948107522461, + -1.4679255572758165, + -2.147676268012719, + 0.06601590617635271, + -0.6973927602559834, + -0.04544164035744429 + ], + [ + -1.5649481935740546, + -1.4684777695004843, + -2.1481353626277175, + 0.06602350396856216, + -0.6972997183816244, + -0.04545089599340428 + ], + [ + -1.5649481099880498, + -1.4690303884455886, + -2.1485954439856827, + 0.06603051727612057, + -0.6972075115137438, + -0.04546070698406791 + ], + [ + -1.5649481111574521, + -1.4695834249523338, + -2.1490563192642296, + 0.06603777293089627, + -0.6971155518126084, + -0.04547017416099907 + ], + [ + -1.5649481123662683, + -1.4701368772639043, + -2.149518056045485, + 0.06604499600385035, + -0.6970240371266824, + -0.04547959810457836 + ], + [ + -1.5649481135839953, + -1.4706907457965217, + -2.149980655336682, + 0.06605218603341921, + -0.6969329677848695, + -0.0454889782469967 + ], + [ + -1.5649482920432458, + -1.471245421356329, + -2.1504448693822384, + 0.06605978323169234, + -0.696843141720066, + -0.04549678101635183 + ], + [ + -1.5649482339359357, + -1.471800921002718, + -2.15091188273419, + 0.06606682325373667, + -0.6967568112852102, + -0.04550911918277596 + ], + [ + -1.5649483323707776, + -1.4723560313832291, + -2.1513774318783154, + 0.06607464548949411, + -0.6966676319991597, + -0.045518945016962686 + ], + [ + -1.5649484221293664, + -1.4729115508821062, + -2.1518438780840787, + 0.06608249595415405, + -0.6965789980396444, + -0.045528800526628034 + ], + [ + -1.5649481140281165, + -1.4734662973449335, + -2.1523065247526243, + 0.06608761224291802, + -0.6964842478065328, + -0.04553519481198353 + ], + [ + -1.564948120940333, + -1.474022745596624, + -2.1527744586306996, + 0.06609462660967538, + -0.6963959510607256, + -0.04554433232963616 + ], + [ + -1.5649481221304642, + -1.4745795515168938, + -2.153243142621887, + 0.06610158222104653, + -0.6963080223743013, + -0.045553402113238405 + ], + [ + -1.5649482087733648, + -1.4751367830188786, + -2.1537126328995604, + 0.0661087833405428, + -0.6962203406827115, + -0.04556212909864078 + ], + [ + -1.5649484823098545, + -1.4756954747423665, + -2.154185155128642, + 0.06611646978483447, + -0.6961352998803589, + -0.045573469539203756 + ], + [ + -1.5649484823098545, + -1.4756954747423665, + -2.154185155128642, + 0.06611646978483447, + -0.6961352998803589, + -0.045573469539203756 + ], + [ + -1.5649475646134356, + -1.471245183907892, + -2.150444935632825, + 0.06605736524038457, + -0.6968446055499057, + -0.04550342055861433 + ], + [ + -1.5649480449012783, + -1.466823482109866, + -2.1467627387982122, + 0.066000894837175, + -0.6975818099987059, + -0.0454224447973474 + ], + [ + -1.5649474667625392, + -1.4624254728371286, + -2.143130796361929, + 0.06593878648987417, + -0.6983460493659368, + -0.045347649489092805 + ], + [ + -1.564949716307383, + -1.4580554263223515, + -2.13955624663632, + 0.06588304259067213, + -0.6991376160157152, + -0.045261786048699355 + ], + [ + -1.5649474517400084, + -1.4537036203780511, + -2.136020785211525, + 0.06581338966775657, + -0.6999501823821501, + -0.04518379550251453 + ], + [ + -1.564947485923767, + -1.4537559747695923, + -2.13606333732605, + 0.06581422686576843, + -0.6999403834342957, + -0.04518473520874977 + ], + [ + -1.4141969565258465, + -1.4809213017344895, + -2.2001881926358684, + 0.06729083863794222, + -0.7303115914212729, + 0.10591315208711764 + ], + [ + -1.4141969565258465, + -1.4809213017344895, + -2.2001881926358684, + 0.06729083863794222, + -0.7303115914212729, + 0.10591315208711764 + ], + [ + -1.4141978870697256, + -1.481471877576713, + -2.200646530654894, + 0.06730380530438475, + -0.7302180165005451, + 0.10590223249996908 + ], + [ + -1.41419757062136, + -1.482028253249448, + -2.2011163326633523, + 0.06730904342475395, + -0.7301303829674767, + 0.10588986294291837 + ], + [ + -1.4141975692835036, + -1.4825842666729008, + -2.2015859235652218, + 0.06731551843249406, + -0.7300441577833724, + 0.10588116719711801 + ], + [ + -1.414197747586191, + -1.4831413879087354, + -2.2020576975684425, + 0.0673224017577685, + -0.7299595821334235, + 0.1058739969421964 + ], + [ + -1.4141978898835175, + -1.4836968629614764, + -2.2025270532300603, + 0.06732959511104, + -0.729874055115475, + 0.10586698299580813 + ], + [ + -1.414197575253648, + -1.4842550928997942, + -2.203000372240604, + 0.06733479191168508, + -0.7297883638182623, + 0.105855296305439 + ], + [ + -1.4141975737060783, + -1.4848129765700706, + -2.203473771198591, + 0.0673411268932772, + -0.7297040720637598, + 0.10584679082362382 + ], + [ + -1.4141975748657705, + -1.4853713271090183, + -2.2039481212743897, + 0.06734744206347905, + -0.7296202630833047, + 0.10583831501681 + ], + [ + -1.4141977492402822, + -1.485930791420338, + -2.2044246671433907, + 0.06735414079623347, + -0.7295381079693453, + 0.10583136461992605 + ], + [ + -1.4141978682978542, + -1.4864886641850361, + -2.2048988998661914, + 0.06736108682196464, + -0.7294549683778198, + 0.10582435493054801 + ], + [ + -1.4141977732474849, + -1.4870497871601847, + -2.2053780280796, + 0.06736665818931714, + -0.7293728043597143, + 0.1058146605375979 + ], + [ + -1.414197832956436, + -1.4876100553874487, + -2.2058582514030176, + 0.06737416040474065, + -0.7292941137377117, + 0.10580242026367835 + ], + [ + -1.4141975946780951, + -1.4881700463706664, + -2.2063342278565967, + 0.06737875877672862, + -0.729208530366867, + 0.10579647977068116 + ], + [ + -1.4141975802686269, + -1.4887313658421808, + -2.206814441984867, + 0.06738458431987071, + -0.7291276706423435, + 0.10578847582430517 + ], + [ + -1.4141975826834785, + -1.489293040573074, + -2.2072955562946808, + 0.0673906578961629, + -0.7290472949144818, + 0.10578032940786233 + ], + [ + -1.4141975837806793, + -1.489855192813287, + -2.207777642027263, + 0.06739668894089945, + -0.7289674112669482, + 0.1057722393518848 + ], + [ + -1.4141975837806793, + -1.489855192813287, + -2.207777642027263, + 0.06739668894089945, + -0.7289674112669482, + 0.1057722393518848 + ], + [ + -1.4120440806190497, + -1.4497028929700961, + -2.127527009577623, + 0.07354406422721979, + -0.6811209138872535, + 0.10146166333638029 + ], + [ + -1.4110086094435692, + -1.4147420775459725, + -2.0569265171438547, + 0.08010350386895544, + -0.6378056252376617, + 0.09568284420658577 + ], + [ + -1.4104795675831459, + -1.3985267123712766, + -2.023932589029312, + 0.08359828773219795, + -0.6171908552729342, + 0.09260275429916602 + ], + [ + -1.4093970803269378, + -1.3681797630603918, + -1.9617237463990107, + 0.09109073771040377, + -0.5776796625110365, + 0.08597135447701527 + ], + [ + -1.4082846488033742, + -1.3401279996972275, + -1.9036407010427987, + 0.09936777998180936, + -0.5400189781725963, + 0.07859928700060811 + ], + [ + -1.4077114307169984, + -1.3268410379030027, + -1.8759177467066435, + 0.10382038679496203, + -0.5217850595358885, + 0.07461091055120525 + ], + [ + -1.4207544848246287, + -1.3300019935398484, + -1.8824253673437168, + 0.11164895399355533, + -0.5334798028718201, + 0.054934818784939506 + ], + [ + -1.4293581962778252, + -1.3323118007527692, + -1.8871397245922494, + 0.11638785264765295, + -0.5414801218339922, + 0.04236765607106759 + ], + [ + -1.4402226506513003, + -1.3301338566568772, + -1.8826239742050537, + 0.11698455439719152, + -0.5404610402942994, + 0.030866935189820033 + ], + [ + -1.4510187381488884, + -1.3281985566708425, + -1.8786083360502113, + 0.1174797879710684, + -0.5396886873658455, + 0.019537363927476754 + ], + [ + -1.4625052462909387, + -1.3272186972754976, + -1.876512667345497, + 0.11997121819210098, + -0.5408599681759068, + 0.005862681278856389 + ], + [ + -1.474001037772417, + -1.3264816667459471, + -1.8749196704605267, + 0.12230960569425492, + -0.5423190639706252, + -0.007680782084646806 + ], + [ + -1.4743837112786242, + -1.326468598556134, + -1.8748903690078458, + 0.12237918724486359, + -0.5423879162112228, + -0.008122117983369745 + ], + [ + -1.4856211358460858, + -1.3253210026675684, + -1.8724668120084484, + 0.12380625184259707, + -0.5426355242864849, + -0.02064329027181623 + ], + [ + -1.4968708636169281, + -1.3244109148091872, + -1.870535954506675, + 0.12512429203461567, + -0.5431579357736952, + -0.033066731897118416 + ], + [ + -1.4977911756823965, + -1.3243470079627797, + -1.870399352095437, + 0.12522629614871988, + -0.543211956095289, + -0.0340790698375632 + ], + [ + -1.5089592435635242, + -1.3234654723158195, + -1.86853774153368, + 0.12616657857549257, + -0.5435149644632538, + -0.046091707696649185 + ], + [ + -1.5186847949739883, + -1.3228798511540194, + -1.867294624412663, + 0.12691202542592817, + -0.5439775655813675, + -0.05647881952551442 + ], + [ + -1.52985749479506, + -1.3223955050097993, + -1.8662597707591988, + 0.12765026418445724, + -0.5446935082034696, + -0.06829603856267459 + ], + [ + -1.538930570998677, + -1.3221667872158698, + -1.8657619756657358, + 0.1281810378625539, + -0.5454570221289797, + -0.07782242150294885 + ], + [ + -1.54987698906037, + -1.3216209105701453, + -1.864624481473026, + 0.12816626093567074, + -0.5455578329735276, + -0.08877249597760623 + ], + [ + -1.5596449922066982, + -1.3213139163872685, + -1.863984948163069, + 0.1281007551134035, + -0.545841113657462, + -0.09848431680018861 + ], + [ + -1.571018283423283, + -1.3220373774196628, + -1.8654292881609655, + 0.12940395036531926, + -0.5483399747315741, + -0.11089622136758734 + ], + [ + -1.582383219257827, + -1.3229943065695855, + -1.8673605065822778, + 0.13058009152160996, + -0.5511071449558151, + -0.12317851249455283 + ], + [ + -1.582777826507011, + -1.3230339959870185, + -1.8674398852818637, + 0.13062207452321442, + -0.5512114694615934, + -0.12360029355275114 + ], + [ + -1.5958195969948301, + -1.3253377469446284, + -1.8719072209027205, + 0.13814442544572514, + -0.5567891086008913, + -0.14284520176666962 + ], + [ + -1.6088281194654643, + -1.3279764692911202, + -1.8770443887926485, + 0.14530878636176, + -0.562829992838872, + -0.1616909662458101 + ], + [ + -1.609841746610557, + -1.3281994238978272, + -1.8774793234100708, + 0.14585306619260097, + -0.5633255471971412, + -0.1631442844739197 + ], + [ + -1.609841746610557, + -1.3281994238978272, + -1.8774793234100708, + 0.14585306619260097, + -0.5633255471971412, + -0.1631442844739197 + ], + [ + -1.6074737036727123, + -1.3065764695633435, + -1.8327869868051208, + 0.14074756759559448, + -0.5371716251021583, + -0.15814872056732 + ], + [ + -1.6050442109996303, + -1.2860160800112965, + -1.7899742880597616, + 0.1350422347018813, + -0.5118456702683242, + -0.1523201931829353 + ], + [ + -1.6037632801014252, + -1.2757511723724029, + -1.7684755524711742, + 0.13183905547518912, + -0.499026112413315, + -0.14897285872165078 + ], + [ + -1.6000828964647504, + -1.2755664012100894, + -1.7681009916543462, + 0.13142537182255573, + -0.4985798540189755, + -0.14494528243589286 + ], + [ + -1.5887325256705112, + -1.2749329421199411, + -1.7667697678732879, + 0.13154321383606168, + -0.4968878526439739, + -0.1337660606250374 + ], + [ + -1.5793164865795246, + -1.274561421437309, + -1.7659914975035709, + 0.1315798671627242, + -0.49565804501549354, + -0.12442903089779263 + ], + [ + -1.5679662497835385, + -1.2743136644687032, + -1.765475123967331, + 0.13158367050045422, + -0.49442000262721914, + -0.11312315693351695 + ], + [ + -1.5569223236815624, + -1.274264932741946, + -1.765380014890354, + 0.13150579424582054, + -0.4934315313672096, + -0.1020381152375317 + ], + [ + -1.5453942471290714, + -1.2740952434477548, + -1.7650512251720512, + 0.13084828883439817, + -0.4918586082957472, + -0.08996710128909337 + ], + [ + -1.533865524642873, + -1.2741326012312073, + -1.7651583990249191, + 0.13007540930489608, + -0.49052257946562594, + -0.077780071135955 + ], + [ + -1.5316392682198376, + -1.2741638546420109, + -1.765229174795895, + 0.129911771262025, + -0.4902919152666686, + -0.07541295008523738 + ], + [ + -1.5200094923337428, + -1.2742346910162345, + -1.7654171487129893, + 0.12874106645056702, + -0.48871488462992224, + -0.06277058447637503 + ], + [ + -1.5091307411814296, + -1.2744920141116278, + -1.7659959592645866, + 0.12752681784327952, + -0.48746244682863904, + -0.05082644884505538 + ], + [ + -1.497926573732543, + -1.2758356238679966, + -1.768832011554236, + 0.12749398528716271, + -0.48849096667439673, + -0.03952390329058556 + ], + [ + -1.4867418772913141, + -1.2773790086350172, + -1.772090790507306, + 0.12740582960719202, + -0.48973711561325256, + -0.028176387317484102 + ], + [ + -1.4860770909721588, + -1.27747665432532, + -1.7722961811608573, + 0.12739223688047221, + -0.48981640329428505, + -0.027495257476624756 + ], + [ + -1.4750699231314668, + -1.2795492016463053, + -1.7766565454449055, + 0.12772950494861054, + -0.4920675646958529, + -0.016662552676553134 + ], + [ + -1.4661679452276384, + -1.281376859093236, + -1.7804990504116085, + 0.12796681628082682, + -0.4940460284360744, + -0.00786232980933779 + ], + [ + -1.4607156474022691, + -1.2821367573516678, + -1.7821212169135707, + 0.12714245700037918, + -0.49436120209811285, + -0.0016440517476743973 + ], + [ + -1.4607156474022691, + -1.2821367573516678, + -1.7821212169135707, + 0.12714245700037918, + -0.49436120209811285, + -0.0016440517476743973 + ], + [ + -1.459160908899742, + -1.2642776939897444, + -1.744289268387834, + 0.1308436270389845, + -0.4738230263202424, + -0.0045759182653586 + ], + [ + -1.4575688358987453, + -1.2471095858695986, + -1.7076719203160229, + 0.1347576285630587, + -0.45381129062888037, + -0.007644392369260821 + ], + [ + -1.4567552134715884, + -1.2387269435636332, + -1.6896991185344392, + 0.1368144967946138, + -0.4439386051902047, + -0.009260377513168282 + ], + [ + -1.4683606686686954, + -1.2370914656808492, + -1.6861740118569053, + 0.13775424399261058, + -0.44293095914543074, + -0.021802213088914425 + ], + [ + -1.473764743338136, + -1.2363973834746702, + -1.6846758678396312, + 0.13816087868267227, + -0.44253866090162003, + -0.0276107801183082 + ], + [ + -1.484935912630273, + -1.2341583436661672, + -1.6798951505396051, + 0.13739455533013042, + -0.43952272456214375, + -0.03822983958802965 + ], + [ + -1.4961411624976155, + -1.2321020963985219, + -1.6755036378198682, + 0.13659969127824628, + -0.4366979501611378, + -0.04884277344267709 + ], + [ + -1.4975445756131769, + -1.231858868290936, + -1.6749839485268978, + 0.1364969278903185, + -0.4363609085569203, + -0.050168429140714434 + ], + [ + -1.5085771122953735, + -1.2296124596651044, + -1.6701938873609858, + 0.13495917738032615, + -0.43269897859049167, + -0.0599467668358334 + ], + [ + -1.5188180555244504, + -1.2276941670518868, + -1.6660997251741594, + 0.13353435102685357, + -0.42947160163646764, + -0.0690130453383575 + ], + [ + -1.530062663777578, + -1.2260948260939744, + -1.6626800578613583, + 0.13247285499009462, + -0.42696651519092543, + -0.07938134028713698 + ], + [ + -1.5413348528583035, + -1.2246783877304777, + -1.659649720827628, + 0.13140604498983066, + -0.4246594326198372, + -0.08976331839355946 + ], + [ + -1.5415262566968748, + -1.2246563951853695, + -1.659604785620913, + 0.13139015885295974, + -0.42462831126757367, + -0.08993639710877757 + ], + [ + -1.5542601948970587, + -1.2264935013532827, + -1.6634941266389531, + 0.13487395014736536, + -0.4307508741849654, + -0.10568448064283928 + ], + [ + -1.5667822560638367, + -1.2285376904024263, + -1.667814653102272, + 0.13792876126624115, + -0.4370840184391215, + -0.12080236165407268 + ], + [ + -1.5791059189781893, + -1.2300292680387224, + -1.6709617069566394, + 0.13974057000044124, + -0.4416526237646702, + -0.13462842905301817 + ], + [ + -1.5896271443323489, + -1.2314766143811655, + -1.6740173845852149, + 0.14109561131475237, + -0.4457694980976831, + -0.14623556563820972 + ], + [ + -1.6013573231643285, + -1.2321057213381046, + -1.6753561761355618, + 0.14119001833540243, + -0.4475379438078377, + -0.15798058205351692 + ], + [ + -1.613076957051701, + -1.2329247093916154, + -1.6771043368207021, + 0.14118349875411695, + -0.44952844547822335, + -0.16960958573836443 + ], + [ + -1.614023327896101, + -1.23299943975429, + -1.677263179339946, + 0.14117699916348136, + -0.44969831604629884, + -0.17054681084657355 + ], + [ + -1.6256814155082906, + -1.2339495884383744, + -1.6792971290767589, + 0.14100219328075253, + -0.45173349817447445, + -0.18195452027379364 + ], + [ + -1.6373201268990982, + -1.235089268795076, + -1.681737620448852, + 0.1407327908498171, + -0.4539861125190841, + -0.1932521488891605 + ], + [ + -1.6395917206873911, + -1.2353310398623754, + -1.6822546753724565, + 0.14067460468820658, + -0.45444156658519447, + -0.1954456627630127 + ], + [ + -1.6395917206873911, + -1.2353310398623754, + -1.6822546753724565, + 0.14067460468820658, + -0.45444156658519447, + -0.1954456627630127 + ], + [ + -1.6429050926495288, + -1.221289011927081, + -1.6515690439767703, + 0.15634916725500045, + -0.44105119505546503, + -0.21388634105003218 + ], + [ + -1.6462886684282336, + -1.2077094256399485, + -1.6216526245254876, + 0.17279741505531784, + -0.4281081032148603, + -0.23324473317257025 + ], + [ + -1.64802556278274, + -1.2010172073695426, + -1.6068168281375583, + 0.18141230028159608, + -0.42173297337351334, + -0.24338982790953725 + ], + [ + -1.6364102284907065, + -1.1999491544729175, + -1.6044881157399482, + 0.18230529218505548, + -0.4196012318419617, + -0.23271524409585745 + ], + [ + -1.6243084857228471, + -1.1986342422498872, + -1.6016569616404597, + 0.18252888005561785, + -0.4165333187292628, + -0.2209885109613832 + ], + [ + -1.612718284775997, + -1.1975510697321083, + -1.599329446898312, + 0.18260043274787202, + -0.41380858407922994, + -0.20960995233639557 + ], + [ + -1.5996868539956526, + -1.1946249743909083, + -1.5931098289364751, + 0.18038350937769942, + -0.40574323947900404, + -0.19494537305233006 + ], + [ + -1.5868414586075636, + -1.1919835236611809, + -1.5874849767545294, + 0.17774731036605623, + -0.39810883714686823, + -0.18003890351245103 + ], + [ + -1.574879772897477, + -1.1919271250697476, + -1.587350481400538, + 0.17830277177330056, + -0.3973703651572478, + -0.1686216778288942 + ], + [ + -1.5629215023748666, + -1.1920461275885612, + -1.5876009536950932, + 0.1787635358915045, + -0.39683754235095087, + -0.1570998898460542 + ], + [ + -1.5622616848761162, + -1.1920573525959464, + -1.5876261291860072, + 0.17878620468936463, + -0.3968141360799304, + -0.1564626226616784 + ], + [ + -1.550736621972365, + -1.1931304675132302, + -1.5899148113460746, + 0.18036841117009383, + -0.39865223018438906, + -0.14629192880752165 + ], + [ + -1.5410221703950744, + -1.1941663371778457, + -1.5921289498808044, + 0.1816734197841715, + -0.4003418366057389, + -0.1376817922248607 + ], + [ + -1.5296168210517425, + -1.1956846050342007, + -1.5953752209219383, + 0.18340578494676738, + -0.4028864168037603, + -0.12772196203104458 + ], + [ + -1.5182645030806028, + -1.1973654849593784, + -1.5989718745306396, + 0.18510730978963086, + -0.40559584001290333, + -0.11776709527184473 + ], + [ + -1.5182645030806028, + -1.1973654849593784, + -1.5989718745306396, + 0.18510730978963086, + -0.40559584001290333, + -0.11776709527184473 + ], + [ + -1.524627907306529, + -1.1746461387432197, + -1.547475354432668, + 0.24260685285396938, + -0.36618770457949545, + -0.18012116920230725 + ], + [ + -1.531234741271906, + -1.1534059117669293, + -1.4981090003762838, + 0.3131914937333925, + -0.32910960167207093, + -0.25613042561530036 + ], + [ + -1.5346626166513857, + -1.143224454706199, + -1.47395291161981, + 0.35489231508840435, + -0.3114307063335892, + -0.3007828603533211 + ], + [ + -1.545553160202557, + -1.142784473559935, + -1.4729636148904053, + 0.35527669032035564, + -0.3119649836291468, + -0.31203769679934895 + ], + [ + -1.5580997108402899, + -1.1425420791564918, + -1.4724128632045472, + 0.3555645804633463, + -0.31310290105415617, + -0.32480987513746723 + ], + [ + -1.5698586937124173, + -1.1424641240730375, + -1.4722350849532544, + 0.3555724771869871, + -0.31434733320649644, + -0.3365118383195312 + ], + [ + -1.5824344394907028, + -1.142517285178185, + -1.4723471848955212, + 0.35551238350904873, + -0.3158016049555742, + -0.34894474031954553 + ], + [ + -1.593004023374311, + -1.1426897532986613, + -1.4727315271127508, + 0.35524815556957107, + -0.3171784736686512, + -0.35917438074142266 + ], + [ + -1.60560980410257, + -1.1431028774855336, + -1.4736660838121005, + 0.35446267362334505, + -0.3191665219887762, + -0.37089332968737265 + ], + [ + -1.618203341046816, + -1.1436817572669704, + -1.4749743271483278, + 0.35340718199227067, + -0.3213462149881441, + -0.3823218473494364 + ], + [ + -1.6186390353153912, + -1.1437051252278458, + -1.4750276169487744, + 0.35336691738405335, + -0.32142731702719407, + -0.3827140393455972 + ], + [ + -1.6190661642212314, + -1.1437290763077652, + -1.4750819108259203, + 0.35332117520068984, + -0.32150851747572173, + -0.38309149896897343 + ], + [ + -1.6190661642212314, + -1.1437290763077652, + -1.4750819108259203, + 0.35332117520068984, + -0.32150851747572173, + -0.38309149896897343 + ], + [ + -1.6236866005094885, + -1.134227111749553, + -1.4526033204298212, + 0.38384790835624805, + -0.3171042035769108, + -0.41731217438476687 + ], + [ + -1.6283965137263632, + -1.1250205613162163, + -1.4305389367565693, + 0.4147863243070221, + -0.3131669336748628, + -0.45212900440038883 + ], + [ + -1.6309567919136814, + -1.1202069949090867, + -1.4188801958898436, + 0.4315090144569117, + -0.3112428472498379, + -0.4710068598185514 + ], + [ + -1.6181696922029358, + -1.1195839426106646, + -1.41744277379594, + 0.4340615856600695, + -0.3089946668191229, + -0.4608588875136733 + ], + [ + -1.605642094768562, + -1.1191300387843854, + -1.41639746245123, + 0.4362736521934094, + -0.3069803632365028, + -0.4506211262065137 + ], + [ + -1.592860647072969, + -1.1188140958697614, + -1.4156605005182594, + 0.43838299026322985, + -0.3050821391031511, + -0.44000971923637744 + ], + [ + -1.580286848589728, + -1.1186571215880792, + -1.4152934270514939, + 0.4401613967634668, + -0.30339579294729796, + -0.4292661491426893 + ], + [ + -1.567490045866042, + -1.1186480248717177, + -1.4152761963766498, + 0.44145051093136817, + -0.3018386311905641, + -0.41780599232634363 + ], + [ + -1.554693268473062, + -1.1187956387596858, + -1.4156201797811379, + 0.44240499372019415, + -0.3004619395774382, + -0.40600289129050837 + ], + [ + -1.5531484839542413, + -1.1188228953304729, + -1.4156864362364874, + 0.4425013998946809, + -0.30030984913486286, + -0.4045535004760275 + ], + [ + -1.5403410464961558, + -1.1191232211332334, + -1.4163799930253438, + 0.4431741714232881, + -0.29905906706182145, + -0.3924493526723851 + ], + [ + -1.5275430189597963, + -1.1195772213751807, + -1.417431169466768, + 0.4435101601563061, + -0.2979862154921824, + -0.38000504860003737 + ], + [ + -1.5273607645731655, + -1.11958619451503, + -1.4174524469789112, + 0.4434981509057357, + -0.2979765878371761, + -0.3798106307309941 + ], + [ + -1.5144902775524811, + -1.120198112517794, + -1.4188785347819495, + 0.4432680847497611, + -0.2970636349907814, + -0.36672627202599334 + ], + [ + -1.5016387800763311, + -1.1209725648523527, + -1.4206774426986597, + 0.4426545039114908, + -0.2963435125077423, + -0.35326657985464144 + ], + [ + -1.4999130736685786, + -1.1210898100698714, + -1.420946269422799, + 0.44254030300161246, + -0.29625968554928017, + -0.35142605743642963 + ], + [ + -1.4892115079743946, + -1.1218766647841745, + -1.4227730335248427, + 0.44168784726120386, + -0.2958264106999816, + -0.33986827294419986 + ], + [ + -1.4892115079743946, + -1.1218766647841745, + -1.4227730335248427, + 0.44168784726120386, + -0.2958264106999816, + -0.33986827294419986 + ], + [ + -1.4874218967064448, + -1.1136153916812381, + -1.4041908742587448, + 0.44072501961577654, + -0.2931486149918422, + -0.3384444956635071 + ], + [ + -1.4855972357957277, + -1.1055427286253279, + -1.385856680241276, + 0.439728449235421, + -0.290531423929722, + -0.3369475301006903 + ], + [ + -1.4846691224214557, + -1.101569087545162, + -1.3767630769474286, + 0.43922097877884386, + -0.28923914164084485, + -0.33615451340402347 + ], + [ + -1.4881132593052264, + -1.1014561170165578, + -1.3767389255086264, + 0.43307517200192025, + -0.2897460534173301, + -0.333677013577836 + ], + [ + -1.4986156711669358, + -1.1002898061264759, + -1.3747227446231256, + 0.4179391704945776, + -0.2886070627849336, + -0.32969858004880404 + ], + [ + -1.5091296891097228, + -1.0992379094499936, + -1.3729464920118115, + 0.40257922874016916, + -0.2875513604939499, + -0.32549992098895336 + ], + [ + -1.510867338813771, + -1.0990767073841765, + -1.3726804586385573, + 0.40001892460396293, + -0.28738986043627623, + -0.32478324393057695 + ], + [ + -1.5238797638117862, + -1.0973961286802094, + -1.3687190369702456, + 0.4043858445818075, + -0.2847443785086869, + -0.342155228497127 + ], + [ + -1.5369398972832984, + -1.0958779190519667, + -1.365128658085829, + 0.40865369766114973, + -0.282317228406539, + -0.3594596988727496 + ], + [ + -1.5380682298229755, + -1.0957549961593576, + -1.3648369486587175, + 0.40901622783578395, + -0.2821172366147911, + -0.3609456060836575 + ], + [ + -1.5511316882262898, + -1.0953013754365415, + -1.3637691501538258, + 0.40866026469825767, + -0.28266483181701907, + -0.37365225948100383 + ], + [ + -1.5618451917683205, + -1.0950440422446823, + -1.3631615698635182, + 0.40812875210692423, + -0.2832525031080799, + -0.3838284701944718 + ], + [ + -1.5748734120453616, + -1.0950255065145225, + -1.3631323881823678, + 0.40608034686467165, + -0.2846071562398439, + -0.3948109331481884 + ], + [ + -1.5865006085241327, + -1.095136083340658, + -1.36340412990649, + 0.4039845357309267, + -0.28596521184503587, + -0.40433649037978614 + ], + [ + -1.5995636492065393, + -1.095430639896297, + -1.3641105022860947, + 0.4012217112680398, + -0.2877433384933091, + -0.41462445426466643 + ], + [ + -1.6126168932134832, + -1.0958772723314014, + -1.3651719365998622, + 0.3981569626697062, + -0.2896996621501818, + -0.42459062323778507 + ], + [ + -1.6139730537791521, + -1.0959327628640552, + -1.365303601577257, + 0.3978231354934768, + -0.28991509401204263, + -0.4256108712916446 + ], + [ + -1.6139730537791521, + -1.0959327628640552, + -1.365303601577257, + 0.3978231354934768, + -0.28991509401204263, + -0.4256108712916446 + ], + [ + -1.6143392151972624, + -1.0917130369521462, + -1.363118827249654, + 0.3951455570903489, + -0.2918304342485889, + -0.42318426040736473 + ], + [ + -1.6147054567587815, + -1.087500296110877, + -1.3609567732091195, + 0.39248467156529854, + -0.2937619137751572, + -0.42077351776070815 + ], + [ + -1.6150716972835493, + -1.0832949171787958, + -1.358818216082453, + 0.38983890861617243, + -0.29571042326131713, + -0.41837695094082644 + ], + [ + -1.6154362901869326, + -1.0790967243941425, + -1.3567043798362735, + 0.38719317243182944, + -0.2976775174897423, + -0.4159779654237545 + ], + [ + -1.6158034152666385, + -1.0749059805111822, + -1.3546109856390907, + 0.38459007260179134, + -0.29965727323414487, + -0.4136227945853148 + ], + [ + -1.6161707170274948, + -1.0707232388518406, + -1.3525433949139796, + 0.3819951166415799, + -0.30165850066020833, + -0.4112771755066804 + ], + [ + -1.6165369357850259, + -1.0665477251561477, + -1.3504984704430194, + 0.37941284358553395, + -0.30367402399614435, + -0.4089385932336301 + ], + [ + -1.6169028549181519, + -1.0623794371243909, + -1.3484766829675396, + 0.37684465946737766, + -0.30570639208573797, + -0.40661450144357253 + ], + [ + -1.6172694640269247, + -1.058219114719232, + -1.3464788441520155, + 0.37429538272105023, + -0.3077566440323067, + -0.4043092563105154 + ], + [ + -1.6176357305279179, + -1.0540661837926995, + -1.3445040096847025, + 0.37176126353933076, + -0.309822995388844, + -0.40201780428602407 + ] + ] +} diff --git a/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_aa408118297d598cb5068cf745e5067b.trajex-totg-replay.json b/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_aa408118297d598cb5068cf745e5067b.trajex-totg-replay.json new file mode 100644 index 0000000..45623d0 --- /dev/null +++ b/src/viam/trajex/totg/streaming/test/data/orbsanding_synthetic_aa408118297d598cb5068cf745e5067b.trajex-totg-replay.json @@ -0,0 +1,2178 @@ +{ + "max_acceleration_vec_rads_per_sec2": [ + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659 + ], + "max_blend_curvature": 100000.0, + "max_velocity_vec_rads_per_sec": [ + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659, + 0.3490658503988659 + ], + "min_blend_curvature": 1e-05, + "path_colinearization_ratio": 0.5, + "path_tolerance_delta_rads": 0.008726646259971648, + "schema_version": 1, + "timestamp": "2026-06-04T01:20:27.954973Z", + "waypoints_rads": [ + [ + -2.136748790740967, + -0.7803743481636047, + -1.7049429416656494, + 5.754814083047677e-06, + -0.8599405288696289, + -5.052760124206542 + ], + [ + -1.6518513639607262, + -1.5769597819018855, + -2.3891194073501363, + 0.012891318898158918, + -0.8129649046742973, + -0.08991475257153778 + ], + [ + -1.6518513639607262, + -1.5769597819018855, + -2.3891194073501363, + 0.012891318898158918, + -0.8129649046742973, + -0.08991475257153778 + ], + [ + -1.6518517577130296, + -1.5776034154474798, + -2.3897657059304436, + 0.012892165251019131, + -0.8129652235379325, + -0.08991594164355501 + ], + [ + -1.6518518052627602, + -1.578253508482353, + -2.3904242762069416, + 0.012892292209507288, + -0.8129732034047359, + -0.08991598144491858 + ], + [ + -1.651851824577836, + -1.5789058664171196, + -2.391087364847267, + 0.012892224721359846, + -0.8129839156880184, + -0.08991584430373599 + ], + [ + -1.6518518180819382, + -1.579559271362936, + -2.3917524579230522, + 0.012892050811177816, + -0.8129956135470103, + -0.08991560066788194 + ], + [ + -1.6518519631192958, + -1.5802180482188923, + -2.392428005833019, + 0.012892334323590703, + -0.8130139798772289, + -0.08991461935833225 + ], + [ + -1.6518516775773324, + -1.5808691984793997, + -2.3930887225688764, + 0.012891276520092145, + -0.8130220235043732, + -0.08991453542105442 + ], + [ + -1.6518520187633996, + -1.5815320965653552, + -2.393772176435477, + 0.01289204569811237, + -0.8130453001087355, + -0.08991308937949347 + ], + [ + -1.6518518324771598, + -1.582182596089238, + -2.394431681077421, + 0.012891383601135288, + -0.8130514340233986, + -0.08991465793908499 + ], + [ + -1.6518518055421831, + -1.5828410255927812, + -2.3951065120594284, + 0.0128910793828947, + -0.8130679204013551, + -0.08991423130832217 + ], + [ + -1.6518519703646937, + -1.583504828164684, + -2.395791811610006, + 0.012891366651255082, + -0.8130910113082886, + -0.08991323220368924 + ], + [ + -1.6518516759474802, + -1.584160890806483, + -2.396462064568582, + 0.012890223333225759, + -0.8131036603875439, + -0.08991304348944183 + ], + [ + -1.6518519854305824, + -1.5848265816100993, + -2.397150992306313, + 0.01289093931705714, + -0.8131284184408818, + -0.08991252875160337 + ], + [ + -1.6518516692828116, + -1.5854846395989581, + -2.397825121924232, + 0.012889703741155688, + -0.8131429623873101, + -0.08991230927438419 + ], + [ + -1.651852023597819, + -1.5861547502693538, + -2.3985225697028385, + 0.012890432874112177, + -0.8131730454826347, + -0.08991082472896508 + ], + [ + -1.6518519335194928, + -1.5868167743478965, + -2.3992044556107315, + 0.01288996101080255, + -0.8131917282385265, + -0.08991153651441908 + ], + [ + -1.6518516834311117, + -1.5874779666322043, + -2.3998846535510716, + 0.012888910074664344, + -0.8132091615966558, + -0.08991116503191769 + ], + [ + -1.651851810904425, + -1.5881443043208494, + -2.400574798554997, + 0.012888998615604929, + -0.8132329103925724, + -0.0899112672471978 + ], + [ + -1.6518518339193078, + -1.5888117985067043, + -2.4012672279815703, + 0.012888759246641821, + -0.8132578059482234, + -0.08991091593129347 + ], + [ + -1.6518518325537874, + -1.589480419260667, + -2.40196183868406, + 0.012888430481961555, + -0.8132837932208334, + -0.08991044423620517 + ], + [ + -1.6518518322097455, + -1.5901500989633508, + -2.4026584998320524, + 0.012888088678542629, + -0.8133107733644774, + -0.08990995490786938 + ], + [ + -1.651851833697077, + -1.5908208335531149, + -2.4033572042470968, + 0.01288774188290287, + -0.8133387378130585, + -0.08990945939511209 + ], + [ + -1.651851834953327, + -1.5914926388825426, + -2.4040579829594724, + 0.012887382778537444, + -0.8133677060142255, + -0.08990894329208456 + ], + [ + -1.6518518358014387, + -1.5921655208673653, + -2.4047608470390665, + 0.012887009427557265, + -0.8133976841507278, + -0.0899084078565078 + ], + [ + -1.651851837034073, + -1.5928394815888998, + -2.4054657997650524, + 0.012886625327971043, + -0.8134286710232393, + -0.08990785558555524 + ], + [ + -1.651851837637902, + -1.5935145291628174, + -2.406172857203854, + 0.012886227493416424, + -0.8134606761848634, + -0.08990728547713502 + ], + [ + -1.6518518386811087, + -1.5941906701232669, + -2.406882031165211, + 0.012885817368314888, + -0.8134937048283397, + -0.08990669539117346 + ], + [ + -1.6518518399163011, + -1.5948679068516707, + -2.4075933261299665, + 0.012885395895761265, + -0.8135277574329822, + -0.08990608998874755 + ], + [ + -1.6518520198486633, + -1.595553857330448, + -2.4083215312045656, + 0.012885407319125214, + -0.8135730696445557, + -0.08990364542374521 + ], + [ + -1.6518518372224267, + -1.5962257099422665, + -2.4090223765571825, + 0.01288450705989368, + -0.8135989548149936, + -0.08990481405410762 + ], + [ + -1.6518520211072831, + -1.5969139141998563, + -2.4097549267419156, + 0.01288449522375636, + -0.8136464015252731, + -0.08990234992460618 + ], + [ + -1.6518518385660377, + -1.5975879726882585, + -2.410460037708797, + 0.012883571244350212, + -0.8136743439669012, + -0.08990346726730883 + ], + [ + -1.6518518258235884, + -1.5982708372140668, + -2.411182205220988, + 0.012883034998999628, + -0.8137137045434748, + -0.08990270137426838 + ], + [ + -1.6518518444236179, + -1.5989547368564367, + -2.4119063870715918, + 0.012882592292901257, + -0.8137539584588069, + -0.08990205715754929 + ], + [ + -1.6518518476307535, + -1.5996398254760915, + -2.4126328716630034, + 0.012882087798514755, + -0.8137953449034748, + -0.08990132815047916 + ], + [ + -1.651851848520455, + -1.6003260687727985, + -2.413361587458664, + 0.012881562960305543, + -0.8138378113861541, + -0.08990057184299347 + ], + [ + -1.651852003352871, + -1.6010185790668805, + -2.4141024738759156, + 0.01288148679168218, + -0.8138879393726862, + -0.08989900616610898 + ], + [ + -1.6518516641465641, + -1.6017023225383078, + -2.414826330249263, + 0.012879892647092061, + -0.8139263907589799, + -0.08989822727223915 + ], + [ + -1.6518520402842016, + -1.6023995201164831, + -2.4155762011166053, + 0.012880384398816365, + -0.8139819928609014, + -0.08989646853344077 + ], + [ + -1.6518518383025054, + -1.6030826942477279, + -2.4162989853858456, + 0.01287929627600841, + -0.8140185351755085, + -0.08989730959403933 + ], + [ + -1.651851833935334, + -1.6037747939268463, + -2.4170390007999387, + 0.012878680489703066, + -0.8140664960073917, + -0.08989642384044456 + ], + [ + -1.651851833935334, + -1.6037747939268463, + -2.4170390007999387, + 0.012878680489703066, + -0.8140664960073917, + -0.08989642384044456 + ], + [ + -1.6518514229257248, + -1.5982724050154333, + -2.4111851610406343, + 0.012881835381240315, + -0.8137155494649798, + -0.08990151950234859 + ], + [ + -1.651851684767872, + -1.5928436665047547, + -2.405473792278473, + 0.0128862043684465, + -0.8134338383014121, + -0.08990671858651648 + ], + [ + -1.6518518825234916, + -1.5874880244126246, + -2.399904146462403, + 0.012889365155086357, + -0.8132228932017904, + -0.08991135997595388 + ], + [ + -1.6518530523801231, + -1.5821770076154342, + -2.3944203991239235, + 0.012894057771881825, + -0.813041475898765, + -0.08991483244620549 + ], + [ + -1.6518521322188697, + -1.576961654275162, + -2.3891221163197307, + 0.012894228930894364, + -0.8129665212962884, + -0.08991289044425621 + ], + [ + -1.6518521308898926, + -1.5770132541656494, + -2.3891744613647456, + 0.012894227169454098, + -0.8129672408103943, + -0.08991290628910065 + ], + [ + -1.3779530172379604, + -1.6141555600617619, + -2.469550404979007, + 0.04624928266381501, + -0.8902495400112755, + 0.1627497470197878 + ], + [ + -1.3779530172379604, + -1.6141555600617619, + -2.469550404979007, + 0.04624928266381501, + -0.8902495400112755, + 0.1627497470197878 + ], + [ + -1.3779515837140364, + -1.61488130040212, + -2.4703537399344886, + 0.046242526277298815, + -0.8903287864139195, + 0.16275668129400866 + ], + [ + -1.377951700182927, + -1.6156038718759047, + -2.471151451939723, + 0.046239947510064515, + -0.8904036566389314, + 0.16276121140364921 + ], + [ + -1.377951683047289, + -1.6163284023403783, + -2.4719528224379808, + 0.046237025911566546, + -0.8904804540892467, + 0.16276574935693294 + ], + [ + -1.3779516877254134, + -1.617054411526963, + -2.4727570394012006, + 0.04623410150951253, + -0.8905585760285781, + 0.16277040671107945 + ], + [ + -1.3779516882063523, + -1.6177819960923803, + -2.473564264750557, + 0.04623111559855774, + -0.8906381362586344, + 0.16277514297904164 + ], + [ + -1.3779516902333984, + -1.618511153073824, + -2.4743744958183065, + 0.046228079571388137, + -0.8907191269604011, + 0.16277996467915512 + ], + [ + -1.377952176166903, + -1.6192409631700246, + -2.4751864127819947, + 0.04622665950353963, + -0.890800375319189, + 0.1627849401141323 + ], + [ + -1.3779519050295435, + -1.619974003693654, + -2.4760041772296137, + 0.04622303741901114, + -0.8908854336124011, + 0.1627895906763231 + ], + [ + -1.3779517629989766, + -1.6207081019515979, + -2.4768233615597373, + 0.04621882143901235, + -0.8909707001297694, + 0.16279507142641292 + ], + [ + -1.3779516889371726, + -1.6214437939020994, + -2.4776460077730484, + 0.046215379093149725, + -0.89105766540236, + 0.16280009604720572 + ], + [ + -1.377950940283176, + -1.6221834171044573, + -2.4784777006384724, + 0.04621023694230852, + -0.8911507935915242, + 0.16280571747979708 + ], + [ + -1.3779520104585912, + -1.6229245693364247, + -2.4793098568893233, + 0.04620994851648608, + -0.8912419727550827, + 0.16281158022716297 + ], + [ + -1.3779517652421993, + -1.6236602134444487, + -2.480132057328338, + 0.04620547460655073, + -0.89132690379102, + 0.16281622205627452 + ], + [ + -1.3779516953321322, + -1.6244025676029366, + -2.480967434223525, + 0.046201819444514396, + -0.8914199434429682, + 0.1628216430444009 + ], + [ + -1.3779517046214578, + -1.6251464142160836, + -2.4818057260045876, + 0.04619830769354342, + -0.8915142747940719, + 0.16282726538815656 + ], + [ + -1.3779517046214578, + -1.6251464142160836, + -2.4818057260045876, + 0.04619830769354342, + -0.8915142747940719, + 0.16282726538815656 + ], + [ + -1.3860600410135528, + -1.6017104731841674, + -2.4366770356890326, + 0.04460979407873451, + -0.866851118196579, + 0.15496807113274488 + ], + [ + -1.3942518027941833, + -1.5869162824543581, + -2.4081054660835894, + 0.04294225909176761, + -0.8503847338650569, + 0.14740386466963526 + ], + [ + -1.4032586587823257, + -1.5770887732300398, + -2.3890669892811847, + 0.04057507534433853, + -0.8406370393612136, + 0.1396862050554967 + ], + [ + -1.4123007324129042, + -1.568071576059768, + -2.371568152377243, + 0.038163211848741566, + -0.8315854771127228, + 0.13202006578906608 + ], + [ + -1.4140127429436604, + -1.5664565679629652, + -2.368429923046782, + 0.03770015006939966, + -0.8299535980882595, + 0.13057782878671156 + ], + [ + -1.4229492795225953, + -1.5583819174111384, + -2.3527342741450394, + 0.03476001136195873, + -0.8217716111564402, + 0.12344184519063182 + ], + [ + -1.4319181975651756, + -1.5509263810752598, + -2.338220199522258, + 0.03178205451772589, + -0.8141177988698508, + 0.1163453320322081 + ], + [ + -1.4332694784131503, + -1.5498541549713043, + -2.3361315380820056, + 0.031330688466805826, + -0.8130081016095465, + 0.11528190886797263 + ], + [ + -1.44224654721111, + -1.5425547107321749, + -2.3218987959811273, + 0.028204218374106277, + -0.8051610335967067, + 0.1083270794008446 + ], + [ + -1.449467854345754, + -1.537090159012891, + -2.311229612119206, + 0.02567112572588242, + -0.7991987955979717, + 0.10277640597470893 + ], + [ + -1.4586492420285608, + -1.53029508141916, + -2.2979419104861365, + 0.02293811330045311, + -0.7914732849565019, + 0.09541507761254774 + ], + [ + -1.4669165541187419, + -1.5246183631384667, + -2.2868251952906147, + 0.020471581939524305, + -0.7848996456377201, + 0.08882128825471584 + ], + [ + -1.4769230696291158, + -1.5187610165751457, + -2.2753219628013377, + 0.020148722002057598, + -0.7779839075636474, + 0.07898154952113218 + ], + [ + -1.4854324120631808, + -1.514176213114189, + -2.2663067744812238, + 0.019894663097922025, + -0.7724755006869134, + 0.07060873561991043 + ], + [ + -1.495959876513356, + -1.509965796208198, + -2.258004018178778, + 0.021109084141881323, + -0.7676312289067568, + 0.05916220828452992 + ], + [ + -1.5065123971749406, + -1.5062018124473402, + -2.2505731996110128, + 0.0223508574995135, + -0.7632338562351142, + 0.04767055991047977 + ], + [ + -1.5066144608322631, + -1.5061675419956537, + -2.2505055439638215, + 0.02236303972705425, + -0.7631932865682797, + 0.0475595140666772 + ], + [ + -1.5164036033196457, + -1.5002437960034416, + -2.238845411051409, + 0.02115342636251954, + -0.7549799439734179, + 0.038588886237118154 + ], + [ + -1.5228960816712616, + -1.4965732010934134, + -2.2316103605698796, + 0.020377485717481218, + -0.749769402117508, + 0.03263298820358273 + ], + [ + -1.5228960816712616, + -1.4965732010934134, + -2.2316103605698796, + 0.020377485717481218, + -0.749769402117508, + 0.03263298820358273 + ], + [ + -1.529639843911598, + -1.4688175949519855, + -2.1761312566316016, + 0.04539673556948922, + -0.7252426797737189, + 0.006432923816822031 + ], + [ + -1.5365064360709082, + -1.4437761122215662, + -2.12547650914551, + 0.07160817174721604, + -0.7034009976794917, + -0.021536470757176945 + ], + [ + -1.5400094235897517, + -1.4320165315176785, + -2.1014630567854957, + 0.08522885367394697, + -0.6932830178129793, + -0.03624683373482049 + ], + [ + -1.5303442696909686, + -1.4318686809142271, + -2.101104067886642, + 0.08829742745960587, + -0.6916600791505266, + -0.02896613717235672 + ], + [ + -1.5226092756093037, + -1.4319264409352013, + -2.1011669666464736, + 0.09071515641073197, + -0.6905135548771113, + -0.023103875475352587 + ], + [ + -1.5117788867487532, + -1.4312151492787, + -2.0998170113776244, + 0.08981689438568423, + -0.6873948828749427, + -0.011624601343713651 + ], + [ + -1.5024761701075333, + -1.4308481109407138, + -2.099144318400037, + 0.0889518328308646, + -0.6849718440733249, + -0.0016790864660232029 + ], + [ + -1.4918878662573567, + -1.4334910712327946, + -2.1044771777689064, + 0.08814431404794583, + -0.687611703659214, + 0.009658915314122387 + ], + [ + -1.481774937929085, + -1.436316445201425, + -2.110170119786447, + 0.08735616454719843, + -0.6904376100933242, + 0.020524696073300163 + ], + [ + -1.471593160836446, + -1.440203744964603, + -2.117945528246349, + 0.08762618485976695, + -0.6948521768320504, + 0.03069063199156425 + ], + [ + -1.4614421429978128, + -1.444417944486833, + -2.1263676336242305, + 0.08788569172789323, + -0.6995806995710542, + 0.04085428985677391 + ], + [ + -1.460020066917211, + -1.4450353575131554, + -2.127601158838016, + 0.08792198858376594, + -0.7002690894996222, + 0.04227989589146426 + ], + [ + -1.4499484018686088, + -1.449482203562746, + -2.1364753897304922, + 0.08833643082906475, + -0.705091817973714, + 0.0522629105477542 + ], + [ + -1.4399104025937015, + -1.4542731572419096, + -2.1460277178627765, + 0.08873450693992199, + -0.7102393485742754, + 0.06224757033504548 + ], + [ + -1.4397268446459384, + -1.4543649658593478, + -2.1462106979778905, + 0.08874230613578417, + -0.7103383188466693, + 0.062429881694896376 + ], + [ + -1.430627913327942, + -1.4608310484772542, + -2.1589774219553783, + 0.09188470590842698, + -0.7181528001150161, + 0.0695023690154044 + ], + [ + -1.4218158018051132, + -1.4675027081477894, + -2.1721309856047673, + 0.09492369460930648, + -0.7260801070096979, + 0.07640476577344664 + ], + [ + -1.4129079631523649, + -1.4749602627226874, + -2.186800885112113, + 0.09829777425364554, + -0.7349031538322626, + 0.08324502238699356 + ], + [ + -1.4040467933657446, + -1.4828602973118012, + -2.2023151084343486, + 0.10162406804036848, + -0.7440881359856399, + 0.09014237462394793 + ], + [ + -1.4023778333381345, + -1.484404071600189, + -2.2053438581231055, + 0.10224201812169098, + -0.7458642763273301, + 0.0914521954307723 + ], + [ + -1.39240775516216, + -1.4912111274340236, + -2.218833826301271, + 0.10189916299595324, + -0.7527312099815144, + 0.10211303896640953 + ], + [ + -1.384333810270168, + -1.497105703790296, + -2.230499744652469, + 0.10160229844117043, + -0.7586497512037043, + 0.11078537327989477 + ], + [ + -1.3742251063681823, + -1.5016274731862518, + -2.2394950981114294, + 0.10117774607721819, + -0.7611259006961768, + 0.12148359429591311 + ], + [ + -1.3654896563543693, + -1.5058848759307755, + -2.2479517959565487, + 0.10073938669598081, + -0.7635959781582641, + 0.13080260860993584 + ], + [ + -1.3559293966232495, + -1.510242035324809, + -2.2565391030979614, + 0.10201897478280825, + -0.7652821339887947, + 0.1397036711387411 + ], + [ + -1.3548280165911617, + -1.510768736275163, + -2.2575763267889255, + 0.10215930573454861, + -0.7654979973621908, + 0.14073539497517518 + ], + [ + -1.3548280165911617, + -1.510768736275163, + -2.2575763267889255, + 0.10215930573454861, + -0.7654979973621908, + 0.14073539497517518 + ], + [ + -1.3511333166401347, + -1.4739012489507686, + -2.1847226047965864, + 0.1016034074709833, + -0.7258413933270087, + 0.1424309614126046 + ], + [ + -1.3473519477728555, + -1.4411655521231317, + -2.1195167496617744, + 0.10069068865224273, + -0.6897000046237048, + 0.14484843937550054 + ], + [ + -1.34542104970751, + -1.4259311944987185, + -2.0889956232372424, + 0.10011386443184776, + -0.6725671568847664, + 0.14629885515000715 + ], + [ + -1.3415021823862754, + -1.3974960230488642, + -2.0317140223799792, + 0.09870714845649263, + -0.6400510795511662, + 0.1496610105206085 + ], + [ + -1.3374907040748119, + -1.37118202403504, + -1.9783171216707427, + 0.09699982246794712, + -0.6092999138658616, + 0.15358671948114969 + ], + [ + -1.3354416211427191, + -1.3586525878332112, + -1.952752637925139, + 0.0960187431230645, + -0.5944246387687446, + 0.1557697501292073 + ], + [ + -1.3445135917030473, + -1.3539900373799336, + -1.9431789116519855, + 0.09685228782991681, + -0.5896689205686209, + 0.14574697090426084 + ], + [ + -1.3549962747825903, + -1.3488905921938958, + -1.9326949938252698, + 0.0977548036847158, + -0.5844612862206413, + 0.13422897555570093 + ], + [ + -1.364231819033554, + -1.3446544438525898, + -1.9239718749975159, + 0.09853035601910404, + -0.5801380183810279, + 0.12410747554493391 + ], + [ + -1.3747777812102353, + -1.3400730198422097, + -1.9145305287816028, + 0.09927379011096395, + -0.5754523862590327, + 0.11268294508285985 + ], + [ + -1.3849349776419744, + -1.3359310444645338, + -1.905983025537732, + 0.09996768930381454, + -0.5712197996616144, + 0.1017118517005565 + ], + [ + -1.395537953454532, + -1.3320066889004494, + -1.8978864779204403, + 0.10017846041659985, + -0.5673806821394207, + 0.09071365917276306 + ], + [ + -1.4060923998516819, + -1.3283610822688166, + -1.8903555297447199, + 0.10034820757874248, + -0.5638236706808933, + 0.07981514918614761 + ], + [ + -1.4167945925768441, + -1.3249444897105662, + -1.8832900247054905, + 0.10053664218087101, + -0.5605259652482043, + 0.06876774944210025 + ], + [ + -1.42753553173782, + -1.3217656286703592, + -1.8767101998825888, + 0.10068637935344242, + -0.5574724682086628, + 0.05772792900584669 + ], + [ + -1.428013241072088, + -1.3216298802600268, + -1.8764289025404424, + 0.10069190570682553, + -0.557342484893767, + 0.05723791513789356 + ], + [ + -1.4390443351280955, + -1.319089840051064, + -1.8711399346215218, + 0.10167217555482844, + -0.5554307212230913, + 0.04525019501963561 + ], + [ + -1.4485893841719324, + -1.3170864671334612, + -1.8669623053021396, + 0.10247231778229388, + -0.5539861581009545, + 0.034926661844488706 + ], + [ + -1.4594754318872267, + -1.3148673304577272, + -1.862356460530612, + 0.10257352512348336, + -0.5522838847049636, + 0.02384522949338712 + ], + [ + -1.4703858680586614, + -1.3128711180788533, + -1.858209537551498, + 0.10262014572726266, + -0.5508158220262541, + 0.012797984217051266 + ], + [ + -1.4705551851635068, + -1.312841295854157, + -1.8581473748827955, + 0.10262028703946238, + -0.550793743557304, + 0.012626514334830589 + ], + [ + -1.481500070526635, + -1.3110730624691267, + -1.854471097280203, + 0.102653640159618, + -0.5495724220679112, + 0.001568161981048174 + ], + [ + -1.4924657662740402, + -1.3095233494598777, + -1.8512475129375943, + 0.10263240844479633, + -0.5485818082791453, + -0.009449672638970784 + ], + [ + -1.4929997328157012, + -1.3094534423664281, + -1.851102063418735, + 0.10262969113379924, + -0.5485395148128454, + -0.009984668888090128 + ], + [ + -1.5041132674428874, + -1.3080944302565944, + -1.8482599196680929, + 0.10308783204690727, + -0.547713354781682, + -0.02155083145167154 + ], + [ + -1.5152426301378865, + -1.3069560731377863, + -1.845876275098002, + 0.10349092685172546, + -0.5471285104363967, + -0.033073479015311776 + ], + [ + -1.5160949357535773, + -1.3068777916518393, + -1.8457121996240968, + 0.10351959169895902, + -0.5470932508946783, + -0.0339536467346364 + ], + [ + -1.5272428559015467, + -1.3059518607214666, + -1.8437701438294425, + 0.10388794815579752, + -0.5467128254406436, + -0.045453602525106 + ], + [ + -1.5384012590700267, + -1.3052441475966179, + -1.8422824210708535, + 0.10420158195192275, + -0.5465714976915051, + -0.056904721351918604 + ], + [ + -1.5495667336017382, + -1.3047536337346972, + -1.8412469408952659, + 0.10445813343923016, + -0.5466678281741336, + -0.06830267969221582 + ], + [ + -1.5607377263086961, + -1.3044805671110438, + -1.8406641535187507, + 0.10466270457707681, + -0.5470025953057445, + -0.07964556489468023 + ], + [ + -1.561798557644212, + -1.3044659026397283, + -1.8406332450242353, + 0.10467889240035982, + -0.5470466274784784, + -0.08072187631742717 + ], + [ + -1.572974844340853, + -1.3044175637960902, + -1.8405202993548297, + 0.10483607643031259, + -0.5476148256436797, + -0.09201673468059304 + ], + [ + -1.582523308400878, + -1.3045473051559713, + -1.8407803915613712, + 0.10492520235447057, + -0.5482864291509866, + -0.10161756094176388 + ], + [ + -1.5936987473373165, + -1.304887716726331, + -1.8414770211419123, + 0.10499423356985067, + -0.5492628998022818, + -0.11281676494121216 + ], + [ + -1.6048687556529797, + -1.3054458990472109, + -1.8426278068820734, + 0.10501425478239797, + -0.5504784784443505, + -0.1239497515682417 + ], + [ + -1.6069897271618747, + -1.3055762648536533, + -1.8428985819028743, + 0.10501096975030211, + -0.5507374385443448, + -0.1260591183235679 + ], + [ + -1.61789633810418, + -1.3065413660643994, + -1.8449234887522048, + 0.10389289873362138, + -0.5525531143091226, + -0.13593510170658127 + ], + [ + -1.6287894666787632, + -1.3077163177084756, + -1.8473847489819974, + 0.10272761675737423, + -0.5545858873499289, + -0.14574373887909675 + ], + [ + -1.6396662528804515, + -1.3091024340936037, + -1.850286251278142, + 0.10151287829075427, + -0.5568381654136904, + -0.15548644933884317 + ], + [ + -1.6505248608929348, + -1.3107017627139188, + -1.8536292136609438, + 0.10025380842426466, + -0.5593075278789803, + -0.16516091456899204 + ], + [ + -1.6613623066536334, + -1.3125158691807488, + -1.857417193669492, + 0.09895122456132936, + -0.5619966906711003, + -0.1747684766833701 + ], + [ + -1.672174716629767, + -1.3145460228101533, + -1.8616514458048345, + 0.09760273661341534, + -0.564905063565727, + -0.18430395903819458 + ], + [ + -1.6725586130291004, + -1.3146223142514282, + -1.861810833986361, + 0.09755936235866035, + -0.5650127759862341, + -0.18464548208536113 + ], + [ + -1.6830620693571299, + -1.3172042162851023, + -1.8672124722893673, + 0.09496158067238888, + -0.5688614373668508, + -0.19277955899700713 + ], + [ + -1.6929896238716637, + -1.3198509174708972, + -1.8727410092615009, + 0.09247172397851944, + -0.572698139419676, + -0.20043587737729307 + ], + [ + -1.6969862069736295, + -1.321147669933261, + -1.8754535218556458, + 0.0909367426576124, + -0.5746506137546142, + -0.20305638928103803 + ], + [ + -1.6969862069736295, + -1.321147669933261, + -1.8754535218556458, + 0.0909367426576124, + -0.5746506137546142, + -0.20305638928103803 + ], + [ + -1.7032561167096836, + -1.312862756783282, + -1.8573845342183266, + 0.10890730482774277, + -0.5786332175745509, + -0.22453219209351627 + ], + [ + -1.709583086623097, + -1.305003063792527, + -1.8398813626617654, + 0.1263010988154269, + -0.5830229586271197, + -0.24562368976730875 + ], + [ + -1.7127654596603605, + -1.3012276400891043, + -1.8313314248729609, + 0.13476821859231464, + -0.5853618631267432, + -0.25601085821825603 + ], + [ + -1.7013511899276443, + -1.2985181896534037, + -1.8257359471927883, + 0.13504924842397514, + -0.5815125635976656, + -0.24509046021758535 + ], + [ + -1.6899014027586046, + -1.2960468960631835, + -1.820632705401014, + 0.13525955793585295, + -0.5779254572946986, + -0.2340562048261185 + ], + [ + -1.6892882875165725, + -1.295917864875435, + -1.8203658912245386, + 0.135276955142974, + -0.5777332178085498, + -0.23346963412502658 + ], + [ + -1.6778576283881925, + -1.2937017398988555, + -1.8157838631270398, + 0.13559749875060292, + -0.5744440025927472, + -0.2225261914212314 + ], + [ + -1.666398529146172, + -1.2917147610026138, + -1.8116785912738116, + 0.13585210317629043, + -0.5714078170265928, + -0.21147985286535223 + ], + [ + -1.654916420439183, + -1.2899468238418477, + -1.808030114153458, + 0.1360459155798458, + -0.5686065565707804, + -0.2003426911487653 + ], + [ + -1.6434100786662973, + -1.2884071800722878, + -1.8048583288263866, + 0.1361621344643412, + -0.5660635461767312, + -0.18909752609647815 + ], + [ + -1.6427707442374393, + -1.2883286301600505, + -1.8046963202709219, + 0.1361670890174316, + -0.5659297866824017, + -0.18846993500358566 + ], + [ + -1.6313243829601864, + -1.2870260965580522, + -1.8020096462871327, + 0.13647144134086003, + -0.5636625237194043, + -0.17742376626725123 + ], + [ + -1.6211593850652875, + -1.2860551871868946, + -1.800011793824654, + 0.13668887675314167, + -0.5618574432192531, + -0.16755590793145547 + ], + [ + -1.6097639479791128, + -1.2851608984830427, + -1.7981721234093735, + 0.13709736983949813, + -0.5600505227691673, + -0.156617058142283 + ], + [ + -1.599638916012615, + -1.2845476862711043, + -1.7969162795046396, + 0.13740924299246007, + -0.5586462900881168, + -0.14683914247920024 + ], + [ + -1.5883636986960459, + -1.284054217985975, + -1.7959013800859858, + 0.13805681546361573, + -0.5572929846114594, + -0.13619204148210398 + ], + [ + -1.5770806229077043, + -1.2837658465238229, + -1.7953191326286209, + 0.13863013576620722, + -0.5561617865719779, + -0.12546008868471048 + ], + [ + -1.5769116571403254, + -1.2837626673943177, + -1.7953125085867883, + 0.13863931758735595, + -0.5561451062096966, + -0.12529744280672578 + ], + [ + -1.5656971357182994, + -1.2836681165486241, + -1.795116494432872, + 0.13970798216183444, + -0.5551811494728911, + -0.11504206563502388 + ], + [ + -1.554484130434464, + -1.2837769532435117, + -1.795350458555963, + 0.1407114454459861, + -0.5544337853046656, + -0.10471504390746897 + ], + [ + -1.5532891399335955, + -1.2838004544485202, + -1.7954007456468262, + 0.14081449980940544, + -0.5543668736311829, + -0.10361095878295654 + ], + [ + -1.5421043351769816, + -1.2841322042522418, + -1.7960998179978485, + 0.14181487121525527, + -0.5538438787986506, + -0.09329171749164517 + ], + [ + -1.530922641243324, + -1.2846671644760588, + -1.797227380945284, + 0.1427414481911666, + -0.5535359228198895, + -0.0828989350515006 + ], + [ + -1.5288245336883832, + -1.2847908464790538, + -1.7974867384053723, + 0.14290595810148915, + -0.5535024743320603, + -0.08093955936892523 + ], + [ + -1.517660894529029, + -1.2855680840174273, + -1.7991223527744962, + 0.14375805673121989, + -0.5534494072694991, + -0.07047934409243778 + ], + [ + -1.5065100704771224, + -1.2865508518382238, + -1.801188256578978, + 0.14453359269228996, + -0.5536138836809815, + -0.05995014849820628 + ], + [ + -1.5058221888668946, + -1.286618559428004, + -1.8013297326741884, + 0.14457730381858228, + -0.5536313093655894, + -0.05929750845097173 + ], + [ + -1.4946882368497987, + -1.28787070490174, + -1.8039644833838382, + 0.14511470980937904, + -0.5541369801463809, + -0.048555898225460324 + ], + [ + -1.4835733151965018, + -1.2893309275131872, + -1.8070338685942298, + 0.14557517409596457, + -0.5548641894769951, + -0.03775089110589057 + ], + [ + -1.4825071077959107, + -1.289481125800811, + -1.8073502189587076, + 0.14561494881687942, + -0.5549465425435343, + -0.036705297293395285 + ], + [ + -1.4714105749221116, + -1.291185375959819, + -1.8109275279087955, + 0.14595593894586661, + -0.5559458701254212, + -0.025801332751657517 + ], + [ + -1.460339413553976, + -1.2930998279348787, + -1.8149475795416616, + 0.14622857491967572, + -0.557172214051984, + -0.014838353942250634 + ], + [ + -1.4597822408019057, + -1.2932022827232044, + -1.815161975547911, + 0.146237293015351, + -0.557240726044103, + -0.014283582449135813 + ], + [ + -1.4487386492049086, + -1.2953421913555987, + -1.8196499185327535, + 0.1464309454150397, + -0.5587060049295626, + -0.0032613120777947083 + ], + [ + -1.4377363952996152, + -1.297696527286673, + -1.8245814530490707, + 0.14654254349153503, + -0.5603994956966197, + 0.0078044541345678314 + ], + [ + -1.4267486730459098, + -1.300303258470693, + -1.8300356168837766, + 0.14659722960826815, + -0.5623941960421412, + 0.018929057110612765 + ], + [ + -1.415795931419086, + -1.3031326875491929, + -1.8359500512631355, + 0.14657772191384758, + -0.5646226684114205, + 0.030100936810224688 + ], + [ + -1.413666838543325, + -1.3037096010006886, + -1.837155861056071, + 0.14656639874029698, + -0.5650837545153041, + 0.032281121609139714 + ], + [ + -1.4027524273788472, + -1.3068933261069937, + -1.8437979582633988, + 0.1464684829698597, + -0.567779966677345, + 0.043515499877647267 + ], + [ + -1.3924404673539192, + -1.3101270257789368, + -1.8505369250781936, + 0.1463113106989274, + -0.5705595636663108, + 0.0542036159061809 + ], + [ + -1.3816439319864506, + -1.3136349801256242, + -1.8578408554449986, + 0.1462063314802153, + -0.5734411775771077, + 0.0653483785644894 + ], + [ + -1.370891545128764, + -1.3173804475656363, + -1.8656300150754872, + 0.1460264840838276, + -0.5765665300866246, + 0.0765325167281859 + ], + [ + -1.3601853987294183, + -1.3213684395054908, + -1.8739132567431247, + 0.14577274097387197, + -0.5799396493555501, + 0.0877532399636573 + ], + [ + -1.3530955419795387, + -1.3241545568446054, + -1.8796935963842014, + 0.14556363280036044, + -0.582316549116605, + 0.09523056878793795 + ], + [ + -1.3425331705725376, + -1.3285199765058802, + -1.8887365610124147, + 0.14532960019646957, + -0.5860397007969109, + 0.10632210003244336 + ], + [ + -1.3325318005766102, + -1.3329081792185258, + -1.8978146894006285, + 0.14504322271127654, + -0.5898149189863285, + 0.11690226938069632 + ], + [ + -1.3221468066158906, + -1.33818121301917, + -1.9086863660490236, + 0.14475457120455798, + -0.5949013940084185, + 0.12795746074553505 + ], + [ + -1.319276834124944, + -1.3396907222997556, + -1.911796033095275, + 0.14466858389158074, + -0.5963575915327647, + 0.1310214610530947 + ], + [ + -1.319276834124944, + -1.3396907222997556, + -1.911796033095275, + 0.14466858389158074, + -0.5963575915327647, + 0.1310214610530947 + ], + [ + -1.3195321623522709, + -1.3207563568663472, + -1.8720654694233003, + 0.1606536891820035, + -0.5770742516902163, + 0.11493567612076291 + ], + [ + -1.3197914849857821, + -1.3027105722055377, + -1.8338970106506667, + 0.1774806119791839, + -0.5586140585245779, + 0.09790765831431418 + ], + [ + -1.3199174677989636, + -1.2939955065865407, + -1.8153509869518072, + 0.1862013478988435, + -0.5496759272549572, + 0.08904806855363745 + ], + [ + -1.329273901078509, + -1.2900797535166428, + -1.8071380922530593, + 0.18682140966495436, + -0.5463252187067842, + 0.07879284603157231 + ], + [ + -1.3400892795179944, + -1.285790216348104, + -1.798130442471835, + 0.18744350947287908, + -0.5426838941514118, + 0.06704293586366432 + ], + [ + -1.3509587301585884, + -1.2817338099475297, + -1.7895995237305398, + 0.18797062408941315, + -0.539283094579401, + 0.05534520075705745 + ], + [ + -1.351580423119658, + -1.2815090066865453, + -1.7891267777933382, + 0.18799807727845372, + -0.5390959618345945, + 0.05467961344040143 + ], + [ + -1.3625044349590556, + -1.2776910544841817, + -1.781085706499462, + 0.18841804245368313, + -0.5359397901041137, + 0.04304338455944468 + ], + [ + -1.3713556973246057, + -1.2747727637814479, + -1.7749326326192634, + 0.18868937087499846, + -0.5335619255978209, + 0.033693881586505714 + ], + [ + -1.3823755544576164, + -1.2713481050495645, + -1.7677035173075348, + 0.18893610106831255, + -0.5308098043635967, + 0.022152029661802075 + ], + [ + -1.3928760862785923, + -1.268297871986939, + -1.7612576411867307, + 0.18908211536299144, + -0.5284072826834022, + 0.0112545430945931 + ], + [ + -1.4040325168532255, + -1.26529077774785, + -1.7548931108095862, + 0.1892051358606495, + -0.5260985728815445, + -0.00027078329181855296 + ], + [ + -1.4152303271038429, + -1.2624939350671625, + -1.7489668758071413, + 0.18922643754858448, + -0.5240113377245303, + -0.01172800247548736 + ], + [ + -1.417062997384267, + -1.2620594509281842, + -1.7480455430097397, + 0.1892199033131927, + -0.523697053311833, + -0.013591851554722676 + ], + [ + -1.4283156162725212, + -1.2595204821771362, + -1.7426547658136822, + 0.189198507620876, + -0.5219054718000506, + -0.02503846356165837 + ], + [ + -1.4396042278713623, + -1.2571883710139706, + -1.7376967483219727, + 0.18907695185929396, + -0.5203354161615287, + -0.036409715805891876 + ], + [ + -1.4509267120975806, + -1.255058305841824, + -1.7331667127748567, + 0.18885027154810732, + -0.5189845880830465, + -0.04770600931026468 + ], + [ + -1.4622799712843702, + -1.253129585512297, + -1.7290599349188864, + 0.18852499146051258, + -0.5178490714885207, + -0.05892334393745496 + ], + [ + -1.4736613104918714, + -1.2513999597821088, + -1.725373988725855, + 0.18809914665911856, + -0.5169281746639873, + -0.07005972415435828 + ], + [ + -1.4847097529308528, + -1.2499129012840964, + -1.7222024743473163, + 0.18759118657967136, + -0.5162395621490745, + -0.08076746777926012 + ], + [ + -1.496136179695707, + -1.248589738561756, + -1.7193746243564754, + 0.1870111335144864, + -0.5157840621024348, + -0.09177103164079298 + ], + [ + -1.5067259197097695, + -1.2475371833913653, + -1.7171218661945058, + 0.18638848886360257, + -0.5155459553933026, + -0.10187562630663761 + ], + [ + -1.5067259197097695, + -1.2475371833913653, + -1.7171218661945058, + 0.18638848886360257, + -0.5155459553933026, + -0.10187562630663761 + ], + [ + -1.5059322422602817, + -1.231160186875764, + -1.6820919810461668, + 0.19258696348715387, + -0.4971875905676845, + -0.10801645344739425 + ], + [ + -1.5051142464045115, + -1.2153504085149696, + -1.6480459154677838, + 0.19910961568742286, + -0.4792622110619917, + -0.11443543827480974 + ], + [ + -1.50469728872372, + -1.2076033930169747, + -1.6312768090482537, + 0.2025348473979787, + -0.4704004437135364, + -0.11779223854747356 + ], + [ + -1.4997466978577394, + -1.2080282617730462, + -1.6321986714445478, + 0.20293679158811898, + -0.47044635841243665, + -0.11317752708543492 + ], + [ + -1.4879790441038092, + -1.2091703727284546, + -1.634679970423815, + 0.2037893805656545, + -0.47070635713310677, + -0.10210642645724836 + ], + [ + -1.4762327876477304, + -1.2104974125523793, + -1.6375598223462517, + 0.20451841788614988, + -0.4711713768151183, + -0.09092710610438495 + ], + [ + -1.4746012586283892, + -1.210696566472509, + -1.637991724119823, + 0.20461065834304218, + -0.4712520332265354, + -0.0893643687019248 + ], + [ + -1.4628813231683868, + -1.2122351289929705, + -1.6413285834238236, + 0.2051939515027754, + -0.47195187917523007, + -0.07805508909741857 + ], + [ + -1.4529960382424199, + -1.213681347042816, + -1.6444616480525038, + 0.2055915252683347, + -0.47270410482981007, + -0.0684173824699704 + ], + [ + -1.44133776415712, + -1.2156020011820239, + -1.6486153767689993, + 0.2059545274517251, + -0.4738774600505169, + -0.05693048497649076 + ], + [ + -1.4307371235474187, + -1.2175182394867465, + -1.652755814110239, + 0.2061822390141255, + -0.4751286237625639, + -0.0463769784949106 + ], + [ + -1.419157739034576, + -1.2198174426833286, + -1.6577163764315412, + 0.2063485690244924, + -0.47673946297586794, + -0.034751312985169765 + ], + [ + -1.4076194612140767, + -1.2223088803132722, + -1.6630862028468876, + 0.20639917573998062, + -0.478559701842252, + -0.023042839188922717 + ], + [ + -1.4062944009012106, + -1.2226083366583889, + -1.6637290711566841, + 0.20639473012550857, + -0.4787822545004263, + -0.021690038840733974 + ], + [ + -1.3948137178672622, + -1.225322240843062, + -1.6695716686494508, + 0.20634701514234366, + -0.4808532452274503, + -0.009924712305408248 + ], + [ + -1.3833812518111122, + -1.228233009285262, + -1.6758279683442243, + 0.20618099571363113, + -0.4831362465163805, + 0.0019148981939632636 + ], + [ + -1.3824630854999445, + -1.2284759003064654, + -1.6763497638257954, + 0.20616320104788705, + -0.4833291992188877, + 0.002870860313204395 + ], + [ + -1.3711266018607817, + -1.2315837179422344, + -1.683020718974387, + 0.20591567948513567, + -0.48580597065312253, + 0.014710445513609317 + ], + [ + -1.3614354322903255, + -1.2344117923194458, + -1.6890838661159306, + 0.2056150133393524, + -0.4881032004577394, + 0.024929149146083678 + ], + [ + -1.3501894140497346, + -1.2378813833464501, + -1.6965169025396964, + 0.20513476375347664, + -0.4909505022821, + 0.03692834538141693 + ], + [ + -1.3394638289736123, + -1.241403832036355, + -1.704050117944715, + 0.20455122686380556, + -0.4938880298221193, + 0.04849613450949891 + ], + [ + -1.3283142731394795, + -1.2452692193823962, + -1.7123094466987592, + 0.2037979993011906, + -0.49713388552650617, + 0.06067697715485498 + ], + [ + -1.3270004164599893, + -1.2457395601675614, + -1.7133123512149586, + 0.20370107007029767, + -0.49753089928723604, + 0.06211957520890088 + ], + [ + -1.3270004164599893, + -1.2457395601675614, + -1.7133123512149586, + 0.20370107007029767, + -0.49753089928723604, + 0.06211957520890088 + ], + [ + -1.3272779854525774, + -1.2418490931099124, + -1.7111185359583454, + 0.20304726370961632, + -0.49921874403580385, + 0.06258084027361932 + ], + [ + -1.3275555994178694, + -1.2379697867233845, + -1.7089514532866799, + 0.20239442913174358, + -0.5009211448857431, + 0.06304279956101666 + ], + [ + -1.327833182736896, + -1.2341022039797012, + -1.7068123295165358, + 0.20174032965917943, + -0.5026396708948777, + 0.06350695655976323 + ], + [ + -1.3281107357497701, + -1.230246322783446, + -1.7047010617641847, + 0.20108506538874438, + -0.5043742563432921, + 0.06397316849835222 + ], + [ + -1.3283882593607628, + -1.2264021270973438, + -1.7026175518779976, + 0.2004287495386526, + -0.506124826624619, + 0.06444131630408384 + ], + [ + -1.3286657550946999, + -1.2225696021429278, + -1.7005617038927192, + 0.19977149517694354, + -0.5078913070509908, + 0.06491128250146763 + ], + [ + -1.3289432239868917, + -1.2187487350753008, + -1.6985334241959869, + 0.19911341375317673, + -0.5096736241493884, + 0.06538295071348893 + ], + [ + -1.3292206685198464, + -1.2149395147692221, + -1.696532622568591, + 0.1984546176960847, + -0.5114717062295612, + 0.06585620322364366 + ], + [ + -1.3294980914455379, + -1.2111419320616237, + -1.6945592117033754, + 0.19779521869083744, + -0.5132854832858244, + 0.06633092251253807 + ], + [ + -1.3297754974451481, + -1.2073559800660911, + -1.692613108123125, + 0.19713533270706277, + -0.5151148881742637, + 0.06680698521976179 + ] + ] +} diff --git a/src/viam/trajex/totg/streaming/test/main.cpp b/src/viam/trajex/totg/streaming/test/main.cpp new file mode 100644 index 0000000..7a51764 --- /dev/null +++ b/src/viam/trajex/totg/streaming/test/main.cpp @@ -0,0 +1,8 @@ +// Boost.Test entry point for the streaming session test binary. + +#define BOOST_TEST_MODULE streaming_session_test + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wnull-dereference" +#include +#pragma GCC diagnostic pop diff --git a/src/viam/trajex/totg/streaming/test/session.cpp b/src/viam/trajex/totg/streaming/test/session.cpp new file mode 100644 index 0000000..89e53e3 --- /dev/null +++ b/src/viam/trajex/totg/streaming/test/session.cpp @@ -0,0 +1,672 @@ +// Streaming session tests. +// +// These tests drive the design of `viam::trajex::totg::streaming::session`. The class is +// currently stubbed out; all tests that exercise real behavior are expected to fail until +// the implementation lands. Tests are organized in the order of the test plan: +// +// A. Empty session behavior +// B. First extend & construction error paths +// C. Sampling primitives +// D. Single-trajectory equivalence +// E. Seam validation +// F. Pivot semantics +// G. Stage and rebase +// H. Multi-extend stress +// I. End-of-stream behavior + +#if __has_include() +#include +#else +#include +#endif + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace { + +using viam::trajex::totg::path; +using viam::trajex::totg::trajectory; +using viam::trajex::totg::waypoint_accumulator; +namespace streaming = viam::trajex::totg::streaming; +namespace types = viam::trajex::types; + +// === Fixture defaults === +// +// A small but real fixture: 2 DOF, modest velocity / acceleration limits, and a 100 Hz +// sample rate. The waypoint sets below produce trajectories long enough that pulling a +// dozen samples at 100 Hz stays well within the trajectory's duration. + +constexpr double k_sample_rate_hz = 100.0; + +types::hertz default_sample_rate() { + return types::hertz{k_sample_rate_hz}; +} + +trajectory::options default_trajectory_options() { + trajectory::options topt; + topt.max_velocity = xt::xarray{2.0, 2.0}; + topt.max_acceleration = xt::xarray{5.0, 5.0}; + return topt; +} + +path::options default_path_options() { + path::options popt; + popt.set_max_blend_deviation(0.05); + return popt; +} + +xt::xarray three_waypoints() { + return xt::xarray{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}}; +} + +xt::xarray six_waypoints() { + return xt::xarray{ + {0.0, 0.0}, + {1.0, 0.0}, + {1.0, 1.0}, + {2.0, 1.0}, + {2.0, 2.0}, + {3.0, 2.0}, + }; +} + +// Builds a trajectory directly from waypoints with the same options the session uses. +// This is the reference any session sample stream should agree with where the active +// trajectory's geometry matches the full merged waypoint set. +trajectory reference_trajectory(const xt::xarray& waypoints) { + path p = path::create(waypoints, default_path_options()); + return trajectory::create(std::move(p), default_trajectory_options()); +} + +// Pins a waypoints xarray and a waypoint_accumulator over it together so the pair can be +// handed to session.extend() in a single expression without lifetime hazards. +// +// waypoint_accumulator holds views into its source xarray and explicitly deletes the +// rvalue-source constructor; ad-hoc factories returning an accumulator over a temporary +// xarray won't compile. This wrapper stores both pieces and pins itself. +// +// TODO(streaming-test-utils): hoist alongside other shared test helpers once a second +// streaming test file needs the same scaffolding. Until then, duplication here is cheap. +class pinned_waypoints { + public: + explicit pinned_waypoints(xt::xarray data) : data_(std::move(data)), accumulator_(data_) {} + + pinned_waypoints(const pinned_waypoints&) = delete; + pinned_waypoints& operator=(const pinned_waypoints&) = delete; + pinned_waypoints(pinned_waypoints&&) = delete; + pinned_waypoints& operator=(pinned_waypoints&&) = delete; + + const waypoint_accumulator& accumulator() const noexcept { + return accumulator_; + } + const xt::xarray& data() const noexcept { + return data_; + } + + private: + xt::xarray data_; + waypoint_accumulator accumulator_; +}; + +// Returns true if every element of `a` is within `tolerance` of `b`. Used for direct +// configuration / velocity / acceleration comparisons between session samples and +// reference trajectory samples. +bool configs_match(const xt::xarray& a, const xt::xarray& b, double tolerance = 1e-9) { + if (a.shape(0) != b.shape(0)) { + return false; + } + for (std::size_t i = 0; i < a.shape(0); ++i) { + if (std::abs(a(i) - b(i)) > tolerance) { + return false; + } + } + return true; +} + +// Helper for the cornerstone equivalence test: verify that every sample the session +// emitted agrees with what a directly-built reference trajectory would produce at the +// same time. The reference is queried via `trajectory::sample(t)` which is random-access. +void check_samples_match_reference(const std::vector& session_samples, const trajectory& reference) { + for (const auto& s : session_samples) { + if (s.time < trajectory::seconds{0.0} || s.time > reference.duration()) { + continue; // sample is past the reference's coverage; nothing to compare against + } + const auto expected = reference.sample(s.time); + BOOST_CHECK(configs_match(s.configuration, expected.configuration)); + BOOST_CHECK(configs_match(s.velocity, expected.velocity)); + BOOST_CHECK(configs_match(s.acceleration, expected.acceleration)); + } +} + +streaming::session fresh_session() { + return streaming::session{default_path_options(), default_trajectory_options(), default_sample_rate()}; +} + +} // namespace + +BOOST_AUTO_TEST_SUITE(streaming_session_tests) + +BOOST_AUTO_TEST_SUITE(empty_session) + +BOOST_AUTO_TEST_CASE(fresh_session_has_zero_current_time) { + auto sess = fresh_session(); + BOOST_CHECK_EQUAL(sess.current_time().count(), 0.0); +} + +BOOST_AUTO_TEST_CASE(fresh_session_has_no_active_trajectory) { + auto sess = fresh_session(); + BOOST_CHECK(sess.active_trajectory() == nullptr); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), 0.0); + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 0U); +} + +BOOST_AUTO_TEST_CASE(fresh_session_sample_next_returns_empty) { + auto sess = fresh_session(); + const auto samples = sess.sample_next(5); + BOOST_CHECK(samples.empty()); +} + +BOOST_AUTO_TEST_CASE(fresh_session_sample_at_least_returns_empty) { + auto sess = fresh_session(); + const auto samples = sess.sample_at_least(trajectory::seconds{1.0}); + BOOST_CHECK(samples.empty()); +} + +BOOST_AUTO_TEST_SUITE_END() // empty_session + +BOOST_AUTO_TEST_SUITE(first_extend) + +BOOST_AUTO_TEST_CASE(first_extend_with_valid_batch_creates_active_trajectory) { + auto sess = fresh_session(); + const pinned_waypoints wp(three_waypoints()); + + sess.extend(wp.accumulator()); + + BOOST_CHECK(sess.active_trajectory() != nullptr); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), 0.0); + BOOST_CHECK_EQUAL(sess.current_time().count(), 0.0); + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 1U); +} + +BOOST_AUTO_TEST_CASE(first_extend_with_single_waypoint_propagates_invalid_argument) { + auto sess = fresh_session(); + const pinned_waypoints wp(xt::xarray{{0.0, 0.0}}); + + BOOST_CHECK_THROW(sess.extend(wp.accumulator()), std::invalid_argument); + + // Failed extend leaves the session unchanged. + BOOST_CHECK(sess.active_trajectory() == nullptr); + BOOST_CHECK_EQUAL(sess.current_time().count(), 0.0); + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 0U); +} + +BOOST_AUTO_TEST_CASE(first_extend_with_dof_mismatch_against_options_propagates_invalid_argument) { + // Default options have 2-DOF velocity / acceleration limits; provide 3-DOF waypoints + // so that trajectory construction rejects the result. + auto sess = fresh_session(); + const pinned_waypoints wp(xt::xarray{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}, {2.0, 2.0, 2.0}}); + + BOOST_CHECK_THROW(sess.extend(wp.accumulator()), std::invalid_argument); + + BOOST_CHECK(sess.active_trajectory() == nullptr); + BOOST_CHECK_EQUAL(sess.current_time().count(), 0.0); + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 0U); +} + +BOOST_AUTO_TEST_SUITE_END() // first_extend + +BOOST_AUTO_TEST_SUITE(sampling_primitives) + +BOOST_AUTO_TEST_CASE(sample_next_default_emits_one_sample) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + const auto samples = sess.sample_next(); + BOOST_CHECK_EQUAL(samples.size(), 1U); +} + +BOOST_AUTO_TEST_CASE(sample_next_n_emits_n_samples) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + const auto samples = sess.sample_next(10); + BOOST_CHECK_EQUAL(samples.size(), 10U); +} + +BOOST_AUTO_TEST_CASE(sample_at_least_advances_at_least_horizon) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + const auto horizon = trajectory::seconds{0.1}; // 100 ms + const auto samples = sess.sample_at_least(horizon); + BOOST_REQUIRE(!samples.empty()); + BOOST_CHECK_GE(sess.current_time().count(), horizon.count()); +} + +BOOST_AUTO_TEST_CASE(sample_at_least_zero_horizon_returns_exactly_one_sample) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + // Zero horizon: the first sample's time equals current_time + dt > current_time + 0, + // so the stopping condition is satisfied after exactly one sample is emitted. + const auto samples = sess.sample_at_least(trajectory::seconds{0.0}); + BOOST_CHECK_EQUAL(samples.size(), 1U); +} + +BOOST_AUTO_TEST_CASE(current_time_tracks_last_emitted_sample) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + const auto samples = sess.sample_next(7); + BOOST_REQUIRE_EQUAL(samples.size(), 7U); + BOOST_CHECK_EQUAL(sess.current_time().count(), samples.back().time.count()); +} + +BOOST_AUTO_TEST_SUITE_END() // sampling_primitives + +BOOST_AUTO_TEST_SUITE(single_trajectory_equivalence) + +BOOST_AUTO_TEST_CASE(session_with_one_extend_matches_direct_trajectory) { + auto sess = fresh_session(); + const auto waypoints = six_waypoints(); + const pinned_waypoints wp(waypoints); + sess.extend(wp.accumulator()); + + const auto reference = reference_trajectory(waypoints); + const auto samples = sess.sample_at_least(reference.duration()); + BOOST_REQUIRE(!samples.empty()); + + check_samples_match_reference(samples, reference); +} + +BOOST_AUTO_TEST_SUITE_END() // single_trajectory_equivalence + +BOOST_AUTO_TEST_SUITE(seam_validation) + +BOOST_AUTO_TEST_CASE(second_extend_with_seam_mismatch_throws_invalid_argument) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + + // The last stored waypoint is {1.0, 1.0}. Provide a batch whose first waypoint differs. + const pinned_waypoints mismatched(xt::xarray{{9.0, 9.0}, {2.0, 2.0}}); + BOOST_CHECK_THROW(sess.extend(mismatched.accumulator()), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(second_extend_with_dof_mismatch_throws_invalid_argument) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + + const pinned_waypoints wrong_dof(xt::xarray{{1.0, 1.0, 0.0}, {2.0, 2.0, 0.0}}); + BOOST_CHECK_THROW(sess.extend(wrong_dof.accumulator()), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(second_extend_with_bit_exact_seam_matches_merged_reference) { + // The merged waypoint set after seam-drop is {{0,0},{1,0},{1,1},{2,1},{2,2}}. + // The sample stream after both extends should agree with a reference trajectory + // built directly over that merged set. + auto sess = fresh_session(); + const pinned_waypoints initial(xt::xarray{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}}); + sess.extend(initial.accumulator()); + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + const xt::xarray merged{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}; + const auto reference = reference_trajectory(merged); + + const auto samples = sess.sample_at_least(reference.duration()); + BOOST_REQUIRE(!samples.empty()); + + check_samples_match_reference(samples, reference); +} + +BOOST_AUTO_TEST_SUITE_END() // seam_validation + +BOOST_AUTO_TEST_SUITE(pivot) + +BOOST_AUTO_TEST_CASE(extend_with_branch_ahead_of_watermark_pivots) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + // One sample's worth of watermark advancement: far below where the branch will lie + // (the branch sits near the prefix's terminal blend, which is most of a trajectory away). + sess.sample_next(1); + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // Generation incremented: a new active trajectory was produced (pivot). + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK(sess.active_trajectory() != nullptr); +} + +BOOST_AUTO_TEST_CASE(pivot_preserves_active_epoch) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + sess.sample_next(1); + + const auto pre_extend_epoch = sess.active_epoch(); + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // Confirm a pivot actually happened, then assert epoch is preserved across it. + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), pre_extend_epoch.count()); +} + +BOOST_AUTO_TEST_CASE(pivot_preserves_current_time) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + sess.sample_next(3); + + const auto pre_extend_time = sess.current_time(); + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // Confirm a pivot actually happened, then assert current_time is preserved across it. + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK_EQUAL(sess.current_time().count(), pre_extend_time.count()); +} + +BOOST_AUTO_TEST_SUITE_END() // pivot + +BOOST_AUTO_TEST_SUITE(stage_and_rebase) + +BOOST_AUTO_TEST_CASE(extend_with_branch_behind_watermark_stages) { + // Sampling all the way to the active trajectory's terminal pushes the watermark + // past where the divergence between the initial and merged trajectories sits, so + // the second extend must stage rather than pivot. + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + const auto* initial_active = sess.active_trajectory(); + BOOST_REQUIRE(initial_active != nullptr); + sess.sample_at_least(initial_active->duration()); + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // Stage: no new trajectory became active, so the generation count is unchanged. + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 1U); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), 0.0); +} + +BOOST_AUTO_TEST_CASE(staged_batch_rebases_when_sampling_past_terminal) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + const auto initial_duration = sess.active_trajectory()->duration(); + + sess.sample_at_least(initial_duration); // exhaust the active before extending + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // After extend: stage. Generation count is still 1. + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + // Sampling further triggers the rebase from the original trajectory's terminal pose. + sess.sample_next(1); + + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK(sess.active_trajectory() != nullptr); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), initial_duration.count()); +} + +BOOST_AUTO_TEST_CASE(rebase_seam_configuration_is_continuous) { + // The last sample of the original chain and the first sample of the rebased chain + // should report the same joint configuration -- both correspond to the original + // trajectory's terminal pose, by the rest-to-rest invariant. + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + + const auto initial_duration = sess.active_trajectory()->duration(); + // Capture the terminal pose directly from the trajectory the session is about to leave. + const auto terminal_sample = sess.active_trajectory()->sample(initial_duration); + + sess.sample_at_least(initial_duration); + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + const auto post_rebase_samples = sess.sample_next(1); + BOOST_REQUIRE_EQUAL(post_rebase_samples.size(), 1U); + // Confirm rebase actually happened. + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + + // The first post-rebase sample lives exactly one sample period into the new trajectory + // by construction of quantized_starting_at(new_active, rate, sample_period_). So its + // configuration differs from the terminal pose by the motion the trajectory plans over + // one sample period starting from rest: bounded above by 0.5 * max_accel * sample_period_^2 + // = 0.5 * 5.0 * 0.01^2 = 2.5e-4 rad per joint, with blend curvature potentially adding + // a bit. 1e-3 leaves an order of magnitude of margin; tighter would be brittle. + BOOST_CHECK(configs_match(post_rebase_samples.front().configuration, terminal_sample.configuration, 1e-3)); +} + +BOOST_AUTO_TEST_CASE(rebase_seam_time_keeps_flowing_forward) { + // Across a rebase the epoch advances and the new active trajectory has a fresh + // local-time origin. The session must add the new epoch when reporting sample + // times so the global clock keeps moving forward rather than restarting near zero. + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + const auto initial_duration = sess.active_trajectory()->duration(); + sess.sample_at_least(initial_duration); + const auto pre_rebase_time = sess.current_time(); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + const auto post_rebase_samples = sess.sample_next(1); + BOOST_REQUIRE_EQUAL(post_rebase_samples.size(), 1U); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK_GT(post_rebase_samples.front().time.count(), pre_rebase_time.count()); +} + +BOOST_AUTO_TEST_SUITE_END() // stage_and_rebase + +BOOST_AUTO_TEST_SUITE(multi_extend) + +BOOST_AUTO_TEST_CASE(repeated_admissible_extends_compose_into_long_trajectory) { + // Three extends in a row, each issued while the watermark is at zero (so each pivots). + // The resulting sample stream should agree with a direct trajectory built over the + // fully merged waypoint set. + auto sess = fresh_session(); + + const pinned_waypoints batch_1(xt::xarray{{0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}}); + sess.extend(batch_1.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + const pinned_waypoints batch_2(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(batch_2.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + + const pinned_waypoints batch_3(xt::xarray{{2.0, 2.0}, {3.0, 2.0}}); + sess.extend(batch_3.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 3U); + + const xt::xarray merged{ + {0.0, 0.0}, + {1.0, 0.0}, + {1.0, 1.0}, + {2.0, 1.0}, + {2.0, 2.0}, + {3.0, 2.0}, + }; + const auto reference = reference_trajectory(merged); + + const auto samples = sess.sample_at_least(reference.duration()); + BOOST_REQUIRE(!samples.empty()); + + check_samples_match_reference(samples, reference); +} + +BOOST_AUTO_TEST_CASE(mixed_pivot_and_stage_eventually_drains_all_input) { + // Pivot once, then sample to terminal to force the next extend to stage, then sample + // past terminal to rebase. Generation count progression: 1 (initial), 2 (pivot), + // 2 (stage, no new active), 3 (rebase). + auto sess = fresh_session(); + + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + // Sample one tick, then extend -- pivot. + sess.sample_next(1); + const pinned_waypoints pivot_batch(xt::xarray{{1.0, 1.0}, {2.0, 1.0}}); + sess.extend(pivot_batch.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + + // Sample past where the next extend's branch will lie, forcing it to stage. + const auto duration_before_stage = sess.active_trajectory()->duration(); + sess.sample_at_least(duration_before_stage); + const pinned_waypoints stage_batch(xt::xarray{{2.0, 1.0}, {2.0, 2.0}}); + sess.extend(stage_batch.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); // staged, not pivoted + + // Sample further to trigger the rebase. + sess.sample_next(1); + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 3U); + BOOST_CHECK(sess.active_trajectory() != nullptr); + BOOST_CHECK_EQUAL(sess.active_epoch().count(), duration_before_stage.count()); +} + +BOOST_AUTO_TEST_SUITE_END() // multi_extend + +BOOST_AUTO_TEST_SUITE(end_of_stream) + +BOOST_AUTO_TEST_CASE(sample_next_after_exhaustion_returns_empty) { + auto sess = fresh_session(); + const pinned_waypoints wp(three_waypoints()); + sess.extend(wp.accumulator()); + + const auto* active = sess.active_trajectory(); + BOOST_REQUIRE(active != nullptr); + sess.sample_at_least(active->duration() * 2.0); // sample well past terminal + + // No staging exists, so further pulls drain to empty. + const auto samples = sess.sample_next(5); + BOOST_CHECK(samples.empty()); +} + +BOOST_AUTO_TEST_CASE(extend_after_exhaustion_eventually_starts_new_chain) { + // When the active is exhausted and no staging exists, an arriving extend stages. + // The next sample then triggers a rebase from the terminal pose, producing a new + // active trajectory and advancing the epoch. + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + const auto initial_duration = sess.active_trajectory()->duration(); + + sess.sample_at_least(initial_duration * 2.0); // drain to empty + + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + + // Extend on an exhausted session: stages, no new active built yet. + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + sess.sample_next(1); + + BOOST_CHECK_EQUAL(sess.trajectory_generation_count(), 2U); + BOOST_CHECK(sess.active_trajectory() != nullptr); + BOOST_CHECK_GE(sess.active_epoch().count(), initial_duration.count()); +} + +BOOST_AUTO_TEST_SUITE_END() // end_of_stream + +// These tests pin down a property that uniform_sampler's quantized-for-duration mode is +// supposed to give us at the trajectory level, and that the session needs to preserve +// across rebases: the last emitted sample lands exactly at the active trajectory's +// terminal, with zero joint velocity and zero joint acceleration by the rest-to-rest +// invariant. +BOOST_AUTO_TEST_SUITE(terminal_sampling) + +BOOST_AUTO_TEST_CASE(final_emitted_sample_in_single_trajectory_lies_at_terminal_at_rest) { + auto sess = fresh_session(); + const pinned_waypoints wp(six_waypoints()); + sess.extend(wp.accumulator()); + + const auto duration = sess.active_trajectory()->duration(); + const auto samples = sess.sample_at_least(duration); + BOOST_REQUIRE(!samples.empty()); + + const auto& last = samples.back(); + + BOOST_CHECK_EQUAL(last.time.count(), duration.count()); + BOOST_REQUIRE_EQUAL(last.velocity.shape(0), 2U); + BOOST_REQUIRE_EQUAL(last.acceleration.shape(0), 2U); + for (std::size_t i = 0; i < last.velocity.shape(0); ++i) { + BOOST_CHECK_EQUAL(last.velocity(i), 0.0); + } + for (std::size_t i = 0; i < last.acceleration.shape(0); ++i) { + BOOST_CHECK_EQUAL(last.acceleration(i), 0.0); + } +} + +BOOST_AUTO_TEST_CASE(final_emitted_sample_after_rebase_lies_at_rebased_terminal_at_rest) { + auto sess = fresh_session(); + const pinned_waypoints initial(three_waypoints()); + sess.extend(initial.accumulator()); + + const auto initial_duration = sess.active_trajectory()->duration(); + sess.sample_at_least(initial_duration); // drain the initial chain through its terminal + + // Stage an extension by extending while the watermark sits at the terminal. + const pinned_waypoints extension(xt::xarray{{1.0, 1.0}, {2.0, 1.0}, {2.0, 2.0}}); + sess.extend(extension.accumulator()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 1U); + + // Drain the rest with a generous horizon to fire the rebase and run out the new chain. + const auto post_rebase_samples = sess.sample_at_least(trajectory::seconds{1000.0}); + BOOST_REQUIRE(!post_rebase_samples.empty()); + BOOST_REQUIRE_EQUAL(sess.trajectory_generation_count(), 2U); + + const auto& last = post_rebase_samples.back(); + const auto rebased_duration = sess.active_trajectory()->duration(); + const auto rebased_epoch = sess.active_epoch(); + + BOOST_CHECK_EQUAL(last.time.count(), (rebased_epoch + rebased_duration).count()); + BOOST_REQUIRE_EQUAL(last.velocity.shape(0), 2U); + BOOST_REQUIRE_EQUAL(last.acceleration.shape(0), 2U); + for (std::size_t i = 0; i < last.velocity.shape(0); ++i) { + BOOST_CHECK_EQUAL(last.velocity(i), 0.0); + } + for (std::size_t i = 0; i < last.acceleration.shape(0); ++i) { + BOOST_CHECK_EQUAL(last.acceleration(i), 0.0); + } +} + +BOOST_AUTO_TEST_SUITE_END() // terminal_sampling + +BOOST_AUTO_TEST_SUITE_END() // streaming_session_tests diff --git a/src/viam/trajex/totg/streaming/test/sim/streaming_sim_main.cpp b/src/viam/trajex/totg/streaming/test/sim/streaming_sim_main.cpp new file mode 100644 index 0000000..f49c106 --- /dev/null +++ b/src/viam/trajex/totg/streaming/test/sim/streaming_sim_main.cpp @@ -0,0 +1,394 @@ +// Streaming session simulator. +// +// Loads a canonical replay JSON record, sweeps a (W_c, W_r) grid by simulating a +// firehose caller on top of streaming::session, and writes a CSV summarizing each +// cell. See orbsanding-streaming-session-simulation-plan.md at the repo root for +// the full design. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +#if __has_include() +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace { + +using viam::trajex::totg::parse_replay_record; +using viam::trajex::totg::path; +using viam::trajex::totg::trajectory; +using viam::trajex::totg::waypoint_accumulator; +namespace streaming = viam::trajex::totg::streaming; +namespace types = viam::trajex::types; + +struct sim_config { + std::filesystem::path replay_path; + std::filesystem::path output_path; + double w_c_min = 0.5; + double w_c_max = 32.0; + std::size_t w_c_count = 7; + double w_r_min = 0.01; + double w_r_max = 1.0; + std::size_t w_r_count = 7; + double speed_factor = 1.0; + std::size_t batch_size = 2; + double sample_rate_hz = 100.0; +}; + +void usage_and_exit(const char* argv0, int code) { + std::cerr << "usage: " << argv0 << " --replay --output " + << "[--w-c-min N] [--w-c-max N] [--w-c-count N] " + << "[--w-r-min N] [--w-r-max N] [--w-r-count N] " + << "[--speed-factor N] [--batch-size N] [--sample-rate N]\n"; + std::exit(code); // NOLINT(concurrency-mt-unsafe) +} + +sim_config parse_args(int argc, char* argv[]) { + sim_config cfg; + auto next = [&](int& i) -> const char* { + if (i + 1 >= argc) { + usage_and_exit(argv[0], 2); + } + return argv[++i]; + }; + for (int i = 1; i < argc; ++i) { + const char* a = argv[i]; + if (std::strcmp(a, "--replay") == 0) { + cfg.replay_path = next(i); + } else if (std::strcmp(a, "--output") == 0) { + cfg.output_path = next(i); + } else if (std::strcmp(a, "--w-c-min") == 0) { + cfg.w_c_min = std::stod(next(i)); + } else if (std::strcmp(a, "--w-c-max") == 0) { + cfg.w_c_max = std::stod(next(i)); + } else if (std::strcmp(a, "--w-c-count") == 0) { + cfg.w_c_count = std::stoul(next(i)); + } else if (std::strcmp(a, "--w-r-min") == 0) { + cfg.w_r_min = std::stod(next(i)); + } else if (std::strcmp(a, "--w-r-max") == 0) { + cfg.w_r_max = std::stod(next(i)); + } else if (std::strcmp(a, "--w-r-count") == 0) { + cfg.w_r_count = std::stoul(next(i)); + } else if (std::strcmp(a, "--speed-factor") == 0) { + cfg.speed_factor = std::stod(next(i)); + } else if (std::strcmp(a, "--batch-size") == 0) { + cfg.batch_size = std::stoul(next(i)); + } else if (std::strcmp(a, "--sample-rate") == 0) { + cfg.sample_rate_hz = std::stod(next(i)); + } else if (std::strcmp(a, "--help") == 0 || std::strcmp(a, "-h") == 0) { + usage_and_exit(argv[0], 0); + } else { + std::cerr << "unknown flag: " << a << "\n"; + usage_and_exit(argv[0], 2); + } + } + if (cfg.replay_path.empty() || cfg.output_path.empty()) { + usage_and_exit(argv[0], 2); + } + if (cfg.batch_size < 2) { + throw std::invalid_argument("batch-size must be at least 2 (seam point counts)"); + } + return cfg; +} + +// Build a geometric sweep of `count` values from min to max, inclusive at both ends. +// count == 1 yields {min}. +std::vector geometric_grid(double lo, double hi, std::size_t count) { + std::vector values; + values.reserve(count); + if (count == 0) { + return values; + } + if (count == 1) { + values.push_back(lo); + return values; + } + const double log_lo = std::log(lo); + const double log_hi = std::log(hi); + for (std::size_t i = 0; i < count; ++i) { + const double t = static_cast(i) / static_cast(count - 1); + values.push_back(std::exp(log_lo + (t * (log_hi - log_lo)))); + } + return values; +} + +// Build path::options the same way planner.cpp reconstructs it from a replay record. +path::options path_options_from_replay(const viam::trajex::totg::planner_base::config& cfg) { + auto opts = path::options{}.set_max_blend_deviation(cfg.path_blend_tolerance); + if (cfg.colinearization_ratio) { + opts.set_max_linear_deviation(cfg.path_blend_tolerance * *cfg.colinearization_ratio); + } + if (cfg.min_blend_curvature) { + opts.set_min_blend_curvature(*cfg.min_blend_curvature); + } + if (cfg.max_blend_curvature) { + opts.set_max_blend_curvature(*cfg.max_blend_curvature); + } + return opts; +} + +trajectory::options trajectory_options_from_replay(const viam::trajex::totg::planner_base::config& cfg) { + trajectory::options topts; + topts.max_velocity = cfg.velocity_limits; + topts.max_acceleration = cfg.acceleration_limits; + return topts; +} + +struct cell_result { + double w_c{}; + double w_r{}; + int rebases{}; + std::optional starved_at_waypoint; + // True when the cell failure was a trajex exception during extend()/rebase, not a + // genuine starve. Currently lumped into starved_at_waypoint for the CSV schema's + // sake; logged to stderr so the operator can see the distinction. The CSV reader + // sees a red cell either way. + bool extend_threw{false}; + std::string extend_throw_message; +}; + +cell_result simulate_cell(const xt::xarray& workload, + const path::options& popts, + const trajectory::options& topts, + double sample_rate_hz, + double w_c, + double w_r, + double speed_factor, + std::size_t batch_size) { + cell_result result; + result.w_c = w_c; + result.w_r = w_r; + + const std::size_t n = workload.shape(0); + const double sample_period = 1.0 / sample_rate_hz; + + // Declared above the try so the catch can report which waypoint we were processing + // when trajex threw. A trajex exception during extend() or during the rebase that + // sample_next() may trigger fails the cell; the catch records the failure point in + // starved_at_waypoint so the plot paints red. The distinction from a real starve is + // recorded in extend_threw and logged to stderr. + std::size_t next_wp_idx = 0; + std::size_t active_wp_count = 0; + std::size_t staged_wp_count = 0; + + try { + streaming::session sess(popts, topts, types::hertz{sample_rate_hz}); + + // Bootstrap: the first extend must deliver at least two waypoints to make a valid + // trajectory. Use exactly two so subsequent batches can default to batch_size=2 with + // one new waypoint each. + { + const xt::xarray bootstrap = xt::view(workload, xt::range(std::size_t{0}, std::size_t{2}), xt::all()); + const waypoint_accumulator acc(bootstrap); + sess.extend(acc); + next_wp_idx = 2; + active_wp_count = 2; + } + + // Pre-fill the watermark to one replan-budget ahead of t=0, mirroring a real consumer + // that primes its buffer before letting the arm begin executing. Without this, the + // very first tick would advance the arm into the future before any samples exist. + { + const double target = w_r; + while (sess.current_time().count() < target) { + const auto pre = sess.trajectory_generation_count(); + auto s = sess.sample_next(1); + const auto post = sess.trajectory_generation_count(); + if (post > pre) { + result.rebases += static_cast(post - pre); + active_wp_count += staged_wp_count; + staged_wp_count = 0; + } + if (s.empty()) { + break; + } + } + } + + double arm_time = 0.0; + + // Steady-state loop. Termination conditions: + // - All workload waypoints delivered AND staging empty AND arm reached active terminal. + // - Starve detected during an extend() call (returns early). + // - Session unexpectedly drained (defensive break). + auto active_terminal_global = [&]() { + const auto* a = sess.active_trajectory(); + return sess.active_epoch().count() + (a ? a->duration().count() : 0.0); + }; + + while (true) { + const bool all_delivered = (next_wp_idx >= n); + const bool staging_empty = (staged_wp_count == 0); + if (all_delivered && staging_empty && arm_time >= active_terminal_global()) { + break; + } + + // Tick: advance arm by one sample period. + arm_time += sample_period; + + // Refill the watermark to >= arm_time + w_r. Each sample_next(1) may trigger a + // rebase internally; track the generation count delta to count rebases. + while (sess.current_time().count() < arm_time + w_r) { + const auto pre = sess.trajectory_generation_count(); + auto s = sess.sample_next(1); + const auto post = sess.trajectory_generation_count(); + if (post > pre) { + result.rebases += static_cast(post - pre); + active_wp_count += staged_wp_count; + staged_wp_count = 0; + } + if (s.empty()) { + break; + } + } + + // Deliver waypoints while the commit window has headroom. + while (next_wp_idx < n) { + const auto* active = sess.active_trajectory(); + if (active == nullptr) { + break; // shouldn't happen after bootstrap, but guard for safety + } + const double active_dur = active->duration().count(); + const double per_wp_dur = active_dur / static_cast(std::max(active_wp_count, 1)); + const double est_staged_dur = static_cast(staged_wp_count) * per_wp_dur; + const double est_commit_horizon_global = sess.active_epoch().count() + active_dur + est_staged_dur; + const double headroom = est_commit_horizon_global - arm_time; + if (headroom >= w_c) { + break; + } + + // Build the next batch: [seam_point, new_waypoints...]. Seam is at index + // (next_wp_idx - 1); batch covers [next_wp_idx - 1, batch_end). + const std::size_t batch_start = next_wp_idx - 1; + const std::size_t batch_end = std::min(next_wp_idx + batch_size - 1, n); + const xt::xarray batch_data = xt::view(workload, xt::range(batch_start, batch_end), xt::all()); + const waypoint_accumulator acc(batch_data); + + const double watermark_before = sess.current_time().count(); + const auto pre_gen = sess.trajectory_generation_count(); + + const auto start = std::chrono::steady_clock::now(); + sess.extend(acc); // may throw; let it propagate up + const auto stop = std::chrono::steady_clock::now(); + const double elapsed_real = std::chrono::duration(stop - start).count(); + const double arm_advance = elapsed_real * speed_factor; + + // Starve check: if the arm would advance past the watermark during this extend, + // the consumer is asking for a sample the session hasn't produced. The starve + // point is recorded as the waypoint index this extend was delivering. + if (arm_time + arm_advance > watermark_before) { + result.starved_at_waypoint = static_cast(next_wp_idx); + return result; + } + arm_time += arm_advance; + + const auto post_gen = sess.trajectory_generation_count(); + const std::size_t new_wps = batch_end - next_wp_idx; + if (post_gen > pre_gen) { + active_wp_count += new_wps; + } else { + staged_wp_count += new_wps; + } + next_wp_idx = batch_end; + } + } + } catch (const std::exception& e) { + result.extend_threw = true; + result.extend_throw_message = e.what(); + result.starved_at_waypoint = static_cast(next_wp_idx); + } + + return result; +} + +void write_csv(const sim_config& cfg, std::size_t n_waypoints, const std::vector& cells) { + std::ofstream out(cfg.output_path); + if (!out) { + throw std::runtime_error("failed to open output file: " + cfg.output_path.string()); + } + out << "# workload: " << cfg.replay_path.filename().string() << "\n"; + out << "# n_waypoints: " << n_waypoints << "\n"; + out << "# sample_rate_hz: " << cfg.sample_rate_hz << "\n"; + out << "# speed_factor: " << cfg.speed_factor << "\n"; + out << "# batch_size: " << cfg.batch_size << "\n"; + out << "commit_window,replan_budget,rebases,starved_at_waypoint\n"; + for (const auto& c : cells) { + out << c.w_c << "," << c.w_r << "," << c.rebases << ","; + if (c.starved_at_waypoint) { + out << *c.starved_at_waypoint; + } + out << "\n"; + } +} + +} // namespace + +int main(int argc, char* argv[]) try { + const sim_config cfg = parse_args(argc, argv); + + auto [planner_cfg, workload] = parse_replay_record(cfg.replay_path); + const std::size_t n = workload.shape(0); + if (n < 2) { + throw std::runtime_error("replay record must contain at least 2 waypoints"); + } + + const auto popts = path_options_from_replay(planner_cfg); + const auto topts = trajectory_options_from_replay(planner_cfg); + + const auto w_c_values = geometric_grid(cfg.w_c_min, cfg.w_c_max, cfg.w_c_count); + const auto w_r_values = geometric_grid(cfg.w_r_min, cfg.w_r_max, cfg.w_r_count); + + std::vector cells; + cells.reserve(w_c_values.size() * w_r_values.size()); + + const std::size_t total = w_c_values.size() * w_r_values.size(); + std::size_t done = 0; + for (const double w_c : w_c_values) { + for (const double w_r : w_r_values) { + ++done; + std::cerr << "[" << done << "/" << total << "] W_c=" << w_c << " W_r=" << w_r << " ... " << std::flush; + cell_result r = simulate_cell(workload, popts, topts, cfg.sample_rate_hz, w_c, w_r, cfg.speed_factor, cfg.batch_size); + cells.push_back(r); + std::cerr << "rebases=" << r.rebases; + if (r.extend_threw) { + std::cerr << " EXTEND-THREW@" << *r.starved_at_waypoint << ": " << r.extend_throw_message; + } else if (r.starved_at_waypoint) { + std::cerr << " STARVED@" << *r.starved_at_waypoint; + } + std::cerr << "\n"; + } + } + + write_csv(cfg, n, cells); + std::cerr << "wrote " << cfg.output_path.string() << "\n"; + return 0; +} catch (const std::exception& e) { + std::cerr << "error: " << e.what() << "\n"; + return 1; +} diff --git a/src/viam/trajex/totg/test/integration.cpp b/src/viam/trajex/totg/test/integration.cpp index 4aad5a0..9e4bc49 100644 --- a/src/viam/trajex/totg/test/integration.cpp +++ b/src/viam/trajex/totg/test/integration.cpp @@ -2582,6 +2582,53 @@ BOOST_AUTO_TEST_CASE(lab_sander_05072026_backward_integration_exceeded_limit_cur BOOST_AUTO_TEST_SUITE_END() // replay_regression_tests +BOOST_AUTO_TEST_SUITE(online_extension_verification) + +// Confirms that integration points generated from a prefix of a waypoint set form a +// bit-exact prefix of the integration points generated from the full set, except for a +// short divergent tail around the prefix's terminal where the path coalesces differently. +BOOST_AUTO_TEST_CASE(lab_sander_prefix_integration_points_bit_exact) { + const auto fixture_path = + std::filesystem::path(VIAM_TRAJEX_TEST_DATA_DIR) / "lab_sander_backward_integration_exceeded-20260507.trajex-totg-replay.json"; + + // The lab_sander fixture has 3920 waypoints. Using the first half gives a non-trivial + // 1960-waypoint prefix while leaving plenty of path beyond it in the full trajectory. + constexpr std::size_t k_fixture_waypoint_count = 3920; + constexpr std::size_t k_prefix_waypoint_count = k_fixture_waypoint_count / 2; + + auto run = [&fixture_path](std::optional prefix) { + auto planner = viam::trajex::totg::replay_planner::create(fixture_path, prefix); + auto outcome = planner.execute([](const auto&, auto tx, const auto&) { return tx; }); + BOOST_REQUIRE_MESSAGE(outcome.receiver.has_value(), "trajectory generation failed"); + BOOST_REQUIRE(outcome.receiver->traj.has_value()); + return std::make_pair(std::move(*outcome.receiver->traj), planner.processed_waypoint_count()); + }; + + auto [traj_full, full_waypoint_count] = run(std::nullopt); + // Tripwire: if the fixture's waypoint count ever changes, force the test author back here. + BOOST_REQUIRE_EQUAL(full_waypoint_count, k_fixture_waypoint_count); + + auto [traj_prefix, prefix_waypoint_count] = run(k_prefix_waypoint_count); + BOOST_REQUIRE_EQUAL(prefix_waypoint_count, k_prefix_waypoint_count); + + const auto& points_full = traj_full.get_integration_points(); + const auto& points_prefix = traj_prefix.get_integration_points(); + + const auto [it_prefix, it_full] = std::ranges::mismatch(points_prefix, points_full); + const auto match_count = static_cast(it_prefix - points_prefix.begin()); + const double match_fraction = static_cast(match_count) / static_cast(points_prefix.size()); + + BOOST_TEST_MESSAGE("integration points (full): " << points_full.size()); + BOOST_TEST_MESSAGE("integration points (prefix): " << points_prefix.size()); + BOOST_TEST_MESSAGE("bit-exact common prefix length: " << match_count); + BOOST_TEST_MESSAGE("bit-exact common prefix fraction: " << match_fraction); + + // The match fraction on this fixture is empirically ~96.5%. + BOOST_CHECK_CLOSE(match_fraction, 0.96, 1.0); +} + +BOOST_AUTO_TEST_SUITE_END() // online_extension_verification + BOOST_AUTO_TEST_SUITE(random_trajectory_tests) // Ensure that if we run the random trajectory twice on the same seed, diff --git a/src/viam/trajex/totg/test/uniform_sampler.cpp b/src/viam/trajex/totg/test/uniform_sampler.cpp index 4d1a120..7dda011 100644 --- a/src/viam/trajex/totg/test/uniform_sampler.cpp +++ b/src/viam/trajex/totg/test/uniform_sampler.cpp @@ -161,4 +161,84 @@ BOOST_AUTO_TEST_CASE(no_duplicate_timestamps_at_end) { } } +namespace { + +viam::trajex::totg::trajectory build_unit_duration_trajectory() { + using namespace viam::trajex::totg; + using viam::trajex::arc_acceleration; + using viam::trajex::arc_length; + using viam::trajex::arc_velocity; + + const xt::xarray waypoints = {{0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}}; + path p = path::create(waypoints); + + std::vector points = { + {.time = trajectory::seconds{0.0}, .s = arc_length{0.0}, .s_dot = arc_velocity{0.0}, .s_ddot = arc_acceleration{0.5}}, + {.time = trajectory::seconds{1.0}, .s = p.length(), .s_dot = arc_velocity{0.0}, .s_ddot = arc_acceleration{0.0}}}; + + const trajectory::options opts{.max_velocity = xt::ones({3}), .max_acceleration = xt::ones({3})}; + + return trajectory::create(std::move(p), opts, std::move(points)); +} + +} // namespace + +BOOST_AUTO_TEST_CASE(quantized_for_trajectory_with_start_emits_first_sample_at_start) { + using namespace viam::trajex::totg; + using namespace viam::trajex::types; + + const trajectory traj = build_unit_duration_trajectory(); + const auto start = trajectory::seconds{0.1}; + + auto sampler = uniform_sampler::quantized_for_trajectory(traj, hertz{10.0}, start); + auto cursor = traj.create_cursor(); + + const auto first = sampler.next(cursor); + BOOST_REQUIRE(first.has_value()); + BOOST_CHECK_EQUAL(first->time.count(), start.count()); +} + +BOOST_AUTO_TEST_CASE(quantized_for_trajectory_with_start_emits_last_sample_at_duration) { + using namespace viam::trajex::totg; + using namespace viam::trajex::types; + + const trajectory traj = build_unit_duration_trajectory(); + const auto start = trajectory::seconds{0.1}; + + auto sampler = uniform_sampler::quantized_for_trajectory(traj, hertz{10.0}, start); + auto cursor = traj.create_cursor(); + + std::optional last; + while (auto s = sampler.next(cursor)) { + last = s; + } + BOOST_REQUIRE(last.has_value()); + BOOST_CHECK_EQUAL(last->time.count(), traj.duration().count()); +} + +BOOST_AUTO_TEST_CASE(quantized_for_trajectory_throws_on_negative_start) { + using namespace viam::trajex::totg; + using namespace viam::trajex::types; + + const trajectory traj = build_unit_duration_trajectory(); + BOOST_CHECK_THROW(uniform_sampler::quantized_for_trajectory(traj, hertz{10.0}, trajectory::seconds{-0.1}), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(quantized_for_trajectory_throws_on_start_at_duration) { + using namespace viam::trajex::totg; + using namespace viam::trajex::types; + + const trajectory traj = build_unit_duration_trajectory(); + BOOST_CHECK_THROW(uniform_sampler::quantized_for_trajectory(traj, hertz{10.0}, traj.duration()), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(quantized_for_trajectory_throws_on_start_beyond_duration) { + using namespace viam::trajex::totg; + using namespace viam::trajex::types; + + const trajectory traj = build_unit_duration_trajectory(); + const auto past = traj.duration() + trajectory::seconds{0.5}; + BOOST_CHECK_THROW(uniform_sampler::quantized_for_trajectory(traj, hertz{10.0}, past), std::invalid_argument); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/src/viam/trajex/totg/tools/planner.cpp b/src/viam/trajex/totg/tools/planner.cpp index 5f68f33..b215d36 100644 --- a/src/viam/trajex/totg/tools/planner.cpp +++ b/src/viam/trajex/totg/tools/planner.cpp @@ -50,12 +50,19 @@ std::string planner_base::serialize_for_replay(const waypoint_accumulator& waypo std::ostringstream ts; ts << std::put_time(&buf, "%FT%T") << "." << std::setw(6) << std::setfill('0') << delta_us.count() << "Z"; - // Reconstruct path::options the same way run_totg_ does, so we can read off the - // min/max blend curvature defaults that are currently not configurable via get_config(). + // Reconstruct path::options the same way run_totg_ does so the curvature bounds + // recorded into the replay match what the totg run actually used: configured values + // when get_config() carries them, defaults otherwise. auto path_opts = path::options{}.set_max_blend_deviation(get_config().path_blend_tolerance); if (get_config().colinearization_ratio) { path_opts.set_max_linear_deviation(get_config().path_blend_tolerance * *get_config().colinearization_ratio); } + if (get_config().min_blend_curvature) { + path_opts.set_min_blend_curvature(*get_config().min_blend_curvature); + } + if (get_config().max_blend_curvature) { + path_opts.set_max_blend_curvature(*get_config().max_blend_curvature); + } Json::Value root; root["schema_version"] = 1; diff --git a/src/viam/trajex/totg/tools/planner.hpp b/src/viam/trajex/totg/tools/planner.hpp index 6321419..efe7f78 100644 --- a/src/viam/trajex/totg/tools/planner.hpp +++ b/src/viam/trajex/totg/tools/planner.hpp @@ -41,6 +41,10 @@ class planner_base { xt::xarray acceleration_limits; double path_blend_tolerance = 0.0; std::optional colinearization_ratio; + // Curvature bounds for blend construction. nullopt leaves path::options at its + // built-in defaults (k_default_min_blend_curvature / k_default_max_blend_curvature). + std::optional min_blend_curvature; + std::optional max_blend_curvature; bool segment_totg = true; trajectory::integration_observer* observer = nullptr; // totg only; ignored by legacy }; @@ -336,6 +340,12 @@ class planner : public planner_base { if (get_config().colinearization_ratio) { path_opts.set_max_linear_deviation(get_config().path_blend_tolerance * *get_config().colinearization_ratio); } + if (get_config().min_blend_curvature) { + path_opts.set_min_blend_curvature(*get_config().min_blend_curvature); + } + if (get_config().max_blend_curvature) { + path_opts.set_max_blend_curvature(*get_config().max_blend_curvature); + } auto p = path::create(segment, path_opts); diff --git a/src/viam/trajex/totg/tools/replay.cpp b/src/viam/trajex/totg/tools/replay.cpp index 0ff25b2..692869e 100644 --- a/src/viam/trajex/totg/tools/replay.cpp +++ b/src/viam/trajex/totg/tools/replay.cpp @@ -21,10 +21,6 @@ namespace viam::trajex::totg { -namespace { - -// Parse a JSON replay record stream into a planner_base::config and a waypoints -// xarray. Throws std::runtime_error on malformed input. std::pair> parse_replay_record(std::istream& in) { Json::Value root; const Json::CharReaderBuilder reader; @@ -81,24 +77,53 @@ std::pair> parse_replay_record(std::ist planner_base::config cfg; cfg.velocity_limits = std::move(velocity_limits); cfg.acceleration_limits = std::move(acceleration_limits); - cfg.path_blend_tolerance = require("path_tolerance_delta_rads").asDouble(); + if (root.isMember("path_tolerance_delta_rads")) { + cfg.path_blend_tolerance = root["path_tolerance_delta_rads"].asDouble(); + } if (root.isMember("path_colinearization_ratio")) { cfg.colinearization_ratio = root["path_colinearization_ratio"].asDouble(); } + if (root.isMember("min_blend_curvature")) { + cfg.min_blend_curvature = root["min_blend_curvature"].asDouble(); + } + if (root.isMember("max_blend_curvature")) { + cfg.max_blend_curvature = root["max_blend_curvature"].asDouble(); + } return {std::move(cfg), std::move(waypoints)}; } -} // namespace +std::pair> parse_replay_record(const std::filesystem::path& path) { + std::ifstream in(path); + if (!in) { + throw std::runtime_error("failed to open replay record file: " + path.string()); + } + return parse_replay_record(in); +} replay_planner::replay_planner(config cfg, std::unique_ptr collector) : planner(std::move(cfg)), collector_(std::move(collector)) { mutable_config().observer = collector_.get(); } -replay_planner replay_planner::create(std::istream& in) { +replay_planner replay_planner::create(std::istream& in, std::optional prefix_waypoint_count) { auto [cfg, waypoints] = parse_replay_record(in); + // Validate prefix request up-front so callers see a clear error before the planner is constructed. + // When a prefix shorter than the full set is requested, materialize the leading rows into a fresh + // xarray and drop the original. The remainder of this function then runs unchanged against `waypoints`. + const auto total_waypoints = waypoints.shape(0); + if (prefix_waypoint_count.has_value()) { + if (*prefix_waypoint_count == 0 || *prefix_waypoint_count > total_waypoints) { + throw std::out_of_range("replay_planner prefix_waypoint_count " + std::to_string(*prefix_waypoint_count) + + " is out of range for a record with " + std::to_string(total_waypoints) + " waypoints"); + } + if (*prefix_waypoint_count < total_waypoints) { + xt::xarray prefix = xt::view(waypoints, xt::range(std::size_t{0}, *prefix_waypoint_count), xt::all()); + waypoints = std::move(prefix); + } + } + auto collector = std::make_unique(); replay_planner p(std::move(cfg), std::move(collector)); @@ -112,12 +137,12 @@ replay_planner replay_planner::create(std::istream& in) { return p; } -replay_planner replay_planner::create(const std::filesystem::path& path) { +replay_planner replay_planner::create(const std::filesystem::path& path, std::optional prefix_waypoint_count) { std::ifstream in(path); if (!in) { throw std::runtime_error("failed to open replay record file: " + path.string()); } - return create(in); + return create(in, prefix_waypoint_count); } const trajectory_integration_event_collector& replay_planner::collector() const noexcept { diff --git a/src/viam/trajex/totg/tools/replay.hpp b/src/viam/trajex/totg/tools/replay.hpp index b407d8b..1e844ff 100644 --- a/src/viam/trajex/totg/tools/replay.hpp +++ b/src/viam/trajex/totg/tools/replay.hpp @@ -4,6 +4,13 @@ #include #include #include +#include + +#if __has_include() +#include +#else +#include +#endif #include #include @@ -11,6 +18,26 @@ namespace viam::trajex::totg { +/// +/// Parses a canonical replay JSON record from `in` into a planner config and +/// a (num_waypoints, dof) xarray of waypoints. Exposed for use by tools and +/// tests that want the raw record without instantiating a planner. +/// +/// @param in Stream containing a canonical JSON replay record +/// @return Pair of (config, waypoints) +/// @throws std::runtime_error if the stream cannot be parsed or required fields are missing +/// +std::pair> parse_replay_record(std::istream& in); + +/// +/// File-path overload of parse_replay_record. +/// +/// @param path Path to a canonical JSON replay record file +/// @return Pair of (config, waypoints) +/// @throws std::runtime_error if the file cannot be opened or parsed +/// +std::pair> parse_replay_record(const std::filesystem::path& path); + /// /// Receiver for replay_planner. Holds the most recently generated /// trajectory so callers can access it after execute() completes. @@ -48,18 +75,27 @@ class replay_planner : public planner { /// /// Constructs a replay planner from a replay record stream. /// + /// When `prefix_waypoint_count` is supplied, the planner runs over only the + /// first N waypoints of the record instead of the full set. This is intended + /// for tests that compare a trajectory generated from a full waypoint set + /// against one generated from a prefix of the same set. + /// /// @param in Stream containing a canonical JSON replay record + /// @param prefix_waypoint_count Optional cap on the number of leading waypoints to use /// @throws std::runtime_error if the stream cannot be parsed or required fields are missing + /// @throws std::out_of_range if `prefix_waypoint_count` is zero or exceeds the record's waypoint count /// - static replay_planner create(std::istream& in); + static replay_planner create(std::istream& in, std::optional prefix_waypoint_count = std::nullopt); /// /// Constructs a replay planner from a replay record file path. /// /// @param path Path to a canonical JSON replay record file + /// @param prefix_waypoint_count Optional cap on the number of leading waypoints to use; see stream overload /// @throws std::runtime_error if the file cannot be opened or parsed + /// @throws std::out_of_range if `prefix_waypoint_count` is zero or exceeds the record's waypoint count /// - static replay_planner create(const std::filesystem::path& path); + static replay_planner create(const std::filesystem::path& path, std::optional prefix_waypoint_count = std::nullopt); /// /// Returns the event collector populated during execute(). diff --git a/src/viam/trajex/totg/tools/replay_main.cpp b/src/viam/trajex/totg/tools/replay_main.cpp index c3ad49b..5256e25 100644 --- a/src/viam/trajex/totg/tools/replay_main.cpp +++ b/src/viam/trajex/totg/tools/replay_main.cpp @@ -20,6 +20,16 @@ int main(int argc, char* argv[]) try { auto outcome = p.execute([](const auto&, auto tx, const auto&) { return tx; }); + // If trajex failed before any observer hook fired (e.g. path::create threw), the + // collector captured nothing and there is no failure JSON to emit. Rethrow so the + // outer catch reports the error and we exit non-zero. The trajex-internal failure + // path (on_failed observer fires, collector retains invalid_trajectory / events) is + // preserved: in that case we fall through to write_trajectory_json, which uses the + // collector contents to emit a complete failure record with exit 0. + if (outcome.error && !p.collector().invalid_trajectory() && !p.collector().invalid_exception()) { + std::rethrow_exception(outcome.error); + } + const viam::trajex::totg::trajectory* traj = nullptr; if (outcome.receiver && outcome.receiver->traj) { traj = &*outcome.receiver->traj; diff --git a/src/viam/trajex/totg/tools/test/planner.cpp b/src/viam/trajex/totg/tools/test/planner.cpp index 07592c1..44bb485 100644 --- a/src/viam/trajex/totg/tools/test/planner.cpp +++ b/src/viam/trajex/totg/tools/test/planner.cpp @@ -24,6 +24,8 @@ planner::config simple_config() { .acceleration_limits = xt::xarray{1.0, 1.0, 1.0}, .path_blend_tolerance = 0.001, .colinearization_ratio = std::nullopt, + .min_blend_curvature = std::nullopt, + .max_blend_curvature = std::nullopt, }; } diff --git a/src/viam/trajex/totg/uniform_sampler.cpp b/src/viam/trajex/totg/uniform_sampler.cpp index b4675c9..e0540ef 100644 --- a/src/viam/trajex/totg/uniform_sampler.cpp +++ b/src/viam/trajex/totg/uniform_sampler.cpp @@ -7,7 +7,7 @@ namespace viam::trajex::totg { -uniform_sampler::uniform_sampler(std::size_t num_samples) : num_samples_{num_samples} {} +uniform_sampler::uniform_sampler(std::size_t num_samples, trajectory::seconds start) : num_samples_{num_samples}, start_{start} {} std::size_t uniform_sampler::calculate_quantized_samples(double duration_sec, double frequency_hz) { if (duration_sec <= 0.0 || frequency_hz <= 0.0) { @@ -36,8 +36,16 @@ uniform_sampler uniform_sampler::quantized_for_duration(trajectory::seconds dura return uniform_sampler{calculate_quantized_samples(duration.count(), frequency.value)}; } -uniform_sampler uniform_sampler::quantized_for_trajectory(const trajectory& traj, types::hertz frequency) { - return quantized_for_duration(traj.duration(), frequency); +uniform_sampler uniform_sampler::quantized_for_trajectory(const trajectory& traj, types::hertz frequency, trajectory::seconds start) { + const auto duration = traj.duration(); + if (start < trajectory::seconds{0.0}) { + throw std::invalid_argument{"start must be non-negative"}; + } + if (start >= duration) { + throw std::invalid_argument{"start must be strictly less than the trajectory's duration"}; + } + const auto span = duration - start; + return uniform_sampler{calculate_quantized_samples(span.count(), frequency.value), start}; } std::optional uniform_sampler::next(trajectory::cursor& cursor) { @@ -45,11 +53,12 @@ std::optional uniform_sampler::next(trajectory::curso return std::nullopt; } - // Compute target time via `linspace` with a special case for the exact endpoint. + // Compute target time via `linspace` over [start_, duration] with a special case for + // the exact endpoint. const auto when = (next_sample_ == num_samples_ - 1) ? 1.0 : static_cast(next_sample_) / static_cast(num_samples_ - 1); // Seek cursor to computed time and sample - cursor.seek(trajectory::seconds{std::lerp(0.0, cursor.trajectory().duration().count(), when)}); + cursor.seek(trajectory::seconds{std::lerp(start_.count(), cursor.trajectory().duration().count(), when)}); auto sample = cursor.sample(); ++next_sample_; diff --git a/src/viam/trajex/totg/uniform_sampler.hpp b/src/viam/trajex/totg/uniform_sampler.hpp index f79bcba..ef832f4 100644 --- a/src/viam/trajex/totg/uniform_sampler.hpp +++ b/src/viam/trajex/totg/uniform_sampler.hpp @@ -10,17 +10,20 @@ namespace viam::trajex::totg { /// /// Uniform time-step sampler. /// -/// Samples trajectory at regular time intervals. -/// Returns samples at t=0, dt, 2*dt, ... until trajectory duration is exceeded. +/// Samples a trajectory at regular time intervals over the closed range `[start, duration]`, +/// where `start` defaults to zero and `duration` is the trajectory's duration. Non-zero +/// `start` values support chaining samplers across trajectory boundaries without +/// duplicating the seam sample. /// class uniform_sampler { public: /// - /// Constructs uniform sampler with sample count. + /// Constructs uniform sampler with sample count and a starting time. /// /// @param num_samples Total number of samples to generate + /// @param start Time at which the first sample lands. Defaults to zero. /// - explicit uniform_sampler(std::size_t num_samples); + explicit uniform_sampler(std::size_t num_samples, trajectory::seconds start = trajectory::seconds{0.0}); /// /// Creates uniform sampler with adjusted timestep to hit duration endpoint exactly. @@ -36,15 +39,27 @@ class uniform_sampler { static uniform_sampler quantized_for_duration(trajectory::seconds duration, types::hertz frequency); /// - /// Creates uniform sampler with adjusted timestep to hit trajectory endpoint exactly. + /// Creates uniform sampler whose first sample lands at `start` and whose last sample + /// lands exactly at the trajectory's duration. /// - /// Convenience wrapper that extracts duration from trajectory. + /// Quantization adjusts the internal sample spacing so an integer number of samples + /// covers `[start, traj.duration()]` with both endpoints hit. The effective dt is + /// approximately `1 / frequency` but is adjusted per-call to fit the requested span. /// - /// @param traj Trajectory to sample (used for duration) - /// @param frequency Desired sampling frequency - /// @return uniform_sampler with dt adjusted to align with trajectory duration + /// With the default `start = 0`, this is the standard "sample the whole trajectory" + /// behavior. Non-zero `start` lets a caller chain samplers across trajectory + /// transitions by skipping past the seam (which the previous sampler already emitted + /// as its terminal sample). + /// + /// @param traj Trajectory to sample + /// @param frequency Target sample rate, subject to per-trajectory quantization + /// @param start Time at which the first sample lands. Defaults to zero. + /// @return uniform_sampler covering [start, traj.duration()] + /// @throws std::invalid_argument if `start` is negative or is not strictly less than the trajectory's duration /// - static uniform_sampler quantized_for_trajectory(const trajectory& traj, types::hertz frequency); + static uniform_sampler quantized_for_trajectory(const trajectory& traj, + types::hertz frequency, + trajectory::seconds start = trajectory::seconds{0.0}); /// /// Calculates adjusted timestep for quantized sampling. @@ -81,6 +96,7 @@ class uniform_sampler { private: std::size_t num_samples_; std::size_t next_sample_ = 0; + trajectory::seconds start_; }; // Verify that uniform_sampler satisfies the sampler concept