Physics simulation is unstable and different from that of pybullet

rishiagarwal2000 commented 1 year ago

Habitat-Sim version

main

🐛 Bug

I tried simulating a urdf from here in habitat-sim and pybullet. The outputs for the simulation are attached. Apparently, the simulations differ significantly which seems unexpected to me because habitat-sim uses the same pybullet simulation internally. Also, the simulation is highly unstable. I am not sure if this is a bug, or if I am using it incorrectly. It would be great if you could help me with this.

Steps to Reproduce

Pybullet snippet

import pybullet as p
import time
import pybullet_data
from scipy.spatial.transform import Rotation as R
import sys
import imageio

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.8)
planeId = p.loadURDF("plane.urdf")
startPos = [0,0,1.1]
startOrientation = p.getQuaternionFromEuler([0,0,0])

humanId = p.loadURDF("CIRCLE_assets/subjects/2.urdf", startPos, startOrientation) #R.from_euler('xyz', [0, 0, 0], degrees=True).as_quat())
jointIndices = [i for i in range(p.getNumJoints(humanId)) if p.getJointInfo(humanId, i)[2] == p.JOINT_SPHERICAL]
print([p.changeDynamics(humanId, i, linearDamping=0, angularDamping=0, jointDamping=0.0) for i in jointIndices])

print("starting sim loop")

for i in range (100):
    p.stepSimulation()
    time.sleep(1./240.)

p.disconnect()

Habitat-sim snippet

import habitat_sim
from habitat_sim.logging import logger
from settings import default_sim_settings, make_cfg

import habitat_sim.physics as phy
from habitat_sim.utils.common import quat_from_magnum, quat_to_magnum
from scipy.spatial.transform import Rotation as R

import magnum as mn
import numpy as np
from typing import Any, Callable, Dict, List, Optional, Tuple
import pathlib
import json

import os
import imageio

def global_correction_quat(up_v: mn.Vector3, forward_v: mn.Vector3) -> mn.Quaternion:
    """
    Given the upward direction and the forward direction of a local space frame, this methd produces
    the correction quaternion to convert the frame to global space (+Y up, -Z forward).
    """
    if up_v.normalized() != mn.Vector3.y_axis():
        angle1 = mn.math.angle(up_v.normalized(), mn.Vector3.y_axis())
        axis1 = mn.math.cross(up_v.normalized(), mn.Vector3.y_axis())
        rotation1 = mn.Quaternion.rotation(angle1, axis1)
        forward_v = rotation1.transform_vector(forward_v)
    else:
        rotation1 = mn.Quaternion()

    forward_v = forward_v * (mn.Vector3(1.0, 1.0, 1.0) - mn.Vector3.y_axis())
    angle2 = mn.math.angle(forward_v.normalized(), -1 * mn.Vector3.z_axis())
    axis2 = mn.Vector3.y_axis()
    rotation2 = mn.Quaternion.rotation(angle2, axis2)

    return rotation2 * rotation1

