PyBullet planar friction demo

pyb_friction_demo.py

import numpy as np
import pybullet as pyb
import pybullet_data


def make_box(
    position, half_extents, orientation=(0, 0, 0, 1), mass=1.0, color=(1, 0, 0, 1)
):
    collision_uid = pyb.createCollisionShape(
        shapeType=pyb.GEOM_BOX,
        halfExtents=half_extents,
    )
    visual_uid = pyb.createVisualShape(
        shapeType=pyb.GEOM_BOX,
        halfExtents=half_extents,
        rgbaColor=color,
    )
    return pyb.createMultiBody(
        baseMass=mass,
        baseCollisionShapeIndex=collision_uid,
        baseVisualShapeIndex=visual_uid,
        basePosition=position,
        baseOrientation=orientation,
    )


def get_contact_friction_force(bodyA, bodyB):
    """Total friction force exerted between bodyA and bodyB."""
    points = pyb.getContactPoints(bodyA, bodyB)
    force = np.zeros(3)
    for point in points:
        ff1 = point[10] * np.array(point[11])
        ff2 = point[12] * np.array(point[13])
        force += ff1 + ff2
    return force


def main():
    pyb.connect(pyb.GUI)
    pyb.setGravity(0, 0, -10)
    pyb.setAdditionalSearchPath(pybullet_data.getDataPath())

    ground_uid = pyb.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True)
    box_uid = make_box(position=[0, 0, 0.1], half_extents=[0.5, 0.5, 0.1])

    # combined friction coefficient is 0.5
    pyb.changeDynamics(ground_uid, -1, lateralFriction=1.0)
    pyb.changeDynamics(box_uid, -1, lateralFriction=0.5)

    # let things settle
    for _ in range(10):
        pyb.stepSimulation()

    # apply a force
    pyb.applyExternalForce(
        box_uid,
        -1,
        forceObj=[5, 0, 0],
        posObj=[0, 0, 0],
        flags=pyb.LINK_FRAME,
    )
    pyb.stepSimulation()
    friction_force = get_contact_friction_force(box_uid, ground_uid)
    friction_norm = np.linalg.norm(friction_force)
    print(friction_norm)  # 4.905783443351186

    # now repeat with a yaw angle of 45 deg
    Q = pyb.getQuaternionFromEuler([0, 0, np.pi / 4])
    pyb.resetBasePositionAndOrientation(box_uid, [0, 0, 0.1], Q)

    # let things settle
    for _ in range(10):
        pyb.stepSimulation()

    # apply the exact same force
    pyb.applyExternalForce(
        box_uid,
        -1,
        forceObj=[5, 0, 0],
        posObj=[0, 0, 0],
        flags=pyb.LINK_FRAME,
    )
    pyb.stepSimulation()
    friction_force = get_contact_friction_force(box_uid, ground_uid)
    friction_norm = np.linalg.norm(friction_force)
    print(friction_norm)  # 4.380425825256481

    # this is wrong: the friction force should be the same in both cases, since
    # we are applying the same force with the same contact surface


main()