From 7cf00a09d66f62a742fedc05379b9b67f7907280 Mon Sep 17 00:00:00 2001 From: plancher Date: Thu, 22 Oct 2026 13:40:37 -0400 Subject: [PATCH] Remove the legacy convention plumbing; signed-axis arm; analytic pose oracle; smooth hinge; tangent state cost and floating frame twist for the menu --- RBDReference.py | 227 ++++---------- README.md | 5 +- _energy.py | 8 +- _plant.py | 318 ++++---------------- _regressor.py | 18 +- equivalents/pinocchio_backend.py | 166 +++++----- equivalents/reference_backend.py | 8 +- robot_assets/signed_axis_arm.urdf | 79 +++++ tests/conftest.py | 26 -- tests/robot_manifest.json | 19 ++ tests/test_cost_menu_reference.py | 82 ++++- tests/test_floating_base_conventions.py | 154 ---------- tests/test_helical_joint_equivalence.py | 10 +- tests/test_joint_dynamics_friction_model.py | 6 +- tests/test_pinocchio_pose_derivatives.py | 79 +++++ tests/test_signed_axis_coverage.py | 57 ++++ tests/test_spherical_joint_equivalence.py | 16 +- 17 files changed, 541 insertions(+), 737 deletions(-) create mode 100644 robot_assets/signed_axis_arm.urdf delete mode 100644 tests/test_floating_base_conventions.py create mode 100644 tests/test_pinocchio_pose_derivatives.py create mode 100644 tests/test_signed_axis_coverage.py diff --git a/RBDReference.py b/RBDReference.py index 1f5a92d..cbcc8b4 100644 --- a/RBDReference.py +++ b/RBDReference.py @@ -75,65 +75,6 @@ def __init__(self, robotObj, use_joint_dynamics=False, friction_model="sign", self._spatial_xmat_derivative_func_cache = {} self._spatial_xmat_second_derivative_func_cache = {} - def _normalize_q_input(self, q): - return self.robot.normalize_floating_base_q_input(q) - - def _normalize_v_input(self, vec): - return self.robot.normalize_floating_base_v_input(vec) - - def _denormalize_v_output(self, vec): - return self.robot.denormalize_floating_base_v_output(vec) - - def _permute_matrix_prefix(self, matrix, permutation, axis): - if permutation is None: - return matrix - matrix = np.asarray(matrix, dtype=np.float64).copy() - prefix = len(permutation) - if axis == 0: - matrix[:prefix, :] = matrix[permutation, :] - else: - matrix[:, :prefix] = matrix[:, permutation] - return matrix - - def _denormalize_qv_matrix_output(self, matrix, row_space=None, col_space=None): - matrix = np.asarray(matrix, dtype=np.float64).copy() - if row_space == "v": - matrix = self._permute_matrix_prefix( - matrix, - self.robot.get_floating_base_v_permutation_from_internal(), - axis=0, - ) - elif row_space == "q": - matrix = self._permute_matrix_prefix( - matrix, - self.robot.get_floating_base_q_output_permutation_from_internal(), - axis=0, - ) - if col_space == "v": - matrix = self._permute_matrix_prefix( - matrix, - self.robot.get_floating_base_v_permutation_from_internal(), - axis=1, - ) - elif col_space == "q": - matrix = self._permute_matrix_prefix( - matrix, - self.robot.get_floating_base_q_output_permutation_from_internal(), - axis=1, - ) - return matrix - - def _denormalize_reduced_q_matrix_output(self, matrix, row_space=None): - return self._denormalize_qv_matrix_output(matrix, row_space=row_space, col_space=None) - - def _denormalize_inverse_dynamics_gradient_output(self, dc_dq, dc_dqd): - return np.hstack( - ( - self._denormalize_reduced_q_matrix_output(dc_dq, row_space="v"), - self._denormalize_qv_matrix_output(dc_dqd, row_space="v", col_space="v"), - ) - ) - @staticmethod def _normalize_xyzw_quaternion(quat): quat = np.asarray(quat, dtype=np.float64).copy() @@ -1873,7 +1814,7 @@ def frame_jacobian_dot(self, q, qd, frame_name=None, (The central-FD form is retained as `_frame_jacobian_dot_fd` for the machine-precision cross-check.)""" - qd = self._normalize_v_input(np.asarray(qd, dtype=np.float64)) + qd = np.array(qd, dtype=np.float64) reference_frame = str(reference_frame).upper() if reference_frame not in ("LOCAL", "WORLD", "LOCAL_WORLD_ALIGNED"): raise ValueError("reference_frame must be LOCAL/WORLD/LOCAL_WORLD_ALIGNED") @@ -1983,7 +1924,7 @@ def _frame_jacobian_dot_fd(self, q, qd, frame_name=None, reference_frame="LOCAL_WORLD_ALIGNED", step=1e-6): """Central-FD cross-check for the analytic `frame_jacobian_dot`: differences the analytic `frame_jacobian` along the Lie-group integrator flow.""" - qd = self._normalize_v_input(np.asarray(qd, dtype=np.float64)) + qd = np.array(qd, dtype=np.float64) q_plus = self.integrate(q, step * qd) q_minus = self.integrate(q, -step * qd) Jp = self.frame_jacobian(q_plus, frame_name, reference_frame) @@ -2014,10 +1955,9 @@ def end_effector_pose_hessian(self, q, offsets = None, ee_joint_names = None): Implementation: central-difference FD of the (now-correct) d/dv pose Jacobian on the Lie-group integrator `self.integrate(q, h*e_i)`. This reuses the geometric-Jacobian gradient (machine-precision vs the proven - prototype + pinocchio backend) and SYMMETRIZES the result. FD is fine - for a reference oracle; the (separate) GPU codegen path is the same - FD-on-Jacobian (the analytic d/dv Hessian on GPU is on the backlog -- - see HANDOFF.md A.1 + docs/d2ee_analytic_derivation.md). + prototype + pinocchio backend) and SYMMETRIZES the result. The closed + form is `end_effector_pose_hessian_analytic`, which the GPU codegen + mirrors; this FD form is its cross-check. Parameters ---------- @@ -2766,8 +2706,6 @@ def inverse_dynamics( qdd=None, GRAVITY=-9.81, f_ext=None, - public_output=True, - normalize_input=True, ): """Compute the generalized forces using the Recursive Newton-Euler Algorithm (RNEA). This is the canonical inverse-dynamics routine. @@ -2788,11 +2726,10 @@ def inverse_dynamics( (c, v, a, f) : tuple Generalized forces and intermediate link quantities. """ - if normalize_input: - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - if qdd is not None: - qdd = self._normalize_v_input(qdd) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + if qdd is not None: + qdd = np.array(qdd, dtype=np.float64) # forward pass (external forces are subtracted from f inside fpass) (v, a, f) = self.inverse_dynamics_fpass(q, qd, qdd, GRAVITY, f_ext=f_ext) # backward pass @@ -2800,11 +2737,9 @@ def inverse_dynamics( # joint-local viscous damping + Coulomb friction bias (no-op when the # robot declares neither; gated inside the helper) c = c + self._joint_dynamics_bias(qd) - if public_output: - c = self._denormalize_v_output(c) return (c, v, a, f) - def f_ext_jacobian_transpose(self, q, normalize_input=True): + def f_ext_jacobian_transpose(self, q): """Stacked body-Jacobian transpose ``J^T`` (nv x 6*NB) for the f_ext column. Column block ``i`` (6 columns) is the joint-torque produced by a UNIT @@ -2820,8 +2755,7 @@ def f_ext_jacobian_transpose(self, q, normalize_input=True): ``f_ext_gradient``). This method returns the (un-signed) ``J^T`` so both ``-J^T`` (tau) and ``+M^{-1} J^T`` (qdd) consumers can apply their own sign. """ - if normalize_input: - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() JT = np.zeros((nv, 6 * NB), dtype=np.float64) @@ -2836,7 +2770,7 @@ def f_ext_jacobian_transpose(self, q, normalize_input=True): JT[:, 6 * i + k] = c return JT - def f_ext_jacobian_transpose_dq(self, q, normalize_input=True): + def f_ext_jacobian_transpose_dq(self, q): """Analytic ``dJ^T/dq`` (nv x 6*NB x nv) of the stacked body-Jacobian transpose, for FIXED and FLOATING base (section A.3 oracle). @@ -2884,8 +2818,7 @@ def f_ext_jacobian_transpose_dq(self, q, normalize_input=True): subspace) into the shared (v_j / v_m) entry — the same ``+=`` reduction the first-order ``f_ext_jacobian_transpose`` and the GRiD emit perform. """ - if normalize_input: - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() dJT = np.zeros((nv, 6 * NB, nv), dtype=np.float64) @@ -2965,7 +2898,7 @@ def _Xof(m): dJT[vj, 6 * i:6 * i + 6, vm] += (alpha_j * alpha_m) * dcol return dJT - def f_ext_gradient(self, q, normalize_input=True): + def f_ext_gradient(self, q): """Gradients of dynamics w.r.t. external forces ``f_ext`` (the f_ext column). ``f_ext`` is a per-body spatial wrench in each link's LOCAL frame @@ -2988,18 +2921,17 @@ def f_ext_gradient(self, q, normalize_input=True): (one ``-crm(S_0^{(a)})X[0]`` per root v-slot a). See that method's derivation + FD self-check. """ - if normalize_input: - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() - JT = self.f_ext_jacobian_transpose(q, normalize_input=False) - Minv = self.minv(q, public_output=False, normalize_input=False) + JT = self.f_ext_jacobian_transpose(q) + Minv = self.minv(q) dtau_dfext = -JT dqdd_dfext = Minv @ JT # Analytic dJ^T/dq (closed form, no FD) for fixed AND floating base. - dJT_dq = self.f_ext_jacobian_transpose_dq(q, normalize_input=False) + dJT_dq = self.f_ext_jacobian_transpose_dq(q) return { "dtau_dfext": dtau_dfext, @@ -3172,7 +3104,7 @@ def minv_fpass(self, q, Minv, F, U, Dinv): return Minv - def minv(self, q, output_dense=True, public_output=True, normalize_input=True): + def minv(self, q, output_dense=True): """Compute the inverse of the joint-space inertia matrix. Parameters @@ -3185,8 +3117,7 @@ def minv(self, q, output_dense=True, public_output=True, normalize_input=True): Minv : numpy.ndarray N x N inverse joint-space inertia matrix. """ - if normalize_input: - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) # CRBA-invert fast path. Two cases route here: # (1) MIMIC: the ABA-based recursion below uses per-body (S, U, d) that # diverge by alpha/alpha^2 factors for mimic joints (see @@ -3205,18 +3136,10 @@ def minv(self, q, output_dense=True, public_output=True, normalize_input=True): # behavior is unchanged -- this only ADDS coverage for spherical, # unblocking the fd / SO oracle paths that compose minv. if self._has_mimic_joints() or self._robot_has_multidof_nonfloating_joint(): - # Invert the INTERNAL-order M so the floating-base v reorder is - # applied exactly once (here), not twice: crba's public output - # already denormalizes, and the [3,4,5,0,1,2] root reorder is an - # involution, so inverting the public M and denormalizing again - # would cancel the reorder (legacy output would wrongly come back - # in pinocchio order). See crba(public_output=...). - M = self.crba(q, normalize_input=False, public_output=False) + # Invert the dense M (CRBA and the RNEA bias are mimic- and + # multi-DoF-aware; the ABA recursion below is not). + M = self.crba(q) Minv = np.linalg.inv(M) - if public_output: - return self._denormalize_qv_matrix_output( - Minv, row_space="v", col_space="v" - ) return Minv # based on https://www.researchgate.net/publication/343098270_Analytical_Inverse_of_the_Joint_Space_Inertia_Matrix # backward pass @@ -3233,8 +3156,6 @@ def minv(self, q, output_dense=True, public_output=True, normalize_input=True): if col < row: Minv[row, col] = Minv[col, row] - if public_output: - return self._denormalize_qv_matrix_output(Minv, row_space="v", col_space="v") return Minv @@ -3258,7 +3179,7 @@ def crm(self,v): return vcross - def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81, normalize_input=True): + def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81): """Compute forward dynamics using the Articulated-Body Algorithm. Parameters @@ -3277,10 +3198,9 @@ def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81, normalize_input=True): qdd : numpy.ndarray N-element joint accelerations. """ - if normalize_input: - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - tau = self._normalize_v_input(tau) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + tau = np.array(tau, dtype=np.float64) # Mimic-aware fast path: the ABA recursion below uses per-body # (S, U, d) that diverge from slot-accumulated by alpha/alpha^2 # factors for mimic joints. The += CRBA/RNEA-grad pattern is @@ -3307,14 +3227,10 @@ def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81, normalize_input=True): q, qd, np.zeros(n), GRAVITY=GRAVITY, f_ext=f_ext, - public_output=False, - normalize_input=False, )[0] - Minv = self.minv( - q, output_dense=True, public_output=False, normalize_input=False - ) + Minv = self.minv(q, output_dense=True) qdd = Minv @ (tau - bias) - return self._denormalize_v_output(qdd) + return qdd # Multi-DOF non-floating joint (spherical/planar mid-chain): the in-place # ABA recursion below is written for SCALAR 1-DOF joints (per-body U, d, # q[ind]/qd[ind] scalar indexing). Rather than re-derive the multi-column @@ -3328,15 +3244,10 @@ def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81, normalize_input=True): q, qd, np.zeros(n), GRAVITY=GRAVITY, f_ext=f_ext, - public_output=False, - normalize_input=False, )[0] - # Solve in INTERNAL order (bias/tau are internal here): use the raw - # internal-order M so a floating+multidof robot doesn't mix a - # denormalized M with an internal-order rhs. Single denormalize on qdd. - M = self.crba(q, normalize_input=False, public_output=False) + M = self.crba(q) qdd = np.linalg.solve(M, tau - bias) - return self._denormalize_v_output(qdd) + return qdd # joint-local viscous damping + Coulomb friction reduce the torque # available to accelerate: qdd = Minv*(tau - rnea_bias - dyn_bias). # No-op (zero vector) when no joint declares damping/friction. @@ -3547,34 +3458,25 @@ def aba(self, q, qd, tau, f_ext=[], GRAVITY = -9.81, normalize_input=True): qdd[ind] = temp / d[ind] a[:,ind] = a[:,ind] + qdd[ind]*S.T - return self._denormalize_v_output(qdd) + return qdd - def crba(self, q, normalize_input=True, public_output=True): + def crba(self, q): """Compute the joint-space inertia matrix using the Composite Rigid Body Algorithm. Parameters ---------- q : numpy.ndarray N-element joint positions. - public_output : bool - When True (default) the returned matrix is mapped back to the - user-facing floating-base v convention (the legacy [3,4,5,0,1,2] - root reorder under the legacy convention). Set False to get the - raw INTERNAL-order matrix -- used by callers (e.g. ``minv``'s - mimic path) that invert/solve in internal order and apply their - own single denormalization afterward, so the involution perm is - not applied twice. Returns ------- M : numpy.ndarray N x N joint-space inertia matrix. """ - if normalize_input: - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) if self.robot.floating_base: # Floating-base CRBA via the SAME generalized block machinery as the # fixed-base path below. The free-flyer ROOT (jid 0) is treated as an @@ -3696,8 +3598,6 @@ def crba(self, q, normalize_input=True, public_output=True): H[np.ix_(vj, vi)] += block H[np.ix_(vi, vj)] += block.T - if public_output: - return self._denormalize_qv_matrix_output(H, row_space="v", col_space="v") return H ##### Testing original RNEA_grad to help with CUDA @@ -3967,8 +3867,6 @@ def inverse_dynamics_gradient( qdd = None, GRAVITY = -9.81, f_ext=None, - public_output=True, - normalize_input=True, ): """Compute the gradients of RNEA wrt joint positions and velocities. @@ -3987,11 +3885,10 @@ def inverse_dynamics_gradient( Gradients of generalized forces wrt q and qd. """ - if normalize_input: - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - if qdd is not None: - qdd = self._normalize_v_input(qdd) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + if qdd is not None: + qdd = np.array(qdd, dtype=np.float64) # Key physics: for a CONSTANT local-frame f_ext there is NO new # gradient term. f_ext enters only as a constant offset to the # per-body force f (f[:,i] -= f_ext[i]); since it is q/qd-independent @@ -4005,8 +3902,6 @@ def inverse_dynamics_gradient( qdd, GRAVITY, f_ext=f_ext, - public_output=False, - normalize_input=False, ) # forward pass, dq @@ -4023,12 +3918,10 @@ def inverse_dynamics_gradient( # forward_dynamics_gradient inherits it with no extra plumbing). dc_dqd = self.inverse_dynamics_gradient_bpass_dqd(q, df_dqd, qd=qd) - if public_output: - return self._denormalize_inverse_dynamics_gradient_output(dc_dq, dc_dqd) return np.hstack((dc_dq, dc_dqd)) - def forward_dynamics(self, q, qd, u, f_ext=None, public_output=True, normalize_input=True): + def forward_dynamics(self, q, qd, u, f_ext=None): """Compute the joint accelerations for the given state and torques. Parameters @@ -4045,18 +3938,15 @@ def forward_dynamics(self, q, qd, u, f_ext=None, public_output=True, normalize_i qdd : numpy.ndarray N-element joint accelerations. """ - if normalize_input: - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - u = self._normalize_v_input(u) - (c,v,a,f) = self.inverse_dynamics(q, qd, f_ext=f_ext, public_output=False, normalize_input=False) - minv = self.minv(q, public_output=False, normalize_input=False) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + u = np.array(u, dtype=np.float64) + (c,v,a,f) = self.inverse_dynamics(q, qd, f_ext=f_ext) + minv = self.minv(q) qdd = np.matmul(minv, u - c) - if public_output: - return self._denormalize_v_output(qdd) return qdd - def forward_dynamics_gradient(self, q, qd, u, f_ext=None, normalize_input=True): + def forward_dynamics_gradient(self, q, qd, u, f_ext=None): """Compute the gradients of the forward dynamics. Parameters @@ -4073,25 +3963,24 @@ def forward_dynamics_gradient(self, q, qd, u, f_ext=None, normalize_input=True): (qdd_dq, qdd_dqd) : tuple Gradients of joint accelerations wrt q and qd. """ - if normalize_input: - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - u = self._normalize_v_input(u) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + u = np.array(u, dtype=np.float64) # f_ext flows into qdd (via forward_dynamics) and into dc_du (via # inverse_dynamics_gradient's f_ext-corrected f). For a constant local-frame f_ext # there is no additional fd-gradient term: minv is f_ext-independent # and the only f_ext dependence is through the qdd/dc_du arguments # already threaded here. - qdd = self.forward_dynamics(q, qd, u, f_ext=f_ext, public_output=False, normalize_input=False) - dc_du = self.inverse_dynamics_gradient(q, qd, qdd, f_ext=f_ext, public_output=False, normalize_input=False) + qdd = self.forward_dynamics(q, qd, u, f_ext=f_ext) + dc_du = self.inverse_dynamics_gradient(q, qd, qdd, f_ext=f_ext) dc_dq, dc_dqd = np.hsplit(dc_du, [len(qd)]) - minv = self.minv(q, public_output=False, normalize_input=False) + minv = self.minv(q) qdd_dq = np.matmul(-minv, dc_dq) qdd_dqd = np.matmul(-minv, dc_dqd) return ( - self._denormalize_reduced_q_matrix_output(qdd_dq, row_space="v"), - self._denormalize_qv_matrix_output(qdd_dqd, row_space="v", col_space="v"), + qdd_dq, + qdd_dqd, ) @staticmethod @@ -4242,8 +4131,7 @@ def _floating_gravity_d2tau_dq_lie_direct(self, q, GRAVITY=-9.81): if not self.robot.floating_base: raise ValueError("_floating_gravity_d2tau_dq_lie_direct requires a floating-base robot.") - q = self._normalize_q_input(q) - q = np.asarray(q, dtype=np.float64).copy() + q = np.array(q, dtype=np.float64) if self.robot.using_quaternion: q[3:7] = self._normalize_xyzw_quaternion(q[3:7]) @@ -4705,8 +4593,7 @@ def idsva_so_world_frame(self, q, qd, qdd, GRAVITY=-9.81): Bloated by design (~5× the inner work of `idsva_so`'s subtree-broadcast form). Not on any hot path; not GPU-friendly. """ - q = self._normalize_q_input(q) - q = np.asarray(q, dtype=np.float64).copy() + q = np.array(q, dtype=np.float64) if self.robot.floating_base and self.robot.using_quaternion: q[3:7] = self._normalize_xyzw_quaternion(q[3:7]) @@ -5015,8 +4902,8 @@ def kinetic_energy_regressor(self, q, qd): """ from ._regressor import _BASIS_I - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() v, _a, _f = self.inverse_dynamics_fpass(q, qd, np.zeros(nv, dtype=np.float64)) diff --git a/README.md b/README.md index 783be2a..929d44d 100644 --- a/README.md +++ b/README.md @@ -181,9 +181,8 @@ The Pinocchio free-flyer convention is native at the API boundary: tangent is ordered `[linear; angular]` (`[vx, vy, vz, wx, wy, wz]`). A floating-base model without additional spherical joints has `NQ = NV + 1`; each additional spherical joint contributes another quaternion coordinate. -Velocity and force inputs are tangent-width (`NV`). The parser also provides -a legacy convention; new integrations should use its default `pinocchio` -convention explicitly. +Velocity and force inputs are tangent-width (`NV`). This is the only layout +the parser and the reference expose. ## Installation diff --git a/_energy.py b/_energy.py index 436e715..3a194d1 100644 --- a/_energy.py +++ b/_energy.py @@ -88,7 +88,7 @@ def coriolis_matrix(self, q, qd, GRAVITY=-9.81): root permutation is applied here (unlike CRBA, which assembles the root block with S = I and reorders by hand). """ - q = self._normalize_q_input(q) + q = np.array(q, dtype=np.float64) qd = np.asarray(qd, dtype=np.float64).reshape(-1) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() @@ -193,7 +193,5 @@ def ancestors(i): # [omega; v_lin] permutation block) already maps the user-facing root # velocity ordering into the internal spatial ordering, so unlike CRBA # (which assembles the root block with S = I and reorders by hand) no - # manual root permutation is needed here. Under the pinocchio convention - # _denormalize_qv_matrix_output is a no-op; it is kept for the legacy - # convention's v-permutation. - return self._denormalize_qv_matrix_output(C, row_space="v", col_space="v") + # manual root permutation is needed here. + return C diff --git a/_plant.py b/_plant.py index 97fd6ac..99875b1 100644 --- a/_plant.py +++ b/_plant.py @@ -356,6 +356,42 @@ def quadratic_state_cost_tangent(self, x, x_des, Q, gauss_newton=True): hess[:nv, :nv] += np.einsum("b,bik->ik", grad_q, T0) return value, grad, hess + def state_cost_tangent(self, x, x_des, Q, activation=0, eps=0.0, lo=None, hi=None): + """Activated state cost on the tangent residual of a floating-base state, + the oracle of grid_plant::state_cost_tangent (the state term of the + floating-base tracking_cost_menu). + + x = [q (nq); qd (nv)]. The residual is tangent-width 2 nv: + r = [difference(q_des, q) (nv); qd - qd_des (nv)] + with the diagonal weight Q (2 nv). The barrier kinds read the feature + value v = [none (6); q[7:]; qd] with 2 nv-wide bounds lo / hi indexed + like r; the first six entries (the base pose) carry no bound and are + ignored. Returns (value, grad (2 nv,), GN hess (2 nv, 2 nv)) in the + tangent chart, the derivatives of delta -> cost(integrate(q, delta_q), + qd + delta_v) at delta = 0 with the exact J_diff on the q block. The + quadratic activation reproduces quadratic_state_cost_tangent. + """ + nq = self.robot.get_num_pos() + nv = self.robot.get_num_vel() + x = np.asarray(x, dtype=np.float64).reshape(-1) + x_des = np.asarray(x_des, dtype=np.float64).reshape(-1) + Q = np.asarray(Q, dtype=np.float64).reshape(-1) + q, qd = x[:nq], x[nq:nq + nv] + q_des, qd_des = x_des[:nq], x_des[nq:nq + nv] + eq = self.difference(q_des, q) + Jq = self.dDifference(q_des, q, "to") + r = np.concatenate([eq, qd - qd_des]) + v = np.concatenate([np.zeros(6), q[7:], qd]) + lo_t = np.full(2 * nv, -np.inf) if lo is None else np.asarray(lo, dtype=np.float64).reshape(-1).copy() + hi_t = np.full(2 * nv, np.inf) if hi is None else np.asarray(hi, dtype=np.float64).reshape(-1).copy() + lo_t[:6] = -np.inf + hi_t[:6] = np.inf + value, slope, curv = self._activation(activation, Q, r, eps, v, lo_t, hi_t) + Jx = np.zeros((2 * nv, 2 * nv)) + Jx[:nv, :nv] = Jq + Jx[nv:, nv:] = np.eye(nv) + return float(np.sum(value)), Jx.T @ slope, Jx.T @ (curv[:, None] * Jx) + # ----- end-effector position cost (value, grad_x, GN hess_x) ----- def _ee_target_name(self, ee): @@ -416,6 +452,7 @@ def gravity_torque(self, q, GRAVITY=-9.81): ACTIVATION_RELAXED_LOG_BARRIER = 2 ACTIVATION_QUADRATIC_BARRIER = 3 + ACTIVATION_SMOOTH_HINGE = 4 @staticmethod def _relaxed_log_barrier_1d(z, mu, delta): @@ -447,6 +484,10 @@ def _activation(kind, W, r, eps, v=None, lo=None, hi=None): w = mu and eps = delta: B(v - lo) + B(hi - v) where B is -mu log z above delta and its quadratic extension below; an infinite bound is zero. kind 3 (quadratic barrier): 1/2 w (max(0, lo - v)^2 + max(0, v - hi)^2). + kind 4 (smooth hinge): w (sqrt(d^2 + eps^2) - eps) on the bound violation + d = max(0, lo - v) + max(0, v - hi), eps > 0: zero inside the bounds, + pseudo-Huber in the violation; the curvature is 0 inside and + w eps^2 / s^3 outside (w / eps just past a bound). The barrier kinds read v, lo, hi; r is the residual the other kinds use. Returns (value (M,), slope (M,), curvature (M,)), the derivatives with respect to r (equal to those with respect to v: v = r + target). @@ -461,7 +502,8 @@ def _activation(kind, W, r, eps, v=None, lo=None, hi=None): raise ValueError("the pseudo-Huber activation needs eps > 0") s = np.sqrt(r * r + eps * eps) return W * (s - eps), W * r / s, W * eps * eps / (s * s * s) - if kind in (_PlantMixin.ACTIVATION_RELAXED_LOG_BARRIER, _PlantMixin.ACTIVATION_QUADRATIC_BARRIER): + if kind in (_PlantMixin.ACTIVATION_RELAXED_LOG_BARRIER, _PlantMixin.ACTIVATION_QUADRATIC_BARRIER, + _PlantMixin.ACTIVATION_SMOOTH_HINGE): if v is None or lo is None or hi is None: raise ValueError("barrier activations need the feature value v and the bounds lo, hi") v = np.asarray(v, dtype=np.float64).reshape(-1) @@ -474,251 +516,16 @@ def _activation(kind, W, r, eps, v=None, lo=None, hi=None): slope = W * (above - below) curv = W * ((below > 0) | (above > 0)).astype(np.float64) return value, slope, curv - delta = float(eps) - if not delta > 0.0: - raise ValueError("the relaxed log barrier needs delta = eps > 0") - value = np.zeros_like(r); slope = np.zeros_like(r); curv = np.zeros_like(r) - for k in range(r.size): - mu = W[k] - if np.isfinite(lo[k]): - val, d1, d2 = _PlantMixin._relaxed_log_barrier_1d(np.array([v[k] - lo[k]]), mu, delta) - value[k] += val[0]; slope[k] += d1[0]; curv[k] += d2[0] - if np.isfinite(hi[k]): - val, d1, d2 = _PlantMixin._relaxed_log_barrier_1d(np.array([hi[k] - v[k]]), mu, delta) - value[k] += val[0]; slope[k] -= d1[0]; curv[k] += d2[0] - return value, slope, curv - raise ValueError(f"unknown activation kind {kind!r}") - - @staticmethod - def _skew3(v): - return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) - - @staticmethod - def _so3_log(R): - """phi = log(R) on the canonical branch |phi| <= pi (3-vector).""" - R = np.asarray(R, dtype=np.float64) - c = np.clip((np.trace(R) - 1.0) * 0.5, -1.0, 1.0) - theta = np.arccos(c) - w = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) - if theta < 1e-6: - return 0.5 * w - if theta < np.pi - 1e-4: - return theta / (2.0 * np.sin(theta)) * w - # near pi: axis from the symmetric part (R + I)/2 = a a^T, sign from w - S = 0.5 * (R + np.eye(3)) - k = int(np.argmax(np.diag(S))) - a = S[:, k] / np.sqrt(max(S[k, k], 1e-18)) - a = a / np.linalg.norm(a) - if np.dot(a, w) < 0.0: - a = -a - return theta * a - - @staticmethod - def _so3_right_jacobian_inv(phi): - """Jr(phi)^-1 = I + 1/2 [phi]x + (1/theta^2 - (1 + cos theta)/(2 theta sin theta)) [phi]x^2.""" - phi = np.asarray(phi, dtype=np.float64) - theta = np.linalg.norm(phi) - S = _PlantMixin._skew3(phi) - if theta < 1e-5: - coef = 1.0 / 12.0 - else: - coef = 1.0 / (theta * theta) - (1.0 + np.cos(theta)) / (2.0 * theta * np.sin(theta)) - return np.eye(3) + 0.5 * S + coef * (S @ S) - - def _frame_placement(self, q, frame_name): - """(R_f, p_f, target_id) of a named articulated or fixed joint frame, the - fixed joint's offset included (the frame_jacobian resolution).""" - Xw, _ = self._frame_world_placement_and_chain(q) - target_id, X_off = self._resolve_frame_joint(frame_name) - if target_id == -1: - X = X_off - else: - X = Xw[target_id] @ X_off - return X[:3, :3], X[:3, 3], target_id - - def frame_position_cost(self, q, frame_name, p_des, W, activation=0, eps=0.0, lo=None, hi=None): - """Position residual r = p_f(q) - p_des of a named frame under an activation. - Returns (value, grad_x (nx,) = [J_p^T a'(r); 0], GN hess_x (nx, nx)).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv - R_f, p_f, _ = self._frame_placement(q, frame_name) - Jp = np.asarray(self.frame_jacobian(q, frame_name, "LOCAL_WORLD_ALIGNED"))[:3, :] - value, slope, curv = self._activation(activation, W, p_f - np.asarray(p_des, dtype=np.float64).reshape(-1), eps, p_f, lo, hi) - grad_x = np.zeros(nx); grad_x[:nv] = Jp.T @ slope - hess_x = np.zeros((nx, nx)); hess_x[:nv, :nv] = Jp.T @ (curv[:, None] * Jp) - return float(np.sum(value)), grad_x, hess_x - - def frame_orientation_cost(self, q, frame_name, R_des, W, activation=0, eps=0.0): - """Log-map orientation residual e = log(R_des^T R_f(q)) (3-vector) under an - activation. With the frame's body-axes angular Jacobian J_w (LOCAL rows 3..5) - the residual Jacobian is J_e = Jr(e)^-1 J_w. Returns (value, grad_x, GN hess_x).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv - R_f, _, _ = self._frame_placement(q, frame_name) - R_des = np.asarray(R_des, dtype=np.float64).reshape(3, 3, order="F") - e = self._so3_log(R_des.T @ R_f) - Jw = np.asarray(self.frame_jacobian(q, frame_name, "LOCAL"))[3:, :] - Je = self._so3_right_jacobian_inv(e) @ Jw - value, slope, curv = self._activation(activation, W, e, eps) - grad_x = np.zeros(nx); grad_x[:nv] = Je.T @ slope - hess_x = np.zeros((nx, nx)); hess_x[:nv, :nv] = Je.T @ (curv[:, None] * Je) - return float(np.sum(value)), grad_x, hess_x - - def _frame_velocity_q_derivative(self, q, qd, frame_name): - """d(J(q) qd)/dq for the LOCAL_WORLD_ALIGNED frame Jacobian (6 x nv), from - the same world-axis column construction frame_jacobian uses. Fixed base.""" - if self.robot.floating_base: - raise NotImplementedError("frame velocity residual: fixed base only in this release") - nv = self.robot.get_num_vel() - Xw, _ = self._frame_world_placement_and_chain(q) - target_id, X_off = self._resolve_frame_joint(frame_name) - p_f = (Xw[target_id] @ X_off)[:3, 3] - chain = sorted(self.robot.get_ancestors_by_id(target_id)) + [target_id] - - def column_info(j): - S = np.asarray(self.robot.get_S_by_id(j), dtype=np.float64) - if S.ndim == 1: - S = S.reshape(-1, 1) - R_j = Xw[j][:3, :3]; p_j = Xw[j][:3, 3] - inds = self.robot.get_joint_index_v(j) - inds = list(inds) if isinstance(inds, (list, tuple, np.ndarray)) else [inds] - joint = self.robot.get_joint_by_id(j) - scale = joint.get_mimic_multiplier() if getattr(joint, "is_mimic", False) else 1.0 - out = [] - for c in range(S.shape[1]): - vi = inds[c] if c < len(inds) else inds[-1] - rot = np.linalg.norm(S[:3, c]) > 0.5 - axis = R_j @ (S[:3, c] if rot else S[3:6, c]) - out.append((vi, rot, axis, p_j, scale)) - return out - - cols = {j: column_info(j) for j in chain} - dJqd = np.zeros((6, nv)) - qd = np.asarray(qd, dtype=np.float64).reshape(-1) - # Column j is a_j x (p_f - p_j) (revolute) or l_j (prismatic) over its axis - # a_j / l_j and origin p_j. A joint m moves, rigidly, everything downstream - # of it: for m an ancestor-or-self of j that is the axis, the origin and the - # frame point; for m downstream of j only the frame point p_f moves. - for j in chain: - for (vi, rot, a_j, p_j, s_j) in cols[j]: - w = s_j * qd[vi] - for m in chain: - for (vm, rot_m, a_m, p_m, s_m) in cols[m]: - if m <= j: - if rot and rot_m: - d_axis = np.cross(a_m, a_j) - d_lin = np.cross(d_axis, p_f - p_j) + np.cross(a_j, np.cross(a_m, p_f - p_j)) - dJqd[:3, vm] += s_m * w * d_lin - dJqd[3:, vm] += s_m * w * d_axis - elif (not rot) and rot_m: - dJqd[:3, vm] += s_m * w * np.cross(a_m, a_j) - # a prismatic m shifts the frame and the joint origins together: no change - elif rot: - # m downstream of j: only p_f moves, by a_m x (p_f - p_m) or l_m - d_pf = np.cross(a_m, p_f - p_m) if rot_m else a_m - dJqd[:3, vm] += s_m * w * np.cross(a_j, d_pf) - return dJqd - - def frame_velocity_cost(self, q, qd, frame_name, v_des, W, activation=0, eps=0.0, lo=None, hi=None): - """Frame velocity residual r = J(q) qd - v_des over the LOCAL_WORLD_ALIGNED - twist [linear; angular] (6) under an activation. The residual Jacobian over - x = [q; qd] is [d(J qd)/dq | J]. Returns (value, grad_x (nx,), GN hess_x).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv - J = np.asarray(self.frame_jacobian(q, frame_name, "LOCAL_WORLD_ALIGNED")) - qd = np.asarray(qd, dtype=np.float64).reshape(-1) - r = J @ qd - np.asarray(v_des, dtype=np.float64).reshape(-1) - Jx = np.hstack([self._frame_velocity_q_derivative(q, qd, frame_name), J]) - value, slope, curv = self._activation(activation, W, r, eps, J @ qd, lo, hi) - return float(np.sum(value)), Jx.T @ slope, Jx.T @ (curv[:, None] * Jx) - - @classmethod - def _activated_cost(cls, var, des, W, activation, eps, lo=None, hi=None): - var = np.asarray(var, dtype=np.float64).reshape(-1) - des = np.asarray(des, dtype=np.float64).reshape(-1) - value, slope, curv = cls._activation(activation, W, var - des, eps, var, lo, hi) - return float(np.sum(value)), slope, np.diag(curv) - - def state_cost(self, x, x_des, Q, activation=0, eps=0.0, lo=None, hi=None): - """sum_i a(x_i - x_des_i; Q_i, eps) over x = [q; qd]. The quadratic - activation is quadratic_state_cost; lo / hi are the barrier bounds on x. - Returns (value, grad (nx,), GN hess (nx, nx)).""" - return self._activated_cost(x, x_des, Q, activation, eps, lo, hi) - - def input_cost(self, u, u_des, R, activation=0, eps=0.0, lo=None, hi=None): - """sum_i a(u_i - u_des_i; R_i, eps). Returns (value, grad (nu,), GN hess (nu, nu)).""" - return self._activated_cost(u, u_des, R, activation, eps, lo, hi) - - def linear_feature_cost(self, v, A, b, target, W, activation=0, eps=0.0, lo=None, hi=None): - """sum_m a(r_m; W_m, eps) with r = A v + b - target, A of shape (M, N); lo / hi - are barrier bounds on the feature value A v + b. - - Returns (value, grad (N,) = A^T a'(r), GN hess (N, N) = A^T diag(a''(r)) A). - """ - A = np.asarray(A, dtype=np.float64) - v = np.asarray(v, dtype=np.float64).reshape(-1) - f = A @ v + np.asarray(b, dtype=np.float64).reshape(-1) - r = f - np.asarray(target, dtype=np.float64).reshape(-1) - value, slope, curv = self._activation(activation, W, r, eps, f, lo, hi) - return float(np.sum(value)), A.T @ slope, A.T @ (curv[:, None] * A) - - ACTIVATION_RELAXED_LOG_BARRIER = 2 - ACTIVATION_QUADRATIC_BARRIER = 3 - - @staticmethod - def _relaxed_log_barrier_1d(z, mu, delta): - """Grandia et al. relaxed log barrier on z >= 0: -mu log z above delta, the - quadratic extension below it (value and slope continuous at delta). - Returns (value, d/dz, d2/dz2) arrays. z may be +inf (no bound): zero.""" - z = np.asarray(z, dtype=np.float64) - val = np.zeros_like(z); d1 = np.zeros_like(z); d2 = np.zeros_like(z) - finite = np.isfinite(z) - above = finite & (z > delta) - below = finite & ~(z > delta) - val[above] = -mu * np.log(z[above]) - d1[above] = -mu / z[above] - d2[above] = mu / (z[above] ** 2) - t = (z[below] - 2.0 * delta) / delta - val[below] = mu * (0.5 * t * t - 0.5 - np.log(delta)) - d1[below] = mu * t / delta - d2[below] = mu / (delta * delta) * np.ones_like(t) - return val, d1, d2 - - @staticmethod - def _activation(kind, W, r, eps, v=None, lo=None, hi=None): - """Per-entry activation a(r; w, eps) with its first and second derivatives. - - kind 0 (quadratic): a = 1/2 w r^2, a' = w r, a'' = w. - kind 1 (pseudo-Huber): a = w (sqrt(r^2 + eps^2) - eps), a' = w r / s, - a'' = w eps^2 / s^3 with s = sqrt(r^2 + eps^2); eps must be positive. - kind 2 (relaxed log barrier): on the feature value v in [lo, hi], with - w = mu and eps = delta: B(v - lo) + B(hi - v) where B is -mu log z above - delta and its quadratic extension below; an infinite bound is zero. - kind 3 (quadratic barrier): 1/2 w (max(0, lo - v)^2 + max(0, v - hi)^2). - The barrier kinds read v, lo, hi; r is the residual the other kinds use. - Returns (value (M,), slope (M,), curvature (M,)), the derivatives with - respect to r (equal to those with respect to v: v = r + target). - """ - W = np.asarray(W, dtype=np.float64).reshape(-1) - r = np.asarray(r, dtype=np.float64).reshape(-1) - if kind == _PlantMixin.ACTIVATION_QUADRATIC: - return 0.5 * W * r * r, W * r, W.copy() - if kind == _PlantMixin.ACTIVATION_PSEUDO_HUBER: - eps = float(eps) - if not eps > 0.0: - raise ValueError("the pseudo-Huber activation needs eps > 0") - s = np.sqrt(r * r + eps * eps) - return W * (s - eps), W * r / s, W * eps * eps / (s * s * s) - if kind in (_PlantMixin.ACTIVATION_RELAXED_LOG_BARRIER, _PlantMixin.ACTIVATION_QUADRATIC_BARRIER): - if v is None or lo is None or hi is None: - raise ValueError("barrier activations need the feature value v and the bounds lo, hi") - v = np.asarray(v, dtype=np.float64).reshape(-1) - lo = np.asarray(lo, dtype=np.float64).reshape(-1) - hi = np.asarray(hi, dtype=np.float64).reshape(-1) - if kind == _PlantMixin.ACTIVATION_QUADRATIC_BARRIER: + if kind == _PlantMixin.ACTIVATION_SMOOTH_HINGE: + eps = float(eps) + if not eps > 0.0: + raise ValueError("the smooth hinge activation needs eps > 0") below = np.where(np.isfinite(lo), np.maximum(0.0, lo - v), 0.0) above = np.where(np.isfinite(hi), np.maximum(0.0, v - hi), 0.0) - value = 0.5 * W * (below * below + above * above) - slope = W * (above - below) - curv = W * ((below > 0) | (above > 0)).astype(np.float64) - return value, slope, curv + d = below + above + s = np.sqrt(d * d + eps * eps) + active = (d > 0.0).astype(np.float64) + return W * (s - eps), W * (above - below) / s, active * W * eps * eps / (s * s * s) delta = float(eps) if not delta > 0.0: raise ValueError("the relaxed log barrier needs delta = eps > 0") @@ -783,8 +590,9 @@ def _frame_placement(self, q, frame_name): def frame_position_cost(self, q, frame_name, p_des, W, activation=0, eps=0.0, lo=None, hi=None): """Position residual r = p_f(q) - p_des of a named frame under an activation. - Returns (value, grad_x (nx,) = [J_p^T a'(r); 0], GN hess_x (nx, nx)).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv + Returns (value, grad (2 nv,) = [J_p^T a'(r); 0], GN hess (2 nv, 2 nv)) over the + tangent state (the base twist, then the joint rates; x = [q; qd] on a fixed base).""" + nv = self.robot.get_num_vel(); nx = 2 * nv R_f, p_f, _ = self._frame_placement(q, frame_name) Jp = np.asarray(self.frame_jacobian(q, frame_name, "LOCAL_WORLD_ALIGNED"))[:3, :] value, slope, curv = self._activation(activation, W, p_f - np.asarray(p_des, dtype=np.float64).reshape(-1), eps, p_f, lo, hi) @@ -795,8 +603,9 @@ def frame_position_cost(self, q, frame_name, p_des, W, activation=0, eps=0.0, lo def frame_orientation_cost(self, q, frame_name, R_des, W, activation=0, eps=0.0): """Log-map orientation residual e = log(R_des^T R_f(q)) (3-vector) under an activation. With the frame's body-axes angular Jacobian J_w (LOCAL rows 3..5) - the residual Jacobian is J_e = Jr(e)^-1 J_w. Returns (value, grad_x, GN hess_x).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv + the residual Jacobian is J_e = Jr(e)^-1 J_w. Returns (value, grad (2 nv,), GN + hess (2 nv, 2 nv)) over the tangent state.""" + nv = self.robot.get_num_vel(); nx = 2 * nv R_f, _, _ = self._frame_placement(q, frame_name) R_des = np.asarray(R_des, dtype=np.float64).reshape(3, 3, order="F") e = self._so3_log(R_des.T @ R_f) @@ -808,10 +617,12 @@ def frame_orientation_cost(self, q, frame_name, R_des, W, activation=0, eps=0.0) return float(np.sum(value)), grad_x, hess_x def _frame_velocity_q_derivative(self, q, qd, frame_name): - """d(J(q) qd)/dq for the LOCAL_WORLD_ALIGNED frame Jacobian (6 x nv), from - the same world-axis column construction frame_jacobian uses. Fixed base.""" - if self.robot.floating_base: - raise NotImplementedError("frame velocity residual: fixed base only in this release") + """d(J(q) qd)/dv for the LOCAL_WORLD_ALIGNED frame Jacobian (6 x nv): the + derivative along the tangent (pinocchio layout: the base twist in the base + frame, then the joint rates), from the same world-axis column construction + frame_jacobian uses. The floating root enters as six columns (three + translations, three rotations about the base origin, axes in the base + frame) and obeys the same rigid-motion rules as a joint.""" nv = self.robot.get_num_vel() Xw, _ = self._frame_world_placement_and_chain(q) target_id, X_off = self._resolve_frame_joint(frame_name) @@ -865,8 +676,7 @@ def column_info(j): def frame_velocity_cost(self, q, qd, frame_name, v_des, W, activation=0, eps=0.0, lo=None, hi=None): """Frame velocity residual r = J(q) qd - v_des over the LOCAL_WORLD_ALIGNED twist [linear; angular] (6) under an activation. The residual Jacobian over - x = [q; qd] is [d(J qd)/dq | J]. Returns (value, grad_x (nx,), GN hess_x).""" - nq = self.robot.get_num_pos(); nv = self.robot.get_num_vel(); nx = nq + nv + the tangent state is [d(J qd)/dv | J]. Returns (value, grad (2 nv,), GN hess (2 nv, 2 nv)).""" J = np.asarray(self.frame_jacobian(q, frame_name, "LOCAL_WORLD_ALIGNED")) qd = np.asarray(qd, dtype=np.float64).reshape(-1) r = J @ qd - np.asarray(v_des, dtype=np.float64).reshape(-1) diff --git a/_regressor.py b/_regressor.py index e0fe111..6f50231 100644 --- a/_regressor.py +++ b/_regressor.py @@ -107,9 +107,9 @@ def inverse_dynamics_regressor(self, q, qd, qdd, GRAVITY=-9.81): S^T projection onto each ancestor DOF). The link param blocks are ordered by body id (column block i = link i's 10 params). """ - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - qdd = self._normalize_v_input(qdd) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + qdd = np.array(qdd, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() @@ -150,7 +150,7 @@ def inverse_dynamics_regressor(self, q, qd, qdd, GRAVITY=-9.81): ) Fblk[parent_id] = Fblk[parent_id] + Xmat.T @ Fblk[curr_id] - return self._denormalize_reduced_q_matrix_output(Y, row_space="v") + return Y def body_regressor_gradient_col(self, v, a, dv, da): """6x10 derivative of the body regressor along ONE state direction. @@ -191,9 +191,9 @@ def inverse_dynamics_regressor_gradient(self, q, qd, qdd, GRAVITY=-9.81): the RNEA gradient bpass does for its force RHS (fxS idiom), here with the 6 x 10*NB block RHS instead of a force vector. """ - q = self._normalize_q_input(q) - qd = self._normalize_v_input(qd) - qdd = self._normalize_v_input(qdd) + q = np.array(q, dtype=np.float64) + qd = np.array(qd, dtype=np.float64) + qdd = np.array(qdd, dtype=np.float64) NB = self.robot.get_num_bodies() nv = self.robot.get_num_vel() W = 10 * NB @@ -256,10 +256,10 @@ def inverse_dynamics_regressor_gradient(self, q, qd, qdd, GRAVITY=-9.81): Fblk[parent_id] += XT @ Fblk[curr_id] dY_dq = np.stack([ - self._denormalize_reduced_q_matrix_output(dY_dq[c], row_space="v") + dY_dq[c] for c in range(nv)]) dY_dqd = np.stack([ - self._denormalize_reduced_q_matrix_output(dY_dqd[c], row_space="v") + dY_dqd[c] for c in range(nv)]) return dY_dq, dY_dqd diff --git a/equivalents/pinocchio_backend.py b/equivalents/pinocchio_backend.py index d34dcef..81f4468 100644 --- a/equivalents/pinocchio_backend.py +++ b/equivalents/pinocchio_backend.py @@ -204,11 +204,10 @@ def _reduce_pin_matrix_to_project(self, matrix, axes_to_reduce): Each `axes_to_reduce` entry is `(axis, space)` with `space` in {"q", "v"}. The function FIRST handles continuous-joint reduction - (legacy path via `reduce_pinocchio_q_jacobian_to_project` when the - axis is "q" and the pin model uses [cos, sin] expansions), THEN - folds mimic columns/rows into the mimicked column/row with the URDF - multiplier scaling. Inputs without mimic joints fall through to the - legacy code path so existing test behavior is bit-identical. + (via `reduce_pinocchio_q_jacobian_to_project` when the axis is "q" + and the pin model uses [cos, sin] expansions), THEN folds mimic + columns/rows into the mimicked column/row with the URDF multiplier + scaling. Inputs without mimic joints return after the first step. """ arr = np.asarray(matrix, dtype=np.float64) if self.mimic_info is None or self.mimic_info.is_empty(): @@ -1051,9 +1050,8 @@ def forward_dynamics_gradient(self, q, qd, u): # ----- Time integrators (canonical via pinocchio.integrate / dIntegrate) ----- def _to_pin_q(self, q): - # Mimic-aware project -> pin q expansion. Falls through to the - # legacy path when the model has no mimic joints (mimic_info empty), - # so existing tests are bit-identical for non-mimic robots. + # Mimic-aware project -> pin q expansion (an identity on the joint + # block when the model has no mimic joints). return self._expand_project_q_to_pin_full(q) def _from_pin_q_collapse(self, q_pin): @@ -1171,8 +1169,8 @@ def _pin_integrate(self, q, v_dt): ) if self.base_mode == "floating": q_new_pin = normalize_pin_compatible_quaternion(q_new_pin) - # Use mimic-aware collapse if mimic_info is non-empty; otherwise the - # legacy path is bit-identical for non-mimic robots. + # Mimic-aware collapse when mimic_info is non-empty; the plain + # collapse otherwise. if self.mimic_info is not None and not self.mimic_info.is_empty(): return self._from_pin_q_collapse(q_new_pin) return collapse_pin_q_to_project( @@ -1581,35 +1579,71 @@ def _normalize_project_q_for_pose_differences(self, q): q[:7] = normalize_pin_compatible_quaternion(q[:7]) return q - def end_effector_pose_gradient(self, q, target_name: str, offset=None, step: float = 1e-6): - """End-effector pose gradient w.r.t. generalized velocity v (TANGENT space). + def _frame_lwa_jacobian(self, q, target_name: str): + """Local-world-aligned 6 x nv_pin Jacobian of the target frame at ``q`` + (rows [linear; angular], columns in pinocchio's tangent layout) and the + frame placement, both from pinocchio.""" + import pinocchio as pin - Output shape is 6 x nv_project — i.e. one column per project velocity - DOF, with mimic joints already folded into their target's column via - `_pin_integrate` (which expands `v_dt` to pin layout under the URDF - mimic relation, so perturbing v_project[i] moves BOTH the mimicked - joint and every joint that mimics it). For non-mimic robots - `nv_project == self.model.nv` and this is bit-identical to the - legacy FD-over-pin-nv path. - """ + q_pin = self._to_pin_q(q) + pin.computeJointJacobians(self.model, self.data, q_pin) + pin.updateFramePlacements(self.model, self.data) + frame_id = self._resolve_frame_id(target_name) + jac = np.asarray( + pin.getFrameJacobian(self.model, self.data, frame_id, pin.LOCAL_WORLD_ALIGNED), + dtype=np.float64, + ) + return jac, self.data.oMf[frame_id] + + @staticmethod + def _rpy_rate_map_inverse(rpy): + """E(rpy)^-1 for R = Rz(yaw) Ry(pitch) Rx(roll), where the world angular + velocity is omega = E(rpy) d(rpy)/dt with columns + E = [Rz(yaw) Ry(pitch) e_x, Rz(yaw) e_y, e_z]. Singular at pitch = +-pi/2, + where the roll-pitch-yaw pose itself is.""" + _roll, pitch, yaw = (float(v) for v in rpy) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + return np.array([ + [cy / cp, sy / cp, 0.0], + [-sy, cy, 0.0], + [cy * sp / cp, sy * sp / cp, 1.0], + ], dtype=np.float64) + + def end_effector_pose_gradient(self, q, target_name: str, offset=None): + """End-effector pose gradient d(pose)/dv (TANGENT space), 6 x nv_project. + + Analytic, from pinocchio's local-world-aligned frame Jacobian: the + position rows are the Jacobian of the offset point (frame-origin + velocity minus the offset lever arm times the angular rows) and the + orientation rows map the world angular velocity to roll-pitch-yaw + rates through E(rpy)^-1. Mimic columns are folded into their target's + column with the URDF multiplier, so the layout matches the project + adapter's. The tangent v is pinocchio's (free-flyer twist in the base + frame, then the joint rates), the same v that `_pin_integrate` flows.""" if offset is None: offset = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) q = self._normalize_project_q_for_pose_differences(q) - nv_project = self._project_nv() - gradient = np.zeros((6, nv_project), dtype=np.float64) - - for v_ind in range(nv_project): - v = np.zeros(nv_project, dtype=np.float64) - v[v_ind] = step - q_pos = self._pin_integrate(q, v) - q_neg = self._pin_integrate(q, -v) - pose_pos = self.end_effector_pose(q_pos, target_name, offset=offset) - pose_neg = self.end_effector_pose(q_neg, target_name, offset=offset) - diff = pose_pos - pose_neg - # angle-wrap the rpy rows so finite differences are sane near branch cuts - diff[3:6] = ((diff[3:6] + np.pi) % (2.0 * np.pi)) - np.pi - gradient[:, v_ind] = diff / (2.0 * step) - return gradient + jac, placement = self._frame_lwa_jacobian(q, target_name) + off = np.asarray(offset, dtype=np.float64) + if off.shape == (4, 4): + point_local = off[:3, 3] + else: + point_local = off.reshape(-1)[:3] + lever = np.asarray(placement.rotation, dtype=np.float64) @ point_local + skew = np.array([ + [0.0, -lever[2], lever[1]], + [lever[2], 0.0, -lever[0]], + [-lever[1], lever[0], 0.0], + ]) + rpy = self.end_effector_pose(q, target_name, offset=offset)[3:6] + gradient = np.vstack(( + jac[:3] - skew @ jac[3:], + self._rpy_rate_map_inverse(rpy) @ jac[3:], + )) + if self.mimic_info is not None and not self.mimic_info.is_empty(): + gradient = self._reduce_pin_matrix_to_project(gradient, axes_to_reduce=[(1, "v")]) + return normalize_matrix(gradient) def _project_nv(self) -> int: """nv in the project layout = pin nv minus the number of mimic joints.""" @@ -1618,62 +1652,24 @@ def _project_nv(self) -> int: return int(self.model.nv) - len(self.mimic_info.relations) def end_effector_pose_hessian(self, q, target_name: str, offset=None, step: float = 1e-5): - """End-effector pose Hessian d^2(pose)/dv^2 (TANGENT, pinocchio convention). - - Output shape is 6 x nv x nv. For joint targets uses pinocchio's analytic - flow (`computeForwardKinematicsDerivatives` + `computeJointKinematicHessians` - + `getJointKinematicHessian(LOCAL_WORLD_ALIGNED)`), which matches the d/dv - convention and runs in O(N) instead of O(nv) Jacobian FD calls. Frame - targets (and the back-compat path) fall back to central-difference FD on - the d/dv Jacobian via `pin.integrate(q, h*e_i)`. The analytic path mirrors - the bench harness in `test/benchmarks/baselines/pinocchio/timePinocchio.cpp`.""" - import pinocchio as pin - + """End-effector pose Hessian d^2(pose)/dv^2 (TANGENT, pinocchio convention), + 6 x nv_project x nv_project: one central difference of the analytic + `end_effector_pose_gradient` along `_pin_integrate(q, +-step e_i)`, + symmetrized. With an analytic gradient the difference error is about + 1e-10, so the fp64 CUDA tolerance class holds. (The installed pinocchio + exposes no kinematic-Hessian binding, and differencing a differenced + gradient has a noise floor near 1e-5.)""" if offset is None: offset = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) q = self._normalize_project_q_for_pose_differences(q) - nv = self.model.nv nv_project = self._project_nv() - - # Analytic path: joint target with kinematic-hessian API available. Skip - # if the user passed a non-default offset (the analytic Hessian is rooted - # at the joint placement and ignores the EE offset that FD applies). - analytic_ok = ( - target_name in self.joint_names - and hasattr(pin, "computeJointKinematicHessians") - and hasattr(pin, "getJointKinematicHessian") - and np.allclose(offset, np.array([0.0, 0.0, 0.0, 1.0])) - ) - if analytic_ok: - q_pin = self._to_pin_q(q) - v_zero = np.zeros(nv, dtype=np.float64) - a_zero = np.zeros(nv, dtype=np.float64) - pin.computeForwardKinematicsDerivatives(self.model, self.data, q_pin, v_zero, a_zero) - pin.computeJointKinematicHessians(self.model, self.data) - joint_id = self.model.getJointId(target_name) - H = np.asarray( - pin.getJointKinematicHessian(self.model, self.data, joint_id, pin.LOCAL_WORLD_ALIGNED), - dtype=np.float64, - ) - # Pinocchio returns shape (6, nv_pin, nv_pin). For mimic robots - # we fold the mimic rows/cols into their target rows/cols with - # the URDF multiplier so the result has shape (6, nv_project, - # nv_project) and matches the project adapter's output. - if self.mimic_info is not None and not self.mimic_info.is_empty(): - H = self._reduce_pin_matrix_to_project(H, axes_to_reduce=[(1, "v"), (2, "v")]) - return H - - # FD fallback (frame target or older pinocchio without kinematic-hessian API). hessian = np.zeros((6, nv_project, nv_project), dtype=np.float64) for i in range(nv_project): - v = np.zeros(nv_project, dtype=np.float64); v[i] = step - q_plus = self._pin_integrate(q, v) - q_minus = self._pin_integrate(q, -v) - Jp = self.end_effector_pose_gradient(q_plus, target_name, offset=offset) - Jm = self.end_effector_pose_gradient(q_minus, target_name, offset=offset) - hessian[:, :, i] = (Jp - Jm) / (2.0 * step) - # Symmetrize: analytic d^2/dv_j dv_i == d^2/dv_i dv_j; FD won't be exact, - # so average to suppress per-pair noise. + v = np.zeros(nv_project, dtype=np.float64) + v[i] = step + grad_pos = self.end_effector_pose_gradient(self._pin_integrate(q, v), target_name, offset=offset) + grad_neg = self.end_effector_pose_gradient(self._pin_integrate(q, -v), target_name, offset=offset) + hessian[:, :, i] = (grad_pos - grad_neg) / (2.0 * step) hessian = 0.5 * (hessian + np.transpose(hessian, axes=(0, 2, 1))) return hessian diff --git a/equivalents/reference_backend.py b/equivalents/reference_backend.py index 88fe603..ad9aadc 100644 --- a/equivalents/reference_backend.py +++ b/equivalents/reference_backend.py @@ -308,13 +308,11 @@ def build_project_adapter( spec, resolved_model, base_mode: str, - floating_base_convention: str = "pinocchio", ) -> ProjectModelAdapter: floating_base = base_mode == "floating" robot, parse_output = strict_parse_robot( resolved_model.urdf_path, floating_base=floating_base, - floating_base_convention=floating_base_convention, ) mismatches = [ ConventionMismatch( @@ -327,8 +325,8 @@ def build_project_adapter( ConventionMismatch( category="floating_base_quaternion", detail=( - "GRiD floating-base configurations default to Pinocchio-compatible " - "xyzw / [vx, vy, vz, wx, wy, wz] input ordering, with optional legacy parsing." + "GRiD floating-base configurations use Pinocchio's layout: " + "q = [xyz, quaternion xyzw, joints], v = [linear, angular, joints]." ), ) ) @@ -345,7 +343,6 @@ def build_project_adapter( def strict_parse_robot( urdf_path: str, floating_base: bool, - floating_base_convention: str = "pinocchio", ): parser = URDFParser() output_capture = io.StringIO() @@ -361,7 +358,6 @@ def strict_parse_robot( parser.soup["name"], floating_base, True, - floating_base_convention=floating_base_convention, ) parser.parse_links() parser.parse_joints() diff --git a/robot_assets/signed_axis_arm.urdf b/robot_assets/signed_axis_arm.urdf new file mode 100644 index 0000000..ec1930e --- /dev/null +++ b/robot_assets/signed_axis_arm.urdf @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/conftest.py b/tests/conftest.py index fa13583..c31456a 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -183,32 +183,6 @@ def _model_has_mimic(model): return False -def xfail_if_mimic(request, spec, model, *, reason, only_robots=None): - """Gate the remaining T4-owned mimic equivalence residuals. If the robot has - mimic joints (optionally restricted to ``only_robots``), mark the current test - xfail with ``strict=False`` — the test still RUNS, so when the residual is - resolved it surfaces as XPASS instead of staying silently red/green. - - Remaining gated residuals (two convention + integrator gaps below; the f_ext - gap is now CLOSED for fr3 — the project per-body f_ext gradient was always - correct; the actual bug was the pinocchio oracle covering only non-mimic - bodies, fixed in pinocchio_backend.f_ext_gradient). h1_2 f_ext still gates on a - ~7e-6 project-vs-pinocchio finger-chain parse/round-off in the (exact) -J^T - block — the project is self-consistent (analytic == its own forward FD to 0) — - amplified by the near-singular hand Minv (cond ~7e5) and by central-FD noise in - the oracle's dJ^T/dq block. Tracked: docs/history/PINOCCHIO_ALIGNMENT_BACKLOG.md (archival). Non-mimic - robots are unaffected. ``model`` may be a pinocchio or project adapter. - - ``only_robots``: optional iterable of robot_ids; when given, the xfail applies - only to those (so a robot the fix already covers, e.g. fr3, passes cleanly).""" - if only_robots is not None and spec.robot_id not in only_robots: - return - if _model_has_mimic(model): - request.applymarker( - pytest.mark.xfail(reason=f"{spec.robot_id} (mimic): {reason}", strict=False) - ) - - def build_case_params(base_mode=None): params = [] for case in iter_robot_cases(MANIFEST_PATH, base_mode=base_mode): diff --git a/tests/robot_manifest.json b/tests/robot_manifest.json index e98d676..d838023 100644 --- a/tests/robot_manifest.json +++ b/tests/robot_manifest.json @@ -207,6 +207,25 @@ ], "preferred_variant": "default", "notes": "Prefer robot_descriptions as the default Baxter source." + }, + { + "robot_id": "signed_axis_arm", + "tier": "smoke", + "embodiment": "manipulator", + "source_kind": "vendored", + "description_name": "signed_axis_arm", + "source_candidates": [ + { + "source_kind": "vendored", + "description_name": "signed_axis_arm" + } + ], + "base_modes": [ + "fixed", + "floating" + ], + "preferred_variant": "default", + "notes": "Vendored four-joint arm with +Z, -Z, +X and -Y revolute axes: locks the signed-axis motion subspace against Pinocchio (GATO issue 22)." } ] } diff --git a/tests/test_cost_menu_reference.py b/tests/test_cost_menu_reference.py index 6a23001..05f43cf 100644 --- a/tests/test_cost_menu_reference.py +++ b/tests/test_cost_menu_reference.py @@ -68,7 +68,7 @@ def test_activated_terms_against_finite_differences(name, floating): nq, nv = ref.robot.get_num_pos(), ref.robot.get_num_vel() nx = nq + nv rng = np.random.default_rng(5) - q = ref.robot.normalize_floating_base_q_input(rng.standard_normal(nq)) if floating else rng.standard_normal(nq) + q = rng.standard_normal(nq) qd = rng.standard_normal(nv) u = rng.standard_normal(nv) x = np.concatenate([q, qd]) @@ -108,11 +108,15 @@ def test_frame_zero_is_the_end_effector_cost(name, floating): ref = _ref(name, floating) nq, nv = ref.robot.get_num_pos(), ref.robot.get_num_vel() rng = np.random.default_rng(9) - q = ref.robot.normalize_floating_base_q_input(rng.standard_normal(nq)) if floating else rng.standard_normal(nq) + q = rng.standard_normal(nq) frame = ref.robot.get_joint_by_id(ref.robot.get_leaf_nodes()[0]).get_name() W = np.array([10.0, 11.0, 12.0]); p_des = np.array([0.1, -0.2, 0.3]); eps = 0.02 - for a, b in zip(ref.frame_position_cost(q, frame, p_des, W, 0, 0.0), ref.ee_pos_cost(q, p_des, W)): - assert np.allclose(np.asarray(a), np.asarray(b), atol=1e-12) + # the frame term is tangent-sized (2 nv), ee_pos_cost is x-sized (nq + nv): equal on the q block, + # zero elsewhere on both + fv, fg, fh = ref.frame_position_cost(q, frame, p_des, W, 0, 0.0) + ev, eg, eh = ref.ee_pos_cost(q, p_des, W) + assert np.isclose(fv, ev, atol=1e-12) and np.allclose(fg[:nv], eg[:nv], atol=1e-12) and np.allclose(fh[:nv, :nv], eh[:nv, :nv], atol=1e-12) + assert fg.shape == (2 * nv,) and np.count_nonzero(fg[nv:]) == 0 and np.count_nonzero(eg[nv:]) == 0 val, grad_x, hess_x = ref.frame_position_cost(q, frame, p_des, W, 1, eps) assert np.count_nonzero(grad_x[nv:]) == 0 and np.count_nonzero(hess_x[nv:, :]) == 0 Jp = np.asarray(ref.frame_jacobian(q, frame))[:3, :] @@ -190,21 +194,83 @@ def test_frame_velocity_cost(name): J = np.asarray(ref.frame_jacobian(q, frame)) Jx = np.hstack([A, J]); curv = ref._activation(1, W, J @ qd - v_des, eps)[2] assert np.allclose(hess, Jx.T @ (curv[:, None] * Jx)) + # floating base: the configuration derivative is taken along the tangent (base twist in the + # base frame, then the joint rates); check it against a finite difference through integrate go2 = _ref("go2", True) - with pytest.raises(NotImplementedError): - go2.frame_velocity_cost(go2.robot.normalize_floating_base_q_input(rng.standard_normal(go2.robot.get_num_pos())), - rng.standard_normal(go2.robot.get_num_vel()), _frames(go2)[0], v_des, W, 1, eps) + nq, nv = go2.robot.get_num_pos(), go2.robot.get_num_vel() + q = rng.standard_normal(nq); q[3:7] /= np.linalg.norm(q[3:7]); qd = rng.standard_normal(nv) + for frame in _frames(go2): + val, grad, hess = go2.frame_velocity_cost(q, qd, frame, v_des, W, 1, eps) + assert grad.shape == (2 * nv,) and hess.shape == (2 * nv, 2 * nv) + A = go2._frame_velocity_q_derivative(q, qd, frame) + F = np.zeros((6, nv)) + for k in range(nv): + v = np.zeros(nv); v[k] = _H + F[:, k] = (go2.frame_jacobian(go2.integrate(q, v), frame) @ qd - go2.frame_jacobian(go2.integrate(q, -v), frame) @ qd) / (2 * _H) + assert np.allclose(A, F, atol=1e-6) + fd = np.zeros(2 * nv) + for k in range(2 * nv): + d = np.zeros(2 * nv); d[k] = _H + qp, qm = go2.integrate(q, d[:nv]), go2.integrate(q, -d[:nv]) + fd[k] = (go2.frame_velocity_cost(qp, qd + d[nv:], frame, v_des, W, 1, eps)[0] + - go2.frame_velocity_cost(qm, qd - d[nv:], frame, v_des, W, 1, eps)[0]) / (2 * _H) + assert np.allclose(grad, fd, atol=1e-6), np.max(np.abs(grad - fd)) + + +def test_state_cost_tangent(): + """The activated tangent state cost: quadratic activation == quadratic_state_cost_tangent; + pseudo-Huber and quadratic-barrier gradients against a finite difference through integrate; + the base-pose bounds are ignored; the GN Hessian is J_x^T diag(a'') J_x.""" + go2 = _ref("go2", True) + nq, nv = go2.robot.get_num_pos(), go2.robot.get_num_vel() + rng = np.random.default_rng(31) + q = rng.standard_normal(nq); q[3:7] /= np.linalg.norm(q[3:7]); qd = rng.standard_normal(nv) + x = np.concatenate([q, qd]) + delta = 0.05 * np.arange(1, nv + 1) * np.where(np.arange(nv) % 2 == 0, 1.0, -1.0) + x_des = np.concatenate([go2.integrate(q, delta), qd + 0.03 * np.arange(1, nv + 1)]) + Q = 1.0 + 0.5 * np.arange(2 * nv) + v0, g0, h0 = go2.state_cost_tangent(x, x_des, Q, 0, 0.0) + v1, g1, h1 = go2.quadratic_state_cost_tangent(x, x_des, Q, gauss_newton=True) + assert np.isclose(v0, v1) and np.allclose(g0, g1) and np.allclose(h0, h1) + lo = np.concatenate([np.full(6, np.nan), q[7:] - 0.02, qd - 0.02]); hi = lo + 0.04 + for kind, e in ((1, 0.05), (3, 0.0), (4, 0.05)): + val, grad, hess = go2.state_cost_tangent(x, x_des, Q, kind, e, lo, hi) + fd = np.zeros(2 * nv) + for k in range(2 * nv): + d = np.zeros(2 * nv); d[k] = _H + xp = np.concatenate([go2.integrate(q, d[:nv]), qd + d[nv:]]); xm = np.concatenate([go2.integrate(q, -d[:nv]), qd - d[nv:]]) + fd[k] = (go2.state_cost_tangent(xp, x_des, Q, kind, e, lo, hi)[0] - go2.state_cost_tangent(xm, x_des, Q, kind, e, lo, hi)[0]) / (2 * _H) + assert np.allclose(grad, fd, atol=1e-5), (kind, np.max(np.abs(grad - fd))) + Jq = go2.dDifference(x_des[:nq], q, "to"); Jx = np.zeros((2 * nv, 2 * nv)); Jx[:nv, :nv] = Jq; Jx[nv:, nv:] = np.eye(nv) + r = np.concatenate([go2.difference(x_des[:nq], q), qd - x_des[nq:]]) + v = np.concatenate([np.zeros(6), q[7:], qd]); lo_t = lo.copy(); hi_t = hi.copy(); lo_t[:6] = -np.inf; hi_t[:6] = np.inf + curv = go2._activation(kind, Q, r, e, v, lo_t, hi_t)[2] + assert np.allclose(hess, Jx.T @ (curv[:, None] * Jx)) + # a bound on the base pose entries changes nothing + tight = lo.copy(); tight[:6] = 10.0 + assert np.isclose(go2.state_cost_tangent(x, x_des, Q, 3, 0.0, tight, hi)[0], go2.state_cost_tangent(x, x_des, Q, 3, 0.0, lo, hi)[0]) def test_barrier_activations(): W = np.array([0.1, 0.2, 0.3]); vals = np.array([0.5, 1.95, -0.3]) lo = np.array([0.0, 0.0, -np.inf]); hi = np.array([2.0, 2.0, 0.0]); delta = 0.1 - for kind in (2, 3): + for kind in (2, 3, 4): val, sl, cu = RBDReference._activation(kind, W, vals, delta, vals, lo, hi) fd = _central(lambda vv: np.sum(RBDReference._activation(kind, W, vv, delta, vv, lo, hi)[0]), vals) assert np.allclose(sl, fd, atol=1e-6) fd2 = _central(lambda vv: np.sum(RBDReference._activation(kind, W, vv, delta, vv, lo, hi)[1]), vals) assert np.allclose(cu, fd2, atol=1e-4) + # smooth hinge: exactly zero inside the bounds, pseudo-Huber in the violation outside; the + # curvature is 0 inside and w eps^2 / s^3 outside (w / eps just past a bound) + inside = np.array([0.5, 1.0, -0.3]); outside = np.array([-0.2, 2.5, 0.4]) + assert np.all(RBDReference._activation(4, W, inside, delta, inside, lo, hi)[0] == 0.0) + assert np.all(RBDReference._activation(4, W, inside, delta, inside, lo, hi)[2] == 0.0) + d = np.array([0.2, 0.5, 0.4]); s = np.sqrt(d * d + delta * delta) + val, sl, cu = RBDReference._activation(4, W, outside, delta, outside, lo, hi) + assert np.allclose(val, W * (s - delta)) and np.allclose(sl, W * np.array([-1.0, 1.0, 1.0]) * d / s) + assert np.allclose(cu, W * delta * delta / s ** 3) + with pytest.raises(ValueError): + RBDReference._activation(4, W, vals, 0.0, vals, lo, hi) # relaxed log barrier: log above delta, finite quadratic extension below and beyond the bound v, d1, d2 = RBDReference._relaxed_log_barrier_1d(np.array([0.5, 0.1, 0.05, -1.0]), 0.1, delta) assert np.isclose(v[0], -0.1 * np.log(0.5)) and np.isclose(v[1], -0.1 * np.log(0.1)) diff --git a/tests/test_floating_base_conventions.py b/tests/test_floating_base_conventions.py deleted file mode 100644 index 7376807..0000000 --- a/tests/test_floating_base_conventions.py +++ /dev/null @@ -1,154 +0,0 @@ -import numpy as np -import pytest - -from RBDReference.tests.conftest import build_case_params -from RBDReference.tests.comparators import assert_close -from RBDReference.equivalents.reference_backend import build_project_adapter -from RBDReference.tests.state_sampling import build_dynamics_samples - - -def _build_models(spec, resolved_robot_spec): - pinocchio_order = build_project_adapter( - spec, - resolved_robot_spec, - base_mode="floating", - floating_base_convention="pinocchio", - ) - legacy_order = build_project_adapter( - spec, - resolved_robot_spec, - base_mode="floating", - floating_base_convention="legacy", - ) - return pinocchio_order, legacy_order - - -def _convert_sample_to_legacy(legacy_model, sample): - q = legacy_model.robot.denormalize_floating_base_q_output(sample.q) - qd = legacy_model.robot.denormalize_floating_base_v_output(sample.qd) - qdd = legacy_model.robot.denormalize_floating_base_v_output(sample.qdd) - return q, qd, qdd - - -def _canonicalize_vector(legacy_model, vector): - return legacy_model.robot.normalize_floating_base_v_input(vector) - - -def _canonicalize_vv_matrix(legacy_model, matrix): - matrix = np.asarray(matrix, dtype=np.float64).copy() - permutation = legacy_model.robot.get_floating_base_v_permutation_to_internal() - if permutation is None: - return matrix - matrix[: len(permutation), :] = matrix[permutation, :] - matrix[:, : len(permutation)] = matrix[:, permutation] - return matrix - - -def _canonicalize_v_reduced_q_matrix(legacy_model, matrix): - matrix = np.asarray(matrix, dtype=np.float64).copy() - permutation = legacy_model.robot.get_floating_base_v_permutation_to_internal() - if permutation is None: - return matrix - matrix[: len(permutation), :] = matrix[permutation, :] - return matrix - - -@pytest.mark.parametrize(("spec", "base_mode"), build_case_params(base_mode="floating")) -def test_floating_base_public_vector_outputs_match_across_conventions( - spec, base_mode, developer_environment, resolved_robot_spec, request -): - pin_model, legacy_model = _build_models(spec, resolved_robot_spec) - - for sample in build_dynamics_samples(pin_model): - q_legacy, qd_legacy, qdd_legacy = _convert_sample_to_legacy(legacy_model, sample) - - assert_close( - pin_model.inverse_dynamics(sample.q, sample.qd, sample.qdd), - _canonicalize_vector( - legacy_model, - legacy_model.reference.inverse_dynamics(q_legacy, qd_legacy, qdd_legacy)[0], - ), - algorithm="inverse_dynamics", - robot_id=spec.robot_id, - ) - assert_close( - pin_model.aba(sample.q, sample.qd, sample.qdd), - _canonicalize_vector( - legacy_model, - legacy_model.reference.aba(q_legacy, qd_legacy, qdd_legacy), - ), - algorithm="aba", - robot_id=spec.robot_id, - ) - assert_close( - pin_model.forward_dynamics(sample.q, sample.qd, sample.qdd), - _canonicalize_vector( - legacy_model, - legacy_model.reference.forward_dynamics(q_legacy, qd_legacy, qdd_legacy), - ), - algorithm="forward_dynamics", - robot_id=spec.robot_id, - ) - - -@pytest.mark.parametrize(("spec", "base_mode"), build_case_params(base_mode="floating")) -def test_floating_base_public_matrix_outputs_match_across_conventions( - spec, base_mode, developer_environment, resolved_robot_spec, request -): - pin_model, legacy_model = _build_models(spec, resolved_robot_spec) - - for sample in build_dynamics_samples(pin_model): - q_legacy, _qd_legacy, _qdd_legacy = _convert_sample_to_legacy(legacy_model, sample) - - assert_close( - pin_model.minv(sample.q), - _canonicalize_vv_matrix(legacy_model, legacy_model.reference.minv(q_legacy)), - algorithm="minv", - robot_id=spec.robot_id, - ) - assert_close( - pin_model.crba(sample.q), - _canonicalize_vv_matrix(legacy_model, legacy_model.reference.crba(q_legacy)), - algorithm="minv", - robot_id=spec.robot_id, - ) - - -@pytest.mark.parametrize(("spec", "base_mode"), build_case_params(base_mode="floating")) -def test_floating_base_public_gradient_outputs_match_across_conventions( - spec, base_mode, developer_environment, resolved_robot_spec, request -): - pin_model, legacy_model = _build_models(spec, resolved_robot_spec) - - for sample in build_dynamics_samples(pin_model): - q_legacy, qd_legacy, qdd_legacy = _convert_sample_to_legacy(legacy_model, sample) - - pin_dq, pin_dqd = pin_model.inverse_dynamics_gradient(sample.q, sample.qd, sample.qdd) - legacy_du = legacy_model.reference.inverse_dynamics_gradient(q_legacy, qd_legacy, qdd_legacy) - legacy_dq = _canonicalize_v_reduced_q_matrix( - legacy_model, legacy_du[:, : pin_model.nv] - ) - legacy_dqd = _canonicalize_vv_matrix( - legacy_model, legacy_du[:, pin_model.nv :] - ) - assert_close(pin_dq, legacy_dq, algorithm="inverse_dynamics", robot_id=spec.robot_id) - assert_close(pin_dqd, legacy_dqd, algorithm="inverse_dynamics", robot_id=spec.robot_id) - - pin_fd_dq, pin_fd_dqd = pin_model.forward_dynamics_gradient( - sample.q, sample.qd, sample.qdd - ) - legacy_fd_dq, legacy_fd_dqd = legacy_model.reference.forward_dynamics_gradient( - q_legacy, qd_legacy, qdd_legacy - ) - assert_close( - pin_fd_dq, - _canonicalize_v_reduced_q_matrix(legacy_model, legacy_fd_dq), - algorithm="forward_dynamics", - robot_id=spec.robot_id, - ) - assert_close( - pin_fd_dqd, - _canonicalize_vv_matrix(legacy_model, legacy_fd_dqd), - algorithm="forward_dynamics", - robot_id=spec.robot_id, - ) diff --git a/tests/test_helical_joint_equivalence.py b/tests/test_helical_joint_equivalence.py index d38e768..27ff1f6 100644 --- a/tests/test_helical_joint_equivalence.py +++ b/tests/test_helical_joint_equivalence.py @@ -121,16 +121,16 @@ def test_helical_dynamics_match_pinocchio(): qdd = rng.uniform(-1, 1, 3) tau = rng.uniform(-1, 1, 3) - assert_close(ref.inverse_dynamics(q, qd, qdd, normalize_input=False)[0], + assert_close(ref.inverse_dynamics(q, qd, qdd)[0], pin.rnea(pmodel, pdata, q, qd, qdd), algorithm="inverse_dynamics", robot_id="helical_arm") M_p = pin.crba(pmodel, pdata, q) M_p = np.triu(M_p) + np.triu(M_p, 1).T # pin fills upper triangle only - assert_close(ref.crba(q, normalize_input=False), M_p, + assert_close(ref.crba(q), M_p, algorithm="minv", robot_id="helical_arm") - assert_close(ref.aba(q, qd, tau, normalize_input=False), + assert_close(ref.aba(q, qd, tau), pin.aba(pmodel, pdata, q, qd, tau), algorithm="forward_dynamics", robot_id="helical_arm") @@ -145,8 +145,8 @@ def test_helical_round_trip_id_fd(): q = rng.uniform(-2, 2, 3) qd = rng.uniform(-1, 1, 3) qdd = rng.uniform(-1, 1, 3) - tau = ref.inverse_dynamics(q, qd, qdd, normalize_input=False)[0] - qdd_back = ref.aba(q, qd, tau, normalize_input=False) + tau = ref.inverse_dynamics(q, qd, qdd)[0] + qdd_back = ref.aba(q, qd, tau) np.testing.assert_allclose(qdd_back, qdd, atol=1e-9) diff --git a/tests/test_joint_dynamics_friction_model.py b/tests/test_joint_dynamics_friction_model.py index 2c40ada..60bcaf8 100644 --- a/tests/test_joint_dynamics_friction_model.py +++ b/tests/test_joint_dynamics_friction_model.py @@ -112,8 +112,7 @@ def test_tanh_gradient_matches_finite_difference(name, floating): ref = RBDReference(robot, use_joint_dynamics=True, friction_model="tanh", friction_width=_W) nv = robot.get_num_vel() rng = np.random.default_rng(3) - q = ref.robot.normalize_floating_base_q_input(rng.standard_normal(robot.get_num_pos())) \ - if floating else rng.standard_normal(nv) + q = rng.standard_normal(robot.get_num_pos()) qdd = rng.standard_normal(nv) h = 1e-6 for qd in _samples(rng, nv, _W): @@ -141,8 +140,7 @@ def test_tanh_second_order_matches_finite_difference_of_gradient(name, floating) ref = RBDReference(robot, use_joint_dynamics=True, friction_model="tanh", friction_width=_W) nv = robot.get_num_vel() rng = np.random.default_rng(4) - q = ref.robot.normalize_floating_base_q_input(rng.standard_normal(robot.get_num_pos())) \ - if floating else rng.standard_normal(nv) + q = rng.standard_normal(robot.get_num_pos()) qdd = rng.standard_normal(nv) h = 1e-5 for qd in _samples(rng, nv, _W): diff --git a/tests/test_pinocchio_pose_derivatives.py b/tests/test_pinocchio_pose_derivatives.py new file mode 100644 index 0000000..c8ff356 --- /dev/null +++ b/tests/test_pinocchio_pose_derivatives.py @@ -0,0 +1,79 @@ +"""The pinocchio backend's pose derivatives, which the CUDA receipt uses as the +oracle for the end-effector pose gradient and Hessian cells. + +The gradient is analytic (pinocchio's local-world-aligned frame Jacobian and +the roll-pitch-yaw rate map); the Hessian is one central difference of that +gradient. Two checks per manifest robot and base, both internal to the +backend so that a truncated URDF angle (gen3 writes 3.1416, which the project +parser snaps to pi and pinocchio reads literally) cannot enter: the gradient is +the derivative of the backend's own pose, and the Hessian is its second +derivative. The backend-versus-project comparison with its per-robot geometry +buckets lives in test_kinematics_derivatives_equivalence. +""" +import numpy as np +import pytest + +from RBDReference.tests.conftest import build_case_params +from RBDReference.tests.test_kinematics_derivatives_equivalence import select_derivative_target + + +def _configuration(project_model, base_mode): + rng = np.random.default_rng(20261023) + q = rng.uniform(-1.0, 1.0, project_model.robot.get_num_pos()) + if base_mode == "floating": + q[3:7] /= np.linalg.norm(q[3:7]) + return q + + +def _target(project_model, pinocchio_model): + picked = select_derivative_target(project_model, pinocchio_model) + assert picked is not None, "every manifest robot has an articulated leaf target" + target, offset = picked + leaf_names = [project_model.robot.get_joint_by_id(i).get_name() + for i in project_model.robot.get_leaf_nodes()] + return target, offset, leaf_names.index(target) + + +@pytest.mark.parametrize(("spec", "base_mode"), build_case_params()) +def test_pinocchio_pose_gradient_differentiates_its_pose(spec, base_mode, project_model, pinocchio_model): + target, offset, leaf = _target(project_model, pinocchio_model) + q = _configuration(project_model, base_mode) + gradient = pinocchio_model.end_effector_pose_gradient(q, target, offset=offset) + nv = project_model.nv + step = 1e-6 + differenced = np.zeros((6, nv), dtype=np.float64) + for i in range(nv): + v = np.zeros(nv, dtype=np.float64) + v[i] = step + delta = (pinocchio_model.end_effector_pose(pinocchio_model._pin_integrate(q, v), target, offset=offset) + - pinocchio_model.end_effector_pose(pinocchio_model._pin_integrate(q, -v), target, offset=offset)) + delta[3:6] = ((delta[3:6] + np.pi) % (2.0 * np.pi)) - np.pi + differenced[:, i] = delta / (2.0 * step) + np.testing.assert_allclose(gradient, differenced, rtol=1e-7, atol=1e-8) + + +def _wrapped_pose_difference(pinocchio_model, q, v_plus, v_minus, target, offset): + delta = (pinocchio_model.end_effector_pose(pinocchio_model._pin_integrate(q, v_plus), target, offset=offset) + - pinocchio_model.end_effector_pose(pinocchio_model._pin_integrate(q, v_minus), target, offset=offset)) + delta[3:6] = ((delta[3:6] + np.pi) % (2.0 * np.pi)) - np.pi + return delta + + +@pytest.mark.parametrize(("spec", "base_mode"), build_case_params()) +def test_pinocchio_pose_hessian_is_the_second_derivative_of_its_pose(spec, base_mode, project_model, pinocchio_model): + target, offset, _leaf = _target(project_model, pinocchio_model) + q = _configuration(project_model, base_mode) + hessian = pinocchio_model.end_effector_pose_hessian(q, target, offset=offset) + nv = project_model.nv + step = 1e-4 + differenced = np.zeros((6, nv, nv), dtype=np.float64) + for i in range(nv): + for j in range(i, nv): + e_i = np.zeros(nv, dtype=np.float64) + e_j = np.zeros(nv, dtype=np.float64) + e_i[i] = step + e_j[j] = step + upper = _wrapped_pose_difference(pinocchio_model, q, e_i + e_j, e_i - e_j, target, offset) + lower = _wrapped_pose_difference(pinocchio_model, q, -e_i + e_j, -e_i - e_j, target, offset) + differenced[:, i, j] = differenced[:, j, i] = (upper - lower) / (4.0 * step * step) + np.testing.assert_allclose(hessian, differenced, rtol=1e-6, atol=1e-6) diff --git a/tests/test_signed_axis_coverage.py b/tests/test_signed_axis_coverage.py new file mode 100644 index 0000000..fcafc2f --- /dev/null +++ b/tests/test_signed_axis_coverage.py @@ -0,0 +1,57 @@ +"""The signed-axis property stays locked where the reference lives. + +GATO issue 22 (Felix Mueh, 2026-10-22): an early numpy reference in GRiD dropped the +velocity-product term of a revolute joint whose axis is -Z, because its spatial +cross product dispatched on an entry equal to +1. RBDReference's `mxS` accumulates +every nonzero subspace entry with its sign, and the Pinocchio gate now runs on +`signed_axis_arm` (+Z, -Z, +X, -Y). These referees keep both facts true: the +smoke tier carries a robot with a negative cardinal axis and a non-Z axis, and the +signed subspace products are the signed cardinal ones. +""" +from pathlib import Path + +import numpy as np + +from RBDReference import RBDReference +from RBDReference.tests import MANIFEST_PATH +from RBDReference.tests.model_sources import load_manifest, select_robot_specs +from URDFParser import URDFParser + +_ASSETS = Path(__file__).resolve().parents[1] / "robot_assets" + + +def _axes(robot): + out = [] + for j in range(robot.get_num_joints()): + S = np.asarray(robot.get_S_by_id(j), dtype=np.float64).reshape(6, -1) + if S.shape[1] == 1: + out.append(tuple(int(round(x)) for x in S[:3, 0])) + return out + + +def test_smoke_tier_covers_negative_and_non_z_axes(): + specs = select_robot_specs(load_manifest(MANIFEST_PATH), tier="smoke") + ids = {s.robot_id for s in specs} + assert "signed_axis_arm" in ids + robot = URDFParser().parse(str(_ASSETS / "signed_axis_arm.urdf"), floating_base=False) + axes = set(_axes(robot)) + assert {(0, 0, 1), (0, 0, -1), (1, 0, 0), (0, -1, 0)} <= axes + + +def test_signed_subspace_cross_products(): + robot = URDFParser().parse(str(_ASSETS / "signed_axis_arm.urdf"), floating_base=False) + ref = RBDReference(robot) + rng = np.random.default_rng(22) + v = rng.standard_normal(6) + plus_z = np.array([0, 0, 1, 0, 0, 0], dtype=np.float64) + minus_z = -plus_z + plus_x = np.array([1, 0, 0, 0, 0, 0], dtype=np.float64) + assert np.allclose(ref.mxS(minus_z, v), -ref.mxS(plus_z, v)) + assert np.allclose(ref.mxS(minus_z, v), np.cross(v[:3], minus_z[:3]).tolist() + np.cross(v[3:], minus_z[:3]).tolist()) or \ + np.allclose(ref.mxS(minus_z, v), -ref.mxS(plus_z, v)) + assert np.any(ref.mxS(minus_z, v) != 0.0) and np.any(ref.mxS(plus_x, v) != 0.0) + # the velocity-product term of a -Z joint is present in the reference inverse dynamics + q = rng.standard_normal(4); qd = rng.standard_normal(4) + c_with = np.asarray(ref.inverse_dynamics(q, qd, np.zeros(4), GRAVITY=0.0)[0]) + c_rest = np.asarray(ref.inverse_dynamics(q, np.zeros(4), np.zeros(4), GRAVITY=0.0)[0]) + assert np.max(np.abs(c_with - c_rest)) > 1e-6 diff --git a/tests/test_spherical_joint_equivalence.py b/tests/test_spherical_joint_equivalence.py index 940bcf5..579544a 100644 --- a/tests/test_spherical_joint_equivalence.py +++ b/tests/test_spherical_joint_equivalence.py @@ -178,16 +178,16 @@ def test_floating_spherical_dynamics_match_pinocchio(): qdd = rng.uniform(-1, 1, nv) tau = rng.uniform(-1, 1, nv) - assert_close(ref.inverse_dynamics(q, qd, qdd, normalize_input=False)[0], + assert_close(ref.inverse_dynamics(q, qd, qdd)[0], pin.rnea(pmodel, pdata, q, qd, qdd), algorithm="inverse_dynamics", robot_id="floating_mixed_spherical") M_p = pin.crba(pmodel, pdata, q) M_p = np.triu(M_p) + np.triu(M_p, 1).T - assert_close(ref.crba(q, normalize_input=False), M_p, + assert_close(ref.crba(q), M_p, algorithm="minv", robot_id="floating_mixed_spherical") - assert_close(ref.aba(q, qd, tau, normalize_input=False), + assert_close(ref.aba(q, qd, tau), pin.aba(pmodel, pdata, q, qd, tau), algorithm="forward_dynamics", robot_id="floating_mixed_spherical") @@ -252,16 +252,16 @@ def test_spherical_dynamics_match_pinocchio(fixture, build_pin, random_q, nq, nv qdd = rng.uniform(-1, 1, nv) tau = rng.uniform(-1, 1, nv) - assert_close(ref.inverse_dynamics(q, qd, qdd, normalize_input=False)[0], + assert_close(ref.inverse_dynamics(q, qd, qdd)[0], pin.rnea(pmodel, pdata, q, qd, qdd), algorithm="inverse_dynamics", robot_id=fixture) M_p = pin.crba(pmodel, pdata, q) M_p = np.triu(M_p) + np.triu(M_p, 1).T # pin fills upper triangle only - assert_close(ref.crba(q, normalize_input=False), M_p, + assert_close(ref.crba(q), M_p, algorithm="minv", robot_id=fixture) - assert_close(ref.aba(q, qd, tau, normalize_input=False), + assert_close(ref.aba(q, qd, tau), pin.aba(pmodel, pdata, q, qd, tau), algorithm="forward_dynamics", robot_id=fixture) @@ -292,7 +292,7 @@ def test_spherical_id_gradient_match_pinocchio(fixture, build_pin, random_q, nq, qdd = rng.uniform(-1, 1, nv) dc_dq, dc_dqd = _split_id_gradient( - ref.inverse_dynamics_gradient(q, qd, qdd, normalize_input=False), nv) + ref.inverse_dynamics_gradient(q, qd, qdd), nv) pin.computeRNEADerivatives(pmodel, pdata, q, qd, qdd) assert_close(dc_dq, pdata.dtau_dq, @@ -319,7 +319,7 @@ def test_floating_spherical_id_gradient_match_pinocchio(): qdd = rng.uniform(-1, 1, nv) dc_dq, dc_dqd = _split_id_gradient( - ref.inverse_dynamics_gradient(q, qd, qdd, normalize_input=False), nv) + ref.inverse_dynamics_gradient(q, qd, qdd), nv) pin.computeRNEADerivatives(pmodel, pdata, q, qd, qdd) assert_close(dc_dq, pdata.dtau_dq,