Physics run really slow?

[nicholaspalomo]

Hi! Thanks again for your work on gym-ignition, ScenarIO, and the whole suite of tools developed by the team at IIT. They're super helpful to my work/ research. Thanks also for your responsiveness these last few times I've asked questions.

I had a question about some odd behavior I've noticed: Sometimes in Ignition it seems like the robots move unrealistically slow. Have you seen this before or do you know what might be the reason for this?

To demonstrate this behavior, I modified your C++ example given here using the C++ API for ScenarIO. Except this time I did it for the ANYmal robot. The simulation is simple: It spawns ANYmal 1 meter above the ground, and then integrates the simulation forward for 30 seconds (as you do for the pendulum example). The URDF and meshes come from here which I have used many times and which I am sure is correct.

I put my code in a separate repository here as a minimum working example: https://github.com/nicholaspalomo/test_ignition

The behavior I expect is that the robot quickly falls to the ground - certainly in less than 30 seconds.

What I actually see is that the robot never even reaches the ground within the 30 seconds integrated by the simulator.

I was wondering if you all had any idea what might be the reason for this seemingly unphysical behavior?

Thanks again!

[diegoferigo]

Oh wow indeed the behavior is quite odd! It took me a while to figure out what's the cause.

I initially thought that the problem was the anymal model. I tried disabling all the torque and joint velocity limits, switching to a pure velocity control, etc but nothing changed. Then, I tried to replace the quadruped with a simple cube, and the same behavior occurs. Something else was off.

To facilitate and speed up debugging, I ported your C++ example to Python, you can find the script below. I found out that the problem occurred only when the base velocity of the model is reset.

Digging more into the rabbit hole, I isolated the source of the problem to the following lines:

https://github.com/robotology/gym-ignition/blob/fa14f7f8f667cf84bf226756cea158ec0f5c3d23/scenario/src/plugins/Physics/Physics.cc#L2952-L2964

In short, after the first time that from the API the {Angular|Linear}VelocityCmd components are created, instead of being deleted in the next steps, they are zeroed. This explains the slow falling, because in a nutshell the physics engine was correctly computing the new model velocity by integrating the dynamics, but after the steps these lines were resetting the model velocity to 0. Therefore, every physics steps of the simulated horizon was starting with a zero velocity, producing the slow falling.

Checking the history, those lines were introduced in #365, and they reflect the changes landed upstream in gazebosim/gz-sim#427. This last upstream PR targets TPE, that is their kinematics physics engine, maybe in TPE having a zero velocity command would produce the right behavior. Instead, in DART, it causes the problems we experienced here. Apparently this went under the radars in both theirs and our testing pipeline.

For the moment, you can solve by just commenting out the lines where you reset the base velocity. The default is zero. I will open a PR to fix it, but since it will touch the vendored Physics system, I want to take advantage to align it with upstream. The process is a bit cumbersome, I'm not sure I'll find enough time to do it in the next few days. I'll put it in my TODO list.

Thanks for providing a clear MWE! It helped a lot the process. In general, I suggest to produce a single Python file since it's easier to share, no need to create a repo with all the CMake stuff, but if you're more comfortable with C++, keep doing it also the next times 😉

example.py

Must be stored in the same folder of example.cpp.

import time
from pathlib import Path

import numpy as np
from scenario import core as scenario_core
from scenario import gazebo as scenario_gazebo

scenario_gazebo.set_verbosity(scenario_gazebo.Verbosity_debug)

if __name__ == "__main__":

    # Create the simulator
    gazebo = scenario_gazebo.GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=10)

    # Get the integration time
    dt = gazebo.step_size() * gazebo.steps_per_run()

    # Initialize the simulator
    assert gazebo.initialize()

    # Get the default world
    world = gazebo.get_world()

    # Select the physics engine
    assert world.set_physics_engine(scenario_gazebo.PhysicsEngine_dart)

    # Explicitly set the gravity
    assert world.set_gravity([0, 0, -10.0])

    # Get the path of the build folder
    build_folder_path = Path(__file__).parent.absolute() / "build"

    # Insert the ground plane
    assert world.insert_model(str(build_folder_path / "ground_plane.sdf"))

    # Open the GUI
    gazebo.gui(0)
    time.sleep(3)
    gazebo.run(paused=True)

    # Insert a robot
    model_file = "anymal.urdf"
    model_name = "my_model"
    if not world.insert_model(
        str(build_folder_path / model_file),
        scenario_core.Pose([0, 0, 1.0], [3.0, 0, 0, 0]),
        model_name,
    ):
        raise RuntimeError

    # Get the robot
    robot = world.get_model(model_name=model_name)

    # Disable any limit set in the model file
    _ = [j.set_velocity_limit(np.finfo(float).max) for j in robot.joints()]
    _ = [j.set_max_generalized_force(np.finfo(float).max) for j in robot.joints()]

    # Process model insertion
    assert gazebo.run(paused=True)
    time.sleep(5)

    # Set the joint control mode
    assert robot.set_joint_control_mode(
        scenario_core.JointControlMode_velocity_follower_dart
    )

    # Configure the joint controller
    assert robot.set_controller_period(gazebo.step_size())

    # Initialize the robot state
    assert robot.to_gazebo().reset_base_world_linear_velocity([0.0, 0, 5.0])
    assert robot.to_gazebo().reset_base_world_angular_velocity([0.0, 0, 0.0])

    # Simulate for 30 seconds
    _ = [gazebo.run() for _ in np.arange(start=0, stop=30, step=dt)]

    # Close the simulator
    time.sleep(5)
    gazebo.close()

[diegoferigo]

If you want to test the fix, you can temporarily use the fix/reset_base_velocity branch. Note that this is not the branch that will be merged, it's there just for testing purposes.

[nicholaspalomo]

Dude, this is brilliant! Thanks, @diegoferigo, for the detailed answer. This all makes a lot of sense. I tried the fix/reset_base_velocity branch and it works for me. For a little context - I first noticed this when I was doing some reinforcement learning for a floating base robot recently and noticed that the robot was moving the legs fine but that the base frame was moving extremely slow. In my RL simulations, I call robot->resetBaseWorldLinearVelocity(velocity), robot->resetBaseWorldAngularVelocity(velocity), etc. at the start of each rollout to "reset the simulation" and set the initial state.

In general, I suggest to produce a single Python file since it's easier to share, no need to create a repo with all the CMake stuff, but if you're more comfortable with C++, keep doing it also the next times

Yes, certainly, I can do a Python file next time. I chose C++ this time since that's how I've been using the ScenarIO API in my RL setup. :)

Thanks again! This resolves my issue.