Yuval Tassa
f0fa3d8260
The gyroscopic (bias) derivatives applied to standalone free bodies by the implicitfast integrator provide comparable stability for spinning bodies, with none of midpoint's restrictions: they apply under contacts, fluid forces and constraints, and preserve the linear force-velocity relation required by discrete-time inverse dynamics. The invdiscrete flag reverts to its original single meaning and no longer affects forward dynamics. Restore implicitfast coverage in the DiscreteInverseMatch test, removed when midpoint made discrete inverse dynamics untestable. Add implicit gyroscopic (bias) derivatives for free bodies in implicitfast. The implicitfast integrator drops the RNE (bias) derivative to stay on the symmetric Cholesky path, so fast-spinning free bodies integrate gyroscopic forces explicitly and can gain energy. Symmetrizing the gyroscopic Jacobian is not an option: its stabilizing content is the antisymmetric part, and adding only the symmetric part is destabilizing. Instead, exploit the fact that for a standalone free body the 6x6 block of M - h*D is decoupled from the rest of the system (qDeriv sparsity is tree-local): after the global solve, rebuild the block with the exact bias derivative in closed form (mjd_freeBias_vel) and re-solve it with dense unsymmetric LU, overwriting the block's rows of qacc. For lone spinning bodies this makes implicitfast match implicit to rounding, at ~150ns per eligible body: cheaper than the midpoint machinery it will replace. Eligibility is structural only; contacts, fluid and constraints need no gating. The same block is mirrored in discrete inverse dynamics (mj_discreteAcc), making invdiscrete exact for spinning free bodies. PiperOrigin-RevId: 948472495 Change-Id: I813ef3d98c7b399881bc8603b9f9208cfb02eb58
93 lines
3.1 KiB
C
93 lines
3.1 KiB
C
// Copyright 2021 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_FORWARD_H_
|
|
#define MUJOCO_SRC_ENGINE_ENGINE_FORWARD_H_
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjexport.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjtype.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
// check positions, velocities, accelerations; reset if bad
|
|
MJAPI void mj_checkPos(const mjModel* m, mjData* d);
|
|
MJAPI void mj_checkVel(const mjModel* m, mjData* d);
|
|
MJAPI void mj_checkAcc(const mjModel* m, mjData* d);
|
|
|
|
|
|
//-------------------------------- top-level API ---------------------------------------------------
|
|
|
|
// advance simulation: use control callback, no external force, RK4 available
|
|
MJAPI void mj_step(const mjModel* m, mjData* d);
|
|
|
|
// advance simulation in two steps: before external force/control is set by user
|
|
MJAPI void mj_step1(const mjModel* m, mjData* d);
|
|
|
|
// advance simulation in two steps: after external force/control is set by user
|
|
MJAPI void mj_step2(const mjModel* m, mjData* d);
|
|
|
|
// forward dynamics
|
|
MJAPI void mj_forward(const mjModel* m, mjData* d);
|
|
|
|
// forward dynamics with skip; skipstage is mjtStage
|
|
MJAPI void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor);
|
|
|
|
|
|
//-------------------------------- integrators -----------------------------------------------------
|
|
|
|
// Runge Kutta explicit order-N integrator
|
|
MJAPI void mj_RungeKutta(const mjModel* m, mjData* d, int N);
|
|
|
|
// Euler integrator, semi-implicit in velocity
|
|
MJAPI void mj_Euler(const mjModel* m, mjData* d);
|
|
|
|
// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
|
|
MJAPI void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor);
|
|
|
|
// fully implicit in velocity
|
|
MJAPI void mj_implicit(const mjModel *m, mjData *d);
|
|
|
|
// fully implicit in velocity, possibly skipping factorization
|
|
MJAPI void mj_implicitSkip(const mjModel *m, mjData *d, int skipfactor);
|
|
|
|
|
|
//-------------------------------- solver components -----------------------------------------------
|
|
|
|
// all kinematics-like computations
|
|
MJAPI void mj_fwdKinematics(const mjModel* m, mjData* d);
|
|
|
|
// computations that depend only on qpos
|
|
MJAPI void mj_fwdPosition(const mjModel* m, mjData* d);
|
|
|
|
// computations that depend only on qpos and qvel
|
|
MJAPI void mj_fwdVelocity(const mjModel* m, mjData* d);
|
|
|
|
// compute actuator force
|
|
MJAPI void mj_fwdActuation(const mjModel* m, mjData* d);
|
|
|
|
// add up all non-constraint forces, compute qacc_smooth
|
|
MJAPI void mj_fwdAcceleration(const mjModel* m, mjData* d);
|
|
|
|
// forward constraint
|
|
MJAPI void mj_fwdConstraint(const mjModel* m, mjData* d);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif // MUJOCO_SRC_ENGINE_ENGINE_FORWARD_H_
|