[Bug]: Inconsistent performance (anisotropy) when object gets pushed with different initial pose

[ZhuoyunZhong]

trafficstars

Bug Description

This is a continuation of the same setup of a closed issue. But we ran into a different bug this time.

Simply put, the initial pose of an object (orientation to be specific) affects the results of it being pushed on the surface.

I set up the same object to start with the same position but different orientations (from 0 -> 2pi, 12 in total). I then set up a pusher object (simulating a robot end effector) to push from the same local position and direction w.r.t. where the object is. See the video for a demonstration.

However, despite everything being identical except for the initial yaw rotation of the box, the final relative poses of the box vary significantly. This was unexpected because I anticipated very similar final poses (allowing for minor noise) when the local push is identical.

Please check the output log. You will find that, when objects are with orientation 0, 90, 180, 270 degrees, their performances are very close (rotated by around -1.3). Also similar for 30, 120, 210, 300 (rotated by around 0.26), etc. But, the differences among 0, 30, and 60 are huge, with some rotated a lot and some barely rotated.

I suspect that the direction of the object gets pushed somehow affects the dynamic computation? The friction/dynamic is not isotropic?

Steps to Reproduce

import numpy as np
from scipy.spatial.transform import Rotation as R
import genesis as gs


def main():
    # Init
    gs.init(backend=gs.gpu)
    scene = gs.Scene(
        show_viewer=True,
        show_FPS=False,
        sim_options=gs.options.SimOptions(substeps=8),
    )

    # Table
    friction = 0.2
    table = scene.add_entity(
        gs.morphs.Box(pos=(0.0, 0, -0.25), size=(2, 2, 0.5), fixed=True),
        material=gs.materials.Rigid(friction=friction),
    )

    # Push objects
    obj = scene.add_entity(
        gs.morphs.Box(pos=(0, 0, 0.05), size=(0.1, 0.2, 0.1)),
        surface=gs.surfaces.Default(color=(1, 0, 0)),
        material=gs.materials.Rigid(friction=friction),
    )
    pusher = scene.add_entity(
        # a bit higher in z to avoid any friction
        gs.morphs.Cylinder(pos=(-0.2, 0.01, 0.06), height=0.1, radius=0.02),
        surface=gs.surfaces.Default(color=(0, 0, 1)),
        # remove gravity
        material=gs.materials.Rigid(gravity_compensation=1.0),
    )

    # Build Scene
    n_envs = 12  # every 360 / 12 = 30 degrees
    scene.build(n_envs=n_envs, env_spacing=(3.0, 3.0))

    #############################################
    # Set object to different starting orientations to be pushed
    # Set the pusher to push from the same
    # local position and direction w.r.t. the object

    # Set object init orientation (0 -> 2pi)
    obj_init_pos = np.array([0, 0, 0.05])
    obj_init_angles = np.linspace(0, 2 * np.pi, n_envs, endpoint=False)
    obj_init_quats = [euler_to_quat([0, 0, yaw]) for yaw in obj_init_angles]
    # Set and create SE3 homogeneous transformation for each environment
    obj_init_poses = [get_homogeneous(obj_init_pos, q) for q in obj_init_quats]
    obj.set_quat(np.array(obj_init_quats))

    # Set pusher init position (same local position w.r.t. the object)
    local_pos = np.array([-0.2, 0.01, 0.01])
    # Transform the local pusher position using each object's initial transform
    push_pos = [(h @ np.append(local_pos, 1))[:3] for h in obj_init_poses]
    pusher.set_pos(np.array(push_pos))
    scene.step()

    # Set pusher push velocity (same local velocity w.r.t. the object)
    local_vel = np.array([0.5, 0, 0])
    push_vel = [h[:3, :3] @ local_vel for h in obj_init_poses]
    # Append zero angular velocity
    push_vel = np.array([np.concatenate([v, np.zeros(3)]) for v in push_vel])

    #############################################
    # Execute push
    input("Start?")
    for i in range(300):
        if i < 100:
            pusher.control_dofs_velocity(push_vel)
        else:
            pusher.control_dofs_velocity(
                np.tile([0, 0, 0, 0, 0, 0], (n_envs, 1))
            )
        scene.step()

    #############################################
    # Read final object pose w.r.t. the initial pose
    positions = obj.get_pos().cpu().numpy()
    rotations = obj.get_quat().cpu().numpy()

    final_relative_poses = []
    for i in range(n_envs):
        # Create final homogeneous transformation matrix
        T_final = get_homogeneous(positions[i], rotations[i])
        T_rel = obj_init_poses[i].T @ T_final

        # Extract pos and euler
        trans = T_rel[:3, 3]
        euler = R.from_matrix(T_rel[:3, :3]).as_euler("xyz", degrees=False)
        final_relative_pose = np.concatenate((trans, euler))
        final_relative_poses.append(final_relative_pose)

    # Print final relative poses for each environment
    for i, (angle, pose) in enumerate(
        zip(obj_init_angles, final_relative_poses)
    ):
        angle = round(angle / np.pi * 180, 1)
        print(f"Env {i}:\t Initial Angle - {angle} Final Pose: {pose}")


