Joint pos, vel, acc difference when applying muscle force in two different ways

[Yingfan99327]

Intro

Hi!

I am a graduate student working on human robot interactions, I use MuJoCo for my research.

My setup

I used mujoco, especially the muscle tendon model supported in mujoco for my research and I found some problems I am not clear about. So, I ask for help from the community.

My question

Generally speaking, the problem I encounted in my experiment is that the joint pos, velocity, acceleration results are different when I applying muscle actuation force in different ways.

What I aim to do is that I want to bypass the way of calculating muscle force in mujoco and implement the way of calculating muscle actuation force with my own implementation. For the first step, I checked Mujoco's way of calculating muscle force in the document as well as in the source code of engine_forward.c. Then, I reproduced the Mujoco's way of calculating muscle force in Python and I used this as the ctrl signal by adjusting the actuator from tendon to motor in the model file.

The model file I pasted below is the reference model file consisting of a single muscle and a single joint with the actuator defined as shown below.

<actuator>
    <general name="DELT1" tendon="DELT1_tendon" biasprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" class="upperarm_muscle" gainprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" lengthrange="0.123363 0.240922"/>
</actuator>

For the reference model, I used the way of calculating muscle actuation force in mujoco by defining the ctrl singal as the muscle excitation signal and write a simple for-loop as below:

for i in range(10)
    data.ctrl = i/10
    mujoco.mj_step(model, data)

So, I use the reference model to collect the actuator_length, actuator_velocity, act_dot, act, actuator_force, qfrc_actuator, qpos, qvel, qacc data to check whether my own implementation of caculating muscle force is correct or not.

Then, I implemented the way of calculating muscle force with Python. And I modified the model file in only one place which is that I changed the actuator as

<actuator>
        <motor name="shoulder_joint" tendon="DELT1_tendon" />
</actuator>

For the data of gainprm, biasprm, lengthrange of this muscle, I extracted them and fed them for calculation in the codes. So, with this model, the ctrl signal will be the actuation_force calculated with my Python implementation and the for loop is shown as below,

for i in range(10)
    ctrl = i/10
    F_actuation = Func_calc_muscle_force(ctrl, act, actuator_length, actuator_velocity)
    data.ctrl = F_actuation
    mujoco.mj_step(model, data)

Then, I record the actuator_force, qfrc_actuator, qpos, qvel, qacc data and compared them with the data collected from the reference model to check my Python codes of calculating the muscle force is the same as that in mujoco. So, this is basic the experiment set-up.

First, I am sure that the implementation of Func_calc_muscle_force is correct, I compared the some intermediate results by calling the mju_muscleGain and mju_muscleBias function to reduce the artifact error and I am pretty sure that my implementation is correct. But, what I found is that, for the first two loops, namely, ctrl=0.0, ctrl=0.1, the results matched, while after the third loop, the results have slight difference that

After the second loop
qpos, qvel, qacc data for the reference model afte mj_step in the second loop 
qpos:  [0.00075047]
qvel:   [0.25009656]
qacc:  [62.47850293]

qpos, qvel, qacc data for my implementation model afte mj_step in the second loop 
qpos:  [0.00075047]
qvel:   [0.25009656]
qacc:  [62.47850293]


After the third loop
qpos, qvel, qacc data for the reference model afte mj_step in the third loop 
qpos:  [0.00163072]
qvel:  [0.44012368]
qacc:  [95.65911787]

qpos, qvel, qacc data for my implementation model afte mj_step in the third loop 
qpos:  [0.0016333]
qvel:  [0.44141479]
qacc:  [95.65911787]

First, I checked the actuation force, qfrc_actuator etc, they match. The qpos, qvel, qacc after the second loop, they also match, which means that the caculation is correct after the first two loops. Things went wrong in the third loop and the weird thing is that, you can see, the qacc is the same, while the qvel and qpos are different. The integrator I used in both model files is "implicitfast" which is recommended in the document for muscle-tendon.

