Issue in friction force estimation while contact with plane

[eshafeeqe]

Hi

I was experimenting with single leg urdf for understanding the response when it touches ground. I was interested about the friction and normal forces when the leg collide (come into contact) with ground plane. My single leg system have three joints 1) prismatic and two revolute link joints. The initial configuration of single leg URDF is shown below.

And I constantly rotated the two revolute joints with constant torque of 5N. Since the entire system is moving in x direction I was expecting a high friction along x axis and less friction along y axis. However when I plot the friction forces along x and y axis (with help of getContactPoints method), I got higher friction along y axis and smaller friction along the x axis. The plot given below.

Here my question is why pybullet giving higher (orders of magnitude) friction force in y direction even though the motion is mainly in x direction. Secondly we can see the friction force in x direction died out after some time, but friction in y direction become is non zero throughout the simulation, why is that ? Is that because of the Coulomb friction model used under in the impulse based contact modelling.

The experiment can be reproduce using the below script.

import pybullet as p
import time
import pybullet_data
import matplotlib.pyplot as plt

cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
  cid = p.connect(p.GUI)

p.setAdditionalSearchPath(pybullet_data.getDataPath())

single_leg_urdf = "single_leg_prismatic.urdf"

initial_positions = [0, 0.75, 0, 0] #

plane = p.loadURDF("plane.urdf")
robot_id = p.loadURDF(single_leg_urdf, [0, 0, 0.195], flags=p.URDF_USE_INERTIA_FROM_FILE,
                    useFixedBase=1)

num_joints = p.getNumJoints(robot_id) # 4
for i in range(num_joints):
    p.setJointMotorControl2(robot_id, i,
                                   controlMode=p.VELOCITY_CONTROL,
                                   force=0.5)
for i in range(num_joints):
   p.resetJointState(robot_id, i, initial_positions[i])

joints = [1,2]

cam_info = p.getDebugVisualizerCamera()
distance, yaw, pitch = 0.35, 0, -20
position = [0, 0, 0.15]
p.resetDebugVisualizerCamera(distance, yaw, pitch, position)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)

p.setGravity(0, 0, -10)

t_curr = 0
t_max  = 3
t_delta = 0.001
p.setTimeStep(t_delta)

lateral_friction1 = []
lateral_friction2 = []

lateral_frictionDir1 = []
lateral_frictionDir2 = []
time_array = []

while t_curr < t_max:

    for joint_id in joints:
        p.setJointMotorControl2(robot_id, joint_id,
                                   controlMode=p.TORQUE_CONTROL,
                                   force=-5.0)
    p.stepSimulation()

    contact_info = p.getContactPoints(robot_id, plane)

    if len(contact_info) > 0:
        lateral_friction1.append(contact_info[0][10])
        lateral_friction2.append(contact_info[0][12])
        lateral_frictionDir1.append(contact_info[0][11])
        lateral_frictionDir2.append(contact_info[0][13])

    else:
        lateral_friction1.append(0)
        lateral_friction2.append(0)
        lateral_frictionDir1.append([0,0,0])
        lateral_frictionDir2.append([0,0,0])

    time.sleep(t_delta)
    t_curr += t_delta
    time_array.append(t_curr)

friction_x = []
friction_y = []

for i in range(len(lateral_friction1)):
    friction_x.append(lateral_friction1[i]*lateral_frictionDir1[i][0] + lateral_friction2[i]*lateral_frictionDir2[i][0])
    friction_y.append(lateral_friction1[i] * lateral_frictionDir1[i][1] + lateral_friction2[i] * lateral_frictionDir2[i][1])

f, axs = plt.subplots(2, 1, figsize=(10, 5))
axs[0].plot(time_array, friction_x, 'r.', label="friction_x")
axs[1].plot(time_array, friction_y, 'g.', label="friction_y")

axs[0].set_ylabel("Force (N)")
axs[0].set_xlabel("time (sec)")

axs[1].set_ylabel("Force (N)")
axs[1].set_xlabel("time (sec)")

axs[0].legend()
axs[1].legend()

f.tight_layout()
plt.show()

I am not sure whether this is an issue of simulator or any missing in my understanding of the simulator. URDF is available from: https://drive.google.com/drive/folders/1FOPNfCiao2BGy3KF3_-QEnE7aDCOVPF6?usp=sharing

[erwincoumans]

Thanks for the reproduction/example. There is no degree of freedom in the y direction in your example, is it? Can you try adding it and see if that changes things?

[erwincoumans]

I had a closer look. The friction in the x direction goes to zero, which is correct: there is no velocity anymore due to static friction, the object pushes onto the ground, and there are no forces along the x, since the object can float along the x direction.

The forces along the Y should be ignored, since there are no degrees of freedom. The reported values are due to floating point noise, some denominator reaches 1e-9 and causes the misleading readings.

We could add some tolerance larger than DBL_EPSILON in btMultiBodyConstraintSolver line 708 to filter this noise, then the image looks like this:

                btScalar d = denom0 + denom1 + cfm;
                ///instead of if (d > DBL_EPSILON ) use
                btScalar tolerance  = 1e-7;
        if (d > tolerance)
        {
            solverConstraint.m_jacDiagABInv = relaxation / (d);
        }
        else
        {
            //disable the constraint row to handle singularity/redundant constraint
            solverConstraint.m_jacDiagABInv = 0.f;
        }