# Helpers
def euler_to_quat(euler):
    r = R.from_euler("xyz", euler, degrees=False)
    quat_xyzw = r.as_quat()
    return np.roll(quat_xyzw, 1)


def get_homogeneous(pos, quat):
    homogeneous = np.eye(4)
    quat_xyzw = np.roll(quat, -1)
    rot = R.from_quat(quat_xyzw).as_matrix()
    homogeneous[:3, :3] = rot
    homogeneous[:3, 3] = pos
    return homogeneous


if __name__ == "__main__":
    np.set_printoptions(suppress=True, precision=3)
    main()

Expected Behavior

The end local pose w.r.t. to the initial pose should be close (with some noise) regardless of the initial object pose. The initial orientation should not affect the pushed results.

Screenshots/Videos

https://github.com/user-attachments/assets/b483a064-8fb8-4230-98c8-cd3484c23b3c

Relevant log output

Env 0:   Initial Angle - 0.0 Final Pose: [ 0.249 -0.06   0.    -0.     0.    -1.313]
Env 1:   Initial Angle - 30.0 Final Pose: [ 0.369  0.058  0.    -0.     0.     0.26 ]
Env 2:   Initial Angle - 60.0 Final Pose: [ 0.341 -0.061  0.    -0.001  0.    -0.608]

Env 3:   Initial Angle - 90.0 Final Pose: [ 0.263 -0.057  0.     0.001  0.002 -1.333]
Env 4:   Initial Angle - 120.0 Final Pose: [ 0.364  0.057  0.    -0.     0.001  0.278]
Env 5:   Initial Angle - 150.0 Final Pose: [ 0.344 -0.06   0.     0.     0.    -0.599]

Env 6:   Initial Angle - 180.0 Final Pose: [ 0.253 -0.06   0.     0.     0.001 -1.311]
Env 7:   Initial Angle - 210.0 Final Pose: [ 0.369  0.056  0.    -0.001  0.001  0.23 ]
Env 8:   Initial Angle - 240.0 Final Pose: [ 0.345 -0.06   0.     0.     0.    -0.608]

Env 9:   Initial Angle - 270.0 Final Pose: [ 0.247 -0.061  0.     0.     0.    -1.288]
Env 10:  Initial Angle - 300.0 Final Pose: [ 0.367  0.058  0.    -0.001  0.001  0.265]
Env 11:  Initial Angle - 330.0 Final Pose: [ 0.338 -0.061  0.    -0.     0.    -0.653]

Environment

• OS: [Ubuntu 22.04]
• GPU/CPU [RTX 4070 Ti Super]
• GPU-driver version (550.127.08)
• CUDA / CUDA-toolkit version (12.4)

Release version or Commit ID

PIP genesis-world, 0.2.1

Additional Context

No response

[ZhuoyunZhong]

Two more things I have tried:

1, Even if the obj size is a cube, this problem still happens.

2, If I rotate the table instead of rotating the obj, this problem won't happen.

I assume the friction/dynamic computation along different coordinate directions is somehow different? The same push along the direction (1, 0) will be different than along (1, 1)

[duburcqa]

Anisotropy is expected, because of the pyramid approximation of the friction cone. The side of the pyramid has constant orientation wrt world, and does not depend on the orientation of the object or its linear velocity. See this discussion for reference.

Fixing this discrepancy would require to implement Elliptic friction cone.

[ZhuoyunZhong]

Is there a plan to implement the Elliptic Friction Cone?

[duburcqa]

@YilingQiao What do you think?

[ZhuoyunZhong]

@YilingQiao Just try to follow up on this.

[YilingQiao]

We plan to run some benchmarks to evaluate how important the Elliptic Friction Cone is for robotics applications. Is this feature critical for your use case? Could you tell us more about your application?

[ZhuoyunZhong]

Thanks for following up! I believe this issue is quite critical for our robot learning setup, and I’d like to provide more context on why.