And it could be easily observed that, if I used the actuation force calculated on my own as the data.ctrl, the qvel and qpos is integrated in the "Euler" way, I checked with calculator by hand. And it could be observed that the way of calculating qpos, qvel are different if I used the way of actutating a tendon built in mujoco.

Then, I compared the results if I used different integrators in the two model files with Euler, implicit, implicitfast, RK4, and I found that

1. With "Euler", my results are exactly the same as the reference results which is exactly what I expect
2. With "implicitfast" or "implicit", there would be the same error between my results and the reference resulst as I showed above
3. With "RK4", first, the reference results also vary from those obtained with "implicit" or "implicitfast" with the reference model. I understand, because if I use different integrators for the same model, you could get different results. With "RK4", the error between the reference results and my results are larger.

So, based on the experiment set-up and experiment results as I show above, I guess there would be something wrong with my way of using the actuation force to actuate a tendon and then move the joint because it seems the "implicitfast" integrators didn't work and it just did the integration in "Euler" way.

I don't know what I missed when I implemented the model and I hope to get some feedback from you about the problems I met and whether it is correct to calculate the muslce force in the way I described. I also hope to get some advice on what I need to pay attention to if I hope to get the same results as those calculated with the internal muscle dynamics function in mujoco.

I really like and enjoying using mujoco and I am very happy each time I know more and deeped about it.

Really appreciate your help and wishing you all the best.

Minimal model and/or code that explain my question

<include file="basic_scene.xml"/>

<compiler inertiafromgeom="auto" balanceinertia="true" boundmass="0.001" boundinertia=".0001" angle="radian" meshdir="/home/yingfan/Documents/GitHub/hrenv_with_robot_model_and_controller/assets/meshes" autolimits="true"/>

<size njmax='1000' nconmax='1000'/> 

<visual>
  <global offwidth="1000" offheight="1000"/>
    <headlight diffuse="1 1 1"/>
<scale actuatorwidth=".075"/>
</visual>

<option integrator="implicitfast"/>

<default class="main">

    <geom contype="0" conaffinity="0"/>

    <tendon rgba="0.95 0.3 0.3 41" width="0.001"/>

    <default class="arm">

        <joint limited="true" armature="1e-4" stiffness="0"/>
        <geom margin="0.001" rgba="0.8 0.8 0.8 1" contype="0" conaffinity="0"/>
        <site size="0.0025" group="2"/>
        <tendon rgba="0.95 0.3 0.3 41" width="0.003"/>

        <default class="muscle">
            <general biasprm="0.75 1.05 -1 200 0.5 1.6 1.5 1.3 1.2 0" biastype="muscle" ctrllimited="true" ctrlrange="0 1" dynprm="0.01 0.04 0 0 0 0 0 0 0 0" dyntype="muscle" gainprm="0.75 1.05 -1 200 0.5 1.6 1.5 1.3 1.2 0" gaintype="muscle"/>
        </default>

    </default>

    <default class="upperarm_muscle">
        <general biasprm="0.75 1.05 -1 200 0.5 1.6 1.5 1.3 1.2 0" biastype="muscle"
                 ctrllimited="true" ctrlrange="0 1" dynprm="0.01 0.04 0 0 0 0 0 0 0 0"
                 dyntype="muscle" gainprm="0.75 1.05 -1 200 0.5 1.6 1.5 1.3 1.2 0" gaintype="muscle"/>
        <tendon rgba="0.95 0.3 0.3 41" width="0.007"/>
    </default>

</default>

