Dart soft box

[trns1997]

Currently soft_contacts for box shape do not work in gazebo11. This PR has the DART SoftBodyNode working for box shapes. If the PR looks good then I have another mini PR for other shapes (sphere and cylinder).

Below is the result of having soft_contacts with different parameters causing different behaviours.

Addresses issue #2255

Here is my box model:

<model name="my_model">
    <pose>0 0 1 0 0 0</pose>
    <link name="box_link">
      <inertial>
        <mass>10.0</mass>
        <inertia> <!-- inertias are tricky to compute -->
          <!-- http://gazebosim.org/tutorials?tut=inertia&cat=build_robot -->
          <ixx>6.64</ixx>       <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
          <ixy>0.0</ixy>         <!-- for a box: ixy = 0 -->
          <ixz>0.0</ixz>         <!-- for a box: ixz = 0 -->
          <iyy>6.64</iyy>       <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
          <iyz>0.0</iyz>         <!-- for a box: iyz = 0 -->
          <izz>6.64</izz>       <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>2 2 2</size>
          </box>
        </geometry>
        <surface>
            <soft_contact>
                <dart>
                    <bone_attachment>0.6e10</bone_attachment>
                    <stiffness>0.4e10</stiffness>
                    <damping>0.1e2</damping>
                    <flesh_mass_fraction>3</flesh_mass_fraction>
                </dart>
            </soft_contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>2 2 2</size>
          </box>
        </geometry>
      </visual>

      <sensor name='my_contact' type='contact'>
        <contact>
          <collision>collision</collision>
        </contact>
        <plugin name="model_push" filename="~/GazeboDartTest/build/libtest_bench.so"/>
      </sensor>
    </link>
  </model>

Some results:

Bouncy Mode:

           <soft_contact>
                <dart>
                    <bone_attachment>0.6e10</bone_attachment>
                    <stiffness>0.4e10</stiffness>
                    <damping>0.1e2</damping>
                    <flesh_mass_fraction>3</flesh_mass_fraction>
                </dart>
            </soft_contact>

Gazebo Bouncy Box

Sum of all contact forces:

10kg box so about 98.1 N force when not bouncing

Pudding Mode:

           <soft_contact>
                <dart>
                    <bone_attachment>0.6e3</bone_attachment>
                    <stiffness>0.4e3</stiffness>
                    <damping>0.1e2</damping>
                    <flesh_mass_fraction>3</flesh_mass_fraction>
                </dart>
            </soft_contact>

Gazebo Pudding Box

Sum of all contact forces:

In the gifs you can see a solid core and the rest is basically the soft body of the box. I am not sure if I should go about making a PR. Let me know. Would appreciate some feedback nonetheless, if the results make sense or not.