class Env():
    def __init__(self, sim_settings, ):
        self.sim_settings = sim_settings
        self.urdf_path = "CIRCLE_assets/subjects/1.urdf"
        self.reconfigure_sim()
        self.art_obj_mgr = self.sim.get_articulated_object_manager()
        self.agent_model = self.art_obj_mgr.add_articulated_object_from_urdf(
            filepath=self.urdf_path
        )
        self._add_stage()

        self.agent_model.rotation = global_correction_quat(mn.Vector3.z_axis(), -mn.Vector3.y_axis()) 
        self.init_height = 1.1
        self.agent_model.translation = mn.Vector3(0, self.init_height, 0)

        self.agent_body_node.translation = self.agent_model.translation + mn.Vector3(1, 0.8, 1)
        camera_position = self.agent_body_node.translation
        camera_look_at = self.agent_model.translation
        self.agent_body_node.rotation = mn.Quaternion.from_matrix(
            mn.Matrix4.look_at(
                camera_position, camera_look_at, np.array([0, 1.0, 0])  # up
            ).rotation()
        )

    def reconfigure_sim(self):
        self.cfg = make_cfg(self.sim_settings)
        self.agent_id: int = self.sim_settings["default_agent"]
        self.cfg.agents[self.agent_id] = self.default_agent_config()

        if self.sim_settings["stage_requires_lighting"]:
            logger.info("Setting synthetic lighting override for stage.")
            self.cfg.sim_cfg.override_scene_light_defaults = True
            self.cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY

        self.sim = habitat_sim.Simulator(self.cfg)

        # post reconfigure
        self.active_scene_graph = self.sim.get_active_scene_graph()

        self.default_agent = self.sim.get_agent(self.agent_id)
        self.agent_body_node = self.default_agent.scene_node

        self.render_camera = self.agent_body_node.node_sensor_suite.get("color_sensor")
        # set sim_settings scene name as actual loaded scene
        self.sim_settings["scene"] = self.sim.curr_scene_name

    def _add_stage(self):
        self.stage = self.art_obj_mgr.add_articulated_object_from_urdf(
            filepath="simpleplane.urdf"
        )
        assert self.stage.is_alive
        self.stage.motion_type = phy.MotionType.DYNAMIC
        self.stage.rotation = global_correction_quat(mn.Vector3.z_axis(), mn.Vector3.x_axis()) #self.final_rotation_correction
        self.stage.translation = mn.Vector3(0,0,0) + mn.Vector3(0, -0.2, 0)

    def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration:
        """
        Set up our own agent and agent controls
        """
        make_action_spec = habitat_sim.agent.ActionSpec
        make_actuation_spec = habitat_sim.agent.ActuationSpec
        MOVE, LOOK = 0.07, 1.5

        # all of our possible actions' names
        action_list = [
            "move_left",
            "turn_left",
            "move_right",
            "turn_right",
            "move_backward",
            "look_up",
            "move_forward",
            "look_down",
            "move_down",
            "move_up",
        ]

        action_space: Dict[str, habitat_sim.agent.ActionSpec] = {}

        # build our action space map
        for action in action_list:
            actuation_spec_amt = MOVE if "move" in action else LOOK
            action_spec = make_action_spec(
                action, make_actuation_spec(actuation_spec_amt)
            )
            action_space[action] = action_spec

        sensor_spec: List[habitat_sim.sensor.SensorSpec] = self.cfg.agents[
            self.agent_id
        ].sensor_specifications

        agent_config = habitat_sim.agent.AgentConfiguration(
            height=1.5,
            radius=0.1,
            sensor_specifications=sensor_spec,
            action_space=action_space,
            body_type="cylinder",
        )
        return agent_config
    def step(self):
        for _ in range(4):
                self.sim.step_world()

    def render(self, keys=(0, "color_sensor")):
            self.sim._Simulator__sensors[keys[0]][keys[1]].draw_observation()
            arr = self.sim.get_sensor_observations()[keys[1]]
            arr = self.sim.get_sensor_observations()[keys[1]]
            return arr

if __name__ == '__main__':

    sim_settings = default_sim_settings
    sim_settings["scene"] = "NONE"
    sim_settings.pop("scene_dataset_config_file")

    env = Env(sim_settings)
    # o, info = env.reset()
    frames = [env.render()]
    dir_ = f'gifs-habitat-sim'

    if not os.path.isdir(dir_):
        os.mkdir(dir_)

    path = f'{dir_}/test.gif'
    for _ in range(100):
        env.step()
        frames.append(env.render())
    imageio.mimsave(path, frames)

settings.py imported by habitat-sim snippet

import habitat_sim
import habitat_sim.agent
from scipy.spatial.transform import Rotation as R

default_sim_settings = {
    # settings shared by example.py and benchmark.py
    "max_frames": 1000,
    "width": 640,
    "height": 480,
    "default_agent": 0,
    "sensor_height": 0,
    "hfov": 90,
    "color_sensor": True,  # RGB sensor (default: ON)
    "semantic_sensor": False,  # semantic sensor (default: OFF)
    "depth_sensor": False,  # depth sensor (default: OFF)
    "ortho_rgba_sensor": False,  # Orthographic RGB sensor (default: OFF)
    "ortho_depth_sensor": False,  # Orthographic depth sensor (default: OFF)
    "ortho_semantic_sensor": False,  # Orthographic semantic sensor (default: OFF)
    "fisheye_rgba_sensor": False,
    "fisheye_depth_sensor": False,
    "fisheye_semantic_sensor": False,
    "equirect_rgba_sensor": False,
    "equirect_depth_sensor": False,
    "equirect_semantic_sensor": False,
    "seed": 1,
    "silent": True,  # do not print log info (default: OFF)
    # settings exclusive to example.py
    "save_png": False,  # save the pngs to disk (default: OFF)
    "print_semantic_scene": False,
    "print_semantic_mask_stats": False,
    "compute_shortest_path": False,
    "compute_action_shortest_path": False,
    "scene": "CIRCLE_assets/stages/102815835.glb",
    "test_scene_data_url": "http://dl.fbaipublicfiles.com/habitat/habitat-test-scenes.zip",
    "goal_position": [5.047, 0.199, 11.145],
    "enable_physics": True,
    "enable_gfx_replay_save": False,
    "physics_config_file": "CIRCLE_assets/default.physics_config.json",
    "num_objects": 10,
    "test_object_index": 0,
    "frustum_culling": True,
    "stage_requires_lighting": True,
    "scene_dataset_config_file": "CIRCLE_assets/floorplanner.scene_dataset_config.json",
}

