Weird behavior for simple prismatic joint

[BolunDai0216]

I created a simple system with a block connected to another block using a prismatic joint:

<?xml version="1.0"?>
<robot name="hopper_unactuated">
    <link name="world">
    </link>

    <joint name="world_to_body" type="fixed">
        <parent link="world"/>
        <child link="body"/>
        <origin xyz="0 0 0.0"/>
    </joint>

    <link name="body">
        <visual> 
            <geometry>
                <box size="0.5 0.5 0.5"/>
            </geometry>
            <origin xyz="0.0 0.0 3.0"/>
        </visual>
        <collision>
            <geometry> 
                <box size="0.5 0.5 0.5"/>
            </geometry>
            <origin xyz="0.0 0.0 3.0"/>
        </collision>
        <inertial> 
            <origin xyz="0.0, 0.0, 3.0"/>
            <mass value="0.2"/>
            <inertia ixx="0.167" ixy="0" ixz="0" iyy="0.0" iyz="0.167" izz="0.167"/>
        </inertial>
    </link>

    <joint name="body_to_foot" type="prismatic">
        <parent link="body"/>
        <child link="foot"/>
        <origin xyz="0 0 0.0"/>
        <axis xyz="0.0 0.0 -1.0"/>
        <limit lower="0.0" upper="0.5" effort="1000.0" velocity="1000.0"/>
    </joint>

    <link name="foot">
        <visual> 
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 1.0"/>
        </visual>
        <collision>
            <geometry> 
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 1.0"/>
        </collision>
        <inertial> 
            <origin xyz="0.0, 0.0, 1.0"/>
            <mass value="0.01"/>
            <inertia ixx="0.0" ixy="0" ixz="0" iyy="0.0" iyz="0.0" izz="0.0"/>
        </inertial>
    </link>

</robot>

And I created a script so that the prismatic joint would mimic the behavior of a spring

import time

import numpy as np
import pybullet as p
import pybullet_data


def main():
    client = p.connect(p.GUI)
    p.setGravity(0, 0, -10, physicsClientId=client)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    planeId = p.loadURDF("urdf/plane.urdf")
    uId = p.loadURDF("urdf/hopper_fixed.urdf")
    # p.setRealTimeSimulation(1)
    p.setTimeStep(0.01)

    p.setJointMotorControl2(uId, 1, controlMode=p.VELOCITY_CONTROL, force=0)
    p.setJointMotorControl2(uId, 0, controlMode=p.VELOCITY_CONTROL, force=0)

    while True:
        pos, vel, rf, torque = p.getJointState(uId, 1)
        force = -20 * (pos)
        p.setJointMotorControl2(uId, 1, p.TORQUE_CONTROL, force=force)
        p.stepSimulation()
        time.sleep(0.01)


if __name__ == "__main__":
    main()

I noticed that when I set the mass of the foot link in my urdf to a small value such as 0.00001, the foot link will oscillate crazy and would not stop. Is this the normal behavior due to numerical issues or did I mess up somewhere?

Would apprecitate any help, thanks in advance!

[erwincoumans]

Yes, using huge mass differences may cause such oscillations. You could try using a much smaller time step, 0.01 is large.