Consider a non-prehensile manipulation task (like pushing), our goal is to learn a predictive model of the object’s end pose given the parameterized push (e.g., push direction, offset, force). The push is defined in the object's local frame, regardless object's initial pose. This is under our assumption, also most of the time physically, that the resulting motion should be isotropic.

However, what we observed was that the final poses vary significantly across different initial orientations due to the pyramid friction cone. This can be seen both in the video and in the logs.

This is fine if, whenever collecting data, I always set the object to be the same starting pose. But once I want to plan, for example, a consecutive move (i.e. multiple pushes sequentially), the objects will end up being in a different orientation during the pushing process. Whenever the object has a different orientation, the pushing result will be dramatically different from the prediction.

[duburcqa]

I think that physically the friction coefficient is unknown and friction properties may vary locally. IMHO, relying on the assumption that the behaviour is always the same given some external forces and orientation in the world is wrong in the first place and such predictive model would not be reliable.

[ZhuoyunZhong]

In our problem setting, the table surface is even. That's why we assume it should be isotropic regardless of where we place the object. We actually conducted experiments in the real world, and our method is working as expected. By the way, the friction coefficient is learned through interaction, and we just assume it is pose-independent.

Yes, in the real world, friction properties may still vary locally, but given the even table surface, this variation is small and considered acceptable data noise. However, it does not work in Genesis simply because such variation is so large in simulation and is pose-dependent.

I will provide a quantitative comparison in a bit.

[ZhuoyunZhong]

Here is a simulation test. We run four experiments, and in each experiment, we push from 100 different angles, and repeat it 10 times.

For different experiments, we run four different settings during repetition:

• same position and rotation
• varying position
• varying rotation
• varying both position and rotation

We measure the standard deviation of the pushing outcome:

Experiment: Random Position: False, Random Rotation: False Overall Std: [0.00103 0.00115 0.01171] Experiment: Random Position: True, Random Rotation: False Overall Std: [0.00116 0.00127 0.01461] Experiment: Random Position: False, Random Rotation: True Overall Std: [0.02912 0.02946 0.19879] Experiment: Random Position: True, Random Rotation: True Overall Std: [0.02976 0.03052 0.20354]

Apparently, as you have already pointed out, due to the pyramid implementation, the results here are object rotation-dependent. Whenever we vary the object's starting rotation, the results vary very much.

---

Also, for your reference, our problem setting is similar to this paper: Augmenting Physical Simulators with Stochastic Neural Networks: Case Study of Planar Pushing and Bouncing

I guess this pyramid collision cone is totally fine with many other robotic applications. But it indeed is not usable in this specific problem setting.

---

Scripts to reproduce simulation results

import numpy as np
from scipy.spatial.transform import Rotation as R
import genesis as gs
from tqdm import tqdm


def main(experiments, n_reps):
    """
    Set up the scene and run the experiments
    Args:
        experiments: list of experiments to run, each experiment is a tuple
                     as [random_position, random_rotation]
    """
    # Init
    gs.init(backend=gs.gpu)
    scene = gs.Scene(
        show_FPS=False, sim_options=gs.options.SimOptions(substeps=4)
    )

    # Table
    friction = 0.2
    table = scene.add_entity(
        gs.morphs.Box(pos=(0, 0, -0.25), size=(1.5, 1.5, 0.5), fixed=True),
        material=gs.materials.Rigid(friction=friction),
    )
    # Push objects
    obj = scene.add_entity(
        gs.morphs.Box(pos=(0, 0, 0.05), size=(0.2, 0.2, 0.1)),
        surface=gs.surfaces.Default(color=(1, 0, 0)),
        material=gs.materials.Rigid(friction=friction),
    )
    pusher = scene.add_entity(
        # a bit higher in z to avoid any friction
        gs.morphs.Cylinder(pos=(-0.2, 0.01, 0.06), height=0.1, radius=0.02),
        surface=gs.surfaces.Default(color=(0, 0, 1)),
        # remove gravity
        material=gs.materials.Rigid(gravity_compensation=1.0),
    )

    # Build Scene
    n_envs = 100
    scene.build(n_envs=n_envs, env_spacing=(2.0, 2.0))

    # Run experiments
    # generate evenly different pushes
    pushes = np.linspace(0, 2 * np.pi, n_envs)
    # np.save("pushes.npy", pushes)

    # run
    input("Start Experiments?")
    for experiment in experiments:
        results = run_experiment(
            scene, table, pusher, obj, pushes, experiment, n_reps
        )
        np.save(f"results_{experiment}.npy", results)