<asset>

    <mesh file="arm/clavicle.stl" name="clavicle_geom_1_clavicle"/>
    <mesh file="arm/scapula.stl" name="scapula_geom_1_scapula"/>
    <mesh file="arm/humerus.stl" name="humerus_geom_1_humerus"/>

    <mesh name="clavicle" file="arm/clavicle.stl" scale="1 1 1"/>
    <mesh name="scapula" file="arm/scapula.stl" scale="1 1 1"/>
    <mesh name="humerus" file="arm/humerus.stl" scale="1 1 1"/>

    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .19 .2" rgb2=".1 0.11 0.11" width="50" height="50"/>
    <texture name="texgeom" type="cube" builtin="flat" mark="cross" width="127" height="1278" rgb1="0.7 0.7 0.7" rgb2="0.9 0.9 0.9" markrgb="1 1 1" random="0.01"/>
    <material name="MatPlane" reflectance="0.5" texture="texplane" texrepeat="4 4" texuniform="true"/>
    <material name="geom" texture="texgeom" texuniform="true"/>
    <material name="MatSkin" specular="0.2" shininess=".1" reflectance="0.5" rgba="0.9 .7 .5 1"/>
</asset>

<tendon>
    <spatial name="DELT1_tendon" class="upperarm_muscle">
        <site site="DELT1_DELT1-P1"/>
        <site site="DELT1_DELT1-P2"/>
        <site site="DELT1_DELT1-P3"/>
        <site site="DELT1_DELT1-P4"/>
    </spatial>

</tendon>

<actuator>
    <general name="DELT1" tendon="DELT1_tendon" biasprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" class="upperarm_muscle" gainprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" lengthrange="0.123363 0.240922"/>
</actuator>

<worldbody>

    <body name="clavicle"  pos="0.006325 -0.02547 0.00693" childclass="arm" euler="1.57 -1.57 0">

        <inertial diaginertia="0.001 0.001 0.001" mass="0.156" pos="-0.011096 0.0063723 0.054168" quat="0.906292 0.0334036 0.164622 -0.387838"/>

        <geom mesh="clavicle_geom_1_clavicle" name="clavicle_geom_1" type="mesh" />

        <site name="DELT1_DELT1-P4" pos="-0.014 0.01106 0.08021"/>

        <body name="clavphant"  pos="-0.01433 0.02007 0.1355">

            <inertial diaginertia="1 1 1" mass="0.001" pos="0 0 0"/>

            <body name="scapula"  pos="0 0 0">

                <inertial diaginertia="0.00199576 0.00103993 0.001" mass="0.70396" pos="-0.054694 -0.035032 -0.043734" quat="0.453021 0.77921 0.0744129 0.426692"/>

                <geom mesh="scapula_geom_1_scapula" name="scapula_geom_1" type="mesh"/>

                <site name="DELT1_DELT1-P3" pos="0.04347 -0.03202 0.00499"/>

                <body name="scapphant"  pos="-0.00955 -0.034 0.009">

                    <inertial diaginertia="1 1 1" mass="0.001" pos="0 0 0"/>

                    <body name="humphant" pos="0 0 0">

                        <inertial diaginertia="1 1 1" mass="0.001" pos="0 0 0"/>

                        <body name="humphant1"  pos="0 0 0">

                            <inertial diaginertia="1 1 1" mass="0.001" pos="0 0 0"/>

                            <body name="humerus"  pos="0 0 0">

                                <inertial diaginertia="0.0128576 0.0121576 0.00239265" mass="1.99757" pos="0.018064 -0.140141 -0.012746" quat="0.407851 0.343714 -0.567755 0.627034"/>

                                <joint axis="0 0 1" name="shoulder_rot" pos="0 0 0" range="-1.571 2.094"/>

                                <geom mesh="humerus_geom_1_humerus" name="humerus_geom_1" type="mesh" />

                                <site name="DELT1hh_sidesite" pos="0.01226 -0.01367 -0.009599"/>

                                <site name="DELT1_DELT1-P1" pos="0.00896 -0.11883 0.00585"/>
                                <site name="DELT1_DELT1-P2" pos="0.01623 -0.1033 -0.00412"/>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </body>

