Skip to content
Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 13 additions & 14 deletions mujoco_warp/_src/constraint.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
from mujoco_warp._src import math
from mujoco_warp._src import support
from mujoco_warp._src import types
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN
from mujoco_warp._src.types import ConstraintType
from mujoco_warp._src.types import ContactType
from mujoco_warp._src.types import DisableBit
Expand Down Expand Up @@ -2135,7 +2134,7 @@ def make_constraint(m: types.Model, d: types.Data):
inputs=[d.ne, d.nf, d.nl, d.nefc, efc_nnz],
)

if types.SPARSE_CONSTRAINT_JACOBIAN:
if m.is_sparse:
d.contact.efc_address.fill_(-1)

if not (m.opt.disableflags & types.DisableBit.CONSTRAINT):
Expand Down Expand Up @@ -2163,7 +2162,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.eq_solref,
m.eq_solimp,
m.eq_data,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.eq_connect_adr,
d.qvel,
d.eq_active,
Expand Down Expand Up @@ -2216,7 +2215,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.eq_solref,
m.eq_solimp,
m.eq_data,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.eq_wld_adr,
d.qvel,
d.eq_active,
Expand Down Expand Up @@ -2262,7 +2261,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.eq_solref,
m.eq_solimp,
m.eq_data,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.eq_jnt_adr,
d.qpos,
d.qvel,
Expand Down Expand Up @@ -2304,7 +2303,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.ten_J_colind,
m.tendon_length0,
m.tendon_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.eq_ten_adr,
d.qvel,
d.eq_active,
Expand Down Expand Up @@ -2332,7 +2331,7 @@ def make_constraint(m: types.Model, d: types.Data):
)

