Add delay buffer utilities to engine_util_misc.
PiperOrigin-RevId: 866450284 Change-Id: I86d6dd6a6f7519640f75ca405fb60a12e8619c86
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
60b5b1d59d
commit
84fa527723
@@ -910,6 +910,215 @@ size_t mju_decodeBase64(uint8_t* buf, const char* s) {
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ delay buffers -----------------------------------------------------
|
||||
|
||||
// convert logical index (0=oldest, n-1=newest) to physical index
|
||||
// cursor points to the newest element (logical index n-1)
|
||||
static inline int delayPhysicalIndex(int cursor, int n, int logical) {
|
||||
return (cursor + 1 + logical) % n;
|
||||
}
|
||||
|
||||
|
||||
// find logical index i such that times[i-1] < t <= times[i], using circular binary search
|
||||
// returns 0 if t <= times[oldest], n if t > times[newest]
|
||||
// cursor points to the newest element (logical index n-1)
|
||||
static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
||||
// get oldest and newest timestamps
|
||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
||||
int newest_phys = delayPhysicalIndex(cursor, n, n-1);
|
||||
mjtNum t_oldest = times[oldest_phys];
|
||||
mjtNum t_newest = times[newest_phys];
|
||||
|
||||
// before or at first element
|
||||
if (t <= t_oldest) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// after last element
|
||||
if (t > t_newest) {
|
||||
return n;
|
||||
}
|
||||
|
||||
// circular binary search: find smallest logical i such that times[phys(i)] >= t
|
||||
int lo = 0;
|
||||
int hi = n - 1;
|
||||
while (hi - lo > 1) {
|
||||
int mid = (lo + hi) / 2;
|
||||
int mid_phys = delayPhysicalIndex(cursor, n, mid);
|
||||
if (times[mid_phys] < t) {
|
||||
lo = mid;
|
||||
} else {
|
||||
hi = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return hi;
|
||||
}
|
||||
|
||||
|
||||
// initialize delay buffer with given times and values; times must be strictly increasing
|
||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||
void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNum* values,
|
||||
mjtNum user) {
|
||||
// check strict monotonicity of times
|
||||
for (int i = 0; i < n-1; i++) {
|
||||
if (times[i+1] - times[i] < mjMINVAL) {
|
||||
mjERROR("times must be strictly increasing, got times[%d]=%g >= times[%d]=%g",
|
||||
i, times[i], i+1, times[i+1]);
|
||||
}
|
||||
}
|
||||
|
||||
// buf layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||
buf[0] = user; // user value
|
||||
buf[1] = (mjtNum)(n-1); // cursor points to newest (logical index n-1 = physical index n-1)
|
||||
|
||||
mjtNum* buf_times = buf + 2;
|
||||
mjtNum* buf_values = buf + 2 + n;
|
||||
|
||||
if (times != buf_times) mju_copy(buf_times, times, n);
|
||||
if (values) mju_copy(buf_values, values, n*dim);
|
||||
}
|
||||
|
||||
|
||||
// find insertion slot for time t, maintaining sorted order
|
||||
// if t matches an existing timestamp, returns pointer to that slot
|
||||
// if a new sample is inserted, the oldest sample is dropped
|
||||
// returns pointer to value slot where caller should write dim values
|
||||
mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
||||
int cursor = (int)buf[1];
|
||||
mjtNum* times = buf + 2;
|
||||
mjtNum* values = buf + 2 + n;
|
||||
|
||||
// find logical insertion index: times[i-1] < t <= times[i]
|
||||
int i = delayFindIndex(times, n, cursor, t);
|
||||
|
||||
// exact match at logical i: return pointer to existing slot
|
||||
if (i < n) {
|
||||
int phys_i = delayPhysicalIndex(cursor, n, i);
|
||||
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
||||
return values + phys_i*dim;
|
||||
}
|
||||
}
|
||||
|
||||
// logical i == 0: new sample is older than oldest, replace oldest slot
|
||||
if (i == 0) {
|
||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
||||
times[oldest_phys] = t;
|
||||
return values + oldest_phys*dim;
|
||||
}
|
||||
|
||||
// logical i == n: new sample is newer than newest, advance cursor and write
|
||||
if (i == n) {
|
||||
cursor = (cursor + 1) % n;
|
||||
buf[1] = (mjtNum)cursor;
|
||||
|
||||
// cursor now points to the new newest slot (which was the old oldest)
|
||||
times[cursor] = t;
|
||||
return values + cursor*dim;
|
||||
}
|
||||
|
||||
// 0 < i < n: out-of-order insertion, shift [1, i-1] left (dropping 0), insert at i-1
|
||||
for (int j = 0; j < i-1; j++) {
|
||||
int src_phys = delayPhysicalIndex(cursor, n, j+1);
|
||||
int dst_phys = delayPhysicalIndex(cursor, n, j);
|
||||
times[dst_phys] = times[src_phys];
|
||||
mju_copy(values + dst_phys*dim, values + src_phys*dim, dim);
|
||||
}
|
||||
int insert_phys = delayPhysicalIndex(cursor, n, i-1);
|
||||
times[insert_phys] = t;
|
||||
return values + insert_phys*dim;
|
||||
}
|
||||
|
||||
|
||||
// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline
|
||||
// returns pointer to sample in buffer on exact match or ZOH (res untouched)
|
||||
// returns NULL and writes interpolated result to res on interpolation
|
||||
const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtNum t, int interp) {
|
||||
int cursor = (int)buf[1];
|
||||
const mjtNum* times = buf + 2;
|
||||
const mjtNum* values = buf + 2 + n;
|
||||
|
||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
||||
int newest_phys = delayPhysicalIndex(cursor, n, n-1);
|
||||
mjtNum t_oldest = times[oldest_phys];
|
||||
mjtNum t_newest = times[newest_phys];
|
||||
|
||||
// extrapolate before oldest: return pointer to oldest value
|
||||
if (t <= t_oldest + mjMINVAL) {
|
||||
return values + oldest_phys*dim;
|
||||
}
|
||||
|
||||
// extrapolate after newest: return pointer to newest value
|
||||
if (t >= t_newest - mjMINVAL) {
|
||||
return values + newest_phys*dim;
|
||||
}
|
||||
|
||||
// find bracketing logical index: times[i-1] < t <= times[i]
|
||||
int i = delayFindIndex(times, n, cursor, t);
|
||||
int phys_i = delayPhysicalIndex(cursor, n, i);
|
||||
|
||||
// check for exact match at i
|
||||
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
||||
return values + phys_i*dim;
|
||||
}
|
||||
|
||||
// lo = i-1, hi = i (we know i > 0 because t > t_oldest)
|
||||
int phys_lo = delayPhysicalIndex(cursor, n, i-1);
|
||||
int phys_hi = phys_i;
|
||||
|
||||
// zero-order hold: return pointer to lo (most recent sample <= t)
|
||||
if (interp == 0) {
|
||||
return values + phys_lo*dim;
|
||||
}
|
||||
|
||||
mjtNum dt = times[phys_hi] - times[phys_lo];
|
||||
mjtNum alpha = (t - times[phys_lo]) / dt;
|
||||
|
||||
// piecewise linear interpolation
|
||||
if (interp == 1) {
|
||||
for (int d = 0; d < dim; d++) {
|
||||
res[d] = values[phys_lo*dim+d] + alpha * (values[phys_hi*dim+d] - values[phys_lo*dim+d]);
|
||||
}
|
||||
}
|
||||
|
||||
// cubic spline interpolation
|
||||
else {
|
||||
// Hermite basis functions
|
||||
mjtNum alpha2 = alpha * alpha;
|
||||
mjtNum alpha3 = alpha2 * alpha;
|
||||
mjtNum h00 = 2*alpha3 - 3*alpha2 + 1;
|
||||
mjtNum h10 = alpha3 - 2*alpha2 + alpha;
|
||||
mjtNum h01 = -2*alpha3 + 3*alpha2;
|
||||
mjtNum h11 = alpha3 - alpha2;
|
||||
|
||||
for (int d = 0; d < dim; d++) {
|
||||
// finite differenced catmull-rom slopes, 0 at endpoints (constant extrapolation)
|
||||
|
||||
mjtNum m_lo = 0;
|
||||
if (i > 1) {
|
||||
int phys_lo_prev = delayPhysicalIndex(cursor, n, i-2);
|
||||
mjtNum dt_lo = times[phys_hi] - times[phys_lo_prev];
|
||||
m_lo = (values[phys_hi*dim+d] - values[phys_lo_prev*dim+d]) / dt_lo;
|
||||
}
|
||||
|
||||
mjtNum m_hi = 0;
|
||||
if (i < n - 1) {
|
||||
int phys_hi_next = delayPhysicalIndex(cursor, n, i+1);
|
||||
mjtNum dt_hi = times[phys_hi_next] - times[phys_lo];
|
||||
m_hi = (values[phys_hi_next*dim+d] - values[phys_lo*dim+d]) / dt_hi;
|
||||
}
|
||||
|
||||
res[d] = h00 * values[phys_lo*dim+d] +
|
||||
h10 * dt * m_lo +
|
||||
h01 * values[phys_hi*dim+d] +
|
||||
h11 * dt * m_hi;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ miscellaneous -----------------------------------------------------
|
||||
|
||||
// convert contact force to pyramid representation
|
||||
|
||||
@@ -75,7 +75,7 @@ MJAPI mjtNum mju_evalBasis(const mjtNum x[3], int i, int order);
|
||||
// interpolate a function at x with given interpolation coefficients and order n
|
||||
MJAPI void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order);
|
||||
|
||||
// ----------------------------- Base64 -----------------------------------------------------------
|
||||
// ----------------------------- Base64 ------------------------------------------------------------
|
||||
|
||||
// encode data as Base64 into buf (including padding and null char)
|
||||
// returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1
|
||||
@@ -89,7 +89,31 @@ MJAPI size_t mju_isValidBase64(const char* s);
|
||||
// returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4))
|
||||
MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s);
|
||||
|
||||
//------------------------------ miscellaneous ----------------------------------------------------
|
||||
//------------------------------ delay buffers -----------------------------------------------------
|
||||
|
||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||
// - user: 1 mjtNum reserved for user data (ignored by these functions)
|
||||
// - cursor: 1 mjtNum for circular buffer index (integer stored as mjtNum)
|
||||
// - times: n timestamps, contiguous at buf[2..n+1]
|
||||
// - values: n*dim values, contiguous at buf[n+2..n+2+n*dim-1]
|
||||
// total buffer size: 2 + n*(1 + dim)
|
||||
|
||||
// initialize delay buffer with given times and values; times must be strictly increasing
|
||||
// values is size n x dim
|
||||
MJAPI void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times,
|
||||
const mjtNum* values, mjtNum user);
|
||||
|
||||
// find insertion slot for sample at time t, maintaining sorted order
|
||||
// returns pointer to value slot (size dim) where caller should write
|
||||
MJAPI mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t);
|
||||
|
||||
// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline
|
||||
// returns pointer to sample in buffer on exact match (res untouched)
|
||||
// returns NULL and writes interpolated result to res otherwise
|
||||
MJAPI const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim,
|
||||
mjtNum* res, mjtNum t, int interp);
|
||||
|
||||
//------------------------------ miscellaneous -----------------------------------------------------
|
||||
|
||||
// convert contact force to pyramid representation
|
||||
MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force,
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
// Tests for engine/engine_util_solve.c.
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
@@ -22,6 +23,7 @@
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <gtest/gtest-spi.h>
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_util_misc.h"
|
||||
@@ -722,5 +724,328 @@ TEST_F(Base64Test, decodeAndEncode) {
|
||||
EXPECT_THAT(buffer2.data(), StrEq(s));
|
||||
}
|
||||
|
||||
// --------------------------------- Delay Buffers ----------------------------
|
||||
|
||||
using DelayTest = MujocoTest;
|
||||
|
||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||
// cursor points to newest element (logical index n-1)
|
||||
// after init, cursor=n-1, so physical indices equal logical indices
|
||||
|
||||
TEST_F(DelayTest, Init) {
|
||||
constexpr int n = 4;
|
||||
constexpr int dim = 1;
|
||||
mjtNum buf[2 + n + n*dim];
|
||||
|
||||
std::vector<mjtNum> times = {4, 6, 8, 10};
|
||||
std::vector<mjtNum> values = {99, 99, 99, 99};
|
||||
mju_delayInit(buf, n, dim, times.data(), values.data(), 0.0);
|
||||
|
||||
// check header
|
||||
EXPECT_EQ(buf[0], 0.0); // user
|
||||
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
|
||||
|
||||
// timestamps: [4, 6, 8, 10] (t=10 is newest)
|
||||
// values: [99, 99, 99, 99]
|
||||
// verify via read function (logical order)
|
||||
mjtNum res;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, Init_Vector) {
|
||||
constexpr int n = 3;
|
||||
constexpr int dim = 2;
|
||||
mjtNum buf[2 + n + n*dim];
|
||||
|
||||
std::vector<mjtNum> times = {-2, -1, 0};
|
||||
std::vector<mjtNum> values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.0};
|
||||
mju_delayInit(buf, n, dim, times.data(), values.data(), 0.0);
|
||||
|
||||
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
|
||||
|
||||
// verify via read function
|
||||
mjtNum res[dim];
|
||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, -2.0, 0);
|
||||
ASSERT_NE(ptr, nullptr);
|
||||
EXPECT_EQ(ptr[0], 1.0);
|
||||
EXPECT_EQ(ptr[1], 2.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, Append) {
|
||||
constexpr int n = 4;
|
||||
constexpr int dim = 1;
|
||||
// Initialize buffer properly, then insert
|
||||
mjtNum buf[2 + 2*n];
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
// timestamps: [4, 6, 8, 10]
|
||||
mjtNum times[] = {4, 6, 8, 10};
|
||||
mju_copy(buf + 2, times, n);
|
||||
// values: [0, 0, 0, 0]
|
||||
mju_zero(buf + 2 + n, n);
|
||||
|
||||
// overwrite with specific values
|
||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
||||
|
||||
// now append at t=12
|
||||
*mju_delayInsert(buf, n, dim, 12.0) = 99.0;
|
||||
|
||||
// verify logical order: [6, 8, 10, 12] -> [2, 3, 4, 99]
|
||||
mjtNum res;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 12.0, 0), 99.0);
|
||||
|
||||
// oldest should now be t=6
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 2.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, Append_Multiple) {
|
||||
constexpr int n = 3;
|
||||
constexpr int dim = 1;
|
||||
mjtNum buf[2 + 2*n];
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {-2, -1, 0};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n);
|
||||
|
||||
for (int i = 1; i <= 4; i++) {
|
||||
mjtNum i_real = static_cast<mjtNum>(i);
|
||||
*mju_delayInsert(buf, n, dim, i_real) = i_real;
|
||||
}
|
||||
// Final: logical timestamps [2, 3, 4], values [2, 3, 4]
|
||||
mjtNum res;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 2.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 3.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 4.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, ReadVector_ExactMatch) {
|
||||
constexpr int n = 3;
|
||||
constexpr int dim = 2;
|
||||
mjtNum buf[2 + n + n*dim];
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {0, 1, 2};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n*dim);
|
||||
|
||||
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
|
||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||
|
||||
mjtNum res[dim];
|
||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 1.0, 0);
|
||||
ASSERT_NE(ptr, nullptr);
|
||||
EXPECT_EQ(ptr[0], 3.0);
|
||||
EXPECT_EQ(ptr[1], 4.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, ReadVector_ZOH) {
|
||||
constexpr int n = 3;
|
||||
constexpr int dim = 2;
|
||||
mjtNum buf[2 + n + n*dim];
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {0, 1, 2};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n*dim);
|
||||
|
||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||
|
||||
mjtNum res[dim];
|
||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 0);
|
||||
ASSERT_NE(ptr, nullptr);
|
||||
EXPECT_EQ(ptr[0], 1.0);
|
||||
EXPECT_EQ(ptr[1], 2.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, ReadVector_Linear) {
|
||||
constexpr int n = 3;
|
||||
constexpr int dim = 2;
|
||||
mjtNum buf[2 + n + n*dim];
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {0, 1, 2};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n*dim);
|
||||
|
||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||
|
||||
mjtNum res[dim];
|
||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 1);
|
||||
EXPECT_EQ(ptr, nullptr);
|
||||
EXPECT_THAT(res[0], DoubleNear(2.0, 1e-10)); // (1+3)/2
|
||||
EXPECT_THAT(res[1], DoubleNear(3.0, 1e-10)); // (2+4)/2
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, InsertOutOfOrder) {
|
||||
constexpr int n = 4;
|
||||
constexpr int dim = 1;
|
||||
mjtNum buf[2 + 2*n];
|
||||
mjtNum res;
|
||||
|
||||
auto reset = [&]() {
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {4, 6, 8, 10};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n);
|
||||
|
||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
||||
};
|
||||
|
||||
// insert in middle (between t=8 and t=10)
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 9.0) = 99.0;
|
||||
// logical: [6, 8, 9, 10] -> [2, 3, 99, 4]
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 9.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
|
||||
// insert near start (between t=4 and t=6)
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 5.0) = 99.0;
|
||||
// logical: [5, 6, 8, 10] -> [99, 2, 3, 4]
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 5.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
|
||||
// insert before oldest (t=3 < t=4): replaces oldest
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 3.0) = 99.0;
|
||||
// logical: [3, 6, 8, 10] -> [99, 2, 3, 4]
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 3.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, InsertReplaceOnCollision) {
|
||||
constexpr int n = 4;
|
||||
constexpr int dim = 1;
|
||||
mjtNum buf[2 + 2*n];
|
||||
mjtNum res;
|
||||
|
||||
auto reset = [&]() {
|
||||
// timestamps: [4, 6, 8, 10], values initialized to 0
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {4, 6, 8, 10};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n);
|
||||
|
||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
||||
};
|
||||
|
||||
// collision in middle (t=8)
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 8.0) = 99.0;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
|
||||
// collision at newest (t=10)
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 10.0) = 99.0;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
||||
|
||||
// collision at oldest (t=4)
|
||||
reset();
|
||||
*mju_delayInsert(buf, n, dim, 4.0) = 99.0;
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||
}
|
||||
|
||||
void TriggerDelayInitNonMonotonic() {
|
||||
mjtNum buf[10];
|
||||
mjtNum times[4] = {1, 2, 2, 4}; // not strictly increasing
|
||||
mjtNum values[4] = {0};
|
||||
mju_delayInit(buf, 4, 1, times, values, 0.0);
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, Init_NonMonotonic) {
|
||||
EXPECT_FATAL_FAILURE(TriggerDelayInitNonMonotonic(),
|
||||
"mju_delayInit: times must be strictly increasing");
|
||||
}
|
||||
|
||||
TEST_F(DelayTest, CubicInterpolation) {
|
||||
int n = 2;
|
||||
int dim = 2;
|
||||
mjtNum buf[100]; // 2 + 2 + 2*2 = 8
|
||||
buf[0] = 0.0;
|
||||
buf[1] = n - 1;
|
||||
mjtNum times[] = {-1, 0};
|
||||
mju_copy(buf + 2, times, n);
|
||||
mju_zero(buf + 2 + n, n*dim);
|
||||
|
||||
// Insert (0, 0, 1) and (1, 1, 0).
|
||||
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
|
||||
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3
|
||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
||||
slot0[0] = 0.0; slot0[1] = 1.0;
|
||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
||||
slot1[0] = 1.0; slot1[1] = 0.0;
|
||||
|
||||
mjtNum res[2];
|
||||
|
||||
// Test midpoint x=0.5
|
||||
// Dim 0: 0.5
|
||||
// Dim 1: 1 - 0.5 = 0.5
|
||||
mju_delayRead(buf, n, dim, res, 0.5, 2);
|
||||
EXPECT_NEAR(res[0], 0.5, 1e-9);
|
||||
EXPECT_NEAR(res[1], 0.5, 1e-9);
|
||||
|
||||
// Test x=0.25
|
||||
// Dim 0: 3*0.25^2 - 2*0.25^3
|
||||
// Dim 1: 1 - (3*0.25^2 - 2*0.25^3)
|
||||
mju_delayRead(buf, n, dim, res, 0.25, 2);
|
||||
mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25;
|
||||
EXPECT_NEAR(res[0], expected_0_25, 1e-9);
|
||||
EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9);
|
||||
|
||||
// Test x=0.8
|
||||
// Dim 0: 3*0.8^2 - 2*0.8^3
|
||||
// Dim 1: 1 - (3*0.8^2 - 2*0.8^3)
|
||||
mju_delayRead(buf, n, dim, res, 0.8, 2);
|
||||
mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8;
|
||||
EXPECT_NEAR(res[0], expected_0_8, 1e-9);
|
||||
EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
Reference in New Issue
Block a user