Skip to content

nav: dyn_bike is port-Hamiltonian at every slip, with per-step energy books #831

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
transfix merged 2 commits into from
Oct 27, 2026
Merged

nav: dyn_bike is port-Hamiltonian at every slip, with per-step energy books #831

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
85840fd
nav: dyn_bike is port-Hamiltonian at every slip, with per-step energy…
transfix Oct 27, 2026
3934049
Merge branch 'master' into feat/dyn-bike-passive-ph
transfix Oct 27, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
301 changes: 216 additions & 85 deletions inc/cvc/nav/detail/dyn_bike.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -231,10 +231,18 @@ template <class S> CVC_HD inline S dyn_secant_w(S F, S alpha, S w, S w0, S B, S
return -F / (w - w0);
}

// The twist one step reached before its limiters: the speed before the clamp to [sp_min, vmax],
// (vy, r) before the low-speed blend. dyn_plant fills it when asked; the energy books
// (dyn_plant_books) charge the limiters the kinetic energy between it and the step's result.
template <class S> struct dyn_step_free {
S vx, vy, r;
};

// One step of the plant; see the header comment. grade is data (its gradient is not taken).
template <class S>
CVC_HD inline void dyn_plant(const dyn_bike_params &p, const bike_veh &v, const dyn_state<S> &s,
S delta_cmd, S a_cmd, S grade, dyn_state<S> &o) {
S delta_cmd, S a_cmd, S grade, dyn_state<S> &o,
dyn_step_free<S> *seen = nullptr) {
const S hdt = S(v.hdt), zero = S(0), one = S(1);
const S m = S(p.m), Iz = S(p.Iz), lf = S(p.lf), lr = S(p.lr), h = S(p.h), g = S(p.g);
const S mu = S(p.mu), Cp = S(p.C), L = lf + lr;
Expand Down Expand Up @@ -315,7 +323,13 @@ CVC_HD inline void dyn_plant(const dyn_bike_params &p, const bike_veh &v, const

// 3, corrected: the speed with the front lateral force the implicit step applied.
const S FX = Fxf * cd - Fyf_step * sd + Fxr + F_res;
const S vx1 = dyn_clamp(s.vx + hdt * (FX / m + r1 * vy1), sp_min, vmax);
const S vx1_free = s.vx + hdt * (FX / m + r1 * vy1);
const S vx1 = dyn_clamp(vx1_free, sp_min, vmax);
if (seen) {
seen->vx = vx1_free;
seen->vy = vy1;
seen->r = r1;
}

// 5. Low speed: toward the kinematic bicycle.
if (p.v_blend > 0.0f) {
Expand Down Expand Up @@ -365,34 +379,49 @@ CVC_HD inline void dyn_plant_vjp(const dyn_bike_params &p, const bike_veh &v, co
}

// ── The plant in port-Hamiltonian form ────────────────────────────────────────────────────
// Doc 20's rungs 2 and 3, at the plant. The body is a planar rigid body: the pose q = (x, y, psi)
// on SE(2) and the body momenta p = M v, with v = (vx, vy, r) the body twist at the centre of
// gravity and M = diag(m, m, Iz). In the linear-tire regime dyn_plant integrates
// Doc 20's rungs 2 and 3, at the plant, at every slip. The body is a planar rigid body: the pose
// q = (x, y, psi) on SE(2) and the body momenta p = M v, with v = (vx, vy, r) the body twist at the
// centre of gravity and M = diag(m, m, Iz). dyn_plant's vector field (its step as hdt -> 0, the
// actuators holding) is exactly
//
// H(q, p) = 1/2 p^T M^-1 p + V(q), dH/dp = v
// p' = (J(p) - D(v)) v - dV + G u, u = (F_x, delta), y = G^T v
// H(p) = 1/2 p^T M^-1 p, dH/dp = v
// p' = (J(p) - R(v, delta)) v + G(delta) u + f_eps - m g grade e_x, u = (F_xf, F_xr),
// y = G^T v
//
// J(p) [[0, 0, p_y], [0, 0, -p_x], [-p_y, p_x, 0]]: SE(2)'s coadjoint action, J(p) v = ad*_v p.
// It gives dyn_plant's + r vy in vx' and - r vx in vy' (and, with equal masses, nothing in
// r'). Skew, so it moves no energy.
// D(v) diag(D_x, D_lat). D_x = drag u + c_rr m g / u: the drag and rolling resistance, each
// opposing vx (u = sqrt(vx^2 + eps^2), as the plant smooths them). D_lat = (C_f a a^T +
// C_r b b^T) / u on (vy, r), a = (1, lf), b = (1, -lr): the velocity parts of the
// linearized tires, F_yf = C_f (delta - (vy + lf r) / u), F_yr = -C_r (vy - lr r) / u.
// Positive semi-definite for every u > 0.
// G [[1, 0], [0, C_f], [0, lf C_f]]: the drive and brake force on p_x; the steering's C_f
// delta on p_y and lf C_f delta on p_r.
// dV (m g grade, 0, 0) in the body frame: gravity along the heading (dyn_plant's grade).
// C_f, C_r the tires' cornering stiffness at zero slip, D C B, D the lateral grip the axle has
// left beside its drive or brake force (dyn_lateral_cap).
// It gives the + r vy in vx' and - r vx in vy' (and, with equal masses, nothing in r').
// Skew, so it moves no energy; a Lie-Poisson structure, so Jacobi holds (unlike the
// kinematic bicycle, whose no-slip constraint is not integrable).
// R D_x e_x e_x^T + (S_f / cos delta) n_f n_f^T + S_r n_r n_r^T. D_x = drag u + c_rr m g / u
// is drag and rolling resistance, opposing vx (u = sqrt(vx^2 + eps^2), as the plant
// smooths them). n_f = (-sin delta, cos delta, lf cos delta) and n_r = (0, 1, -lr) carry
// each tire's lateral force onto (vx, vy, r) (so n^T v is its contact patch's sideways
// velocity in the wheel's frame), and S_f, S_r are the tires' secant stiffnesses through
// their roots (dyn_secant_w), >= 0 at any slip because a Pacejka force (C < 2) always
// opposes its slip. So R is positive semi-definite sliding or not, for |delta| < pi / 2.
// G [t_f, t_r], t_f = (cos delta, sin delta, lf sin delta), t_r = (1, 0, 0): each axle's
// drive or brake force (within its grip) along its wheel. y^T u is the power those forces
// deliver at the wheels' ground speed. Steering is not an input: delta only turns n_f and
// t_f, so it does no work on the body.
// f_eps n_f (F_yf + (S_f / cos delta) n_f^T v) + n_r (F_yr + S_r n_r^T v): what the secants
// leave of the tires' forces. The front force's root is at the plant's floored speed,
// vy + lf r = u tan(sg delta) with sg = vx / u, not where n_f^T v = 0 (vy + lf r =
// vx tan delta); f_eps is the difference, S_f (u tan(sg delta) - vx tan delta) n_f, plus
// the secants' own rounding at zero slip. It vanishes as eps / |vx| -> 0 and is booked on
// its own line.
// grade gravity along the heading, booked as the power - m g grade vx (the plant has no side
// slope, so H carries no potential).
//
// So dH/dt = -v^T D v + y^T u - m g grade vx: supplied power y^T u, dissipated v^T D v. The
// inputs are the actuators' outputs (the state's delta and a): their lags are outside the body.
// dyn_plant departs from this by the tires' and the small steering angle's linearization and by
// its own step; nav_dyn_bike_test's DynBikePH gates measure both. Past a tire's peak its force
// falls as slip grows, and the tangent dissipation (dyn_ph_tangent_D) loses definiteness: the
// vehicle slides, which a positive-definite D cannot represent. These are evaluations only;
// dyn_plant does not call them.
// So dH/dt = -v^T R v + y^T u + v^T f_eps - m g grade vx: passive with respect to the axle forces,
// at every slip. The inputs are the actuators' outputs (the state's delta and a): their lags are
// outside the body. dyn_plant's step departs from the vector field by its integrator (implicit
// Euler on (vy, r), a predicted then corrected vx) and by its limiters (the speed clamp, the
// low-speed blend toward the kinematic bicycle); dyn_plant_books charges each step's limiters
// their kinetic energy and leaves the integrator's error, second order per step, as the residual.
// Past a tire's peak its force falls as slip grows and the tangent (dyn_ph_tangent_D), the
// linearization's damping, loses definiteness: the motion can be locally unstable there, but the
// secant R, the energy's, stays positive semi-definite. These are evaluations: dyn_plant's
// arithmetic does not depend on them.

// The kinetic energy 1/2 v^T M v.
template <class S>
Expand Down Expand Up @@ -422,9 +451,11 @@ template <class S> CVC_HD inline void dyn_ph_J(const S pm[3], S J[3][3]) {
J[2][2] = z;
}

// The lateral grip each axle has left (dyn_plant's step 2 at the state's own a): Dyf, Dyr.
// Each axle's drive or brake force within its grip, and the lateral grip it leaves (dyn_plant's
// step 2 at the state's own a): Fxf, Fxr, Dyf, Dyr.
template <class S>
CVC_HD inline void dyn_ph_grip(const dyn_bike_params &p, const dyn_state<S> &s, S &Dyf, S &Dyr) {
CVC_HD inline void dyn_ph_axles(const dyn_bike_params &p, const dyn_state<S> &s, S &Fxf, S &Fxr,
S &Dyf, S &Dyr) {
const S m = S(p.m), g = S(p.g), lf = S(p.lf), lr = S(p.lr), h = S(p.h), L = lf + lr;
const S fz_floor = S(p.fz_min) * m * g;
S Fzf = m * (g * lr - s.a * h) / L, Fzr = m * (g * lf + s.a * h) / L;
Expand All @@ -434,97 +465,197 @@ CVC_HD inline void dyn_ph_grip(const dyn_bike_params &p, const dyn_state<S> &s,
Fzr = fz_floor;
const S cap_f = S(p.mu) * Fzf, cap_r = S(p.mu) * Fzr, F_drive = m * s.a;
const S share = S(s_val(s.a) >= 0.0 ? p.front_drive : p.front_brake);
Dyf = dyn_lateral_cap(dyn_sat(F_drive * share, cap_f), cap_f);
Dyr = dyn_lateral_cap(dyn_sat(F_drive * (S(1) - share), cap_r), cap_r);
Fxf = dyn_sat(F_drive * share, cap_f);
Fxr = dyn_sat(F_drive * (S(1) - share), cap_r);
Dyf = dyn_lateral_cap(Fxf, cap_f);
Dyr = dyn_lateral_cap(Fxr, cap_r);
}

// The cornering stiffnesses at zero slip, D C B.
// The lateral grip each axle has left: Dyf, Dyr.
template <class S>
CVC_HD inline void dyn_ph_cornering(const dyn_bike_params &p, const dyn_state<S> &s, S &Cf, S &Cr) {
CVC_HD inline void dyn_ph_grip(const dyn_bike_params &p, const dyn_state<S> &s, S &Dyf, S &Dyr) {
S Fxf, Fxr;
dyn_ph_axles(p, s, Fxf, Fxr, Dyf, Dyr);
}

// Everything the form is built from, at a state.
template <class S> struct dyn_ph_parts {
S u, sg; // the slips' floored speed sqrt(vx^2 + eps^2), and sg = vx / u
S Fxf, Fxr; // the axles' drive or brake forces: the inputs
S Fyf, Fyr; // the tires' lateral forces
S wf, wr, w0f; // the axles' lateral velocities, and the front force's root
S Sf, Sr; // the secant stiffnesses (dyn_secant_w)
S Dx; // drag and rolling resistance on vx
S cd; // cos delta
S nf[3], nr[3]; // each tire's lateral force on (vx, vy, r)
S tf[3], tr[3]; // each axle's drive force on (vx, vy, r)
};

template <class S>
CVC_HD inline void dyn_ph_parts_at(const dyn_bike_params &p, const dyn_state<S> &s,
dyn_ph_parts<S> &q) {
const S lf = S(p.lf), lr = S(p.lr), Cp = S(p.C), Bf = S(p.B_f), Br = S(p.B_r), z = S(0);
S Dyf, Dyr;
dyn_ph_grip(p, s, Dyf, Dyr);
Cf = Dyf * S(p.C) * S(p.B_f);
Cr = Dyr * S(p.C) * S(p.B_r);
dyn_ph_axles(p, s, q.Fxf, q.Fxr, Dyf, Dyr);
q.u = s_sqrt(s.vx * s.vx + S(p.eps) * S(p.eps));
q.sg = s.vx / q.u;
const S cd = s_cos(s.delta), sd = s_sin(s.delta);
q.cd = cd;
q.wf = s.vy + lf * s.r;
q.wr = s.vy - lr * s.r;
q.w0f = q.u * s_tan(q.sg * s.delta);
const S af = q.sg * s.delta - s_atan2(q.wf, q.u), ar = -s_atan2(q.wr, q.u);
q.Fyf = dyn_lateral(af, Bf, Cp, Dyf);
q.Fyr = dyn_lateral(ar, Br, Cp, Dyr);
q.Sf = dyn_secant_w(q.Fyf, af, q.wf, q.w0f, Bf, Cp, Dyf, q.u);
q.Sr = dyn_secant_w(q.Fyr, ar, q.wr, z, Br, Cp, Dyr, q.u);
q.Dx = S(p.drag) * q.u + S(p.c_rr) * S(p.m) * S(p.g) / q.u;
q.nf[0] = -sd;
q.nf[1] = cd;
q.nf[2] = lf * cd;
q.nr[0] = z;
q.nr[1] = S(1);
q.nr[2] = -lr;
q.tf[0] = cd;
q.tf[1] = sd;
q.tf[2] = lf * sd;
q.tr[0] = S(1);
q.tr[1] = z;
q.tr[2] = z;
}

// R(v, delta), the dissipation: positive semi-definite at every slip.
template <class S>
CVC_HD inline void dyn_ph_R(const dyn_bike_params &p, const dyn_state<S> &s, S R[3][3]) {
dyn_ph_parts<S> q;
dyn_ph_parts_at(p, s, q);
const S kf = q.Sf / q.cd;
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j) {
R[i][j] = kf * (q.nf[i] * q.nf[j]) + q.Sr * (q.nr[i] * q.nr[j]);
R[j][i] = R[i][j];
}
R[0][0] = R[0][0] + q.Dx;
}

// D(v), the linear-tire dissipation (with drag and rolling resistance on vx).
template <class S>
CVC_HD inline void dyn_ph_D(const dyn_bike_params &p, const dyn_state<S> &s, S D[3][3]) {
S Cf, Cr;
dyn_ph_cornering(p, s, Cf, Cr);
const S u = s_sqrt(s.vx * s.vx + S(p.eps) * S(p.eps));
const S lf = S(p.lf), lr = S(p.lr), z = S(0);
D[0][0] = S(p.drag) * u + S(p.c_rr) * S(p.m) * S(p.g) / u;
D[0][1] = z;
D[0][2] = z;
D[1][0] = z;
D[2][0] = z;
D[1][1] = (Cf + Cr) / u;
D[1][2] = (Cf * lf - Cr * lr) / u;
D[2][1] = D[1][2];
D[2][2] = (Cf * lf * lf + Cr * lr * lr) / u;
}

// G, the input map for u = (F_x, delta).
// G(delta), the input map for u = (F_xf, F_xr): each axle's force along its wheel.
template <class S>
CVC_HD inline void dyn_ph_G(const dyn_bike_params &p, const dyn_state<S> &s, S G[3][2]) {
S Cf, Cr;
dyn_ph_cornering(p, s, Cf, Cr);
const S z = S(0);
G[0][0] = S(1);
G[0][1] = z;
G[1][0] = z;
G[1][1] = Cf;
G[2][0] = z;
G[2][1] = S(p.lf) * Cf;
dyn_ph_parts<S> q;
dyn_ph_parts_at(p, s, q);
for (int i = 0; i < 3; ++i) {
G[i][0] = q.tf[i];
G[i][1] = q.tr[i];
}
}

// u = (F_x, delta) from the actuators' state: the drive force m a and the road-wheel angle.
// u = (F_xf, F_xr): the axles' drive or brake forces, from the actuator's a, within their grip.
template <class S>
CVC_HD inline void dyn_ph_input(const dyn_bike_params &p, const dyn_state<S> &s, S u[2]) {
u[0] = S(p.m) * s.a;
u[1] = s.delta;
S Dyf, Dyr;
dyn_ph_axles(p, s, u[0], u[1], Dyf, Dyr);
}

// p' = (J - D) v - dV + G u at the state.
// f_eps, what the secant dissipation leaves of the tires' lateral forces: the speed floor's.
template <class S>
CVC_HD inline void dyn_ph_floor(const dyn_bike_params &p, const dyn_state<S> &s, S f[3]) {
dyn_ph_parts<S> q;
dyn_ph_parts_at(p, s, q);
const S v[3] = {s.vx, s.vy, s.r};
S nfv = S(0), nrv = S(0);
for (int i = 0; i < 3; ++i) {
nfv = nfv + q.nf[i] * v[i];
nrv = nrv + q.nr[i] * v[i];
}
const S ef = q.Fyf + q.Sf / q.cd * nfv, er = q.Fyr + q.Sr * nrv;
for (int i = 0; i < 3; ++i)
f[i] = q.nf[i] * ef + q.nr[i] * er;
}

// p' = (J - R) v + G u + f_eps - m g grade e_x at the state: dyn_plant's vector field.
template <class S>
CVC_HD inline void dyn_ph_rhs(const dyn_bike_params &p, const dyn_state<S> &s, S grade, S pdot[3]) {
S pm[3], J[3][3], D[3][3], G[3][2], u[2];
S pm[3], J[3][3], R[3][3], G[3][2], u[2], f[3];
dyn_ph_momentum(p, s, pm);
dyn_ph_J(pm, J);
dyn_ph_D(p, s, D);
dyn_ph_R(p, s, R);
dyn_ph_G(p, s, G);
dyn_ph_input(p, s, u);
dyn_ph_floor(p, s, f);
const S v[3] = {s.vx, s.vy, s.r};
for (int i = 0; i < 3; ++i) {
S acc = G[i][0] * u[0] + G[i][1] * u[1];
S acc = G[i][0] * u[0] + G[i][1] * u[1] + f[i];
for (int j = 0; j < 3; ++j)
acc = acc + (J[i][j] - D[i][j]) * v[j];
acc = acc + (J[i][j] - R[i][j]) * v[j];
pdot[i] = acc;
}
pdot[0] = pdot[0] - S(p.m) * S(p.g) * grade;
}

// The power balance at the state: dissipated v^T D v and supplied y^T u (y = G^T v).
// The power balance at a state: dH/dt = supplied - dissipated + floor + grade.
template <class S> struct dyn_ph_rates {
S dissipated; // v^T R v >= 0: the tires' slip, drag and rolling resistance
S supplied; // y^T u, y = G^T v: the axles' forces at their wheels' ground speed
S floor; // v^T f_eps: the slips' speed floor
S grade; // - m g grade vx: gravity along the heading
};

template <class S>
CVC_HD inline void dyn_ph_power(const dyn_bike_params &p, const dyn_state<S> &s, S &dissipated,
S &supplied) {
S D[3][3], G[3][2], u[2];
dyn_ph_D(p, s, D);
CVC_HD inline dyn_ph_rates<S> dyn_ph_rates_at(const dyn_bike_params &p, const dyn_state<S> &s,
S grade) {
S R[3][3], G[3][2], u[2], f[3];
dyn_ph_R(p, s, R);
dyn_ph_G(p, s, G);
dyn_ph_input(p, s, u);
dyn_ph_floor(p, s, f);
const S v[3] = {s.vx, s.vy, s.r};
dissipated = S(0);
for (int i = 0; i < 3; ++i)
dyn_ph_rates<S> out;
out.dissipated = S(0);
out.supplied = S(0);
out.floor = S(0);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j)
dissipated = dissipated + v[i] * D[i][j] * v[j];
supplied = S(0);
for (int k = 0; k < 2; ++k) {
S y = S(0);
for (int i = 0; i < 3; ++i)
y = y + G[i][k] * v[i];
supplied = supplied + y * u[k];
out.dissipated = out.dissipated + v[i] * R[i][j] * v[j];
out.supplied = out.supplied + v[i] * (G[i][0] * u[0] + G[i][1] * u[1]);
out.floor = out.floor + v[i] * f[i];
}
out.grade = -S(p.m) * S(p.g) * grade * s.vx;
return out;
}

// One step's energy books, in J: dH = supplied - dissipated + floor + grade + limiter + residual.
// The powers are the trapezoid between the step's start (with the inputs it applied, the
// actuators' outputs) and the twist it reached before its limiters; limiter is the kinetic energy
// the speed clamp and the low-speed blend then added (< 0: removed); residual is the integrator's
// error, second order in hdt per step.
template <class S> struct dyn_ph_books {
S dH, dissipated, supplied, floor, grade, limiter, residual;
};

// dyn_plant's step with its books. The step is dyn_plant's, to the last bit.
template <class S>
CVC_HD inline void dyn_plant_books(const dyn_bike_params &p, const bike_veh &v,
const dyn_state<S> &s, S delta_cmd, S a_cmd, S grade,
dyn_state<S> &o, dyn_ph_books<S> &b) {
dyn_step_free<S> seen;
dyn_plant<S>(p, v, s, delta_cmd, a_cmd, grade, o, &seen);
dyn_state<S> s0 = s, e = o;
s0.delta = o.delta;
s0.a = o.a;
e.vx = seen.vx;
e.vy = seen.vy;
e.r = seen.r;
const dyn_ph_rates<S> r0 = dyn_ph_rates_at(p, s0, grade), r1 = dyn_ph_rates_at(p, e, grade);
const S half = S(0.5) * S(v.hdt);
b.dissipated = half * (r0.dissipated + r1.dissipated);
b.supplied = half * (r0.supplied + r1.supplied);
b.floor = half * (r0.floor + r1.floor);
b.grade = half * (r0.grade + r1.grade);
const S h0 = dyn_ph_hamiltonian(p, s), he = dyn_ph_hamiltonian(p, e),
h1 = dyn_ph_hamiltonian(p, o);
b.dH = h1 - h0;
b.limiter = h1 - he;
b.residual = b.dH - (b.supplied - b.dissipated + b.floor + b.grade + b.limiter);
}

// The tangent dissipation of dyn_plant's Pacejka tires at the state: Dt = -d(Q_y, Q_r)/d(vy, r)
Expand Down
3 changes: 1 addition & 2 deletions inc/cvc/ph/se2_train.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,7 @@ namespace cvc {
class thread_pool;
namespace ph {

// ── data
// ──────────────────────────────────────────────────────────────────────────────────────────
// ── data ──────────────────────────────────────────────────────────────────────────────────

// Equal-length flights: rows [x, y, psi, v_x, v_y, r, u (control_dim)], row k's inputs acting
// from t_k-1 to t_k (cvc::physics::plant_log's layout; row 0's inputs unused).
Expand Down
Loading
Loading