search

google-deepmind / mujoco

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

Measuring forces in tendons #832

Open thawke opened 1 year ago

thawke commented 1 year ago

Hello,

I want to measure the tension in a tendon attached to an otherwise free body. In lieu of a tendonfrc sensor, I'm trying to use force sensors at the tendon attachment sites, with dummy bodies containing the sites.

I expect to see the same force reading from both sensors, with opposite signs, but I'm getting 0 output. I've tried a few configurations and cannot manage to read tendon forces at all by this method. I have obviously misunderstood something.

danglemass

<mujoco>
    <option>
        <flag gravity="enable"/>
    </option>

    <default>
        <site type="sphere" rgba="1 0 0 1" size="0.005"/>
        <tendon rgba="0 1 0 1" stiffness="500" damping="10"/>
    </default>

    <visual>
        <headlight diffuse=".7 .7 .7"/>
    </visual>

    <worldbody>
        <body name="slidebody" pos ="0 0 0">
            <joint name="slide" type="slide" axis="0 0 1" armature="1" damping="100" range="-0.2 0.2" limited="true"/>
            <inertial mass="0.01" pos="0 0 0" diaginertia="1 1 1"/>
            <body name="originbody" pos="0 0 0">
                <site name="origin0" pos="0 0 0"/>
            </body>
        </body>
        <body name="massbody" pos="0 0 -0.3">
            <freejoint/>
            <geom name="massgeom" type="box" size="0.05 0.05 0.05" pos="0 0 0" mass="1"/>
            <body name="insertionbody" pos="0 0 0">
                <site name="insertion0" pos="0 0 0.05"/>
            </body>
        </body>
    </worldbody>

    <tendon>
        <spatial name="rope0" springlength="0 0.25">
            <site site="origin0"/>
            <site site="insertion0"/>
        </spatial>
    </tendon>

    <sensor>
        <force name="tension0" site="origin0"/>
        <force name="tension1" site="insertion0"/>
    </sensor>

    <actuator>
        <position name="verticalpos" joint="slide" kp="10000" />
    </actuator>

</mujoco>

The nesting works as expected in the following example with no tendon:

<mujoco>

    <visual>
            <headlight diffuse=".7 .7 .7"/>
    </visual>

    <worldbody>
        <body name="fixed body">
            <body name="dummy body">
                <site name="force1"/>
                <body name="heavybody">
                    <inertial mass="1" diaginertia="1 1 1" pos="0 0 0"/>
                    <site name="force2"/>
                </body>
            </body>
        </body>
    </worldbody>

    <sensor>
        <force name="f1" site="force1"/>
        <force name="f2" site="force2"/>
    </sensor>

</mujoco>
yuvaltassa commented 1 year ago

You have no misunderstanding and your expectations are valid.

A very cursory examination leads me to conclude that there is a high likelihood of a bug. Examining the function where these forces are computed one can see handling of forces inside the kinematic tree and outside the kinematic tree due to constraints. Tendon forces are outside the kinematic tree, but not due to constraints. In fact, even if one uses a limited tendon (which is a constraint), is appears the related forces are skipped.

So my current assessment is that this is bug. We will look more carefully soon.

thawke commented 1 year ago

Thanks for your reply - be good to hear what you conclude.

You have no misunderstanding and your expectations are valid.

^ I'm going to get this on a teeshirt

yuvaltassa commented 11 months ago

Still not fixed, but the documentation at least acknowledges the bug.

Hopefully we'll get to this soon.

ekchapman commented 10 months ago

@yuvaltassa, thanks for the update. Do you know when we might expect a fix? In the meantime, I have a workaround that others may find useful. Does this workaround look reasonable?

Rather than attaching a tendon endpoint directly to a body B, I attach it instead to a "mounting body" M, which is a free body that gets welded to B. Now when you measure the forces acting on B, you measure the force due to the weld constraint, rather than (trying to) measure the force from the tendon.

You have to tune the inertial properties of the free bodies and the solver parameters of the weld, but otherwise it seems to work fine. Note that, in the example below, sensordata[2] == 9.996 but sensordata[5] == -9.8. This is because the two mounting bodies each have mass 0.01. 

<mujoco>
    <option gravity="0 0 -9.8">
        <flag gravity="enable"/>
    </option>

    <default>
        <site type="sphere" rgba="1 0 0 1" size="0.005"/>
        <tendon rgba="0 1 0 1" stiffness="500" damping="10"/>
    </default>

    <visual>
        <headlight diffuse=".7 .7 .7"/>
    </visual>

    <worldbody>
        <body name="slidebody" pos ="0 0 0">
            <joint name="slide" type="slide" axis="0 0 1" armature="1" damping="100" range="-0.2 0.2" limited="true"/>
            <inertial mass="0.1" pos="0 0 0" diaginertia="1 1 1"/>
            <body name="originbody" pos="0 0 0">
                <site name="origin0" pos="0 0 0"/>
            </body>
        </body>
        <body name="massbody" pos="0 0 -0.3">
            <freejoint/>
            <geom name="massgeom" type="box" size="0.05 0.05 0.05" pos="0 0 0" mass="1" rgba="0.5 0.5 0.5 0.3"/>
            <body name="insertionbody" pos="0 0 0">
                <site name="insertion0" pos="0 0 0.05"/>
            </body>
        </body>

        <body name="tendon_mount_0" pos="0 0 0">
            <freejoint/>
            <inertial mass="0.01" pos="0 0 0" diaginertia="1 1 1"/>
            <site name="tendon_mount_site_0" rgba="0.3 0.6 0.9 0.6" size="0.008"/>
        </body>

        <body name="tendon_mount_1" pos="0 0 -0.25">
            <freejoint/>
            <inertial mass="0.01" pos="0 0 0" diaginertia="1 1 1"/>
            <site name="tendon_mount_site_1" rgba="0.3 0.6 0.9 0.6" size="0.008"/>
        </body>

    </worldbody>

    <tendon>
        <spatial name="rope0" limited="true" range="0 0.25">
            <site site="tendon_mount_site_0"/>
            <site site="tendon_mount_site_1"/>
        </spatial>
    </tendon>

    <equality>
        <weld body1="originbody" body2="tendon_mount_0"/>
        <weld body1="insertionbody" body2="tendon_mount_1"/>
    </equality>

    <sensor>
        <force name="tension0" site="origin0"/>
        <force name="tension1" site="insertion0"/>
    </sensor>

    <actuator>
        <position name="verticalpos" joint="slide" kp="10000" />
    </actuator>

</mujoco>

image

yuvaltassa commented 1 month ago

@thowell and I looked at this again recently. It's surprisingly tricky to implement, but at least we have a workplan đŸ™‚