wp.launch(
_equality_flex(SPARSE_CONSTRAINT_JACOBIAN),
_equality_flex(m.is_sparse),
dim=(d.nworld, m.eq_flex_adr.size, m.nflexedge),
inputs=[
m.nv,
Expand Down Expand Up @@ -2382,7 +2381,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.dof_solimp,
m.dof_frictionloss,
m.dof_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
d.qvel,
d.njmax,
],
Expand Down Expand Up @@ -2419,7 +2418,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.tendon_solimp_fri,
m.tendon_frictionloss,
m.tendon_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
d.qvel,
d.ten_J,
d.njmax,
Expand Down Expand Up @@ -2459,7 +2458,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.jnt_range,
m.jnt_margin,
m.dof_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.jnt_limited_ball_adr,
d.qpos,
d.qvel,
Expand Down Expand Up @@ -2498,7 +2497,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.jnt_range,
m.jnt_margin,
m.dof_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.jnt_limited_slide_hinge_adr,
d.qpos,
d.qvel,
Expand Down Expand Up @@ -2538,7 +2537,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.tendon_range,
m.tendon_margin,
m.tendon_invweight0,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
m.tendon_limited_adr,
d.qvel,
d.ten_J,
Expand Down Expand Up @@ -2584,7 +2583,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.dof_bodyid,
m.dof_parentid,
m.geom_bodyid,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
d.qvel,
d.subtree_com,
d.cdof,
Expand Down Expand Up @@ -2638,7 +2637,7 @@ def make_constraint(m: types.Model, d: types.Data):
m.dof_bodyid,
m.dof_parentid,
m.geom_bodyid,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
d.qvel,
d.subtree_com,
d.cdof,
Expand Down
3 changes: 1 addition & 2 deletions mujoco_warp/_src/constraint_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
import mujoco_warp as mjw
from mujoco_warp import ConeType
from mujoco_warp import test_data
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN

# tolerance for difference between MuJoCo and MJWarp constraint calculations,
# mostly due to float precision
Expand Down Expand Up @@ -59,7 +58,7 @@ def _assert_efc_eq(mjm, m, d, mjd, nefc, name, nv):
mjd_sort_indices = np.lexsort((mjd_efc_pos, mjd_efc_type, mjd_efc_vel, mjd_efc_aref, mjd_efc_d))

# convert sparse to dense if necessary
if SPARSE_CONSTRAINT_JACOBIAN:
if m.is_sparse:
efc_J = np.zeros((nefc, nv))
mujoco.mju_sparse2dense(
efc_J,
Expand Down
3 changes: 1 addition & 2 deletions mujoco_warp/_src/forward_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
from mujoco_warp import GainType
from mujoco_warp import IntegratorType
from mujoco_warp import test_data
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN

# tolerance for difference between MuJoCo and mjwarp smooth calculations - mostly
# due to float precision
Expand Down Expand Up @@ -399,7 +398,7 @@ def _getattr(arr):
nefc = d.nefc.numpy()[0]
if nefc > 0:
nv = m.nv
if SPARSE_CONSTRAINT_JACOBIAN:
if m.is_sparse:
# Reconstruct dense J from sparse representation
d_efc_J = np.zeros((nefc, nv))
mujoco.mju_sparse2dense(
Expand Down
7 changes: 3 additions & 4 deletions mujoco_warp/_src/io.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
from mujoco_warp._src import types
from mujoco_warp._src import warp_util
from mujoco_warp._src.types import MJ_MINVAL
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN
from mujoco_warp._src.types import BiasType
from mujoco_warp._src.types import TrnType
from mujoco_warp._src.types import vec10
Expand Down Expand Up @@ -712,7 +711,7 @@ def make_data(
contact = types.Contact(**{f.name: _create_array(None, f.type, sizes) for f in dataclasses.fields(types.Contact)})
efc = types.Constraint(**{f.name: _create_array(None, f.type, sizes) for f in dataclasses.fields(types.Constraint)})

if SPARSE_CONSTRAINT_JACOBIAN:
if is_sparse(mjm):
efc.J_rownnz = wp.zeros((nworld, njmax), dtype=int)
efc.J_rowadr = wp.zeros((nworld, njmax), dtype=int)
efc.J_colind = wp.zeros((nworld, 1, njmax * mjm.nv), dtype=int)
Expand Down Expand Up @@ -913,7 +912,7 @@ def put_data(

efc = types.Constraint(**efc_kwargs)

if SPARSE_CONSTRAINT_JACOBIAN:
if is_sparse(mjm):
# TODO(team): process efc_J sparsity structure for nv row shift
efc.J_rownnz = wp.array(np.full((nworld, njmax), mjm.nv, dtype=int), dtype=int)
efc.J_rowadr = wp.array(
Expand Down Expand Up @@ -1151,7 +1150,7 @@ def get_data_into(
mujoco.mj_factorM(mjm, result)

if nefc > 0:
if SPARSE_CONSTRAINT_JACOBIAN:
if is_sparse(mjm):
efc_J = np.zeros((nefc, mjm.nv))
mujoco.mju_sparse2dense(
efc_J,
Expand Down
3 changes: 1 addition & 2 deletions mujoco_warp/_src/io_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@
from mujoco_warp import test_data
from mujoco_warp._src import warp_util
from mujoco_warp._src.io import set_length_range
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN


def _assert_eq(a, b, name):
Expand Down Expand Up @@ -1613,7 +1612,7 @@ def test_put_data_nefc_zero_dense(self):
m = mjwarp.put_model(mjm)
d = mjwarp.put_data(mjm, mjd)

if SPARSE_CONSTRAINT_JACOBIAN:
if m.is_sparse:
self.assertEqual(d.efc.J.shape[2], d.njmax * m.nv)
else:
self.assertEqual(d.efc.J.shape[2], m.nv_pad)
Expand Down
3 changes: 1 addition & 2 deletions mujoco_warp/_src/smooth.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
from mujoco_warp._src import util_misc
from mujoco_warp._src.types import MJ_MAXVAL
from mujoco_warp._src.types import MJ_MINVAL
from mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN
from mujoco_warp._src.types import CamLightType
from mujoco_warp._src.types import ConeType
from mujoco_warp._src.types import Data
Expand Down Expand Up @@ -2622,7 +2621,7 @@ def transmission(m: Model, d: Data):
d.efc.J_colind,
d.efc.J,
d.nacon,
SPARSE_CONSTRAINT_JACOBIAN,
m.is_sparse,
],
outputs=[d.actuator_moment, ncon],
)
Expand Down
Loading