reset_base_world_linear_velocity has no effect

[FirefoxMetzger]

I'm trying to use scenario to spawn an object into the environment and give it initial velocity. The snippet that spawns the model is an adaptation of the manipulation example:

cube_sdf = scenario_gazebo.get_model_file_from_fuel(
    uri="https://fuel.ignitionrobotics.org/openrobotics/models/wood cube 5cm",
    use_cache=True,
)

def spawn_cube(position):
    # Get a unique name
    model_name = gym_ignition.utils.scenario.get_unique_model_name(
        world=world, model_name="cube"
    )

    # Insert the model
    assert world.insert_model(
        cube_sdf, scenario_core.Pose(position, [1.0, 0, 0, 0]), model_name
    )

    cube = world.get_model(model_name)
    velocity = scenario_core.Array3d((1, 2, 3))
    assert cube.to_gazebo().reset_base_world_linear_velocity(velocity)

As the assert passes, I would expect the model to have some velocity (quite a lot), so I should be able to see it flying away (or at least bouncing. Instead, it just falls down without any initial velocity. I made a quick video to demonstrate the issue (spawning cubes periodically above the end-effector).

https://user-images.githubusercontent.com/4402489/112521107-5d308a00-8d59-11eb-9da5-d2e9eb129c86.mp4

Any ideas what might cause this behavior?

[FirefoxMetzger]

Solved!

It was an issue with the physics plugin. I "outsourced" the physics plugin into an .sdf from which I load the world

gazebo = scenario_gazebo.GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1)
assert gazebo.insert_world_from_sdf("./panda_world_expanded.sdf")
gazebo.initialize()

This didn't have any obvious negative effect previously, so I assumed it is fine for my case. However, removing the plugin from the .sdf and instead adding physics manually

gazebo = scenario_gazebo.GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1)
assert gazebo.insert_world_from_sdf("./panda_world_expanded.sdf")
gazebo.initialize()
world = gazebo.get_world()
assert world.set_physics_engine(scenario_gazebo.PhysicsEngine_dart)

solved the issue. Now cubes are flying away as expected:

https://user-images.githubusercontent.com/4402489/112522959-76d2d100-8d5b-11eb-9ea4-167696f45673.mp4

If it doesn't cause too much trouble, I would still be very interested in learning about the underlying reason why this fails though. @diegoferigo If you could shed some light on this, it would be much appreciated :)

[diegoferigo]

Can you please report also the sdf file? I suspect there's something missing in the definition of the physics plugin. To explain myself better, we do not use the Physics system from upstream, but we vendor a custom version that extends few functionalities. When you load the plugin from code, our vendored version is loaded. If you prefer, you can load our plugin also from sdf, just make sure that 1) the plugin is in the search path and 2) you specify the proper lib and class names.

[FirefoxMetzger]

Both files differ by a single line

<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>

which is removed in the working version (in favor of manually loading the physics plugin).

