Gazebo joint velocities do not agree with joint displacement

Question

In a simulation that I am running, which is described below, the angular velocities that I get from the joint_states topic, do not give me the angular displacement I calculate from the joints angular positions.

Firstly, this happens when the robot is is in contact with the ground! In simulations that do not involve contacts, the velocities give the measured displacements.

The joint velocities should be zero, as the <max_vel> element in the collision properties is large. And according to this, if the contact tangential friction ($F_T$) is less than $\mu F_{normal}$ the the contact surfaces do not move relative to each other.

However, the joint velocities aren't zero, and actually give an end-effector velocity of 0.0012 m/s in the direction normal to the ground (With that velocity, the end_effector "wants" to leave the contact).

Inital hypothersis, which turned out not to be true:

Initially, i thought it had to do with the ros_control plugin and the way that it publishes velocities. But, i get the joint velocities from gazebo itself (last plot: Gazebo Velocity Plots) and they agree with the velocities from the joint_states topic.

Question:

Why don't the joint velocities agree with the measured displacements?

Simulation experiment description:

I am running some contact simulations (below I include the simulation parameters) for a robotic leg in gazebo (11.11) using ros (noetic). The experiment I am running is the following: The leg starts touching the ground, like the figure below, and an effort controller is activated with the following command:

$$ \tau = \mathbf{J}^T h_{desired} + \mathbf{G} $$

Then I am getting the joint positions and velocities in order to compare them to an other simulation. However, the joint positions and velocities do not make sense. I present them, at steady state, below.

Data:

From the velocity plots, the mean velocities are: $ q_{t,2} = 3.59e-3 $ which means, that $ \Delta q_2 \approx (3.59e-3) \cdot 0.8 = 0.0029 \ rad $. Also, $ q_{t,3} = -8.62e-3 $ which means, that $ \Delta q_3 \approx (-8.62e-3) \cdot 0.8 = -0.0069 \ rad $.

However, from the position plots: $ \Delta q_2 \approx -1.20465 + 1.20468 = 3e-5 $ and $ \Delta q_3 \approx -1.22289 + 1.22285 = -4e-5 $.

So physically, this doesn't make sense.

Steady state plots:

Position:

Velocity:

Simulation parameters

Gazebo physics in urdf file (.xacro to be exact):

<physics type="ode" >
  <ode>
      <solver>
      <type>world</type>
        <friction_model>cone_model</friction_model>
      </solver>
  </ode>
  <max_step_size>0.001</max_step_size>
</physics>

and contact parameters:

  <collision>
    <surface>
      <friction>
      <torsional>
        <coefficient> 0 </coefficient>
      </torsional>
        <ode>
          <mu>0.7</mu>
          <mu2>0.7</mu2>
        </ode>
      </friction>
      <contact>
        <ode>
            <kp>1e4</kp>
            <kd>1e2</kd>
            <max_vel>100</max_vel>
        </ode>
      </contact>
    </surface>  
  </collision>

Gazebo Velocity plots:

Here, the velocity plots straight from gazebo are shown. Gazebo still calculates the same velocities that i get from the joint_states topic. So, the problem is from gazebo and not due to a faulty implementation of the ros_control plugin.

(purple : joint 1, blue: joint 2, green: joint3)

Answer 1

It's likely that the contacts are continually being created in one step and removed in the next step. I would first check that you're using a primitive sphere for your collision instead of a mesh. That's critical for having good contact behavior. I would also lower the max_vel since that's the maximum allowed correction velocity when there's interpenetration of surfaces. The next parameter to tune is the cfm. This will help stabilize the contact points and hopefully get rid of your intermittent contacts. The "Contact Parameter" section of https://classic.gazebosim.org/tutorials?tut=physics_params&cat=physics is a good reference.

Edit: Visualizing the contacts in Gazebo using "View -> Contacts" should help in determining if you have intermittent contacts.