Kinetic energy consistently delayed by one step

Intro

Hi!

I am a graduate student, I use MuJoCo to practice control theory in my research.

My setup

MuJoCo - 3.2.3

Python - 3.10.14 | packaged by Anaconda, Inc. | (main, Mar 21 2024, 16:20:14) [MSC v.1916 64 bit (AMD64)]

OS - Windows-10-10.0.22631-SP0

What's happening? What did you expect?

When calculating kinetic energy in a simulation, I noticed a consistent one-step delay between the manually calculated kinetic energy (calc_kinetic_energy) and the kinetic energy stored in data.energy[1] provided by the simulation.

Specifically, the value at step N for calc_kinetic_energy matches the value at step N+1 for data.energy[1], causing a mismatch at every step.

Interestingly, while the potential energy returned by data.energy[0] remains consistent at each step, the kinetic energy seems to be delayed.

Additionally, even when accounting for the one-step delay, there is still a slight discrepancy between the manually calculated kinetic energy and the simulation-provided value. I apologize for the confusion if this is due to a mistake in my calculations or my approach.

I expected the manually calculated kinetic energy (calc_kinetic_energy) to match the simulation-provided kinetic energy from data.energy[1] at each step without any delay.

Steps for reproduction

Install packages.
- pip install mujoco
- pip install robot_descriptions
Run the code below.
Compare the results of the following two loops.
- zip(calc_kinetic_energy, data_kinetic_energy)
- zip(calc_kinetic_energy, data_kinetic_energy[1:])

Minimal model for reproduction

No response

Code required for reproduction

import sys
import platform
import numpy as np
import mujoco
from robot_descriptions import ur5e_mj_description

print('python version:', sys.version)
print('mujoco version:', mujoco.__version__)
print('OS version: ', platform.platform())

# model & data
model = mujoco.MjModel.from_xml_path(ur5e_mj_description.MJCF_PATH)
for i in range(model.nu):
    model.actuator_dynprm[i][:3] = [1,0,0]
    model.actuator_gainprm[i][:3] = [1,0,0]
    model.actuator_biasprm[i][:3] = [0,0,0]

model.opt.enableflags = mujoco.mjtEnableBit.mjENBL_ENERGY
data = mujoco.MjData(model)

calc_kinetic_energy = []
data_kinetic_energy = []

for _ in range(10):

    mujoco.mj_step(model, data)

    # calc_potential = model.body_mass @ data.xipos @ (-model.opt.gravity)

    vec = np.zeros(model.nv)
    mujoco.mj_mulM(m=model, d=data, res=vec, vec=data.qvel)
    calc_kinetic = 0.5*mujoco.mju_dot(vec, data.qvel)

    # energy = np.array([calc_potential, calc_kinetic])
    # print(data.energy - energy)

    calc_kinetic_energy.append(calc_kinetic)
    data_kinetic_energy.append(data.energy[1])

s = f"| {'calc_kinetic':^12} | {'data_kinetic':^12} | {'diff':^11} |"
# compare all steps
print('-'*len(s))
print(s)
for calc_kin, data_kin in zip(calc_kinetic_energy, data_kinetic_energy):
    print(f'| {calc_kin:^12.8f} | {data_kin:^12.8f} | {calc_kin - data_kin:^11.8f} |')
print('-'*len(s))
print()
# compare from the second step onward
print('-'*len(s))
print(s)
for calc_kin, data_kin in zip(calc_kinetic_energy, data_kinetic_energy[1:]):
    print(f'| {calc_kin:^12.8f} | {data_kin:^12.8f} | {calc_kin - data_kin:^11.8f} |')
print('-'*len(s))

Confirmations

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

google-deepmind / mujoco