# build SimulatorConfiguration
def make_cfg(settings):
    sim_cfg = habitat_sim.SimulatorConfiguration()
    if "scene_dataset_config_file" in settings:
        sim_cfg.scene_dataset_config_file = settings["scene_dataset_config_file"]
    sim_cfg.frustum_culling = settings.get("frustum_culling", False)
    if "enable_physics" in settings:
        sim_cfg.enable_physics = settings["enable_physics"]
    if "physics_config_file" in settings:
        sim_cfg.physics_config_file = settings["physics_config_file"]
    if not settings["silent"]:
        print("sim_cfg.physics_config_file = " + sim_cfg.physics_config_file)
    if "scene_light_setup" in settings:
        sim_cfg.scene_light_setup = settings["scene_light_setup"]
    sim_cfg.gpu_device_id = 0
    if not hasattr(sim_cfg, "scene_id"):
        raise RuntimeError(
            "Error: Please upgrade habitat-sim. SimulatorConfig API version mismatch"
        )
    sim_cfg.scene_id = settings["scene"]

    # define default sensor parameters (see src/esp/Sensor/Sensor.h)
    sensor_specs = []

    def create_camera_spec(**kw_args):
        camera_sensor_spec = habitat_sim.CameraSensorSpec()
        camera_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
        camera_sensor_spec.resolution = [settings["height"], settings["width"]]
        camera_sensor_spec.position = [0, settings["sensor_height"], 0]
        # camera_sensor_spec.orientation = [90,0,0]
        for k in kw_args:
            setattr(camera_sensor_spec, k, kw_args[k])
        return camera_sensor_spec

    if settings["color_sensor"]:
        color_sensor_spec = create_camera_spec(
            uuid="color_sensor",
            hfov=settings["hfov"],
            sensor_type=habitat_sim.SensorType.COLOR,
            sensor_subtype=habitat_sim.SensorSubType.PINHOLE,
        )
        sensor_specs.append(color_sensor_spec)

    if settings["depth_sensor"]:
        depth_sensor_spec = create_camera_spec(
            uuid="depth_sensor",
            hfov=settings["hfov"],
            sensor_type=habitat_sim.SensorType.DEPTH,
            channels=1,
            sensor_subtype=habitat_sim.SensorSubType.PINHOLE,
        )
        sensor_specs.append(depth_sensor_spec)

    if settings["semantic_sensor"]:
        semantic_sensor_spec = create_camera_spec(
            uuid="semantic_sensor",
            hfov=settings["hfov"],
            sensor_type=habitat_sim.SensorType.SEMANTIC,
            channels=1,
            sensor_subtype=habitat_sim.SensorSubType.PINHOLE,
        )
        sensor_specs.append(semantic_sensor_spec)

    if settings["ortho_rgba_sensor"]:
        ortho_rgba_sensor_spec = create_camera_spec(
            uuid="ortho_rgba_sensor",
            sensor_type=habitat_sim.SensorType.COLOR,
            sensor_subtype=habitat_sim.SensorSubType.ORTHOGRAPHIC,
        )
        sensor_specs.append(ortho_rgba_sensor_spec)

    if settings["ortho_depth_sensor"]:
        ortho_depth_sensor_spec = create_camera_spec(
            uuid="ortho_depth_sensor",
            sensor_type=habitat_sim.SensorType.DEPTH,
            channels=1,
            sensor_subtype=habitat_sim.SensorSubType.ORTHOGRAPHIC,
        )
        sensor_specs.append(ortho_depth_sensor_spec)

    if settings["ortho_semantic_sensor"]:
        ortho_semantic_sensor_spec = create_camera_spec(
            uuid="ortho_semantic_sensor",
            sensor_type=habitat_sim.SensorType.SEMANTIC,
            channels=1,
            sensor_subtype=habitat_sim.SensorSubType.ORTHOGRAPHIC,
        )
        sensor_specs.append(ortho_semantic_sensor_spec)

    # TODO Figure out how to implement copying of specs
    def create_fisheye_spec(**kw_args):
        fisheye_sensor_spec = habitat_sim.FisheyeSensorDoubleSphereSpec()
        fisheye_sensor_spec.uuid = "fisheye_sensor"
        fisheye_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
        fisheye_sensor_spec.sensor_model_type = (
            habitat_sim.FisheyeSensorModelType.DOUBLE_SPHERE
        )

        # The default value (alpha, xi) is set to match the lens "GoPro" found in Table 3 of this paper:
        # Vladyslav Usenko, Nikolaus Demmel and Daniel Cremers: The Double Sphere
        # Camera Model, The International Conference on 3D Vision (3DV), 2018
        # You can find the intrinsic parameters for the other lenses in the same table as well.
        fisheye_sensor_spec.xi = -0.27
        fisheye_sensor_spec.alpha = 0.57
        fisheye_sensor_spec.focal_length = [364.84, 364.86]

        fisheye_sensor_spec.resolution = [settings["height"], settings["width"]]
        # The default principal_point_offset is the middle of the image
        fisheye_sensor_spec.principal_point_offset = None
        # default: fisheye_sensor_spec.principal_point_offset = [i/2 for i in fisheye_sensor_spec.resolution]
        fisheye_sensor_spec.position = [0, settings["sensor_height"], 0]
        for k in kw_args:
            setattr(fisheye_sensor_spec, k, kw_args[k])
        return fisheye_sensor_spec

    if settings["fisheye_rgba_sensor"]:
        fisheye_rgba_sensor_spec = create_fisheye_spec(uuid="fisheye_rgba_sensor")
        sensor_specs.append(fisheye_rgba_sensor_spec)
    if settings["fisheye_depth_sensor"]:
        fisheye_depth_sensor_spec = create_fisheye_spec(
            uuid="fisheye_depth_sensor",
            sensor_type=habitat_sim.SensorType.DEPTH,
            channels=1,
        )
        sensor_specs.append(fisheye_depth_sensor_spec)
    if settings["fisheye_semantic_sensor"]:
        fisheye_semantic_sensor_spec = create_fisheye_spec(
            uuid="fisheye_semantic_sensor",
            sensor_type=habitat_sim.SensorType.SEMANTIC,
            channels=1,
        )
        sensor_specs.append(fisheye_semantic_sensor_spec)

    def create_equirect_spec(**kw_args):
        equirect_sensor_spec = habitat_sim.EquirectangularSensorSpec()
        equirect_sensor_spec.uuid = "equirect_rgba_sensor"
        equirect_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
        equirect_sensor_spec.resolution = [settings["height"], settings["width"]]
        equirect_sensor_spec.position = [0, settings["sensor_height"], 0]
        for k in kw_args:
            setattr(equirect_sensor_spec, k, kw_args[k])
        return equirect_sensor_spec

    if settings["equirect_rgba_sensor"]:
        equirect_rgba_sensor_spec = create_equirect_spec(uuid="equirect_rgba_sensor")
        sensor_specs.append(equirect_rgba_sensor_spec)

    if settings["equirect_depth_sensor"]:
        equirect_depth_sensor_spec = create_equirect_spec(
            uuid="equirect_depth_sensor",
            sensor_type=habitat_sim.SensorType.DEPTH,
            channels=1,
        )
        sensor_specs.append(equirect_depth_sensor_spec)

    if settings["equirect_semantic_sensor"]:
        equirect_semantic_sensor_spec = create_equirect_spec(
            uuid="equirect_semantic_sensor",
            sensor_type=habitat_sim.SensorType.SEMANTIC,
            channels=1,
        )
        sensor_specs.append(equirect_semantic_sensor_spec)

    # create agent specifications
    agent_cfg = habitat_sim.agent.AgentConfiguration()
    agent_cfg.sensor_specifications = sensor_specs
    agent_cfg.action_space = {
        "move_forward": habitat_sim.agent.ActionSpec(
            "move_forward", habitat_sim.agent.ActuationSpec(amount=0.25)
        ),
        "turn_left": habitat_sim.agent.ActionSpec(
            "turn_left", habitat_sim.agent.ActuationSpec(amount=10.0)
        ),
        "turn_right": habitat_sim.agent.ActionSpec(
            "turn_right", habitat_sim.agent.ActuationSpec(amount=10.0)
        ),
    }

    # override action space to no-op to test physics
    if sim_cfg.enable_physics:
        agent_cfg.action_space = {
            "move_forward": habitat_sim.agent.ActionSpec(
                "move_forward", habitat_sim.agent.ActuationSpec(amount=0.0)
            )
        }

    return habitat_sim.Configuration(sim_cfg, [agent_cfg])

