Rename "delay" buffer functions to "history" buffers.
The functions and associated tests related to managing time-stamped data buffers have been renamed from `mju_delay*` to `mju_history*` to better reflect their general purpose beyond just handling delays. PiperOrigin-RevId: 866978339 Change-Id: I8655e91bce783287ad2adc412d13a1c39aa2c322
This commit is contained in:
committed by
Copybara-Service
parent
e6639f1e29
commit
d680b3d234
@@ -869,7 +869,7 @@ static void mj_advance(const mjModel* m, mjData* d,
|
|||||||
|
|
||||||
// get history buffer pointer and insert ctrl at current time
|
// get history buffer pointer and insert ctrl at current time
|
||||||
mjtNum* buf = d->history + m->actuator_historyadr[i];
|
mjtNum* buf = d->history + m->actuator_historyadr[i];
|
||||||
*mju_delayInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i];
|
*mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
// advance sensor history buffers
|
// advance sensor history buffers
|
||||||
@@ -888,7 +888,7 @@ static void mj_advance(const mjModel* m, mjData* d,
|
|||||||
mjtNum time_prev = buf[0]; // first slot stores previous sensor tick
|
mjtNum time_prev = buf[0]; // first slot stores previous sensor tick
|
||||||
if (time_prev + interval <= d->time) {
|
if (time_prev + interval <= d->time) {
|
||||||
buf[0] += interval; // advance by exact interval (continuous time)
|
buf[0] += interval; // advance by exact interval (continuous time)
|
||||||
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
|
mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
|
||||||
if (delay > 0) {
|
if (delay > 0) {
|
||||||
// have delay, compute sensor
|
// have delay, compute sensor
|
||||||
mj_computeSensor(m, d, i, slot);
|
mj_computeSensor(m, d, i, slot);
|
||||||
@@ -899,11 +899,11 @@ static void mj_advance(const mjModel* m, mjData* d,
|
|||||||
}
|
}
|
||||||
} else if (delay > 0) {
|
} else if (delay > 0) {
|
||||||
// delay-only mode: always compute and insert
|
// delay-only mode: always compute and insert
|
||||||
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
|
mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
|
||||||
mj_computeSensor(m, d, i, slot);
|
mj_computeSensor(m, d, i, slot);
|
||||||
} else {
|
} else {
|
||||||
// history-only mode: copy from sensordata (already computed)
|
// history-only mode: copy from sensordata (already computed)
|
||||||
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
|
mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
|
||||||
mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
|
mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -831,7 +831,7 @@ mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int i
|
|||||||
mjtNum delay = m->actuator_delay[id];
|
mjtNum delay = m->actuator_delay[id];
|
||||||
const mjtNum* buf = d->history + m->actuator_historyadr[id];
|
const mjtNum* buf = d->history + m->actuator_historyadr[id];
|
||||||
mjtNum res;
|
mjtNum res;
|
||||||
const mjtNum* ptr = mju_delayRead(buf, nsample, /*dim=*/1, &res, time - delay, interp);
|
const mjtNum* ptr = mju_historyRead(buf, nsample, /*dim=*/1, &res, time - delay, interp);
|
||||||
return ptr ? *ptr : res;
|
return ptr ? *ptr : res;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -858,7 +858,7 @@ const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum ti
|
|||||||
int dim = m->sensor_dim[id];
|
int dim = m->sensor_dim[id];
|
||||||
mjtNum delay = m->sensor_delay[id];
|
mjtNum delay = m->sensor_delay[id];
|
||||||
const mjtNum* buf = d->history + m->sensor_historyadr[id];
|
const mjtNum* buf = d->history + m->sensor_historyadr[id];
|
||||||
return mju_delayRead(buf, nsample, dim, result, time - delay, interp);
|
return mju_historyRead(buf, nsample, dim, result, time - delay, interp);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -888,7 +888,7 @@ void mj_initCtrlHistory(const mjModel* m, mjData* d, int id,
|
|||||||
mjtNum user = buf[0];
|
mjtNum user = buf[0];
|
||||||
|
|
||||||
// initialize history buffer
|
// initialize history buffer
|
||||||
mju_delayInit(buf, nsample, 1, buf_times, values, user);
|
mju_historyInit(buf, nsample, 1, buf_times, values, user);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -916,5 +916,5 @@ void mj_initSensorHistory(const mjModel* m, mjData* d, int id,
|
|||||||
const mjtNum* buf_times = times ? times : buf + 2;
|
const mjtNum* buf_times = times ? times : buf + 2;
|
||||||
|
|
||||||
// initialize history buffer with provided phase
|
// initialize history buffer with provided phase
|
||||||
mju_delayInit(buf, nsample, dim, buf_times, values, phase);
|
mju_historyInit(buf, nsample, dim, buf_times, values, phase);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -910,11 +910,11 @@ size_t mju_decodeBase64(uint8_t* buf, const char* s) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//------------------------------ delay buffers -----------------------------------------------------
|
//------------------------------ history buffers ---------------------------------------------------
|
||||||
|
|
||||||
// convert logical index (0=oldest, n-1=newest) to physical index
|
// convert logical index (0=oldest, n-1=newest) to physical index
|
||||||
// cursor points to the newest element (logical index n-1)
|
// cursor points to the newest element (logical index n-1)
|
||||||
static inline int delayPhysicalIndex(int cursor, int n, int logical) {
|
static inline int historyPhysicalIndex(int cursor, int n, int logical) {
|
||||||
return (cursor + 1 + logical) % n;
|
return (cursor + 1 + logical) % n;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -922,10 +922,10 @@ static inline int delayPhysicalIndex(int cursor, int n, int logical) {
|
|||||||
// find logical index i such that times[i-1] < t <= times[i], using circular binary search
|
// 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]
|
// returns 0 if t <= times[oldest], n if t > times[newest]
|
||||||
// cursor points to the newest element (logical index n-1)
|
// cursor points to the newest element (logical index n-1)
|
||||||
static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
static int historyFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
||||||
// get oldest and newest timestamps
|
// get oldest and newest timestamps
|
||||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
||||||
int newest_phys = delayPhysicalIndex(cursor, n, n-1);
|
int newest_phys = historyPhysicalIndex(cursor, n, n-1);
|
||||||
mjtNum t_oldest = times[oldest_phys];
|
mjtNum t_oldest = times[oldest_phys];
|
||||||
mjtNum t_newest = times[newest_phys];
|
mjtNum t_newest = times[newest_phys];
|
||||||
|
|
||||||
@@ -944,7 +944,7 @@ static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
|||||||
int hi = n - 1;
|
int hi = n - 1;
|
||||||
while (hi - lo > 1) {
|
while (hi - lo > 1) {
|
||||||
int mid = (lo + hi) / 2;
|
int mid = (lo + hi) / 2;
|
||||||
int mid_phys = delayPhysicalIndex(cursor, n, mid);
|
int mid_phys = historyPhysicalIndex(cursor, n, mid);
|
||||||
if (times[mid_phys] < t) {
|
if (times[mid_phys] < t) {
|
||||||
lo = mid;
|
lo = mid;
|
||||||
} else {
|
} else {
|
||||||
@@ -956,10 +956,10 @@ static int delayFindIndex(const mjtNum* times, int n, int cursor, mjtNum t) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// initialize delay buffer with given times and values; times must be strictly increasing
|
// initialize history buffer with given times and values; times must be strictly increasing
|
||||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
// 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,
|
void mju_historyInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNum* values,
|
||||||
mjtNum user) {
|
mjtNum user) {
|
||||||
// check strict monotonicity of times
|
// check strict monotonicity of times
|
||||||
for (int i = 0; i < n-1; i++) {
|
for (int i = 0; i < n-1; i++) {
|
||||||
if (times[i+1] - times[i] < mjMINVAL) {
|
if (times[i+1] - times[i] < mjMINVAL) {
|
||||||
@@ -984,17 +984,17 @@ void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times, const mjtNu
|
|||||||
// if t matches an existing timestamp, returns pointer to that slot
|
// if t matches an existing timestamp, returns pointer to that slot
|
||||||
// if a new sample is inserted, the oldest sample is dropped
|
// if a new sample is inserted, the oldest sample is dropped
|
||||||
// returns pointer to value slot where caller should write dim values
|
// returns pointer to value slot where caller should write dim values
|
||||||
mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
mjtNum* mju_historyInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
||||||
int cursor = (int)buf[1];
|
int cursor = (int)buf[1];
|
||||||
mjtNum* times = buf + 2;
|
mjtNum* times = buf + 2;
|
||||||
mjtNum* values = buf + 2 + n;
|
mjtNum* values = buf + 2 + n;
|
||||||
|
|
||||||
// find logical insertion index: times[i-1] < t <= times[i]
|
// find logical insertion index: times[i-1] < t <= times[i]
|
||||||
int i = delayFindIndex(times, n, cursor, t);
|
int i = historyFindIndex(times, n, cursor, t);
|
||||||
|
|
||||||
// exact match at logical i: return pointer to existing slot
|
// exact match at logical i: return pointer to existing slot
|
||||||
if (i < n) {
|
if (i < n) {
|
||||||
int phys_i = delayPhysicalIndex(cursor, n, i);
|
int phys_i = historyPhysicalIndex(cursor, n, i);
|
||||||
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
||||||
return values + phys_i*dim;
|
return values + phys_i*dim;
|
||||||
}
|
}
|
||||||
@@ -1002,7 +1002,7 @@ mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
|||||||
|
|
||||||
// logical i == 0: new sample is older than oldest, replace oldest slot
|
// logical i == 0: new sample is older than oldest, replace oldest slot
|
||||||
if (i == 0) {
|
if (i == 0) {
|
||||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
||||||
times[oldest_phys] = t;
|
times[oldest_phys] = t;
|
||||||
return values + oldest_phys*dim;
|
return values + oldest_phys*dim;
|
||||||
}
|
}
|
||||||
@@ -1019,12 +1019,12 @@ mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t) {
|
|||||||
|
|
||||||
// 0 < i < n: out-of-order insertion, shift [1, i-1] left (dropping 0), insert at i-1
|
// 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++) {
|
for (int j = 0; j < i-1; j++) {
|
||||||
int src_phys = delayPhysicalIndex(cursor, n, j+1);
|
int src_phys = historyPhysicalIndex(cursor, n, j+1);
|
||||||
int dst_phys = delayPhysicalIndex(cursor, n, j);
|
int dst_phys = historyPhysicalIndex(cursor, n, j);
|
||||||
times[dst_phys] = times[src_phys];
|
times[dst_phys] = times[src_phys];
|
||||||
mju_copy(values + dst_phys*dim, values + src_phys*dim, dim);
|
mju_copy(values + dst_phys*dim, values + src_phys*dim, dim);
|
||||||
}
|
}
|
||||||
int insert_phys = delayPhysicalIndex(cursor, n, i-1);
|
int insert_phys = historyPhysicalIndex(cursor, n, i-1);
|
||||||
times[insert_phys] = t;
|
times[insert_phys] = t;
|
||||||
return values + insert_phys*dim;
|
return values + insert_phys*dim;
|
||||||
}
|
}
|
||||||
@@ -1033,13 +1033,13 @@ 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
|
// 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 pointer to sample in buffer on exact match or ZOH (res untouched)
|
||||||
// returns NULL and writes interpolated result to res on interpolation
|
// 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) {
|
const mjtNum* mju_historyRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtNum t, int interp) {
|
||||||
int cursor = (int)buf[1];
|
int cursor = (int)buf[1];
|
||||||
const mjtNum* times = buf + 2;
|
const mjtNum* times = buf + 2;
|
||||||
const mjtNum* values = buf + 2 + n;
|
const mjtNum* values = buf + 2 + n;
|
||||||
|
|
||||||
int oldest_phys = delayPhysicalIndex(cursor, n, 0);
|
int oldest_phys = historyPhysicalIndex(cursor, n, 0);
|
||||||
int newest_phys = delayPhysicalIndex(cursor, n, n-1);
|
int newest_phys = historyPhysicalIndex(cursor, n, n-1);
|
||||||
mjtNum t_oldest = times[oldest_phys];
|
mjtNum t_oldest = times[oldest_phys];
|
||||||
mjtNum t_newest = times[newest_phys];
|
mjtNum t_newest = times[newest_phys];
|
||||||
|
|
||||||
@@ -1054,8 +1054,8 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN
|
|||||||
}
|
}
|
||||||
|
|
||||||
// find bracketing logical index: times[i-1] < t <= times[i]
|
// find bracketing logical index: times[i-1] < t <= times[i]
|
||||||
int i = delayFindIndex(times, n, cursor, t);
|
int i = historyFindIndex(times, n, cursor, t);
|
||||||
int phys_i = delayPhysicalIndex(cursor, n, i);
|
int phys_i = historyPhysicalIndex(cursor, n, i);
|
||||||
|
|
||||||
// check for exact match at i
|
// check for exact match at i
|
||||||
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
if (mju_abs(t - times[phys_i]) < mjMINVAL) {
|
||||||
@@ -1063,7 +1063,7 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN
|
|||||||
}
|
}
|
||||||
|
|
||||||
// lo = i-1, hi = i (we know i > 0 because t > t_oldest)
|
// lo = i-1, hi = i (we know i > 0 because t > t_oldest)
|
||||||
int phys_lo = delayPhysicalIndex(cursor, n, i-1);
|
int phys_lo = historyPhysicalIndex(cursor, n, i-1);
|
||||||
int phys_hi = phys_i;
|
int phys_hi = phys_i;
|
||||||
|
|
||||||
// zero-order hold: return pointer to lo (most recent sample <= t)
|
// zero-order hold: return pointer to lo (most recent sample <= t)
|
||||||
@@ -1096,14 +1096,14 @@ const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim, mjtNum* res, mjtN
|
|||||||
|
|
||||||
mjtNum m_lo = 0;
|
mjtNum m_lo = 0;
|
||||||
if (i > 1) {
|
if (i > 1) {
|
||||||
int phys_lo_prev = delayPhysicalIndex(cursor, n, i-2);
|
int phys_lo_prev = historyPhysicalIndex(cursor, n, i-2);
|
||||||
mjtNum dt_lo = times[phys_hi] - times[phys_lo_prev];
|
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;
|
m_lo = (values[phys_hi*dim+d] - values[phys_lo_prev*dim+d]) / dt_lo;
|
||||||
}
|
}
|
||||||
|
|
||||||
mjtNum m_hi = 0;
|
mjtNum m_hi = 0;
|
||||||
if (i < n - 1) {
|
if (i < n - 1) {
|
||||||
int phys_hi_next = delayPhysicalIndex(cursor, n, i+1);
|
int phys_hi_next = historyPhysicalIndex(cursor, n, i+1);
|
||||||
mjtNum dt_hi = times[phys_hi_next] - times[phys_lo];
|
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;
|
m_hi = (values[phys_hi_next*dim+d] - values[phys_lo*dim+d]) / dt_hi;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -89,7 +89,7 @@ MJAPI size_t mju_isValidBase64(const char* s);
|
|||||||
// returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4))
|
// returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4))
|
||||||
MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s);
|
MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s);
|
||||||
|
|
||||||
//------------------------------ delay buffers -----------------------------------------------------
|
//------------------------------ history buffers ---------------------------------------------------
|
||||||
|
|
||||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||||
// - user: 1 mjtNum reserved for user data (ignored by these functions)
|
// - user: 1 mjtNum reserved for user data (ignored by these functions)
|
||||||
@@ -98,20 +98,20 @@ MJAPI size_t mju_decodeBase64(uint8_t* buf, const char* s);
|
|||||||
// - values: n*dim values, contiguous at buf[n+2..n+2+n*dim-1]
|
// - values: n*dim values, contiguous at buf[n+2..n+2+n*dim-1]
|
||||||
// total buffer size: 2 + n*(1 + dim)
|
// total buffer size: 2 + n*(1 + dim)
|
||||||
|
|
||||||
// initialize delay buffer with given times and values; times must be strictly increasing
|
// initialize history buffer with given times and values; times must be strictly increasing
|
||||||
// values is size n x dim
|
// values is size n x dim
|
||||||
MJAPI void mju_delayInit(mjtNum* buf, int n, int dim, const mjtNum* times,
|
MJAPI void mju_historyInit(mjtNum* buf, int n, int dim, const mjtNum* times,
|
||||||
const mjtNum* values, mjtNum user);
|
const mjtNum* values, mjtNum user);
|
||||||
|
|
||||||
// find insertion slot for sample at time t, maintaining sorted order
|
// find insertion slot for sample at time t, maintaining sorted order
|
||||||
// returns pointer to value slot (size dim) where caller should write
|
// returns pointer to value slot (size dim) where caller should write
|
||||||
MJAPI mjtNum* mju_delayInsert(mjtNum* buf, int n, int dim, mjtNum t);
|
MJAPI mjtNum* mju_historyInsert(mjtNum* buf, int n, int dim, mjtNum t);
|
||||||
|
|
||||||
// read vector value at time t; interp: 0=zero-order-hold, 1=linear, 2=cubic spline
|
// 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 pointer to sample in buffer on exact match (res untouched)
|
||||||
// returns NULL and writes interpolated result to res otherwise
|
// returns NULL and writes interpolated result to res otherwise
|
||||||
MJAPI const mjtNum* mju_delayRead(const mjtNum* buf, int n, int dim,
|
MJAPI const mjtNum* mju_historyRead(const mjtNum* buf, int n, int dim,
|
||||||
mjtNum* res, mjtNum t, int interp);
|
mjtNum* res, mjtNum t, int interp);
|
||||||
|
|
||||||
//------------------------------ miscellaneous -----------------------------------------------------
|
//------------------------------ miscellaneous -----------------------------------------------------
|
||||||
|
|
||||||
|
|||||||
@@ -724,22 +724,22 @@ TEST_F(Base64Test, decodeAndEncode) {
|
|||||||
EXPECT_THAT(buffer2.data(), StrEq(s));
|
EXPECT_THAT(buffer2.data(), StrEq(s));
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------------------------------- Delay Buffers ----------------------------
|
// --------------------------------- History Buffers ---------------------------
|
||||||
|
|
||||||
using DelayTest = MujocoTest;
|
using HistoryTest = MujocoTest;
|
||||||
|
|
||||||
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
// buffer layout: [user(1), cursor(1), times(n), values(n*dim)]
|
||||||
// cursor points to newest element (logical index n-1)
|
// cursor points to newest element (logical index n-1)
|
||||||
// after init, cursor=n-1, so physical indices equal logical indices
|
// after init, cursor=n-1, so physical indices equal logical indices
|
||||||
|
|
||||||
TEST_F(DelayTest, Init) {
|
TEST_F(HistoryTest, Init) {
|
||||||
constexpr int n = 4;
|
constexpr int n = 4;
|
||||||
constexpr int dim = 1;
|
constexpr int dim = 1;
|
||||||
mjtNum buf[2 + n + n*dim];
|
mjtNum buf[2 + n + n*dim];
|
||||||
|
|
||||||
std::vector<mjtNum> times = {4, 6, 8, 10};
|
std::vector<mjtNum> times = {4, 6, 8, 10};
|
||||||
std::vector<mjtNum> values = {99, 99, 99, 99};
|
std::vector<mjtNum> values = {99, 99, 99, 99};
|
||||||
mju_delayInit(buf, n, dim, times.data(), values.data(), 0.0);
|
mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0);
|
||||||
|
|
||||||
// check header
|
// check header
|
||||||
EXPECT_EQ(buf[0], 0.0); // user
|
EXPECT_EQ(buf[0], 0.0); // user
|
||||||
@@ -749,30 +749,30 @@ TEST_F(DelayTest, Init) {
|
|||||||
// values: [99, 99, 99, 99]
|
// values: [99, 99, 99, 99]
|
||||||
// verify via read function (logical order)
|
// verify via read function (logical order)
|
||||||
mjtNum res;
|
mjtNum res;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 99.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, Init_Vector) {
|
TEST_F(HistoryTest, Init_Vector) {
|
||||||
constexpr int n = 3;
|
constexpr int n = 3;
|
||||||
constexpr int dim = 2;
|
constexpr int dim = 2;
|
||||||
mjtNum buf[2 + n + n*dim];
|
mjtNum buf[2 + n + n*dim];
|
||||||
|
|
||||||
std::vector<mjtNum> times = {-2, -1, 0};
|
std::vector<mjtNum> times = {-2, -1, 0};
|
||||||
std::vector<mjtNum> values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.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);
|
mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0);
|
||||||
|
|
||||||
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
|
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
|
||||||
|
|
||||||
// verify via read function
|
// verify via read function
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, -2.0, 0);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, -2.0, 0);
|
||||||
ASSERT_NE(ptr, nullptr);
|
ASSERT_NE(ptr, nullptr);
|
||||||
EXPECT_EQ(ptr[0], 1.0);
|
EXPECT_EQ(ptr[0], 1.0);
|
||||||
EXPECT_EQ(ptr[1], 2.0);
|
EXPECT_EQ(ptr[1], 2.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, Append) {
|
TEST_F(HistoryTest, Append) {
|
||||||
constexpr int n = 4;
|
constexpr int n = 4;
|
||||||
constexpr int dim = 1;
|
constexpr int dim = 1;
|
||||||
// Initialize buffer properly, then insert
|
// Initialize buffer properly, then insert
|
||||||
@@ -786,26 +786,26 @@ TEST_F(DelayTest, Append) {
|
|||||||
mju_zero(buf + 2 + n, n);
|
mju_zero(buf + 2 + n, n);
|
||||||
|
|
||||||
// overwrite with specific values
|
// overwrite with specific values
|
||||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
*mju_historyInsert(buf, n, dim, 4.0) = 1.0;
|
||||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
*mju_historyInsert(buf, n, dim, 6.0) = 2.0;
|
||||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
*mju_historyInsert(buf, n, dim, 8.0) = 3.0;
|
||||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
*mju_historyInsert(buf, n, dim, 10.0) = 4.0;
|
||||||
|
|
||||||
// now append at t=12
|
// now append at t=12
|
||||||
*mju_delayInsert(buf, n, dim, 12.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 12.0) = 99.0;
|
||||||
|
|
||||||
// verify logical order: [6, 8, 10, 12] -> [2, 3, 4, 99]
|
// verify logical order: [6, 8, 10, 12] -> [2, 3, 4, 99]
|
||||||
mjtNum res;
|
mjtNum res;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 6.0, 0), 2.0);
|
EXPECT_EQ(*mju_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 12.0, 0), 99.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 12.0, 0), 99.0);
|
||||||
|
|
||||||
// oldest should now be t=6
|
// oldest should now be t=6
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 2.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 2.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, Append_Multiple) {
|
TEST_F(HistoryTest, Append_Multiple) {
|
||||||
constexpr int n = 3;
|
constexpr int n = 3;
|
||||||
constexpr int dim = 1;
|
constexpr int dim = 1;
|
||||||
mjtNum buf[2 + 2*n];
|
mjtNum buf[2 + 2*n];
|
||||||
@@ -817,16 +817,16 @@ TEST_F(DelayTest, Append_Multiple) {
|
|||||||
|
|
||||||
for (int i = 1; i <= 4; i++) {
|
for (int i = 1; i <= 4; i++) {
|
||||||
mjtNum i_real = static_cast<mjtNum>(i);
|
mjtNum i_real = static_cast<mjtNum>(i);
|
||||||
*mju_delayInsert(buf, n, dim, i_real) = i_real;
|
*mju_historyInsert(buf, n, dim, i_real) = i_real;
|
||||||
}
|
}
|
||||||
// Final: logical timestamps [2, 3, 4], values [2, 3, 4]
|
// Final: logical timestamps [2, 3, 4], values [2, 3, 4]
|
||||||
mjtNum res;
|
mjtNum res;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 2.0, 0), 2.0);
|
EXPECT_EQ(*mju_historyRead(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_historyRead(buf, n, dim, &res, 3.0, 0), 3.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 4.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, ReadVector_ExactMatch) {
|
TEST_F(HistoryTest, ReadVector_ExactMatch) {
|
||||||
constexpr int n = 3;
|
constexpr int n = 3;
|
||||||
constexpr int dim = 2;
|
constexpr int dim = 2;
|
||||||
mjtNum buf[2 + n + n*dim];
|
mjtNum buf[2 + n + n*dim];
|
||||||
@@ -837,21 +837,21 @@ TEST_F(DelayTest, ReadVector_ExactMatch) {
|
|||||||
mju_zero(buf + 2 + n, n*dim);
|
mju_zero(buf + 2 + n, n*dim);
|
||||||
|
|
||||||
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
|
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
|
||||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 1.0, 0);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0);
|
||||||
ASSERT_NE(ptr, nullptr);
|
ASSERT_NE(ptr, nullptr);
|
||||||
EXPECT_EQ(ptr[0], 3.0);
|
EXPECT_EQ(ptr[0], 3.0);
|
||||||
EXPECT_EQ(ptr[1], 4.0);
|
EXPECT_EQ(ptr[1], 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, ReadVector_ZOH) {
|
TEST_F(HistoryTest, ReadVector_ZOH) {
|
||||||
constexpr int n = 3;
|
constexpr int n = 3;
|
||||||
constexpr int dim = 2;
|
constexpr int dim = 2;
|
||||||
mjtNum buf[2 + n + n*dim];
|
mjtNum buf[2 + n + n*dim];
|
||||||
@@ -861,21 +861,21 @@ TEST_F(DelayTest, ReadVector_ZOH) {
|
|||||||
mju_copy(buf + 2, times, n);
|
mju_copy(buf + 2, times, n);
|
||||||
mju_zero(buf + 2 + n, n*dim);
|
mju_zero(buf + 2 + n, n*dim);
|
||||||
|
|
||||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 0);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0);
|
||||||
ASSERT_NE(ptr, nullptr);
|
ASSERT_NE(ptr, nullptr);
|
||||||
EXPECT_EQ(ptr[0], 1.0);
|
EXPECT_EQ(ptr[0], 1.0);
|
||||||
EXPECT_EQ(ptr[1], 2.0);
|
EXPECT_EQ(ptr[1], 2.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, ReadVector_Linear) {
|
TEST_F(HistoryTest, ReadVector_Linear) {
|
||||||
constexpr int n = 3;
|
constexpr int n = 3;
|
||||||
constexpr int dim = 2;
|
constexpr int dim = 2;
|
||||||
mjtNum buf[2 + n + n*dim];
|
mjtNum buf[2 + n + n*dim];
|
||||||
@@ -885,21 +885,21 @@ TEST_F(DelayTest, ReadVector_Linear) {
|
|||||||
mju_copy(buf + 2, times, n);
|
mju_copy(buf + 2, times, n);
|
||||||
mju_zero(buf + 2 + n, n*dim);
|
mju_zero(buf + 2 + n, n*dim);
|
||||||
|
|
||||||
mjtNum* slot0 = mju_delayInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0; slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0; slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_delayInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0; slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_delayRead(buf, n, dim, res, 0.5, 1);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1);
|
||||||
EXPECT_EQ(ptr, nullptr);
|
EXPECT_EQ(ptr, nullptr);
|
||||||
EXPECT_THAT(res[0], DoubleNear(2.0, 1e-10)); // (1+3)/2
|
EXPECT_THAT(res[0], DoubleNear(2.0, 1e-10)); // (1+3)/2
|
||||||
EXPECT_THAT(res[1], DoubleNear(3.0, 1e-10)); // (2+4)/2
|
EXPECT_THAT(res[1], DoubleNear(3.0, 1e-10)); // (2+4)/2
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, InsertOutOfOrder) {
|
TEST_F(HistoryTest, InsertOutOfOrder) {
|
||||||
constexpr int n = 4;
|
constexpr int n = 4;
|
||||||
constexpr int dim = 1;
|
constexpr int dim = 1;
|
||||||
mjtNum buf[2 + 2*n];
|
mjtNum buf[2 + 2*n];
|
||||||
@@ -912,41 +912,41 @@ TEST_F(DelayTest, InsertOutOfOrder) {
|
|||||||
mju_copy(buf + 2, times, n);
|
mju_copy(buf + 2, times, n);
|
||||||
mju_zero(buf + 2 + n, n);
|
mju_zero(buf + 2 + n, n);
|
||||||
|
|
||||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
*mju_historyInsert(buf, n, dim, 4.0) = 1.0;
|
||||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
*mju_historyInsert(buf, n, dim, 6.0) = 2.0;
|
||||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
*mju_historyInsert(buf, n, dim, 8.0) = 3.0;
|
||||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
*mju_historyInsert(buf, n, dim, 10.0) = 4.0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// insert in middle (between t=8 and t=10)
|
// insert in middle (between t=8 and t=10)
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 9.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 9.0) = 99.0;
|
||||||
// logical: [6, 8, 9, 10] -> [2, 3, 99, 4]
|
// 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_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 9.0, 0), 99.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
|
|
||||||
// insert near start (between t=4 and t=6)
|
// insert near start (between t=4 and t=6)
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 5.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 5.0) = 99.0;
|
||||||
// logical: [5, 6, 8, 10] -> [99, 2, 3, 4]
|
// 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_historyRead(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_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
|
|
||||||
// insert before oldest (t=3 < t=4): replaces oldest
|
// insert before oldest (t=3 < t=4): replaces oldest
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 3.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 3.0) = 99.0;
|
||||||
// logical: [3, 6, 8, 10] -> [99, 2, 3, 4]
|
// 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_historyRead(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_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, InsertReplaceOnCollision) {
|
TEST_F(HistoryTest, InsertReplaceOnCollision) {
|
||||||
constexpr int n = 4;
|
constexpr int n = 4;
|
||||||
constexpr int dim = 1;
|
constexpr int dim = 1;
|
||||||
mjtNum buf[2 + 2*n];
|
mjtNum buf[2 + 2*n];
|
||||||
@@ -960,50 +960,50 @@ TEST_F(DelayTest, InsertReplaceOnCollision) {
|
|||||||
mju_copy(buf + 2, times, n);
|
mju_copy(buf + 2, times, n);
|
||||||
mju_zero(buf + 2 + n, n);
|
mju_zero(buf + 2 + n, n);
|
||||||
|
|
||||||
*mju_delayInsert(buf, n, dim, 4.0) = 1.0;
|
*mju_historyInsert(buf, n, dim, 4.0) = 1.0;
|
||||||
*mju_delayInsert(buf, n, dim, 6.0) = 2.0;
|
*mju_historyInsert(buf, n, dim, 6.0) = 2.0;
|
||||||
*mju_delayInsert(buf, n, dim, 8.0) = 3.0;
|
*mju_historyInsert(buf, n, dim, 8.0) = 3.0;
|
||||||
*mju_delayInsert(buf, n, dim, 10.0) = 4.0;
|
*mju_historyInsert(buf, n, dim, 10.0) = 4.0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// collision in middle (t=8)
|
// collision in middle (t=8)
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 8.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 8.0) = 99.0;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0);
|
EXPECT_EQ(*mju_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 8.0, 0), 99.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
|
|
||||||
// collision at newest (t=10)
|
// collision at newest (t=10)
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 10.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 10.0) = 99.0;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 1.0);
|
EXPECT_EQ(*mju_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 8.0, 0), 3.0);
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 99.0);
|
||||||
|
|
||||||
// collision at oldest (t=4)
|
// collision at oldest (t=4)
|
||||||
reset();
|
reset();
|
||||||
*mju_delayInsert(buf, n, dim, 4.0) = 99.0;
|
*mju_historyInsert(buf, n, dim, 4.0) = 99.0;
|
||||||
EXPECT_EQ(*mju_delayRead(buf, n, dim, &res, 4.0, 0), 99.0);
|
EXPECT_EQ(*mju_historyRead(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_historyRead(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_historyRead(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_historyRead(buf, n, dim, &res, 10.0, 0), 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TriggerDelayInitNonMonotonic() {
|
void TriggerHistoryInitNonMonotonic() {
|
||||||
mjtNum buf[10];
|
mjtNum buf[10];
|
||||||
mjtNum times[4] = {1, 2, 2, 4}; // not strictly increasing
|
mjtNum times[4] = {1, 2, 2, 4}; // not strictly increasing
|
||||||
mjtNum values[4] = {0};
|
mjtNum values[4] = {0};
|
||||||
mju_delayInit(buf, 4, 1, times, values, 0.0);
|
mju_historyInit(buf, 4, 1, times, values, 0.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, Init_NonMonotonic) {
|
TEST_F(HistoryTest, Init_NonMonotonic) {
|
||||||
EXPECT_FATAL_FAILURE(TriggerDelayInitNonMonotonic(),
|
EXPECT_FATAL_FAILURE(TriggerHistoryInitNonMonotonic(),
|
||||||
"mju_delayInit: times must be strictly increasing");
|
"mju_historyInit: times must be strictly increasing");
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DelayTest, CubicInterpolation) {
|
TEST_F(HistoryTest, CubicInterpolation) {
|
||||||
int n = 2;
|
int n = 2;
|
||||||
int dim = 2;
|
int dim = 2;
|
||||||
mjtNum buf[100]; // 2 + 2 + 2*2 = 8
|
mjtNum buf[100]; // 2 + 2 + 2*2 = 8
|
||||||
@@ -1016,9 +1016,9 @@ TEST_F(DelayTest, CubicInterpolation) {
|
|||||||
// Insert (0, 0, 1) and (1, 1, 0).
|
// Insert (0, 0, 1) and (1, 1, 0).
|
||||||
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
|
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
|
||||||
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 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);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 0.0; slot0[1] = 1.0;
|
slot0[0] = 0.0; slot0[1] = 1.0;
|
||||||
mjtNum* slot1 = mju_delayInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 1.0; slot1[1] = 0.0;
|
slot1[0] = 1.0; slot1[1] = 0.0;
|
||||||
|
|
||||||
mjtNum res[2];
|
mjtNum res[2];
|
||||||
@@ -1026,14 +1026,14 @@ TEST_F(DelayTest, CubicInterpolation) {
|
|||||||
// Test midpoint x=0.5
|
// Test midpoint x=0.5
|
||||||
// Dim 0: 0.5
|
// Dim 0: 0.5
|
||||||
// Dim 1: 1 - 0.5 = 0.5
|
// Dim 1: 1 - 0.5 = 0.5
|
||||||
mju_delayRead(buf, n, dim, res, 0.5, 2);
|
mju_historyRead(buf, n, dim, res, 0.5, 2);
|
||||||
EXPECT_NEAR(res[0], 0.5, 1e-9);
|
EXPECT_NEAR(res[0], 0.5, 1e-9);
|
||||||
EXPECT_NEAR(res[1], 0.5, 1e-9);
|
EXPECT_NEAR(res[1], 0.5, 1e-9);
|
||||||
|
|
||||||
// Test x=0.25
|
// Test x=0.25
|
||||||
// Dim 0: 3*0.25^2 - 2*0.25^3
|
// Dim 0: 3*0.25^2 - 2*0.25^3
|
||||||
// Dim 1: 1 - (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);
|
mju_historyRead(buf, n, dim, res, 0.25, 2);
|
||||||
mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25;
|
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[0], expected_0_25, 1e-9);
|
||||||
EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9);
|
EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9);
|
||||||
@@ -1041,7 +1041,7 @@ TEST_F(DelayTest, CubicInterpolation) {
|
|||||||
// Test x=0.8
|
// Test x=0.8
|
||||||
// Dim 0: 3*0.8^2 - 2*0.8^3
|
// Dim 0: 3*0.8^2 - 2*0.8^3
|
||||||
// Dim 1: 1 - (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);
|
mju_historyRead(buf, n, dim, res, 0.8, 2);
|
||||||
mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8;
|
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[0], expected_0_8, 1e-9);
|
||||||
EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9);
|
EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9);
|
||||||
|
|||||||
Reference in New Issue
Block a user