Add restitution coefficient support for bouncing

[diegoferigo]

This PR adds the support of simulating bouncing using //collision/surface/bounce/restitution_coefficient. It calls the methods introduced in dartsim/dart#1369.

This is the resulting behaviour simulating 11 spheres with a different restitution coefficient and setting the coefficient of the ground to 1.0:

Script (requires third party libraries)

import numpy as np
from dataclasses import dataclass
from gym_ignition.utils import misc
from scenario import core as scenario_core
from scenario import gazebo as scenario_gazebo
from gym_ignition.utils.scenario import init_gazebo_sim


@dataclass
class SphereURDF:

    mass: float = 5.0
    radius: float = 0.1
    restitution: float = 0

    def urdf(self) -> str:

        i = 2.0 / 5 * self.mass * self.radius * self.radius
        urdf = f"""
            <robot name="sphere_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
                <link name="sphere">
                    <inertial>
                      <origin rpy="0 0 0" xyz="0 0 0"/>
                      <mass value="{self.mass}"/>
                      <inertia ixx="{i}" ixy="0" ixz="0" iyy="{i}" iyz="0" izz="{i}"/>
                    </inertial>
                    <visual>
                      <geometry>
                        <sphere radius="{self.radius}"/>
                      </geometry>
                      <origin rpy="0 0 0" xyz="0 0 0"/>
                    </visual>
                    <collision>
                      <geometry>
                        <sphere radius="{self.radius}"/>
                      </geometry>
                      <origin rpy="0 0 0" xyz="0 0 0"/>
                    </collision>
                </link>
                <gazebo reference="sphere">
                  <collision><surface><bounce>
                    <restitution_coefficient>{self.restitution}</restitution_coefficient>
                  </bounce></surface></collision>
                </gazebo>
            </robot>
            """

        return urdf


# Set the verbosity
scenario_gazebo.set_verbosity(scenario_gazebo.Verbosity_debug)

# Get the default simulator and the default empty world
gazebo, world = init_gazebo_sim()

# Show the GUI
import time
gazebo.gui()
gazebo.run(paused=True)
time.sleep(3)

for restitution in np.linspace(start=0, stop=1.0, num=11):

    # Create a tmp file with the urdf
    sphere_urdf = SphereURDF(restitution=restitution).urdf()
    sphere_urdf_file = misc.string_to_file(sphere_urdf)

    # Insert the sphere
    pose = scenario_core.Pose_identity()
    pose.position = [0, restitution * 3, 1.0]
    assert world.to_gazebo().insert_model(sphere_urdf_file, pose, f"sphere_{restitution}")

gazebo.run(paused=True)

for _ in range(10_000):
    gazebo.run()

time.sleep(5)
gazebo.close()

This PR fills one of the gaps described in https://www.allisonthackston.com/articles/ignition_vs_gazebo.html.

[diegoferigo]

Closes #84

[azeey]

Thanks for the contribution @diegoferigo. The PR looks good. It does demonstrate the limitation of the current API that uses SDFormat objects for construction because it make it possible for two physics engine plugins to implement the Construct* feature, but have different capabilities in reality. i.e, in this case, if there is another physics engine plugin that implements the ConstructCollision feature, but doesn't support restitution, we now have a discrepancy.

For now though, I think we should go forward with this PR. Would it be possible for you to add a test?

[diegoferigo]

@azeey Thanks for the feedback. I get back to this issue to say that lately I'm running a bit low with time, not sure if I manage to add the test in the next days.

It does demonstrate the limitation of the current API that uses SDFormat objects for construction

Yes, I think this is the trade off between API complexity and flexibility. I'm not sure what it the best solution here, I just say that often the flexibility is good to have, especially in this development phase, and it can be compensated with a proper documentation so that downstream users know what they can and cannot expect.

[azeey]

LGTM! I took the liberty to add a small test.

[azeey]

@osrf-jenkins run tests please

[diegoferigo]

LGTM! I took the liberty to add a small test.

Thanks @azeey, few weeks ago I tried to add a test but I couldn't find anything simple enough (and delayed again). I was thinking something about computing the average velocity of the spheres and checking that those with bigger restitution coefficients were higher. Though, it was a bit convoluted and in any case it was not checking against a ground truth.

[azeey]

CI looks good. Merging!