Yuval Tassa
b092563cab
PiperOrigin-RevId: 800870384 Change-Id: I273e0ceec090901e2620febb67e57914928e94bb
74 lines
2.7 KiB
Python
74 lines
2.7 KiB
Python
# Copyright 2025 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.
|
|
# ==============================================================================
|
|
"""Derivative functions."""
|
|
|
|
from typing import Optional
|
|
|
|
import jax
|
|
from jax import numpy as jp
|
|
# pylint: disable=g-importing-member
|
|
from mujoco.mjx._src.types import BiasType
|
|
from mujoco.mjx._src.types import Data
|
|
from mujoco.mjx._src.types import DataJAX
|
|
from mujoco.mjx._src.types import DisableBit
|
|
from mujoco.mjx._src.types import DynType
|
|
from mujoco.mjx._src.types import GainType
|
|
from mujoco.mjx._src.types import Model
|
|
from mujoco.mjx._src.types import ModelJAX
|
|
from mujoco.mjx._src.types import OptionJAX
|
|
# pylint: enable=g-importing-member
|
|
|
|
|
|
def deriv_smooth_vel(m: Model, d: Data) -> Optional[jax.Array]:
|
|
"""Analytical derivative of smooth forces w.r.t. velocities."""
|
|
if (
|
|
not isinstance(m._impl, ModelJAX)
|
|
or not isinstance(d._impl, DataJAX)
|
|
or not isinstance(m.opt._impl, OptionJAX)
|
|
):
|
|
raise ValueError('deriv_smooth_vel requires JAX MJX implementation.')
|
|
|
|
qderiv = None
|
|
|
|
# qDeriv += d qfrc_actuator / d qvel
|
|
if not m.opt.disableflags & DisableBit.ACTUATION:
|
|
affine_bias = m.actuator_biastype == BiasType.AFFINE
|
|
bias_vel = m.actuator_biasprm[:, 2] * affine_bias
|
|
affine_gain = m.actuator_gaintype == GainType.AFFINE
|
|
gain_vel = m.actuator_gainprm[:, 2] * affine_gain
|
|
ctrl = d.ctrl.at[m.actuator_dyntype != DynType.NONE].set(d.act)
|
|
vel = bias_vel + gain_vel * ctrl
|
|
qderiv = d._impl.actuator_moment.T @ jax.vmap(jp.multiply)(
|
|
d._impl.actuator_moment, vel
|
|
)
|
|
|
|
# qDeriv += d qfrc_passive / d qvel
|
|
if not m.opt.disableflags & DisableBit.DAMPER:
|
|
if qderiv is None:
|
|
qderiv = -jp.diag(m.dof_damping)
|
|
else:
|
|
qderiv -= jp.diag(m.dof_damping)
|
|
if m.ntendon:
|
|
qderiv -= d._impl.ten_J.T @ jp.diag(m.tendon_damping) @ d._impl.ten_J
|
|
|
|
if not m.opt.disableflags & (DisableBit.DAMPER | DisableBit.SPRING):
|
|
# TODO(robotics-simulation): fluid drag model
|
|
if m.opt._impl.has_fluid_params: # pytype: disable=attribute-error
|
|
raise NotImplementedError('fluid drag not supported for implicitfast')
|
|
|
|
# TODO(team): rne derivative
|
|
|
|
return qderiv
|