Confirmations

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

[yuvaltassa]

I think you are using the lengths and velocities from the previous timestep. You need to understand the difference between the mj_step sim loop and mj_step1 / mj_step2. Read this carefully.

[Yingfan99327]

Hi Yuval,

Thanks very much for your reply and suggestions.

Yeah, actually I found this error and solved it at the beginning of debugging. I found that the actuator_length and actuator_velocity I used to calculate the actuation force is one timestep behind, so I used some tricks you provided in another discussion (I don't remember which one) that someone asked about the way of calculating the derivative of jacobian, and then I used the right actuator length and velocity to calculate the right actuation force.

I also tried the mj_step1 and mj_step2 for the experiment after your reply as below

for i in range(3):
    mj_step1(model, data)
    F_muscle = Func_calc_muscle_force(actuator_length, actuator_velocity)
    data.ctrl = F_muscle
    mj_step2(model, data)

But I still found the mismatch between qpos, qvel, qacc after the third loop but they match well for the first and second loop. I am curious that if I used the right actuator_length and actuator_velocity to calculate the actuation force and use this to drive the simulation, why the results are still different?

To be more specific, I am wondering why these two methods could present the same results when I use the "Euler" integrators but cannot if I use the "RK4", "implicitfast", "implicit", I also noticed this in the document.

"There is one complication however: this only works with Euler integration. The Runge-Kutta integrator (as well as other advanced integrators we plan to implement) need to evaluate the entire dynamics including the feedback control law multiple times per step, which can only be done using a control callback."

Does this "other advanced integrators we plan to implement" include "implicit" and "implicitfast"?

I also tried using the callback + mj_step, but they mismatched neither.

So, my guess would be that with my way of calculating the muslce actuation force and advancing the simulation, if I chose the integrators from "Euler", "implicitfast" and "implicit", they would all perform in a explicit Euler way. If this is not right, then there would be something I missed to integrate the pos and vel.

I put the results below that you can see the acuation force is corretly calculated, the qacc is the same but the qvel and qpos are different which implies that the "implicit" and "implicitfast" didn't work in my implementation. If this is the case illustrated in the quote I showed above, then it makes sense. If not, I think there would still be some problems.

Sorry for my insist and really appreciate your help. Thanks very much!

The results with my own implementation:

ctrl:  0.2

qpos before step:  [0.00075047]
qvel before step:  [0.25009656]
qacc before step:  [62.47850293]

muscle gain:  [-887.26798455]
act_dot at this step: 28.57142857142857
The active muscle force:  [-35.49071938]
The bias muscle force:  [-73.51931293]
actuator_force： [-109.01003231]
actuator_moment： [[-0.04941076]]
qfrc_actuator： [5.38626857]
actuator_length:  [0.20893689]
actuator_velocity:  [-0.01235746]

**qpos after step:  [0.0016333]
qvel after step:  [0.44141479]
qacc after step:  [95.65911787]**

The results with setting data.ctrl=muscle excitation signal:

ctrl:  0.2
qpos before:  [0.00075047]
qvel before:  [0.25009656]
qacc before:  [62.47850293]

act_dot at this step:  [28.57142857]
muscle gain: -887.2679845476669
muscle bias: -73.51931293026566
actuator_force： [-109.01003231]
actuator_moment： [[-0.04941076]]
qfrc_actuator： [5.38626857]**
actuator_length:  [0.20893689]
actuator_velocity:  [-0.01235746]

qpos after step: [0.00163072]
qvel after step:  [0.44012368]
qacc after step:   [95.65911787]

[yuvaltassa]

I can see the difference, but it's quite hard to help you figure this out from the GitHub issue...

Can you build from source? If you build from source then debugging should be pretty straightforward, e.g. by throwing in some printf statements.

My other suggestion is to use mj_getState and mj_setState and to carefully inspect the intermediate variables in mjData.

One thing that is making me suspicious is that you are not printing out act, the activation states. You do realize these are states just like qpos and qvel, yes?

Did you read mj_fwdActuation carefully? You're sure you understand how activations are integrated? Note that in the current API documentation if you click on a function name you go straight to the source code:

https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mj-fwdactuation

I'm sorry this is not more helpful. I can promise you that the solution to your problem is very simple and that once you figure it out the numbers will match perfectly. At the end of the day the computation is very simple.

[Yingfan99327]

Hi Yuval,

Yeah, when I met this problem first, I tried to build from the source actually. However, I met some problems when I build from the source with Python bindings, I thought I didn't figure out very clearly about the way of building mujoco from the source. But, I will try.

Yeah, I will try to use mj_getState and mj_State to inspect the intermediate variables more carefully in the experiments later. Thanks for your suggestions.

Yeah, I guess I am clear about how the actuation was integrated in the mj_fwdActuation, I will double check it later and be more careful.

I have the same feelings actually. I think it should be a small and simple problem. My first intuition is that maybe I have something wrong with my model file, so I posted all the details about how I carried out the experiment and the model file I used to see whether I miss something or made something wrong with the model file and experiment set-up. But I agree with you, once I figured out the problem, I would find it is a very very simple problem.

Thanks so much for you help, Yuval, for all these helpful suggestions. And, also really appreciate your patience and previous advice on my previous questions. Sometimes, I just used Mujoco to do some very "weird" experiments, lol, but it is really fun to play with Mujoco and use it for my research. As I said, everytime I know more about it, I feel very happy. On the other hand, mujoco also opens the gate of simulation for me indeed.

Really appreciate for your help and this amazing tool. I will keep debugging and see whether I could figure this problem clearly. I will try first.

Wishing you all the best.

[Yingfan99327]

Hi Yuval,

I check the source codes again and kind of make some progress.

I chose the same integrator for them, which is implict and implicitfast. I found there is some difference in this variable qDeriv and qLU after mj_step between my way and Mujoco's way.

My way:
qDeriv after mj_step:  [6.50521303e-19]
qM after mj_step:  [0.05256202]
qH after mj_step:  [0.]
qHDiagInv after mj_step:  [0.]
qLU: [0.05256202]

Mujoco:
qDeriv after mj_step:  [-0.17856273]
qHDiagInv after mj_step:  [0.]
qM  after mj_step:  [0.05256202]
qH  after mj_step:  [0.]
qLU: [0.05291914]

I guess this should be the reason why the qacc is the same while qvel is different, and then I checked the source codes to calculate this part again and find out the problem should be realted to the functions used in mjd_actuator_vel, mjd_passive_vel, mjd_rne_vel in the function mjd_smooth_vel. Because in order to directly use actuation_force to actuate the muscle, I modified the actuator model as

<actuator>
    <motor name="shoulder_joint" tendon="DELT1_tendon" />
</actuator>

instead of

<actuator>
    <general name="DELT1" tendon="DELT1_tendon" biasprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" class="upperarm_muscle" gainprm="0.33707 1.53316 1051.85 1 0 1.8 10 0.518315 1.4 0" lengthrange="0.123363 0.240922"/>
</actuator>

which results in the misalign in the conditions inside the function like mjd_actuator_vel such as the m->actuator_biastype[i], m->actuator_gaintype[i] as well as the difference in actuator_gainprm, actuator_lengthrange, which leads to the difference in gain_vel and bias_vel, finally the qDerive.

I guess this would be the problem if I use integrators like implicit and implicitfast, which is consistent with my initial guess that this "weird" actuator design would probably result in this misalign problems. It could also explain why using Euler is OK.

Sorry to bother you, may I ask what could be the best way to solve it? I tried to reassign the values of actuator_gainprm, actuator_lengthrange etc to the data, but it seems dumb and not work.

I will try other methods as well.

Thanks and wishing you all the best.