Contacts don't work as expected

edit

Hi! I'm facing an issue with gazebo. I'm working on a robot with wheel and rotative arms and when I run the simulation my wheel have good contact physic but my arms are going underground and take so time to just go out of the floor. I'm attaching a screenshot to help you to visualize. The fact is that this issue distorts the simulation.

So here is the issue:

.

As shown the wheel is going undergroung instead of just be in contact with the floor.

I'm sharing some part of my gazebo file and my .world file too.

That's for the gazebo file for the wheels:

    <xacro:macro name = "little_wheel_gazebo" params = "position side">
    <gazebo reference = "${position}_${side}_little_wheel">
    <material>Gazebo/Grey</material>
    <mu1>1</mu1>
    <mu2>1</mu2>
    <kp>100000</kp>
    <kd>100000</kd>
    <self_collide>1</self_collide>
    <fdir1>0 0 1</fdir1>
    <max_contacts>10</max_contacts>
    <min_depth>0.001</min_depth>
    </gazebo>
</xacro:macro>

That's the xacro desrcritpion of the arms:

<xacro:macro name = 'sliding_arm' params='position'>
    <link name ="${position}_arm">
        <visual>
            <xacro:arm_origin position="${position}"/>
            <geometry>
                <box size="${arm_length} ${arm_width} ${arm_height}"/>
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <xacro:arm_origin position="${position}"/>
            <geometry>
                <box size="${arm_length} ${arm_width} ${arm_height}"/>
            </geometry>
        </collision>
        <xacro:box_inertial mass = "${arm_mass}" width="${arm_length}" height="${arm_height}" depth="${arm_width}"/>
    </link>

    <joint name="${position}_arm_joint" type ="prismatic">
        <xacro:arm_origin_joint position = "${position}"/>
        <parent link="base_link"/>
        <child link = "${position}_arm"/>
        <axis xyz ="1 0 0"/>
        <limit effort="5000" velocity="10." upper ="${arm_length+axe2axe_length}" lower="0"/>
    </joint>

</xacro:macro>

And finally for my .world file:

<model name="25cm_stairs">
        <static>1</static>
        <link name='first_stair'>
            <visual name="first_stair_visual">
                <pose>0.8 0 0.125 0 0 0</pose>
                <geometry>
                    <box>
                        <size>0.357 0.8 0.25</size>
                    </box>
                </geometry>
                <material>
                    <script>
                        <uri>file://media/materials/scripts/gazebo.material</uri>
                        <name>Gazebo/Grey</name>
                    </script>
                </material>
            </visual>
            <collision name="first_stair_collision">
                <pose>0.8 0 0.125 0 0 0</pose>
                <geometry>
                    <box>
                        <size>0.357 0.8 0.25</size>
                    </box>
                </geometry>
                <max_contacts>10</max_contacts>
                <surface>
                    <bounce/>
                    <friction>
                        <ode>
                            <mu>100</mu>
                            <mu2>50</mu2>
                        </ode>
                    </friction>
                    <contact>
                        <ode>
                            <kp>100000</kp>
                            <kd>100000</kd>
                        </ode>
                    </contact>
                </surface>
            </collision>
            <velocity_decay>
                <linear>0</linear>
                <angular>0</angular>
            </velocity_decay>
            <self_collide>0</self_collide>
            <kinematic>0</kinematic>
            <gravity>1</gravity>
        </link>
        <link name='second_stair'>
            <visual name="second_stair_visual">
                <pose>1.157 0 0.25 0 0 0</pose>
                <geometry>
                    <box>
                        <size>0.357 0.8 0.5</size>
                    </box>
                </geometry>
                <material>
                    <script>
                        <uri>file://media/materials/scripts/gazebo.material</uri>
                        <name>Gazebo/Grey</name>
                    </script>
                </material>
            </visual>
            <collision name="second_stair_collision">
                <pose>1.157 0 0.25 0 0 0</pose>
                <geometry>
                    <box>
                        <size>0.357 0.8 0.5</size>
                    </box>
                </geometry>
                <max_contacts>10</max_contacts>
                <surface>
                    <bounce/>
                    <friction>
                        <ode>
                            <mu>100</mu>
                            <mu2>50</mu2>
                        </ode>
                    </friction>
                    <contact>
                        <ode/>
                    </contact>
                </surface>
            </collision>
            <velocity_decay>
                <linear>0</linear>
                <angular>0</angular>
            </velocity_decay>
            <self_collide>0</self_collide>
            <kinematic>0</kinematic>
            <gravity>1</gravity>
        </link>

Thanks for you help!