Alessio
55d13aec5f
Every step, the flex block of the implicit effective metric M + K was factorized by sparse Cholesky, because K depends on the configuration. On model/flex/bag.xml, added here, that is roughly half the step, against a comparable share for the constraint solve it exists to accelerate. Keep only the metric's per-vertex 3x3 diagonal blocks, prefactored. Neither consumer needs the exact inverse: the CG constraint solver only wants a preconditioner, and qacc_smooth can come from an iterative solve using those blocks. They are O(n) to build and to apply, but weaker, so CG runs about twice the iterations and qacc_smooth becomes an iteration rather than a direct solve. Net, the bag model steps roughly twice as fast. The preconditioner, by metric state. Inactive, meaning no flex elasticity or an explicit integrator: M^-1, unchanged. Bending only (nefmK == 0): M^-1 plus the exact constant bending factor from mj_setConst on the dofs it covers, unchanged; that factor is built at model compile time and costs nothing per step. Per-step stiffness: M^-1 plus the 3x3 blocks, where before it was a per-step sparse Cholesky, or, when M couples across the flex block, an inner PCG of up to 50 iterations run once per outer CG iteration. Only models carrying per-step stretch stiffness change in wall-clock. Both ponchos hold their timing and take slightly fewer CG iterations than before, because the preconditioner is now symmetric: it applies M^-1 and the covered blocks to disjoint sets of dofs, where previously the two overlapped and the operator was not symmetric, which PCG requires. mjd_effSolve is the accurate solve of (M + K)x = b; what used to carry that name only preconditions and is now mjd_effPrec. Its CG guarded the division by pAp with mjMINVAL, an absolute floor on a quantity that scales with the square of the right-hand side, so a small b aborted the solve while the curvature was healthy: four flex models were quietly left short of tolerance. For an SPD metric the guard is positivity, and with that the same solves converge. The qacc_smooth call site in mj_fwdAcceleration is textually unchanged but now reaches the iterative solve, which converges on opt.tolerance rather than a hardcoded threshold, floored in mjUSESINGLE builds where the squared target is unreachable in float. Reaching the iteration cap names the ill-conditioned flex stiffness and then reports it through mjWARN_INERTIA, rather than returning an under-converged result. Covered dofs are located by walking the covered rows of the stiffness matrix, as they need not be 3-aligned from dof 0: any joint declared before a flexcomp shifts them. mjData.efm_L_rownnz, efm_L_rowadr and efm_L_colind described the sparsity of the deleted factorization and are removed: left NULL with nonzero mjxmacro extents they made the Python bindings hand back uninitialized arrays. efm_active loses the value 2 for the same reason, nothing selects a solve path on preconditioner exactness any more. Both are recorded under breaking changes. model/flex/bag.xml is added because no shipped model carried per-step stretch stiffness. The ponchos are bending-only and trampoline.xml uses an explicit integrator, so the metric never activates there. It is excluded from WriteReadCompareTest: stretch stiffness amplifies rest geometry that XML rounds on save.
115 lines
5.5 KiB
C
115 lines
5.5 KiB
C
// Copyright 2022 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#ifndef MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
|
|
#define MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjexport.h>
|
|
#include <mujoco/mjmodel.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
// derivatives of mju_subQuat w.r.t inputs
|
|
MJAPI void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]);
|
|
|
|
// derivatives of mju_quatIntegrate w.r.t inputs
|
|
MJAPI void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale,
|
|
mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]);
|
|
|
|
// analytical derivative of smooth forces w.r.t velocities:
|
|
// d->qDeriv = d (qfrc_actuator + qfrc_passive - [qfrc_bias]) / d qvel
|
|
MJAPI void mjd_smooth_vel(const mjModel* m, mjData* d, int flg_bias);
|
|
|
|
// add (d qfrc_actuator / d qvel) to qDeriv
|
|
MJAPI void mjd_actuator_vel(const mjModel* m, mjData* d);
|
|
|
|
// add (d qfrc_passive / d qvel) to qDeriv
|
|
MJAPI void mjd_passive_vel(const mjModel* m, mjData* d);
|
|
|
|
// subtract (d qfrc_bias / d qvel) from qDeriv (dense version)
|
|
MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d);
|
|
|
|
// return 1 if body is a standalone free body: a free joint with no children
|
|
mjtBool mj_isFreeBody(const mjModel* m, int body);
|
|
|
|
// 6x6 block B = d qfrc_bias / d qvel for the free joint of a standalone body
|
|
MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]);
|
|
|
|
// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
|
|
// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
|
|
// requires valid d->qDeriv rows for the block, computed with flg_bias = 0
|
|
MJAPI int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]);
|
|
|
|
// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes
|
|
// K_rot_cache: if non-NULL, use pre-cached K_rot (same layout as m->flex_stiffness)
|
|
MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
|
|
mjtNum s1, mjtNum s2, const mjtNum* K_rot_cache);
|
|
|
|
// precompute unscaled K_rot for all elements into cache (same layout as m->flex_stiffness)
|
|
MJAPI void mjd_flexInterp_cacheKrot(const mjModel* m, mjData* d, mjtNum* K_rot_out);
|
|
|
|
// compute res += scale * K_bend * vec for standard (non-interp) flex bending
|
|
// scale = s1 + s2 * flex_damping[f] per flex
|
|
MJAPI void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
|
|
mjtNum s1, mjtNum s2);
|
|
|
|
// compute res += scale * K_stretch * vec for standard (non-interp) flex stretch,
|
|
// K_stretch the Gauss-Newton Hessian of the passive stretch force at the current state
|
|
// scale = s1 + s2 * flex_damping[f] per flex
|
|
MJAPI void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
|
|
mjtNum s1, mjtNum s2);
|
|
|
|
// assemble the standard-flex implicit stiffness (s1 + s2*damping)*(K_bend + K_stretch) into
|
|
// dof-level CSR; phase 1 (colind==NULL) fills rownnz/rowadr and returns total nnz, phase 2
|
|
// fills colind/val. Interp flexes are assembled iff Krot (mjd_flexInterp_cacheKrot cache) is
|
|
// non-NULL and the centered fast path applies (check mjd_flexInterpAssemblable first).
|
|
MJAPI int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr,
|
|
int* colind, mjtNum* val, mjtNum s1, mjtNum s2,
|
|
int flg_bend, int flg_stretch, const mjtNum* Krot);
|
|
|
|
// can all interp flexes be assembled to dof-level CSR? (centered fast path everywhere)
|
|
MJAPI mjtBool mjd_flexInterpAssemblable(const mjModel* m);
|
|
|
|
// does any flex contribute assemblable implicit stiffness? (existence check)
|
|
MJAPI mjtBool mjd_flexStiff_any(const mjModel* m, int flg_interp);
|
|
|
|
// implicit effective metric Mtilde = M + (h^2+h*d)*K: per-step arena object (see mjdata.h efm_*)
|
|
// build (or deactivate, active==0); the gate decision belongs to the caller
|
|
MJAPI void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor);
|
|
|
|
// refresh the metric's smooth-force shift c = h*K*qvel (values only, velocity stage)
|
|
MJAPI void mjd_effShift(const mjModel* m, mjData* d);
|
|
|
|
// res += B*vec (the stiffness part of the metric; caller supplies the M part)
|
|
MJAPI void mjd_effMulAdd(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
|
|
|
|
// solve (M + B) x = b by PCG preconditioned with mjd_effPrec, to opt.tolerance on the relative
|
|
// residual; x = M^-1 b when the metric is inactive. Warns (mjWARN_INERTIA) if the iteration cap
|
|
// is reached before convergence, in which case x is returned under-converged.
|
|
MJAPI void mjd_effSolve(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b);
|
|
|
|
// apply the metric preconditioner: x ~= (M + B)^-1 b, a cheap fixed linear operator, NOT a solve.
|
|
// Exact only when the metric is inactive (x = M^-1 b); otherwise approximate by construction.
|
|
MJAPI void mjd_effPrec(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* b);
|
|
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif // MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
|