Unsymmetrical behavior when bodies have non-zero rpy values in transformation

Hello everyone,

I have this simple skel file, which has four identical boxes floating in space (no gravity). The middle two are free, while the other two are weld to world frame.

    <physics>
        <time_step>0.002</time_step>
        <gravity>0 0 0</gravity>
        <collision_detector>dart</collision_detector>
    </physics>

    <skeleton name="box1">
        <body name="h1">
            <transformation>-0.6 0.1 0.0 0.0 0.0 0.0</transformation>
            <inertia>
                <mass>8</mass>
                <offset>0.0 -0.0 0.0</offset>
            </inertia>
            <visualization_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </visualization_shape>
            <collision_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </collision_shape>                                
        </body>

        <joint type="free" name="j_h1">
            <parent>world</parent>
            <child>h1</child>
        </joint>
    </skeleton>   

    <skeleton name="box2">
        <body name="h1">
            <transformation>0.6 0.1 0.0 0.0 0.0 0.0</transformation>
            <inertia>
                <mass>8</mass>
                <offset>0.0 -0.0 0.0</offset>
            </inertia>
            <visualization_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </visualization_shape>
            <collision_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </collision_shape>                                
        </body>

        <joint type="free" name="j_h1">
            <parent>world</parent>
            <child>h1</child>
        </joint>
    </skeleton> 

    <skeleton name="box3">
        <body name="h1">
            <transformation>-1.6 0.1 0.0 0.0 0.0 0.0</transformation>
            <inertia>
                <mass>8</mass>
                <offset>0.0 -0.0 0.0</offset>
            </inertia>
            <visualization_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </visualization_shape>
            <collision_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </collision_shape>                                
        </body>

        <joint type="weld" name="j_h1">
            <parent>world</parent>
            <child>h1</child>
        </joint>
    </skeleton> 

    <skeleton name="box4">
        <body name="h1">
            <transformation>1.6 0.1 0.0 0.0 0.0 0.0</transformation>
            <inertia>
                <mass>8</mass>
                <offset>0.0 -0.0 0.0</offset>
            </inertia>
            <visualization_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </visualization_shape>
            <collision_shape>
                <transformation>0.0 -0.0 0.0 0 0.000 0.0</transformation>
                <geometry>
                    <box>
                        <size>0.12 0.12 0.12</size>
                    </box>
                </geometry>
            </collision_shape>                                
        </body>

        <joint type="weld" name="j_h1">
            <parent>world</parent>
            <child>h1</child>
        </joint>
    </skeleton>

If I give the middle two boxes a same initial x velocity but with opposite direction, (say (1,0,0) and (-1,0,0)), and set the restitution to 1.0, then in theory the middle two boxes will keep bouncing back and forward between the two weld boxes. And that is indeed what I saw in DART.

However, if I replace all the "0.000" above with pi/2 ("1.570796") - that is, I let all 4 boxes pitch pi/2, in theory that should change nothing. But when I ran the simulation, I saw the middle two boxes quickly losing symmetry after 2 bounces, bypassing the weld boxes, and flying away.

As another example to show this unsymmetrical behavior, I tried to let two spheres free fall, one with no initial velocity and another starting with spinning (say, w_0 = 10 rad/s). In theory spinning should not affect free fall at all (and that is what I see in Mujoco). But from DART output I observed that the spinning ball started to gain +x velocity and position quickly.

I tried to dig into the code base a little bit. I am not sure whether this phenomena is related to the Lie Group operators, which I have absolutely no knowledge of.

Thanks, Yifeng

dartsim / dart

Unsymmetrical behavior when bodies have non-zero rpy values in transformation #870