Expected behavior

Bullet simulation (expected) bullet-sim

Habitat sim (unstable and way different) habitat-sim

Additional context

System Info

You can run the script with:

# For security purposes, please check the contents of collect_env.py before running it.
python src_python/habitat_sim/utils/collect_env.py

OS (e.g., Linux): Linux
How you installed PyTorch (conda, pip, source): pip
Build command you used (if compiling from source): python setup.py develop --bullet
Python version: 3.9.16
GPU models and configuration: P8
CUDA version: 11.7
PIP/conda dependencies packages versions
Any other relevant information:

aclegg3 commented 1 year ago

@rishiagarwal2000 Thanks for the bug report.

First, it is important to note that internally Habitat connects to the Bullet C++ API, not PyBullet (which embeds much of its multi-body dynamics support directly into bindings layers and C Bullet integration). Therefore, the expectation of similar default behavior is somewhat unrealistic.

I've looked into this and noticed a couple of things.

These examples appear to be running at very different frame-rates. Your call to sim.step_world() excludes a time-step so uses the default of 1/60. I assume pybullet steps at the physics timestep of 1/240, so this would explain the first inconsistency. I'm not sure what pybullet sim would look like if it continued simulation. :thinking:

PyBullet adds joint motors by default (I'm not sure if this uses SPD by default) while Habitat only adds damping motors when friction is defined. You can remove any automatically created motors and add one full set of new motors in Habitat like:

            # check removal and auto-creation
            joint_motor_settings = habitat_sim.physics.JointMotorSettings(
                position_target=0.0,
                position_gain=1.0,
                velocity_target=0.0,
                velocity_gain=1.0,
                max_impulse=1000.0,
            )
            existing_motor_ids = ao.existing_joint_motor_ids
            for motor_id in existing_motor_ids:
                ao.remove_joint_motor(motor_id)
            ao.create_all_motors(joint_motor_settings)

Habitat does not have SPD implemented for joint motors currently, instead using the Bullet C++ btMultiBodyJointMotor interface. I'm not completely sure how this difference will affect stability.
After playing with these skeletons a bit, I think a large contributing factor to the instability is that inertia matrices are computed from the collision shapes automatically. These are fairly small and spread out possibly contributing to the unstable behavior. For example, see the AMASS test urdf included with Habitat. It has a very similar structure, but much larger collision shapes and much more stable behavior in-engine. If I manually force link inertia to be loaded from the file, I get a much more stable simulation. I could consider adding this option to the API.

Finally, I suggest you try out your simulations interactively in the python viewer.

Make the following small edit to the examples/viewer.py file at line 546:

elif key == pressed.T:
    # load URDF
    fixed_base = alt_pressed
    urdf_file_path = "data/test_assets/urdf/amass_male.urdf" #or your own filepath

    aom = self.sim.get_articulated_object_manager()
    ao = aom.add_articulated_object_from_urdf(
        urdf_file_path, fixed_base, 1.0, 1.0, True
    )
    ao.translation = (
        self.default_agent.scene_node.transformation.transform_point(
            [0.0, 1.0, -1.5]))
    # check removal and auto-creation
    joint_motor_settings = habitat_sim.physics.JointMotorSettings(
        position_target=0.0,
        position_gain=1.0,
        velocity_target=0.0,
        velocity_gain=1.0,
        max_impulse=1000.0,
    )
    existing_motor_ids = ao.existing_joint_motor_ids
    for motor_id in existing_motor_ids:
        ao.remove_joint_motor(motor_id)
    ao.create_all_motors(joint_motor_settings)

Then run it like: python examples/viewer.py --stage-requires-lighting --scene data/test_assets/scenes/simple_room.glb Press 'm' to enter GRAB mouse mode and then click and drag the skeleton to play around with it like so:

https://user-images.githubusercontent.com/1445143/236301486-70676f66-5754-4c14-8b86-8c1769c0e033.mp4

With your file (default) simulation is unstable :frowning_face: :

https://user-images.githubusercontent.com/1445143/236301559-2f9f074b-e80c-4c38-aa42-b23ac956b374.mp4

With your file inertia loaded from URDF it is very stiff, but stable:

https://user-images.githubusercontent.com/1445143/236301684-ccd60e89-a9c2-489a-ae36-b5e03f24ba18.mp4

With your file inertia loaded from URDF but also scaled to 0.001 (from 1) we get what we might expect:

https://user-images.githubusercontent.com/1445143/236301912-cd326ecb-d32b-4454-9a04-9ea83cf7ac4a.mp4

rishiagarwal2000 commented 1 year ago

Thank you so much @aclegg3. This is super helpful and clarifies a lot of questions. I appreciate your elaborate reply. I will try to incorporate these suggestions and if I face any further issues, I will post them here.

facebookresearch / habitat-sim