Get the whole force on joints

[jommsy]

Hi,

I'm a student and I'm trying to use MuJoCo for hydraulic excavator, combined with Simulink. I'm looking for some help with how to get the whole forces on joint.

In the XML file, I use only slide joint between cylinder tube and piston, doesn't add any cylinder actuator.

I want to get the whole forces on joint (Here I use qfrc_bias and qfrc_constraint) and transmit it to Simulink, which simulate the valve-controlled hydraulic system. And get back the hydraulic force and set it as qfrc_applied. In Simulink I also model a simplified cylinder tube and piston, with same mass in mujoco. My idea is that the motion of the hydraulic cylinders in mujoco and simulink should be roughly synchronized.

The workflow is shown as below,

mj_step1()
for mujoco_run_time < limit_time
    load = qfrc_bias + qfrc_constraint
    qfrc_applied = simulink(load)

    for Mujoco simulate 4 steps
        mj_step()

When I set all valves as closed, which means the cylinder should be locked, qfrc_applied also roughly equals qfrc_bias + qfrc_constraint according record, but the model in mujoco can still move cause gravity. So I am thinking if I miss some other variables of the forces in joint or there is something wrong with my workflow.

Here is the screenshot of my model. And the frames are body frame. The red box identifies the points where I use the equality constraints (<connect> type).

Thanks for your help.

[jommsy]

I experienced this issue previously as well. I believe the cause is that MuJoCo models the constraint as a soft constraint, meaning it's not as rigid as we might expect.

mujoco_equality.mp4

You mean the equality constraint points will separate? I set the solref and timestep as follows, and it can works well

  <default>
    <equality solref="0.010 1" />
  </default>

  <option timestep="0.005" integrator="implicitfast"/>

The equality solref should be at least 2 times of timestep.

And how do you finally fix the problem of force?

The documentation is
mujoco_equality.webm

[Balint-H]

Depending on your model you might also be missing qfrc_passive and qfrc_actuator. Also you might want to apply any forces after mj_step1 but before mj_step2 (or via ctrl callback)

[jommsy]

or via ctrl callback

Sorry I forgot to mention that, I didn't add any properties about spring, damping and gravidity compensator and any actuators. So qfrc_passive and qfrc_actuator are zero. And I record some data in order of code execution while set all cylinder "locked" in simulink. (Here load = qfrc_constraint + qfrc_bias + qfrc_passive + qfrc_actuator )

And I want to use dm_control for reinforcement learning, so I simulate the workflow as dm_control in my demo code, that is execute mj_step1() to compute all variables depend on pos and vel firstly and then mj_step2() compute all depend on force, but I think it doesn't influence.

mujoco_time, 0.05499999999999999
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  1.44575202  68.31653994  21.47776481 -52.42886128]
qfrc_constraint, [-1.97230703e-06  7.87743783e+01 -2.05386420e+02 -2.69614931e+02]
load, [   1.44575004  147.09091819 -183.90865568 -322.04379225]
qfrc_applied, [  -1.44574521 -147.9623971   183.65111425  322.5885231 ]

mujoco_time, 0.05999999999999999
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  1.75499626  68.27353678  21.34614925 -52.53484053]
qfrc_constraint, [-1.86435437e-06  2.04225878e+02 -1.33187111e+02 -3.13217998e+02]
load, [   1.75499439  272.49941493 -111.84096204 -365.75283878]
qfrc_applied, [  -1.44574521 -147.9623971   183.65111425  322.5885231 ]

mujoco_time, 0.06499999999999999
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  2.09766798  68.22476455  21.19820431 -52.64505243]
qfrc_constraint, [-1.92541749e-06  2.14031313e+02 -1.27159740e+02 -3.17103169e+02]
load, [   2.09766605  282.25607762 -105.96153594 -369.74822172]
qfrc_applied, [  -2.09766607 -283.36768242  105.09060857  370.3283967 ]

mujoco_time, 0.06999999999999999
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  2.46906785  68.17137542  21.03489961 -52.7621387 ]
qfrc_constraint, [-1.75064368e-06  3.39750102e+02 -5.45543221e+01 -3.58993285e+02]
load, [   2.4690661   407.92147709  -33.51942249 -411.75542348]
qfrc_applied, [  -2.09766607 -283.36768242  105.09060857  370.3283967 ]

mujoco_time, 0.6450000000000005
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  0.08850392  55.14023422  -7.26324137 -57.56216599]
qfrc_constraint, [ 2474.51420873  7227.89858472 -1068.91126175 -6395.48725397]
load, [ 2474.60271265  7283.03881894 -1076.17450311 -6453.04941996]
qfrc_applied, [-2474.75606577 -7286.57810521  1075.24069832  6450.2398211 ]

mujoco_time, 0.6500000000000005
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [  0.06867819  55.14898646  -7.24320923 -57.55965878]
qfrc_constraint, [ 2441.47287255  7335.76937409 -1073.84496431 -6477.54775569]
load, [ 2441.54155075  7390.91836055 -1081.08817354 -6535.10741446]
qfrc_applied, [-2474.75606577 -7286.57810521  1075.24069832  6450.2398211 ]

mujoco_time, 0.6550000000000005
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [ 4.73683453e-02  5.51545802e+01 -7.23037196e+00 -5.75579982e+01]
qfrc_constraint, [ 2441.72404531  7340.30647204 -1074.88727304 -6482.13511258]
load, [ 2441.77141365  7395.46105229 -1082.117645   -6539.69311076]
qfrc_applied, [-2441.54154665 -7398.42920159  1081.12228084  6536.16267044]

mujoco_time, 0.6600000000000005
qfrc_passive, [0. 0. 0. 0.]
qfrc_actuator, [0. 0. 0. 0.]
qfrc_bias, [ 2.65392668e-02  5.51573725e+01 -7.22380503e+00 -5.75570385e+01]
qfrc_constraint, [ 2415.20546373  7448.2815133  -1081.14448318 -6568.18056041]
load, [ 2415.232003    7503.43888576 -1088.3682882  -6625.73759895]
qfrc_applied, [-2441.54154665 -7398.42920159  1081.12228084  6536.16267044]