search

google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
7.94k stars 797 forks source link

Global cartesian acceleration with `mj_objectAcceleration` #598

Closed ronansgd closed 1 year ago

ronansgd commented 1 year ago

Hi, I am a student using MuJoCo for a university project; I precisely use the version 2.2.2 of the Python bindings on Ubuntu 20.04

I wanted to compute the acceleration in global cartesian coordinates of a body using the function mj_objectAcceleration.

To test that I created a simple pendulum model pendulum.xml, and wrote a python script swing.py letting the pendulum swing.

pendulum.xml
swing.py import os import mujoco as mjc from mujoco import mjtObj, MjModel, MjData import numpy as np model_path = "path/to/pendulum.xml" assert os.path.isfile(model_path) model = MjModel.from_xml_path(model_path) sim = MjData(model) mjc.mj_forward(model, sim) pend_body_id = mjc.mj_name2id(model, mjtObj.mjOBJ_BODY, "pendulum") n_steps = 200 pos_array = np.zeros((n_steps, 3)) vel_array = np.zeros((n_steps, 3)) acc_array = np.zeros((n_steps, 3)) for idx in range(n_steps): mjc.mj_step(model, sim) pos_array[idx] = sim.xipos[pend_body_id] # get velocity res_array = np.zeros(6) mjc.mj_objectVelocity(model, sim, mjtObj.mjOBJ_BODY, pend_body_id, res_array, 0) vel_array[idx] = res_array[-3:] # get acceleration mjc.mj_objectAcceleration( model, sim, mjtObj.mjOBJ_BODY, pend_body_id, res_array, 0) acc_array[idx] = res_array[-3:] approx_vel = (pos_array[1:] - pos_array[:-1]) / model.opt.timestep print("vel error: ", np.linalg.norm(approx_vel - vel_array[1:]) / (n_steps - 1)) approx_acc = (vel_array[1:] - vel_array[:-1]) / model.opt.timestep print("acc error: ", np.linalg.norm(approx_acc - acc_array[1:]) / (n_steps - 1)) print("init acc: ", acc_array[0])

Running the script, I get a vel error of roughly 0.000759, which sounds reasonable.

However, the acc error is close to 0.582496, which doesn't look good. Moreover, the init acc is displayed as [0. 0. 0.], while I would have expected it to be close to gravity (ie [0. 0. -9.81]) given that initially the pendulum is horizontal.

I guess that I misunderstood something about mj_objectAcceleration, can someone point out what I got wrong?

Thank you for your time.

happygaoxiao commented 1 year ago

I have the same issue. The mj_objectVelocity seems to be correct, but mj_objectVelocity always return zero.

yuvaltassa commented 1 year ago

@happygaoxiao Presumably you meant acceleration? Could you try using an accelerometer sensor? If not, please provide a MWE?

Regarding @ronansgd's OP (sorry, clearly dropped the ball here), the errors one would expect for accelerations, given your code, should be 1/dt larger than the velocity errors, so I think this WAI. Do you disagree?

happygaoxiao commented 1 year ago

Hello, sorry, I mean the acceleration always return zero. Here is a simple test. An object falls from a height of 1m.

import numpy as np
import mujoco
import time
from mujoco import viewer

cylinder_falling = """
<mujoco model="cylinder">
  <option>
    <flag gravity="enable"/>
  </option>

  <default class="obj">
    <geom condim="6" friction="1 0.5 0.01" solimp="1.1 1.2 0.001 0.5 2" solref="0.02 2"/>
  </default>

  <worldbody>
    <light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
    <light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>

    <body name="table" pos="0.0 0 0.1">
          <geom name="table1" class="obj"  type="box" size="0.3 0.6 0.1" pos="0 0 0" mass="10"/>
    </body>

    <body name="cylinder1" pos="0 0 1" quat="1  1. 0. 0.">
        <joint name="cylinder1:joint" type="free"/>
        <geom name="cylinder1" class="obj" mass="0.1" pos="0 0 0" rgba="0 1 0 1" size="0.02 0.15" type="cylinder"/>
    </body>
  </worldbody>

</mujoco>
"""

model = mujoco.MjModel.from_xml_string(cylinder_falling)
data = mujoco.MjData(model)
view = viewer.launch_passive(model, data)

mujoco.mj_forward(model, data)

t0 = time.time()
while 1:
    t = time.time() - t0
    if t > 1:
        break
    mujoco.mj_step(model, data)
    view.sync()

    vel = np.zeros(6)
    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder1")
    mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_BODY,
                             body_id, vel, 0)  # 1 for local, 0 for world frame
    acc = np.zeros(6)
    mujoco.mj_objectAcceleration(model, data, mujoco.mjtObj.mjOBJ_BODY,
                                 body_id, acc, 0)

    print(t, vel, acc)
    time.sleep(0.002)

Here is the print output:

2.384185791015625e-06 [0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0.] 0.0027878284454345703 [ 0. 0. 0. 0. 0. -0.01962] [0. 0. 0. 0. 0. 0.] 0.00539851188659668 [ 0. 0. 0. 0. 0. -0.03924] [0. 0. 0. 0. 0. 0.] 0.007817983627319336 [ 0. 0. 0. 0. 0. -0.05886] [0. 0. 0. 0. 0. 0.] 0.01036381721496582 [ 0. 0. 0. 0. 0. -0.07848] [0. 0. 0. 0. 0. 0.] 0.012817621231079102 [ 0. 0. 0. 0. 0. -0.0981] [0. 0. 0. 0. 0. 0.] 0.01527857780456543 [ 0. 0. 0. 0. 0. -0.11772] [0. 0. 0. 0. 0. 0.] 0.01778888702392578 [ 0. 0. 0. 0. 0. -0.13734] [0. 0. 0. 0. 0. 0.] 0.02035379409790039 [ 0. 0. 0. 0. 0. -0.15696] [0. 0. 0. 0. 0. 0.] 0.022930145263671875 [ 0. 0. 0. 0. 0. -0.17658] [0. 0. 0. 0. 0. 0.] 0.02557849884033203 [ 0. 0. 0. 0. 0. -0.1962] [0. 0. 0. 0. 0. 0.] 0.02804088592529297 [ 0. 0. 0. 0. 0. -0.21582] [0. 0. 0. 0. 0. 0.] 0.03049755096435547 [ 0. 0. 0. 0. 0. -0.23544] [0. 0. 0. 0. 0. 0.] 0.03313016891479492 [ 0. 0. 0. 0. 0. -0.25506] [0. 0. 0. 0. 0. 0.] 0.03551840782165527 [ 0. 0. 0. 0. 0. -0.27468] [0. 0. 0. 0. 0. 0.]

I have checked that the model.opt.gravity is [0,0,-9.81], and model.opt.timestep=0.002. The velocity is correct and is increasing 0.002*9.81=0.01962 m/s in each step. But the acceleration is always zero.

yuvaltassa commented 1 year ago

Okay there are two parts to the answer, neither are obvious and should be properly documented, which we will take care of soon. In the meantime this reply can act as documentation.

  1. In order for mj_objectAcceleration to report anything, you need to call mj_rnePostConstraint. Why is this? Recursive Newton Euler is run in the middle part of the pipeline (step 17), not taking into account the constraint forces. In order to calculate things like F/T sensors and accelerometers, another call to RNE is required, taking into account constraint forces and induced accelerations, calculating the quantities:

    // computed by mj_sensorAcc/mj_rnePostConstraint if needed; rotation:translation format
mjtNum* cacc;              // com-based acceleration                           (nbody x 6)
mjtNum* cfrc_int;          // com-based interaction force with parent          (nbody x 6)
mjtNum* cfrc_ext;          // com-based external force on body                 (nbody x 6)

    Because these quantities are not required for the next step, it would be wasteful to always compute them. MuJoCo therefore only calls mj_rnePostConstraint if a sensor which requires these quantities is defined. Of course the user can always call the function manually.

  2. Understanding the computed values is also perhaps unintuitive, and requires a bit of (surprise!) general relativity. Consider the following MWE:

    
xml = """
<mujoco>
<worldbody>
<geom name="floor" type="plane" size="1 1 .1"/>

<body name="falling" pos="0 0 10">
  <freejoint/>
  <geom size="0.1"/>
</body>

<body name="resting" pos="0 0 .1">
  <freejoint/>
  <geom size="0.1"/>
</body>

</worldbody>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

roll out for 200ms so resting body contact stabilises

while data.time < 0.2: mujoco.mj_step(model, data)

call rnePostConstraint to compute cacc

mujoco.mj_rnePostConstraint(model, data)

id_falling = model.body('falling').id id_resting = model.body('resting').id id_world = model.body('world').id

assert id_world == 0

xacc = np.zeros(6) type_body = mujoco.mjtObj.mjOBJ_BODY flg_local = 0 mujoco.mj_objectAcceleration(model, data, type_body, id_world, xacc, flg_local) print('world acceleration', xacc) mujoco.mj_objectAcceleration(model, data, type_body, id_resting, xacc, flg_local) print('resting acceleration', xacc) mujoco.mj_objectAcceleration(model, data, type_body, id_falling, xacc, flg_local) print('falling acceleration', xacc)

This code will output

world acceleration [0. 0. 0. 0. 0. 9.81] resting acceleration [0. 0. 0. 0. 0. 9.81] falling acceleration [0. 0. 0. 0. 0. 0.]


In MuJoCo, *gravity is not implemented as a force*. Instead, we use Einstein's [Equivalence Principle](https://en.wikipedia.org/wiki/Equivalence_principle), modeling gravity as an *upward acceleration of the world frame*. This might seem strange if you're used to high-school mechanics, but is more consistent with reality: Take an accelerometer and throw it in the air. During the flight phase it will report no acceleration, even though the trajectory is curved. Once it reaches the ground and stops moving, it will report gravity, even though it is stationary w.r.t the world. To summarise, accelerations reported by MuJoCo take into account the fact that the *world is accelerating up*. This is actually happening in reality, due to the local curvature between the vertical direction and the temporal direction, induced by the mass of the earth.