def run_experiment(scene, table, pusher, obj, pushes, experiment, n_reps=10):
    """Run the experiment n_reps times and return the results"""
    n_envs = scene.n_envs
    results = np.zeros((n_reps, n_envs, 3))
    init_pos = np.tile(np.array([0, 0, 0.05]), (n_envs, 1))
    init_ori = np.tile(np.array([1, 0, 0, 0]), (n_envs, 1))

    # Initialize object poses
    for rep in tqdm(range(n_reps)):
        # Change object position and rotation given experiment settings
        # Random position if experiment[0] is True
        eulers = np.zeros(n_envs)
        if experiment[0]:
            init_pos = np.random.uniform(-0.2, 0.2, (n_envs, 2))
            init_pos = np.concatenate(
                (init_pos, 0.05 * np.ones((n_envs, 1))), axis=1
            )
        # Random rotation if experiment[1] is True
        if experiment[1]:
            eulers = np.random.uniform(-np.pi, np.pi, n_envs)
            init_ori = np.array(
                [euler_to_quat((0, 0, euler)) for euler in eulers]
            )
        obj_init_poses = [
            get_homogeneous(init_pos[i], init_ori[i]) for i in range(n_envs)
        ]

        # Get pusher position given object position and rotation
        pusher_local_pos = 0.3 * np.array(
            [(np.cos(euler), np.sin(euler), 0) for euler in pushes]
        )
        pusher_pos = np.array(
            [
                h[:3, :3] @ p + h[:3, 3]
                for h, p in zip(obj_init_poses, pusher_local_pos)
            ]
        )

        # Reset
        reset(scene, [(obj, init_pos, init_ori), (pusher, pusher_pos, None)])

        # Get pusher velocity towards object (pos + rot)
        push_vel = [(o - p) for p, o in zip(pusher_pos, init_pos)]
        push_vel = 1.5 * np.array(
            [np.concatenate([v, np.zeros(3)]) for v in push_vel]
        )
        zero_vel = np.tile([0, 0, 0, 0, 0, 0], (n_envs, 1))
        # Execute push
        for i in range(200):
            if i < 100:
                pusher.control_dofs_velocity(push_vel)
            else:
                pusher.control_dofs_velocity(zero_vel)
            scene.step()

        # Read final object pose w.r.t. the initial pose
        positions = obj.get_pos().cpu().numpy()
        rotations = obj.get_quat().cpu().numpy()
        for i in range(n_envs):
            pose_final = get_homogeneous(positions[i], rotations[i])
            pose_rel = np.linalg.inv(obj_init_poses[i]) @ pose_final
            # extract pos and euler
            trans = pose_rel[:3, 3]
            euler = R.from_matrix(pose_rel[:3, :3]).as_euler("xyz")
            final_relative_pose = np.array((trans[0], trans[1], euler[2]))
            results[rep, i] = final_relative_pose

    return results


# Simulation Helpers
def reset(scene, obj_list):
    for obj, pos, quat in obj_list:
        if pos is not None:
            obj.set_pos(pos)
        if quat is not None:
            obj.set_quat(quat)
    scene.step()


# Geometry Helpers
def euler_to_quat(euler):
    r = R.from_euler("xyz", euler, degrees=False)
    quat_xyzw = r.as_quat()
    return np.roll(quat_xyzw, 1)


def get_homogeneous(pos, quat):
    homogeneous = np.eye(4)
    quat_xyzw = np.roll(quat, -1)
    rot = R.from_quat(quat_xyzw).as_matrix()
    homogeneous[:3, :3] = rot
    homogeneous[:3, 3] = pos
    return homogeneous


# Evaluation
def evaluate_results(experiment):
    """Evaluate the results' standard deviation of the experiments"""
    # Compute the mean and std of the results
    results = np.load(f"results_{experiment}.npy")
    std_per_env = np.std(results, axis=0)
    overall_std = np.mean(std_per_env, axis=0)

    print(
        f"\nExperiment: "
        + f"Random Position: {experiment[0]}, "
        + f"Random Rotation: {experiment[1]}"
    )
    print(f"Overall Std: {overall_std}")


if __name__ == "__main__":
    np.set_printoptions(suppress=True, precision=5)
    np.random.seed(0)

    # Run n_reps times of n_envs different pushes
    # experiment (bool, bool) defines if
    # we randomize position or rotation for each repetition
    experiments = [(False, False), (True, False), (False, True), (True, True)]

    main(experiments, n_reps=10)
    for experiment in experiments:
        evaluate_results(experiment)