It is indeed me loading Gazebo's physics plugin instead of the custom one (that I wasn't aware of until now). I recall that scenario is doing something with the physics engine (from https://github.com/robotology/gym-ignition/issues/296#issuecomment-795564631), but I didn't realize that it is a whole dedicated plugin. This is good to know, thanks!

Note that the .sdf files are not exactly transportable, as they depend on machine-local full paths to find meshes.

old sdf (broken

```xml ogre2 1 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 1000 0.9 0.01 0.001 -0.5 0.1 -0.9 0 0 0 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 0.4 0.4 0.4 1 0.7 0.7 0.7 1 1 1 0 0 1 100 100 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 0 0 -0 0 1 0 0 1 0 -0 0 1.5 0.8 0.03 0.6 0.6 0 0 1 0 -0 0 1.5 0.8 0.03 1 1 1 1 https://fuel.ignitionrobotics.org/1.0/openrobotics/models/table/3/files/Table_Diffuse.jpg 0.68 0.38 0.5 0 -0 0 0.02 1 0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 0.38 0.5 0 -0 0 0.02 1 -0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0 0 0 0 -0 1.5708 1 0 0 1 0 -0 0 1.5 0.8 0.03 0.6 0.6 0 0 1 0 -0 0 1.5 0.8 0.03 1 1 1 1 https://fuel.ignitionrobotics.org/1.0/openrobotics/models/table/3/files/Table_Diffuse.jpg 0.68 0.38 0.5 0 -0 0 0.02 1 0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 0.38 0.5 0 -0 0 0.02 1 -0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.794 0 0 0 -0 1.5708 1 2 0 1.75 -0 0.282863 3.14 0.05 0.05 0.05 0 -0 0 0.1 0.1 0.1 1.047 1920 1080 0.1 100 1 30 1 camera ```

new sdf (working)

```xml ogre2 1 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 1000 0.9 0.01 0.001 -0.5 0.1 -0.9 0 0 0 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 0.4 0.4 0.4 1 0.7 0.7 0.7 1 1 1 0 0 1 100 100 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 0 0 -0 0 1 0 0 1 0 -0 0 1.5 0.8 0.03 0.6 0.6 0 0 1 0 -0 0 1.5 0.8 0.03 1 1 1 1 https://fuel.ignitionrobotics.org/1.0/openrobotics/models/table/3/files/Table_Diffuse.jpg 0.68 0.38 0.5 0 -0 0 0.02 1 0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 0.38 0.5 0 -0 0 0.02 1 -0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0 0 0 0 -0 1.5708 1 0 0 1 0 -0 0 1.5 0.8 0.03 0.6 0.6 0 0 1 0 -0 0 1.5 0.8 0.03 1 1 1 1 https://fuel.ignitionrobotics.org/1.0/openrobotics/models/table/3/files/Table_Diffuse.jpg 0.68 0.38 0.5 0 -0 0 0.02 1 0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 -0.68 -0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 -0.68 0.38 0.5 0 -0 0 0.02 1 -0.68 0.38 0.5 0 -0 0 0.02 1 0.5 0.5 0.5 1 0.794 0 0 0 -0 1.5708 1 2 0 1.75 -0 0.282863 3.14 0.05 0.05 0.05 0 -0 0 0.1 0.1 0.1 1.047 1920 1080 0.1 100 1 30 1 camera ```

[diegoferigo]

The following could work:

<plugin name='scenario::plugins::gazebo::Physics' filename='PhysicsSystem'/>

<!-- we should have an alias that allows also the following, but never actually tried -->
<plugin name='ignition::gazebo::systems::Physics' filename='PhysicsSystem'/>

if no file is found, try to add the folder that contains /conda/lib/scenario/plugins/libPhysicsSystem.so (it could be in another prefix in your system) to the environment variable IGN_GAZEBO_SYSTEM_PLUGIN_PATH. That file is the plugin that is dlopened during runtime and provides a system that interfaces with ign-physics plugins (Physics is a plugin that loads a plugin, yes). Note that the Python package of scenario automatically does it when imported.

[FirefoxMetzger]

It does work indeed 🚀 and since the python package populates IGN_GAZEBO_SYSTEM_PLUGIN_PATH upon import this seems like a very smooth solution. I'll definitely keep this in mind.

Speaking off smooth (even though it is a little off-topic), I'm surprised by how stable the controller is, given that it appears to be a chain of PIDs. I guess I have to dive into that code next and see what is happening.

https://user-images.githubusercontent.com/4402489/112539252-01243080-8d6e-11eb-9c29-34f7d9020cfb.mp4

I will close this issue since it is not an issue, but rather a user-error on my part (using the wrong plugin).

[diegoferigo]

It does work indeed rocket and since the python package populates IGN_GAZEBO_SYSTEM_PLUGIN_PATH upon import this seems like a very smooth solution. I'll definitely keep this in mind.

Great :rocket:

Speaking off smooth (even though it is a little off-topic), I'm surprised by how stable the controller is, given that it appears to be a chain of PIDs. I guess I have to dive into that code next and see what is happening. my_video.mp4

The used ComputedTorqueFixedBase is a model-based controller, and while it's in its simplest form, it operates on a dynamics that it almost exact. The only effect not considered should be the friction. This is to say that under these conditions, I expect the feed-forward term with the dynamics being quite precise, and the PID that receives the small error (thanks to the feed-forward) has easy life.

Drawbacks, however, are many. There's no trajectory optimization running under the hood, the trajectories are computed in the state space of the controller. This is to say that self-collisions can happen during movements, and load on the end-effector could degrade tracking performance.