torque/force sensor bug in closed loop system with weld constraints

[Swk6666]

Intro

Hi!

I am a phd in BIT, I use MuJoCo for my research on Collaborative control of dual-arm robot.

My setup

mujoco3.3.0 / mac os 15 / python3.10

What's happening? What did you expect?

When I use a weld constraint in MuJoCo to form a closed-loop constraint system, the data read by the torque/force sensor attached to the joint is inconsistent with data.qfrc_applied, data.qfrc_constraint, data.qfrc_actuator, or their combinations. I expect the value of the torque sensor in the direction of the joint's rotation axis to be consistent with these data. To verify that my assumption is correct, I also built a Simscape Multibody model, and the data obtained there were consistent, which further convinces me that this is a bug.

Steps for reproduction

Reproducing this bug is straightforward. You need to download the zip file I’ve uploaded here and run the scene2_position.py file. The corresponding XML file is also included in the zip file.

I’ve also uploaded the .slx file from Simulink here, which requires MATLAB version >= R2023a.

Minimal model for reproduction

github.zip

Code required for reproduction

No response

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 confused about what this sensor reports. It is not the torque applied by the actuator, but the total torque between the parent and the child. I loaded your model but it is too complex for me to diagnose. Perhaps if you made a simpler model we could understand better the discrepancy.

[Swk6666]

Thank you very much for your reply! I understand that this sensor should represent the total torque between the parent and the child, and actually, I indeed want to use this total torque and force for subsequent RNEA calculations. However, in MuJoCo, I couldn’t find the corresponding quantities (actuator/inverse/constraints) at the joint or their combinations that match the torque in the direction of the rotation axis obtained from the sensor. In other words, I want to find some value in the MuJoCo data that equal the value of the torque sensor in the direction of the rotation axis. Below, I’ve provided a simplified version of the code with comments added to each section, hoping this will help you understand my issue.

github.zip

[yuvaltassa]

Perhaps you need to look at the implementation of mj_rnePostConstraint (click on the function name to be taken to the source code)

[Swk6666]

I’ve read this code before and just reviewed it again. My understanding is that the sensor values are in the frame of the child body, while cfrc_int represents the 6-dimensional force between the two bodies, expressed in the world coordinate system. cfrc_ext takes the weld constraint into account, also relative to the world coordinate system, and these can be transformed using mju_transformSpatial. However, my question is about the torque in the direction of the rotation axis—specifically, the torque in a certain direction from the torque sensor (in robotics, typically the z-axis direction). What does it correspond to? In other words, in qfrc, which value corresponds to it? Thank you～

[yuvaltassa]

the sensor values are in the frame of the child body

The output of the sensor is in the frame of the site it's defined in, not the body.

cfrc_int represents the 6-dimensional force between the two bodies, expressed in the world coordinate system

Not exactly, see here and here.

in qfrc, which value corresponds to it?

As recommended in the docs "Using this sensor often requires creating a dummy body welded to its parent (i.e., having no joint elements)". It is often difficult to interpret force/torque measurements in directions in which the body can move. Try associating the sensor with a body that is fixed wrt to its parent, and see if the outputs make more sense to you.

[Swk6666]

The output of the sensor is in the frame of the site it's defined in, not the body.

I understand your point. This sensor displays the 6-dimensional force at the site. In the following code, the sensor is located at "joint1_2" between "link1_1" and "link1_2". The force transmitted between link1_1 and link1_2, which is measured by the sensor, is expressed in the coordinate system of that site. This 6-dimensional force, except for the direction of the rotation axis, has the other five dimensions canceled out by the physical structure of the joint. The torque in the direction of the rotation axis should be generated by the motor. In this example, the joint has no armature, damping, or gravity. If a closed loop is not formed (in the example I provided, closed=0), the torque in the rotation axis direction equals data.qfrc_actuator. However, if a closed loop is formed using a weld, this equality no longer holds, as other torques in the rotation axis direction are introduced, such as data.qfrc_constraint. My question is, since I need to perform control, I want to know if, in MuJoCo, there is any other physical quantity in the data.qfrc data structure that equals the torque value in the rotation axis direction measured by this sensor.

<body name="link1_1" pos="0 -0.25 0.05">
    <joint name="joint1_1" type="hinge" pos="0 0 0" axis="1 0 0" range="-1.57 1.57"/>
    <geom name="link1_1geom" type="capsule" fromto="0 0 0 0 0 0.3" size="0.04" rgba="0 1 0 1"/>
    <inertial pos="0 0 0.15" mass="1" diaginertia="5 5 0.5"/>
    <body name="link1_2" pos="0 0 0.3">
        <joint name="joint1_2" type="hinge" pos="0 0 0" axis="1 0 0" range="-1.57 1.57"/>
        <site name="sensor1_2" pos="0 0 0" size="0.01"/>
        <geom name="link1_2geom" type="capsule" fromto="0 0 0 0 0 0.3" size="0.04" rgba="0 0 1 1"/>
        <inertial pos="0 0 0.15" mass="1" diaginertia="5 5 0.5"/>
        <site name="end_effector1" pos="0 0 0.3" size="0.01" rgba="1 0 0 1"/>
        <body name="target_object" pos="0 0.25 0.3" quat="1 0 0 0">
            <inertial pos="0 0.1 0" mass="0.0001" diaginertia="0.0001 0.0001 0.0001"/>
            <site name="object_site2" pos="0 0 0" size="0.01" rgba="1 0 0 1"/>
            <geom size="0.05 0.25 0.05" type="box" rgba="1 0 0 1" contype="0" conaffinity="0"/>
        </body>
    </body>
</body>

Try associating the sensor with a body that is fixed wrt to its parent, and see if the outputs make more sense to you.

As mentioned above, the situation is very accurate in the open-loop case, allowing for a good correspondence. However, things get complicated in the closed-loop case.

[lwan]

Is this related to section in the documentation?

https://github.com/google-deepmind/mujoco/blob/main/doc/mjx.rst?plain=1#L250

(ACCELEROMETER, FORCE, TORQUE not supported with connect or weld equality constraints)

@yuvaltassa is this a bug that the team is actively working on resolving?