Having trouble with mjd_transitionFD and contacts #3113

andyabateagility · 2026-02-18T04:31:13Z

andyabateagility
Feb 18, 2026

Intro

Hi, I am trying to make a simple optimal controller using mjd_transitionFD to find a control vector to best minimize IK tracking over a single timestep (to start, and working towards a full rollout).

My method is straightforward: get task Jacobians de/dq and multiply by the dq/du gotten from transitionFD, then solve as a least-squares problem (mju_boxQP(H=J.T@J + damping, g=J.T@e)).

This method works great in free space, and is only a couple lines of code (horray!). However, I am finding that the jacobian from transitionFD is a very poor approximation of dynamics when in contact with an object (i.e. in free space, mj_integratePos(data.qpos, DqDu @ data.ctrl, dt=1) matches data.qpos resulting from an mj_step to 1e-15, but in contact that error is on the order of 1e-2). I find that if I set solref and solimp to give linear soft contacts (see minimal model) and update state with the linearization, my method works through contact.

My setup

MuJoCo 3.4.0, Python, Ubuntu 24.04.4

My question

So, two questions:

How could I make mjd_transitionFD better match the dynamics of my system? It feels like I'm missing something important, because my results feel "glitchy" and "wrong". I would expect this to give good results with default solref and smooth solimp = [0, 1, 0.01, 0.5, 2], but it's quite bad....
As a result of linearizing (solref=[-100,-10]) and making constant (solimp=[1,1,...]) contacts (because that improves the behavior of transitionFD), I'm seeing very weird "sinking"/"pinging" of contacts. They don't seem to settle an equilibrium position, but instead mujoco seems to drive efc_pos to zero, loses the contact, the model penetrates, repeat. See the attached minimal model. How can I prevent this?

Minimal model and/or code that explain my question

<mujoco model="/home/andyabate/Desktop/twitchy-mujoco.blend">
<compiler angle="radian"/>
<option timestep="0.01" integrator="implicitfast"/>
<size nkey="1"/>
<default>
<geom solref="-100 -10" solimp="1 1 0.1"/>
</default>
<worldbody>
<geom name="Cube" size="0.1 0.1 0.1" pos="0 0 -0.100205" type="box" mass="0"/>
<body name="Cube.001" pos="-0.115702 -6.89094e-08 0.341069" quat="0.707107 0.707107 9.72705e-06 -4.04809e-08">
<inertial pos="0.1 0 0" mass="10.0001" diaginertia="1e-12 1e-12 1e-12"/>
<joint name="Cube.001" pos="0 0 0" axis="0 0 1" range="-inf inf" damping="0.1"/>
<site name="Cube.001" pos="0 0 0"/>
<body name="Cube.002" pos="0.2 8.9407e-08 0" quat="0.674842 -0.674842 0.211159 -0.211159">
<inertial pos="0.1 0 0" mass="10.0001" diaginertia="1e-12 1e-12 1e-12"/>
<joint name="Cube.002" pos="0 0 0" type="ball" range="0 inf" damping="0.1"/>
<geom name="Icosphere" size="0.0183352" pos="0.203057 1.86265e-09 7.45058e-09" quat="1 -1.67638e-08 4.65661e-09 -2.23517e-08" mass="0"/>
<site name="Cube.002" pos="0 0 0"/>
</body>
</body>
</worldbody>
</mujoco>

Code (sketch, not runnable):

        data.ctrl[:] = data.actuator_force * 0.99 # use saturated control, give room for epsilon
        mj.mj_step1(model, data)

        # XXX: returns change in _next_ state, but resets data to current state
        mj.mjd_transitionFD(model, data, 1e-3, 0, None, D, None, None)
        Dv = D[nv:2*nv,:] # dqdot/du
        Dx = D[:nv,:] # dq/du

        mj.mj_step2(model, data) # step forward to get the _next_ state's error
        mj.mj_step1(model, data) # XXX: needed before getting errors, even after step

        H = np.zeros((nu,nu))
        g = np.zeros(nu)

        def add_task(w, J, e):
            """J unused if H and g are None"""
            nonlocal H, g
            if H is not None: H += w * J.T@J
            if g is not None: g += w * J.T@e
            return 0.5 * w * sum(e**2)

        for target in self.targets:
            J, e = target.get_J_e(model, data)
            J = J @ Dx # de/dq @ dq/du ==> de/du
            add_task(weight, J, -e)
        
        # stabilize by minimizing joint velocity
        add_task(1e-4, Dv, data.qvel)

        D = np.sqrt(H.diagonal())
        Hi = H.copy()
        Hi[range(nu), range(nu)] += 1e-4 * D
        result = mj.mju_boxQP(du, R, index=None, H=Hi, g=g,
            lower=model.actuator_forcerange[:,0]-data.ctrl, upper=model.actuator_forcerange[:,1]-data.ctrl)
        print(result)
        data.ctrl[:] += du
        print('ctrl', np.linalg.norm(data.ctrl))
        mj.mj_integratePos(model, data.qpos, Dx @ data.ctrl, dt=1) # have already stepped forward, now adjust with DqDu
        data.qvel += Dv @ data.ctrl

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Having trouble with mjd_transitionFD and contacts #3113

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Replies: 0 comments

Select a reply

Uh oh!

Having trouble with mjd_transitionFD and contacts #3113

Uh oh!

Uh oh!

andyabateagility Feb 18, 2026

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 0 comments

andyabateagility
Feb 18, 2026