From 693cfa9fa075af8bb34dda9f8d5355291d241ba6 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 16:32:10 -0700
Subject: [PATCH 1/9] work

---
 .../tutorials/custom_tasks/intro.md           |  7 +++---
 mani_skill/agents/base_agent.py               | 14 ++++++++---
 mani_skill/envs/sapien_env.py                 |  8 ++++++-
 mani_skill/envs/tasks/control/cartpole.py     |  3 ++-
 .../benchmarking/envs/maniskill/cartpole.py   | 12 ++++++----
 mani_skill/utils/building/actor_builder.py    | 23 ++++---------------
 .../utils/building/articulation_builder.py    | 21 +++++------------
 7 files changed, 40 insertions(+), 48 deletions(-)

diff --git a/docs/source/user_guide/tutorials/custom_tasks/intro.md b/docs/source/user_guide/tutorials/custom_tasks/intro.md
index 16ea2d655..9a89fb4de 100644
--- a/docs/source/user_guide/tutorials/custom_tasks/intro.md
+++ b/docs/source/user_guide/tutorials/custom_tasks/intro.md
@@ -96,8 +96,8 @@ class PushCubeEnv(BaseEnv):
                 base_color=[1, 0, 0, 1],
             ),
         )
-        # optionally set an initial pose so that it initially spawns there
-        # self.builder.initial_pose = sapien.Pose(p=[1, 1, 1], q=[1, 0, 0, 0])
+        # strongly recommended to set initial poses for objects, even if you plan to modify them later
+        builder.initial_pose = sapien.Pose(p=[0, 0, 0.02], q=[1, 0, 0, 0])
         self.obj = builder.build(name="cube")
         # PushCube has some other code after this removed for brevity that 
         # spawns a goal object (a red/white target) stored at self.goal_region
@@ -109,8 +109,7 @@ You can build a **kinematic** actor with `builder.build_kinematic` and a **stati
 - Kinematic actors can have their pose changed at any time. Static actors must have an initial pose set before calling `build_static` via `builder.initial_pose = ...`
 - Use static instead of kinematic whenever possible as it saves a lot of GPU memory. Static objects must have an initial pose set before building via `builder.initial_pose = ...`.
 
-Note that by default, if an object does not have an initial pose set in its builder, ManiSkill tries its best to choose initial poses of objects such that they are unlikely to collide with each other during GPU simulation initialization (which uses the initial poses of the builders). These poses are definitely not what you want for task initialization but need to be far apart initially to ensure stable GPU simulation.
-
+Note that by default, if an object does not have an initial pose set in its builder, ManiSkill will set it to a default pose of `q=[1,0,0,0], p=[0,0,0]` and a give a warning. For simple tasks this may not matter but when working with more complex objects and articulations, it is strongly recommended to set initial poses for all objects as GPU simulation might run into bugs/issues if the objects at their initial poses collide.
 
 We also provide some functions that build some more complex shapes that you can use by importing the following:
 ```python
diff --git a/mani_skill/agents/base_agent.py b/mani_skill/agents/base_agent.py
index a11abbb10..d40c901f1 100644
--- a/mani_skill/agents/base_agent.py
+++ b/mani_skill/agents/base_agent.py
@@ -18,6 +18,7 @@
 from mani_skill.sensors.base_sensor import BaseSensor, BaseSensorConfig
 from mani_skill.utils import assets, download_asset, sapien_utils
 from mani_skill.utils.structs import Actor, Array, Articulation
+from mani_skill.utils.structs.pose import Pose
 
 from .controllers.base_controller import (
     BaseController,
@@ -51,6 +52,8 @@ class BaseAgent:
         control_mode (str | None): uid of controller to use
         fix_root_link (bool): whether to fix the robot root link
         agent_idx (str | None): an index for this agent in a multi-agent task setup If None, the task should be single-agent
+        initial_pose (sapien.Pose | Pose | None): the initial pose of the robot. Important to set for GPU simulation to ensure robot
+        does not collide with other objects in the scene during GPU initialization which occurs before `env._initialize_episode` is called
     """
 
     uid: str
@@ -81,6 +84,7 @@ def __init__(
         control_freq: int,
         control_mode: Optional[str] = None,
         agent_idx: Optional[str] = None,
+        initial_pose: Optional[Union[sapien.Pose, Pose]] = None,
     ):
         self.scene = scene
         self._control_freq = control_freq
@@ -93,7 +97,7 @@ def __init__(
         self.sensors: Dict[str, BaseSensor] = dict()
         """The sensors that come with the robot."""
 
-        self._load_articulation()
+        self._load_articulation(initial_pose)
         self._after_loading_articulation()
 
         # Controller
@@ -141,7 +145,9 @@ def _controller_configs(
     def device(self):
         return self.scene.device
 
-    def _load_articulation(self):
+    def _load_articulation(
+        self, initial_pose: Optional[Union[sapien.Pose, Pose]] = None
+    ):
         """
         Loads the robot articulation
         """
@@ -185,7 +191,9 @@ def _load_articulation(self):
                     f"Exiting as assets for robot {self.uid} are not found. Check that this agent is properly registered with the appropriate download asset ids"
                 )
                 exit()
-        self.robot: Articulation = loader.load(asset_path)
+        builder = loader.parse(asset_path)["articulation_builders"][0]
+        builder.initial_pose = initial_pose
+        self.robot = builder.build(self.uid)
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
 
         # Cache robot link names
diff --git a/mani_skill/envs/sapien_env.py b/mani_skill/envs/sapien_env.py
index 785d49530..c601c70b3 100644
--- a/mani_skill/envs/sapien_env.py
+++ b/mani_skill/envs/sapien_env.py
@@ -35,6 +35,7 @@
 from mani_skill.sensors.depth_camera import StereoDepthCamera, StereoDepthCameraConfig
 from mani_skill.utils import common, gym_utils, sapien_utils
 from mani_skill.utils.structs import Actor, Articulation
+from mani_skill.utils.structs.pose import Pose
 from mani_skill.utils.structs.types import Array, SimConfig
 from mani_skill.utils.visualization.misc import tile_images
 from mani_skill.viewer import create_viewer
@@ -379,9 +380,11 @@ def gpu_sim_enabled(self):
     @property
     def _default_sim_config(self):
         return SimConfig()
-    def _load_agent(self, options: dict):
+    def _load_agent(self, options: dict, initial_agent_poses: Optional[Union[sapien.Pose, Pose]] = None):
         agents = []
         robot_uids = self.robot_uids
+        if not isinstance(initial_agent_poses, list):
+            initial_agent_poses = [initial_agent_poses]
         if robot_uids == "none" or robot_uids == ("none", ):
             self.agent = None
             return
@@ -402,6 +405,7 @@ def _load_agent(self, options: dict):
                     self._control_freq,
                     self._control_mode,
                     agent_idx=i if len(robot_uids) > 1 else None,
+                    initial_pose=initial_agent_poses[i] if initial_agent_poses is not None else None,
                 )
                 agents.append(agent)
         if len(agents) == 1:
@@ -640,7 +644,9 @@ def _reconfigure(self, options = dict()):
         self._clear()
         # load everything into the scene first before initializing anything
         self._setup_scene()
+
         self._load_agent(options)
+
         self._load_scene(options)
         self._load_lighting(options)
 
diff --git a/mani_skill/envs/tasks/control/cartpole.py b/mani_skill/envs/tasks/control/cartpole.py
index 82c9fb033..b5cbd874e 100644
--- a/mani_skill/envs/tasks/control/cartpole.py
+++ b/mani_skill/envs/tasks/control/cartpole.py
@@ -111,17 +111,18 @@ def _load_scene(self, options: dict):
         loader = self.scene.create_mjcf_loader()
         actor_builders = loader.parse(MJCF_FILE)["actor_builders"]
         for a in actor_builders:
+            a.initial_pose = sapien.Pose()
             a.build(a.name)
 
         # background visual wall
         self.wall = self.scene.create_actor_builder()
         self.wall.add_box_visual(
             half_size=(1e-3, 20, 10),
-            pose=sapien.Pose(p=[0, 1, 1], q=euler2quat(0, 0, np.pi / 2)),
             material=sapien.render.RenderMaterial(
                 base_color=np.array([0.3, 0.3, 0.3, 1])
             ),
         )
+        self.wall.initial_pose = sapien.Pose(p=[0, 1, 1], q=euler2quat(0, 0, np.pi / 2))
         self.wall.build_static(name="wall")
 
     def evaluate(self):
diff --git a/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py b/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
index 2560c027c..50b70b788 100644
--- a/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
+++ b/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
@@ -10,8 +10,9 @@
 from mani_skill.utils import sapien_utils
 from mani_skill.utils.building.ground import build_ground
 from mani_skill.utils.registration import register_env
+from mani_skill.utils.structs.pose import Pose
 from mani_skill.utils.structs.types import SceneConfig, SimConfig
-
+from typing import Optional, Union
 MJCF_FILE = f"{os.path.join(os.path.dirname(__file__), 'assets/cartpole.xml')}"
 
 
@@ -39,7 +40,7 @@ def _controller_configs(self):
             )
         )
 
-    def _load_articulation(self):
+    def _load_articulation(self, initial_pose: Optional[Union[sapien.Pose, Pose]] = None):
         """
         Load the robot articulation
         """
@@ -49,8 +50,9 @@ def _load_articulation(self):
         loader.name = self.uid
 
         # only need the robot
-        self.robot = loader.parse(asset_path)["articulation_builders"][0].build()
-
+        builder = loader.parse(asset_path)["articulation_builders"][0]
+        builder.initial_pose = initial_pose
+        self.robot = builder.build(name="cartpole")
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
 
         # Cache robot link ids
@@ -109,8 +111,8 @@ def _load_scene(self, options: dict):
         loader = self.scene.create_mjcf_loader()
         actor_builders = loader.parse(MJCF_FILE)["actor_builders"]
         for a in actor_builders:
+            a.initial_pose = sapien.Pose()
             a.build(a.name)
-
         # isaac uses a 0.5mx0.5m grid so we downscale the grid which is 4x4 squares by 2 by assumign the texture square length is 2
         self.ground = build_ground(
             self.scene,
diff --git a/mani_skill/utils/building/actor_builder.py b/mani_skill/utils/building/actor_builder.py
index c7e99cb64..e770c71ee 100644
--- a/mani_skill/utils/building/actor_builder.py
+++ b/mani_skill/utils/building/actor_builder.py
@@ -215,25 +215,10 @@ def build(self, name):
         num_actors = len(self.scene_idxs)
 
         if self.initial_pose is None:
-            if self.physx_body_type == "static":
-                logger.warn(
-                    f"initial pose not set for static object scenes-{self.scene_idxs.tolist()}_{self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]"
-                )
-                self.initial_pose = Pose.create(sapien.Pose())
-            else:
-                initial_ps = []
-                for scene_idx in self.scene_idxs:
-                    if self.scene.parallel_in_single_scene:
-                        sub_scene = self.scene.sub_scenes[0]
-                    else:
-                        sub_scene = self.scene.sub_scenes[scene_idx]
-                    entity_num = len(sub_scene.entities)
-                    # z_height starting at 100 (room for backgrounds, etc)
-                    z_height = SPAWN_SPACING * (entity_num // 2 + 1) + SPAWN_START_GAP
-                    # zdir, switch between -1 and 1 to flip above and below origin
-                    z_height *= 2 * (entity_num % 2) - 1
-                    initial_ps.append([0, 0, z_height])
-                self.initial_pose = Pose.create_from_pq(p=initial_ps)
+            logger.warn(
+                f"initial pose not set for actor {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]."
+            )
+            self.initial_pose = Pose.create(sapien.Pose())
         else:
             self.initial_pose = Pose.create(self.initial_pose)
 
diff --git a/mani_skill/utils/building/articulation_builder.py b/mani_skill/utils/building/articulation_builder.py
index e4d096c2a..13e21e123 100644
--- a/mani_skill/utils/building/articulation_builder.py
+++ b/mani_skill/utils/building/articulation_builder.py
@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import logging
 from typing import TYPE_CHECKING, List, Optional, Sequence, Union
 
 import numpy as np
@@ -132,21 +133,11 @@ def build(
         num_arts = len(self.scene_idxs)
 
         if self.initial_pose is None:
-            initial_ps = []
-            for scene_idx in self.scene_idxs:
-                if self.scene.parallel_in_single_scene:
-                    sub_scene = self.scene.sub_scenes[0]
-                else:
-                    sub_scene = self.scene.sub_scenes[scene_idx]
-                entity_num = len(sub_scene.entities)
-                # z_height starting at 100 (room for backgrounds, etc)
-                z_height = SPAWN_SPACING * (entity_num // 2 + 1) + SPAWN_START_GAP
-                # zdir, switch between -1 and 1 to flip above and below origin
-                z_height *= 2 * (entity_num % 2) - 1
-                initial_ps.append([0, 0, z_height])
-            self.initial_pose = Pose.create_from_pq(p=initial_ps)
-        else:
-            self.initial_pose = Pose.create(self.initial_pose)
+            logging.warning(
+                f"No initial pose set for articulation {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]. There may be simulation issues/bugs if this articulation at it's initial pose collides with other objects at their initial poses."
+            )
+            self.initial_pose = sapien.Pose()
+        self.initial_pose = Pose.create(self.initial_pose)
         initial_pose_b = self.initial_pose.raw_pose.shape[0]
         assert initial_pose_b == 1 or initial_pose_b == num_arts
         initial_pose_np = common.to_numpy(self.initial_pose.raw_pose)

From 130a421ce7c0f63790cc8be0d6e61b4e853c8ea1 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 16:44:48 -0700
Subject: [PATCH 2/9] Update _mjcf_loader.py

---
 mani_skill/utils/building/_mjcf_loader.py | 19 +++++++++++++------
 1 file changed, 13 insertions(+), 6 deletions(-)

diff --git a/mani_skill/utils/building/_mjcf_loader.py b/mani_skill/utils/building/_mjcf_loader.py
index a468d9628..f430721f8 100644
--- a/mani_skill/utils/building/_mjcf_loader.py
+++ b/mani_skill/utils/building/_mjcf_loader.py
@@ -73,6 +73,9 @@ class MJCFTexture:
 DEFAULT_MJCF_OPTIONS = dict(contact=True)
 
 
+WARNED_ONCE = defaultdict(lambda: False)
+
+
 def _parse_int(attrib, key, default):
     if key in attrib:
         return int(attrib[key])
@@ -390,13 +393,17 @@ def _build_geom(
                     )
 
         elif geom_type == "plane":
-            logging.warning(
-                "Currently ManiSkill does not support loading plane geometries from MJCFs"
-            )
+            if not WARNED_ONCE["plane"]:
+                logging.warning(
+                    "Currently ManiSkill does not support loading plane geometries from MJCFs"
+                )
+                WARNED_ONCE["plane"] = True
         elif geom_type == "ellipsoid":
-            logging.warning(
-                "Currently ManiSkill does not support loading ellipsoid geometries from MJCFs"
-            )
+            if not WARNED_ONCE["ellipsoid"]:
+                logging.warning(
+                    "Currently ManiSkill does not support loading ellipsoid geometries from MJCFs"
+                )
+                WARNED_ONCE["ellipsoid"] = True
         elif geom_type == "mesh":
             mesh_name = geom_attrib.get("mesh")
             mesh_attrib = self._meshes[mesh_name].attrib

From 181a097fe9497ae62e05faa6584613cf4d6841da Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 16:53:57 -0700
Subject: [PATCH 3/9] fix mjcf loader bug

---
 mani_skill/utils/building/_mjcf_loader.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/mani_skill/utils/building/_mjcf_loader.py b/mani_skill/utils/building/_mjcf_loader.py
index f430721f8..b89530c73 100644
--- a/mani_skill/utils/building/_mjcf_loader.py
+++ b/mani_skill/utils/building/_mjcf_loader.py
@@ -872,12 +872,13 @@ def has_joint(body):
         #         record = self._parse_constraint(constraint)
         #         builder.mimic_joint_records.append(record)
 
-        # if not self._mjcf_options["contact"]:
-        #     for actor in actor_builders:
-        #         actor.collision_groups[2] |= 1 << 1
-        #     for art in articulation_builders:
-        #         for link in art.link_builders:
-        #             link.collision_groups[2] |= 1 << 1
+        if not self._mjcf_options["contact"]:
+            # means to disable all contacts
+            for actor in actor_builders:
+                actor.collision_groups[2] |= 1 << 1
+            for art in articulation_builders:
+                for link in art.link_builders:
+                    link.collision_groups[2] |= 1 << 1
 
         return articulation_builders, actor_builders, []
 

From 724143fe6c43a4ae9e1a627f91024eec837417f2 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 17:05:30 -0700
Subject: [PATCH 4/9] warns

---
 mani_skill/envs/sapien_env.py                         | 3 +--
 mani_skill/envs/tasks/tabletop/pick_cube.py           | 8 +++++++-
 mani_skill/utils/building/_mjcf_loader.py             | 7 ++++---
 mani_skill/utils/building/actor_builder.py            | 2 +-
 mani_skill/utils/building/articulation_builder.py     | 5 ++---
 mani_skill/utils/scene_builder/table/scene_builder.py | 5 ++++-
 6 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/mani_skill/envs/sapien_env.py b/mani_skill/envs/sapien_env.py
index c601c70b3..f4412f540 100644
--- a/mani_skill/envs/sapien_env.py
+++ b/mani_skill/envs/sapien_env.py
@@ -1,6 +1,5 @@
 import copy
 import gc
-import logging
 import os
 from functools import cached_property
 from typing import Any, Dict, List, Optional, Sequence, Tuple, Union
@@ -198,7 +197,7 @@ def __init__(
         self.reconfiguration_freq = reconfiguration_freq if reconfiguration_freq is not None else 0
         self._reconfig_counter = 0
         if shader_dir is not None:
-            logging.warn("shader_dir argument will be deprecated after ManiSkill v3.0.0 official release. Please use sensor_configs/human_render_camera_configs to set shaders.")
+            logger.warn("shader_dir argument will be deprecated after ManiSkill v3.0.0 official release. Please use sensor_configs/human_render_camera_configs to set shaders.")
             sensor_configs |= dict(shader_pack=shader_dir)
             human_render_camera_configs |= dict(shader_pack=shader_dir)
             viewer_camera_configs |= dict(shader_pack=shader_dir)
diff --git a/mani_skill/envs/tasks/tabletop/pick_cube.py b/mani_skill/envs/tasks/tabletop/pick_cube.py
index 70a1f91b4..e5d477005 100644
--- a/mani_skill/envs/tasks/tabletop/pick_cube.py
+++ b/mani_skill/envs/tasks/tabletop/pick_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 
 import mani_skill.envs.utils.randomization as randomization
@@ -42,7 +43,11 @@ def _load_scene(self, options: dict):
         )
         self.table_scene.build()
         self.cube = actors.build_cube(
-            self.scene, half_size=self.cube_half_size, color=[1, 0, 0, 1], name="cube"
+            self.scene,
+            half_size=self.cube_half_size,
+            color=[1, 0, 0, 1],
+            name="cube",
+            initial_pose=sapien.Pose(p=[0, 0, self.cube_half_size]),
         )
         self.goal_site = actors.build_sphere(
             self.scene,
@@ -51,6 +56,7 @@ def _load_scene(self, options: dict):
             name="goal_site",
             body_type="kinematic",
             add_collision=False,
+            initial_pose=sapien.Pose(),
         )
         self._hidden_objects.append(self.goal_site)
 
diff --git a/mani_skill/utils/building/_mjcf_loader.py b/mani_skill/utils/building/_mjcf_loader.py
index b89530c73..d735899f7 100644
--- a/mani_skill/utils/building/_mjcf_loader.py
+++ b/mani_skill/utils/building/_mjcf_loader.py
@@ -36,7 +36,6 @@
 
 
 """
-import logging
 import math
 import os
 import re
@@ -60,6 +59,8 @@
 )
 from transforms3d import euler, quaternions
 
+from mani_skill import logger
+
 
 @dataclass
 class MJCFTexture:
@@ -394,13 +395,13 @@ def _build_geom(
 
         elif geom_type == "plane":
             if not WARNED_ONCE["plane"]:
-                logging.warning(
+                logger.warn(
                     "Currently ManiSkill does not support loading plane geometries from MJCFs"
                 )
                 WARNED_ONCE["plane"] = True
         elif geom_type == "ellipsoid":
             if not WARNED_ONCE["ellipsoid"]:
-                logging.warning(
+                logger.warn(
                     "Currently ManiSkill does not support loading ellipsoid geometries from MJCFs"
                 )
                 WARNED_ONCE["ellipsoid"] = True
diff --git a/mani_skill/utils/building/actor_builder.py b/mani_skill/utils/building/actor_builder.py
index e770c71ee..7e8c8e79b 100644
--- a/mani_skill/utils/building/actor_builder.py
+++ b/mani_skill/utils/building/actor_builder.py
@@ -216,7 +216,7 @@ def build(self, name):
 
         if self.initial_pose is None:
             logger.warn(
-                f"initial pose not set for actor {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]."
+                f"No initial pose set for actor builder of {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]."
             )
             self.initial_pose = Pose.create(sapien.Pose())
         else:
diff --git a/mani_skill/utils/building/articulation_builder.py b/mani_skill/utils/building/articulation_builder.py
index 13e21e123..e7dee0515 100644
--- a/mani_skill/utils/building/articulation_builder.py
+++ b/mani_skill/utils/building/articulation_builder.py
@@ -1,6 +1,5 @@
 from __future__ import annotations
 
-import logging
 from typing import TYPE_CHECKING, List, Optional, Sequence, Union
 
 import numpy as np
@@ -133,8 +132,8 @@ def build(
         num_arts = len(self.scene_idxs)
 
         if self.initial_pose is None:
-            logging.warning(
-                f"No initial pose set for articulation {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]. There may be simulation issues/bugs if this articulation at it's initial pose collides with other objects at their initial poses."
+            logger.warn(
+                f"No initial pose set for articulation builder of {self.name}, setting to default pose q=[1,0,0,0], p=[0,0,0]. There may be simulation issues/bugs if this articulation at it's initial pose collides with other objects at their initial poses."
             )
             self.initial_pose = sapien.Pose()
         self.initial_pose = Pose.create(self.initial_pose)
diff --git a/mani_skill/utils/scene_builder/table/scene_builder.py b/mani_skill/utils/scene_builder/table/scene_builder.py
index de509a193..e957d88f5 100644
--- a/mani_skill/utils/scene_builder/table/scene_builder.py
+++ b/mani_skill/utils/scene_builder/table/scene_builder.py
@@ -35,6 +35,9 @@ def build(self):
         builder.add_visual_from_file(
             filename=table_model_file, scale=[scale] * 3, pose=table_pose
         )
+        builder.initial_pose = sapien.Pose(
+            p=[-0.12, 0, -0.9196429], q=euler2quat(0, 0, np.pi / 2)
+        )
         table = builder.build_kinematic(name="table-workspace")
         aabb = (
             table._objs[0]
@@ -57,7 +60,7 @@ def initialize(self, env_idx: torch.Tensor):
         # table_height = 0.9196429
         b = len(env_idx)
         self.table.set_pose(
-            sapien.Pose(p=[-0.12, 0, -self.table_height], q=euler2quat(0, 0, np.pi / 2))
+            sapien.Pose(p=[-0.12, 0, -0.9196429], q=euler2quat(0, 0, np.pi / 2))
         )
         if self.env.robot_uids == "panda":
             qpos = np.array(

From 96203bd2f1c958050a146a9c5a41264729f4e4fa Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 17:43:26 -0700
Subject: [PATCH 5/9] provide initial poses for better sim and update docs

---
 .../user_guide/tutorials/custom_tasks/intro.md  | 17 +++++++++++------
 .../tutorials/custom_tasks/loading_objects.md   |  9 +++++++++
 mani_skill/agents/base_agent.py                 |  2 +-
 mani_skill/envs/tasks/control/ant.py            | 10 +++++++---
 mani_skill/envs/tasks/control/cartpole.py       | 10 +++++++---
 mani_skill/envs/tasks/control/hopper.py         | 10 +++++++---
 .../envs/tasks/tabletop/assembling_kits.py      |  4 ++++
 .../envs/tasks/tabletop/lift_peg_upright.py     |  5 +++++
 .../envs/tasks/tabletop/peg_insertion_side.py   |  5 +++++
 .../envs/tasks/tabletop/pick_clutter_ycb.py     |  4 ++++
 mani_skill/envs/tasks/tabletop/pick_cube.py     |  3 +++
 .../envs/tasks/tabletop/pick_single_ycb.py      |  5 +++++
 mani_skill/envs/tasks/tabletop/place_sphere.py  |  3 +++
 mani_skill/envs/tasks/tabletop/plug_charger.py  |  7 +++++--
 mani_skill/envs/tasks/tabletop/poke_cube.py     |  6 ++++++
 mani_skill/envs/tasks/tabletop/pull_cube.py     |  5 +++++
 mani_skill/envs/tasks/tabletop/push_cube.py     |  7 +++++++
 mani_skill/envs/tasks/tabletop/push_t.py        |  5 +++++
 mani_skill/envs/tasks/tabletop/roll_ball.py     |  6 ++++++
 mani_skill/envs/tasks/tabletop/stack_cube.py    | 16 ++++++++++++++--
 mani_skill/envs/tasks/tabletop/turn_faucet.py   |  5 +++++
 .../envs/tasks/tabletop/two_robot_pick_cube.py  |  8 ++++++++
 .../envs/tasks/tabletop/two_robot_stack_cube.py | 14 +++++++++++++-
 .../benchmarking/envs/maniskill/cartpole.py     |  3 +++
 24 files changed, 148 insertions(+), 21 deletions(-)

diff --git a/docs/source/user_guide/tutorials/custom_tasks/intro.md b/docs/source/user_guide/tutorials/custom_tasks/intro.md
index 9a89fb4de..d6070b531 100644
--- a/docs/source/user_guide/tutorials/custom_tasks/intro.md
+++ b/docs/source/user_guide/tutorials/custom_tasks/intro.md
@@ -49,11 +49,11 @@ class PushCubeEnv(BaseEnv):
 
 At the start of any task, you must load in all objects (robots, assets, articulations, lighting etc.) into each parallel environment, also known as a sub-scene. This is also known as **reconfiguration** and generally only ever occurs once. Loading these objects is done in the `_load_scene` function of your custom task class. The objective is to simply load objects in with initial poses that ensure they don't collide on the first step, and nothing else. For GPU simulation at this stage you cannot change any object states (like velocities, qpos), only initial poses can be modified. Changing/randomizing states is done in the section on [episode initialization / randomization](#episode-initialization--randomization).
 
-Building objects in ManiSkill is nearly the exact same as it is in SAPIEN. You create an `ActorBuilder` via `self.scene.create_actor_builder` and via the actor builder add visual and collision shapes. Visual shapes only affect visual rendering processes while collision shapes affect the physical simulation. ManiSkill further will create the actor for you in every sub-scene (unless you use [scene-masks/scene-idxs](./advanced.md#scene-masks), a more advanced feature).
+Building objects in ManiSkill is nearly the exact same as it is in SAPIEN. You create an `ActorBuilder` via `self.scene.create_actor_builder` and via the actor builder add visual and collision shapes. Visual shapes only affect visual rendering processes while collision shapes affect the physical simulation. ManiSkill further will create the actor for you in every sub-scene (unless you use [scene-masks/scene-idxs](./advanced.md#scene-masks), a more advanced feature for enabling heterogeneous simulation).
 
 ### Building Robots
 
-This is the simplest part and requires almost no additional work here. Robots are added in for you automatically and have their base initialized at 0. You can specify the default robot(s) added in via the init function. In PushCube this is done as so by adding `SUPPORTED_ROBOTS` to ensure users can only run your task with the selected robots. You can further add typing if you wish to the `agent` class attribute. 
+This is the simplest part and requires almost no additional work here. Robots are added in for you automatically and by default are initialized at the origin. You can specify the default robot(s) added in via the init function. In PushCube this is done as so by adding `SUPPORTED_ROBOTS` to ensure users can only run your task with the selected robots. You can further add typing if you wish to the `agent` class attribute. 
 
 ```python
 from mani_skill.agents.robots import Fetch, Panda
@@ -68,15 +68,20 @@ class PushCubeEnv(BaseEnv):
         # robot_uids="fetch" is possible, or even multi-robot 
         # setups via robot_uids=("fetch", "panda")
         super().__init__(*args, robot_uids=robot_uids, **kwargs)
+
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 1]))
 ```
+To define the initial pose of the robot you ovverride the `_load_agent` function. This is done in PushCube above. It is recommended to set initial poses for all objects such that if they were spawned there they don't intersect other objects. Here we spawn the robot 1 meter above the ground which won't clash with anything else.
+
 
-Initializing these robots occurs in the initialization / randomization section covered later. With this setup you can later access agent data via `self.agent` and the specific articulation data of the robot via `self.agent.robot`. For multi-robot setups you can access each agent via `self.agent.agents`.
+Initializing/randomizing these robots occurs in the initialization / randomization section covered later. With this setup you can later access agent data via `self.agent` and the specific articulation data of the robot via `self.agent.robot`. For multi-robot setups you can access each agent via `self.agent.agents`.
 
 To create your own custom robots/agents, see the [custom robots tutorial](../custom_robots.md).
 
 ### Building Actors
 
-the `_load_scene` function must be implemented to build objects besides agents. It is also given an `options` dictionary which is the same options dictionary passed to `env.reset` and defaults to an empty dictionary (which may be useful for controlling how to load a scene with just reset arguments).
+The `_load_scene` function must be implemented to build objects besides agents. It is also given an `options` dictionary which is the same options dictionary passed to `env.reset` and defaults to an empty dictionary (which may be useful for controlling how to load a scene with just reset arguments).
 
 Building a **dynamic** actor like a cube in PushCube is done as so
 ```python
@@ -109,7 +114,7 @@ You can build a **kinematic** actor with `builder.build_kinematic` and a **stati
 - Kinematic actors can have their pose changed at any time. Static actors must have an initial pose set before calling `build_static` via `builder.initial_pose = ...`
 - Use static instead of kinematic whenever possible as it saves a lot of GPU memory. Static objects must have an initial pose set before building via `builder.initial_pose = ...`.
 
-Note that by default, if an object does not have an initial pose set in its builder, ManiSkill will set it to a default pose of `q=[1,0,0,0], p=[0,0,0]` and a give a warning. For simple tasks this may not matter but when working with more complex objects and articulations, it is strongly recommended to set initial poses for all objects as GPU simulation might run into bugs/issues if the objects at their initial poses collide.
+Note that by default, if an object does not have an initial pose set in its builder, ManiSkill will set it to a default pose of `q=[1,0,0,0], p=[0,0,0]` and give a warning. For simple tasks this may not matter but when working with more complex objects and articulations, it is strongly recommended to set initial poses for all objects as GPU simulation might run into bugs/issues if the objects at their initial poses collide.
 
 We also provide some functions that build some more complex shapes that you can use by importing the following:
 ```python
@@ -143,7 +148,7 @@ We recommend you to first complete this tutorial before moving onto the next.
 
 ## Episode Initialization / Randomization
 
-Task initialization and randomization is handled in the `_initialize_episode` function and is called whenever `env.reset` is called. The objective here is to set the initial states of all non-static objects, including the robot. Note that objects that do not have initial poses set when building or set during episode initialization won't necessarily spawn at the origin.
+Task initialization and randomization is handled in the `_initialize_episode` function and is called whenever `env.reset` is called. The objective here is to set the initial states of all non-static objects, including the robot.
 
 As the task ideally should be simulatable on the GPU, batched code is unavoidable. Note that furthermore, by default everything in ManiSkill tries to stay batched, even if there is only one element. Finally, like `_load_scene` the options argument is also passed down here if needed.
 
diff --git a/docs/source/user_guide/tutorials/custom_tasks/loading_objects.md b/docs/source/user_guide/tutorials/custom_tasks/loading_objects.md
index ca26e1c43..0a2b91c5b 100644
--- a/docs/source/user_guide/tutorials/custom_tasks/loading_objects.md
+++ b/docs/source/user_guide/tutorials/custom_tasks/loading_objects.md
@@ -11,12 +11,15 @@ ManiSkill provides two ways to load actors, loading directly from existing simul
 ManiSkill supports easily loading assets from existing datasets such as the YCB dataset. In the beta release this is the only asset database available, more will be provided once we finish integrating a 3D asset database system.
 
 ```python
+import sapien
 from mani_skill.utils.building import actors
 def _load_scene(self, options):
     builder = actors.get_actor_builder(
         self.scene,
         id=f"ycb:{model_id}",
     )
+    # choose a reasonable initial pose that doesn't intersect other objects
+    builder.inital_pose = sapien.Pose(p=[0, 0, 0.5])
     builder.build(name="object")
 ```
 
@@ -45,6 +48,9 @@ def _load_scene(self, options):
     builder = articulations.get_articulation_builder(
         self.scene, f"partnet-mobility:{model_id}"
     )
+    # choose a reasonable initial pose that doesn't intersect other objects
+    # this matters a lot for articulations in GPU sim or else simulation bugs can occur
+    builder.inital_pose = sapien.Pose(p=[0, 0, 0.5])
     builder.build(name="object")
 ```
 
@@ -71,6 +77,9 @@ def _load_scene(self, options):
     # actors and cameras but we only use the articulations
     articulation_builders = loader.parse(str(urdf_path))["articulation_builders"]
     builder = articulation_builders[0]
+    # choose a reasonable initial pose that doesn't intersect other objects
+    # this matters a lot for articulations in GPU sim or else simulation bugs can occur
+    builder.initial_pose = sapien.Pose(p=[0, 0, 0.5])
     builder.build(name="my_articulation")
 ```
 
diff --git a/mani_skill/agents/base_agent.py b/mani_skill/agents/base_agent.py
index d40c901f1..afd4c2a46 100644
--- a/mani_skill/agents/base_agent.py
+++ b/mani_skill/agents/base_agent.py
@@ -193,7 +193,7 @@ def _load_articulation(
                 exit()
         builder = loader.parse(asset_path)["articulation_builders"][0]
         builder.initial_pose = initial_pose
-        self.robot = builder.build(self.uid)
+        self.robot = builder.build()
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
 
         # Cache robot link names
diff --git a/mani_skill/envs/tasks/control/ant.py b/mani_skill/envs/tasks/control/ant.py
index 40831f19e..9d5729614 100644
--- a/mani_skill/envs/tasks/control/ant.py
+++ b/mani_skill/envs/tasks/control/ant.py
@@ -1,5 +1,5 @@
 import os
-from typing import Any, Dict, Union
+from typing import Any, Dict, Optional, Union
 
 import numpy as np
 import sapien
@@ -59,7 +59,9 @@ def _controller_configs(self):
             )
         )
 
-    def _load_articulation(self):
+    def _load_articulation(
+        self, initial_pose: Optional[Union[sapien.Pose, Pose]] = None
+    ):
         """
         Load the robot articulation
         """
@@ -68,7 +70,9 @@ def _load_articulation(self):
 
         loader.name = self.uid
 
-        self.robot = loader.parse(asset_path)["articulation_builders"][0].build()
+        builder = loader.parse(asset_path)["articulation_builders"][0]
+        builder.initial_pose = initial_pose
+        self.robot = builder.build()
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
         self.robot_link_ids = [link.name for link in self.robot.get_links()]
 
diff --git a/mani_skill/envs/tasks/control/cartpole.py b/mani_skill/envs/tasks/control/cartpole.py
index b5cbd874e..533361593 100644
--- a/mani_skill/envs/tasks/control/cartpole.py
+++ b/mani_skill/envs/tasks/control/cartpole.py
@@ -1,7 +1,7 @@
 """Adapted from https://github.com/google-deepmind/dm_control/blob/main/dm_control/suite/cartpole.py"""
 
 import os
-from typing import Any, Dict, Union
+from typing import Any, Dict, Optional, Union
 
 import numpy as np
 import sapien
@@ -50,7 +50,9 @@ def _controller_configs(self):
             )
         )
 
-    def _load_articulation(self):
+    def _load_articulation(
+        self, initial_pose: Optional[Union[sapien.Pose, Pose]] = None
+    ):
         """
         Load the robot articulation
         """
@@ -60,7 +62,9 @@ def _load_articulation(self):
         loader.name = self.uid
 
         # only need the robot
-        self.robot = loader.parse(asset_path)["articulation_builders"][0].build()
+        builder = loader.parse(asset_path)["articulation_builders"][0]
+        builder.initial_pose = initial_pose
+        self.robot = builder.build()
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
 
         # Cache robot link ids
diff --git a/mani_skill/envs/tasks/control/hopper.py b/mani_skill/envs/tasks/control/hopper.py
index d3c2ecbff..a6ecc9072 100644
--- a/mani_skill/envs/tasks/control/hopper.py
+++ b/mani_skill/envs/tasks/control/hopper.py
@@ -1,7 +1,7 @@
 """Adapted from https://github.com/google-deepmind/dm_control/blob/main/dm_control/suite/hopper.py"""
 
 import os
-from typing import Any, Dict, Union
+from typing import Any, Dict, Optional, Union
 
 import numpy as np
 import sapien
@@ -75,7 +75,9 @@ def _controller_configs(self):
             )
         )
 
-    def _load_articulation(self):
+    def _load_articulation(
+        self, initial_pose: Optional[Union[sapien.Pose, Pose]] = None
+    ):
         """
         Load the robot articulation
         """
@@ -84,7 +86,9 @@ def _load_articulation(self):
 
         loader.name = self.uid
 
-        self.robot = loader.parse(asset_path)["articulation_builders"][0].build()
+        builder = loader.parse(asset_path)["articulation_builders"][0]
+        builder.initial_pose = initial_pose
+        self.robot = builder.build()
         assert self.robot is not None, f"Fail to load URDF/MJCF from {asset_path}"
         self.robot_link_ids = [link.name for link in self.robot.get_links()]
 
diff --git a/mani_skill/envs/tasks/tabletop/assembling_kits.py b/mani_skill/envs/tasks/tabletop/assembling_kits.py
index 3f818f3c7..64610424a 100644
--- a/mani_skill/envs/tasks/tabletop/assembling_kits.py
+++ b/mani_skill/envs/tasks/tabletop/assembling_kits.py
@@ -80,6 +80,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.3, 0.3, 0.8], [0.0, 0.0, 0.1])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         with torch.device(self.device):
             self.table_scene = TableSceneBuilder(self)
@@ -123,6 +126,7 @@ def _load_scene(self, options: dict):
                         episode["obj_to_place"], color_id=pick_color_ids[i]
                     )
                     .set_scene_idxs(scene_idxs)
+                    .set_initial_pose(sapien.Pose(p=[0, 0, 0.1]))
                     .build(f"obj_{i}")
                 )
                 self.object_ids.append(episode["obj_to_place"])
diff --git a/mani_skill/envs/tasks/tabletop/lift_peg_upright.py b/mani_skill/envs/tasks/tabletop/lift_peg_upright.py
index 15dee9a8c..d708fb82c 100644
--- a/mani_skill/envs/tasks/tabletop/lift_peg_upright.py
+++ b/mani_skill/envs/tasks/tabletop/lift_peg_upright.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 import torch.random
 from transforms3d.euler import euler2quat
@@ -39,6 +40,9 @@ def _default_human_render_camera_configs(self):
         pose = look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             env=self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -54,6 +58,7 @@ def _load_scene(self, options: dict):
             color_2=np.array([12, 42, 160, 255]) / 255,
             name="peg",
             body_type="dynamic",
+            initial_pose=sapien.Pose(p=[0, 0, 0.1]),
         )
 
     def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
diff --git a/mani_skill/envs/tasks/tabletop/peg_insertion_side.py b/mani_skill/envs/tasks/tabletop/peg_insertion_side.py
index 5b8e904e8..34c919bb9 100644
--- a/mani_skill/envs/tasks/tabletop/peg_insertion_side.py
+++ b/mani_skill/envs/tasks/tabletop/peg_insertion_side.py
@@ -89,6 +89,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.5, -0.5, 0.8], [0.05, -0.1, 0.4])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         with torch.device(self.device):
             self.table_scene = TableSceneBuilder(self)
@@ -145,6 +148,7 @@ def _load_scene(self, options: dict):
                     half_size=[length / 2, radius, radius],
                     material=mat,
                 )
+                builder.initial_pose = sapien.Pose(p=[0, 0, 0.1])
                 builder.set_scene_idxs(scene_idxs)
                 peg = builder.build(f"peg_{i}")
 
@@ -158,6 +162,7 @@ def _load_scene(self, options: dict):
                 builder = _build_box_with_hole(
                     self.scene, inner_radius, outer_radius, depth, center=centers[i]
                 )
+                builder.initial_pose = sapien.Pose(p=[0, 1, 0.1])
                 builder.set_scene_idxs(scene_idxs)
                 box = builder.build_kinematic(f"box_with_hole_{i}")
 
diff --git a/mani_skill/envs/tasks/tabletop/pick_clutter_ycb.py b/mani_skill/envs/tasks/tabletop/pick_clutter_ycb.py
index 566569c22..bbd456cdc 100644
--- a/mani_skill/envs/tasks/tabletop/pick_clutter_ycb.py
+++ b/mani_skill/envs/tasks/tabletop/pick_clutter_ycb.py
@@ -98,6 +98,9 @@ def _default_human_render_camera_configs(self):
     def _load_model(self, model_id: str) -> ActorBuilder:
         raise NotImplementedError()
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.scene_builder = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -135,6 +138,7 @@ def _load_scene(self, options: dict):
             name="goal_site",
             body_type="kinematic",
             add_collision=False,
+            initial_pose=sapien.Pose(),
         )
         self._hidden_objects.append(self.goal_site)
 
diff --git a/mani_skill/envs/tasks/tabletop/pick_cube.py b/mani_skill/envs/tasks/tabletop/pick_cube.py
index e5d477005..97e45805c 100644
--- a/mani_skill/envs/tasks/tabletop/pick_cube.py
+++ b/mani_skill/envs/tasks/tabletop/pick_cube.py
@@ -37,6 +37,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
diff --git a/mani_skill/envs/tasks/tabletop/pick_single_ycb.py b/mani_skill/envs/tasks/tabletop/pick_single_ycb.py
index 1cda8ebd0..542d067c4 100644
--- a/mani_skill/envs/tasks/tabletop/pick_single_ycb.py
+++ b/mani_skill/envs/tasks/tabletop/pick_single_ycb.py
@@ -70,6 +70,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         global WARNED_ONCE
         self.table_scene = TableSceneBuilder(
@@ -101,6 +104,7 @@ def _load_scene(self, options: dict):
                 self.scene,
                 id=f"ycb:{model_id}",
             )
+            builder.initial_pose = sapien.Pose(p=[0, 0, 0.1])
             builder.set_scene_idxs([i])
             self._objs.append(builder.build(name=f"{model_id}-{i}"))
         self.obj = Actor.merge(self._objs, name="ycb_object")
@@ -112,6 +116,7 @@ def _load_scene(self, options: dict):
             name="goal_site",
             body_type="kinematic",
             add_collision=False,
+            initial_pose=sapien.Pose(),
         )
         self._hidden_objects.append(self.goal_site)
 
diff --git a/mani_skill/envs/tasks/tabletop/place_sphere.py b/mani_skill/envs/tasks/tabletop/place_sphere.py
index 1f18461ef..9a3095804 100644
--- a/mani_skill/envs/tasks/tabletop/place_sphere.py
+++ b/mani_skill/envs/tasks/tabletop/place_sphere.py
@@ -128,6 +128,9 @@ def _build_bin(self, radius):
         # build the kinematic bin
         return builder.build_kinematic(name="bin")
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         # load the table
         self.table_scene = TableSceneBuilder(
diff --git a/mani_skill/envs/tasks/tabletop/plug_charger.py b/mani_skill/envs/tasks/tabletop/plug_charger.py
index a82bc8326..1e1206f8d 100644
--- a/mani_skill/envs/tasks/tabletop/plug_charger.py
+++ b/mani_skill/envs/tasks/tabletop/plug_charger.py
@@ -86,7 +86,7 @@ def _build_charger(self, peg_size, base_size, gap):
         builder.add_box_visual(
             sapien.Pose([-base_size[0], 0, 0]), base_size, material=mat
         )
-
+        builder.initial_pose = sapien.Pose(p=[0, 0, self._base_size[2]])
         return builder.build(name="charger")
 
     def _build_receptacle(self, peg_size, receptacle_size, gap):
@@ -136,9 +136,12 @@ def _build_receptacle(self, peg_size, receptacle_size, gap):
         builder.add_box_visual(pose, half_size, material=mat)
         pose = sapien.Pose([-receptacle_size[0], gap * 0.5 + peg_size[1], 0])
         builder.add_box_visual(pose, half_size, material=mat)
-
+        builder.initial_pose = sapien.Pose(p=[0, 0, 0.1])
         return builder.build_kinematic(name="receptacle")
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.scene_builder = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
diff --git a/mani_skill/envs/tasks/tabletop/poke_cube.py b/mani_skill/envs/tasks/tabletop/poke_cube.py
index 32e3796bb..df5eba018 100644
--- a/mani_skill/envs/tasks/tabletop/poke_cube.py
+++ b/mani_skill/envs/tasks/tabletop/poke_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 from transforms3d.euler import euler2quat
 
@@ -40,6 +41,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.2, 0.2, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -52,6 +56,7 @@ def _load_scene(self, options: dict):
             color=[1, 0, 0, 1],
             name="cube",
             body_type="dynamic",
+            initial_pose=sapien.Pose(p=[1, 0, self.cube_half_size]),
         )
 
         self.peg = actors.build_twocolor_peg(
@@ -62,6 +67,7 @@ def _load_scene(self, options: dict):
             color_2=np.array([12, 42, 160, 255]) / 255,
             name="peg",
             body_type="dynamic",
+            initial_pose=sapien.Pose(p=[0, 0, self.peg_half_width]),
         )
 
         self.goal_region = actors.build_red_white_target(
diff --git a/mani_skill/envs/tasks/tabletop/pull_cube.py b/mani_skill/envs/tasks/tabletop/pull_cube.py
index f41976bd2..7286049f8 100644
--- a/mani_skill/envs/tasks/tabletop/pull_cube.py
+++ b/mani_skill/envs/tasks/tabletop/pull_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 import torch.random
 from transforms3d.euler import euler2quat
@@ -37,6 +38,9 @@ def _default_human_render_camera_configs(self):
         pose = look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             env=self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -50,6 +54,7 @@ def _load_scene(self, options: dict):
             color=np.array([12, 42, 160, 255]) / 255,
             name="cube",
             body_type="dynamic",
+            initial_pose=sapien.Pose(p=[0, 0, self.cube_half_size]),
         )
 
         # create target
diff --git a/mani_skill/envs/tasks/tabletop/push_cube.py b/mani_skill/envs/tasks/tabletop/push_cube.py
index 1942653d3..991c5f01e 100644
--- a/mani_skill/envs/tasks/tabletop/push_cube.py
+++ b/mani_skill/envs/tasks/tabletop/push_cube.py
@@ -18,6 +18,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 import torch.random
 from transforms3d.euler import euler2quat
@@ -100,6 +101,10 @@ def _default_human_render_camera_configs(self):
             "render_camera", pose=pose, width=512, height=512, fov=1, near=0.01, far=100
         )
 
+    def _load_agent(self, options: dict):
+        # set a reasonable initial pose for the agent that doesn't intersect other objects
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         # we use a prebuilt scene builder class that automatically loads in a floor and table.
         self.table_scene = TableSceneBuilder(
@@ -109,12 +114,14 @@ def _load_scene(self, options: dict):
 
         # we then add the cube that we want to push and give it a color and size using a convenience build_cube function
         # we specify the body_type to be "dynamic" as it should be able to move when touched by other objects / the robot
+        # finally we specify an initial pose for the cube so that it doesn't collide with other objects initially
         self.obj = actors.build_cube(
             self.scene,
             half_size=self.cube_half_size,
             color=np.array([12, 42, 160, 255]) / 255,
             name="cube",
             body_type="dynamic",
+            initial_pose=sapien.Pose(p=[0, 0, self.cube_half_size]),
         )
 
         # we also add in red/white target to visualize where we want the cube to be pushed to
diff --git a/mani_skill/envs/tasks/tabletop/push_t.py b/mani_skill/envs/tasks/tabletop/push_t.py
index ac303ae34..a46d20b55 100644
--- a/mani_skill/envs/tasks/tabletop/push_t.py
+++ b/mani_skill/envs/tasks/tabletop/push_t.py
@@ -173,6 +173,9 @@ def _default_human_render_camera_configs(self):
             "render_camera", pose=pose, width=512, height=512, fov=1, near=0.01, far=100
         )
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         # have to put these parmaeters to device - defined before we had access to device
         # load scene is a convienent place for this one time operation
@@ -247,6 +250,7 @@ def create_tee(name="tee", target=False, base_color=TARGET_RED):
                     base_color=base_color,
                 ),
             )
+            builder.initial_pose = sapien.Pose(p=[0, 0, 0.1])
             if not target:
                 return builder.build(name=name)
             else:
@@ -268,6 +272,7 @@ def create_tee(name="tee", target=False, base_color=TARGET_RED):
                 base_color=np.array([128, 128, 128, 255]) / 255
             ),
         )
+        builder.initial_pose = sapien.Pose(p=[0, 0, 0.1])
         self.ee_goal_pos = builder.build_kinematic(name="goal_ee")
 
         # Rest of function is setting up for Custom 2D "Pseudo-Rendering" function below
diff --git a/mani_skill/envs/tasks/tabletop/roll_ball.py b/mani_skill/envs/tasks/tabletop/roll_ball.py
index 7e65a4620..4f45077ac 100644
--- a/mani_skill/envs/tasks/tabletop/roll_ball.py
+++ b/mani_skill/envs/tasks/tabletop/roll_ball.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict
 
 import numpy as np
+import sapien
 import torch
 from transforms3d.euler import euler2quat
 
@@ -63,6 +64,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([-0.6, 1.3, 0.8], [0.0, 0.13, 0.0])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -74,6 +78,7 @@ def _load_scene(self, options: dict):
             radius=self.ball_radius,
             color=[0, 0.2, 0.8, 1],
             name="ball",
+            initial_pose=sapien.Pose(p=[0, 0, 0.1]),
         )
 
         self.goal_region = actors.build_red_white_target(
@@ -83,6 +88,7 @@ def _load_scene(self, options: dict):
             name="goal_region",
             add_collision=False,
             body_type="kinematic",
+            initial_pose=sapien.Pose(p=[0, 0, 0.1]),
         )
         self.reached_status = torch.zeros(self.num_envs, dtype=torch.float32)
 
diff --git a/mani_skill/envs/tasks/tabletop/stack_cube.py b/mani_skill/envs/tasks/tabletop/stack_cube.py
index f76fb284b..1f616da30 100644
--- a/mani_skill/envs/tasks/tabletop/stack_cube.py
+++ b/mani_skill/envs/tasks/tabletop/stack_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.robots import Fetch, Panda
@@ -36,6 +37,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.cube_half_size = common.to_tensor([0.02] * 3)
         self.table_scene = TableSceneBuilder(
@@ -43,10 +47,18 @@ def _load_scene(self, options: dict):
         )
         self.table_scene.build()
         self.cubeA = actors.build_cube(
-            self.scene, half_size=0.02, color=[1, 0, 0, 1], name="cubeA"
+            self.scene,
+            half_size=0.02,
+            color=[1, 0, 0, 1],
+            name="cubeA",
+            initial_pose=sapien.Pose(p=[0, 0, 0.1]),
         )
         self.cubeB = actors.build_cube(
-            self.scene, half_size=0.02, color=[0, 1, 0, 1], name="cubeB"
+            self.scene,
+            half_size=0.02,
+            color=[0, 1, 0, 1],
+            name="cubeB",
+            initial_pose=sapien.Pose(p=[1, 0, 0.1]),
         )
 
     def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
diff --git a/mani_skill/envs/tasks/tabletop/turn_faucet.py b/mani_skill/envs/tasks/tabletop/turn_faucet.py
index e87853d0e..69955115a 100644
--- a/mani_skill/envs/tasks/tabletop/turn_faucet.py
+++ b/mani_skill/envs/tasks/tabletop/turn_faucet.py
@@ -1,6 +1,7 @@
 from typing import Dict, List, Union
 
 import numpy as np
+import sapien
 import sapien.physx as physx
 import torch
 
@@ -68,6 +69,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.5, 0.5, 1.0], [0.0, 0.0, 0.5])
         return CameraConfig("render_camera", pose=pose, width=512, height=512, fov=1)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.scene_builder = TableSceneBuilder(
             self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -90,6 +94,7 @@ def _load_scene(self, options: dict):
                 urdf_config=dict(density=model_info.get("density", 8e3)),
             )
             builder.set_scene_idxs(scene_idxs=[i])
+            builder.initial_pose = sapien.Pose(p=[0, 0, model_info["offset"][2]])
             faucet = builder.build(name=f"{model_id}-{i}")
             for joint in faucet.active_joints:
                 joint.set_friction(1.0)
diff --git a/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py b/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
index 2411d95ae..17a2bb94b 100644
--- a/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
+++ b/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Tuple
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.multi_agent import MultiAgent
@@ -73,6 +74,11 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([1.4, 0.8, 0.75], [0.0, 0.1, 0.1])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(
+            options, [sapien.Pose(p=[0, -1, 0]), sapien.Pose(p=[0, 1, 0])]
+        )
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(
             env=self, robot_init_qpos_noise=self.robot_init_qpos_noise
@@ -83,6 +89,7 @@ def _load_scene(self, options: dict):
             half_size=self.cube_half_size,
             color=[1, 0, 0, 1],
             name="cube",
+            initial_pose=sapien.Pose(p=[0, 0, self.cube_half_size]),
         )
         self.goal_site = actors.build_sphere(
             self.scene,
@@ -91,6 +98,7 @@ def _load_scene(self, options: dict):
             name="goal_site",
             body_type="kinematic",
             add_collision=False,
+            initial_pose=sapien.Pose(),
         )
         self._hidden_objects.append(self.goal_site)
 
diff --git a/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py b/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
index 62637e94f..e981d0c8c 100644
--- a/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
+++ b/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Tuple
 
 import numpy as np
+import sapien
 import torch
 from transforms3d.euler import euler2quat
 
@@ -78,6 +79,11 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at(eye=[0.6, 0.2, 0.4], target=[-0.1, 0, 0.1])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(
+            options, [sapien.Pose(p=[0, -1, 0]), sapien.Pose(p=[0, 1, 0])]
+        )
+
     def _load_scene(self, options: dict):
         self.cube_half_size = common.to_tensor([0.02] * 3)
         self.table_scene = TableSceneBuilder(
@@ -89,9 +95,14 @@ def _load_scene(self, options: dict):
             half_size=0.02,
             color=np.array([12, 42, 160, 255]) / 255,
             name="cubeA",
+            initial_pose=sapien.Pose(p=[1, 0, self.cube_half_size]),
         )
         self.cubeB = actors.build_cube(
-            self.scene, half_size=0.02, color=[0, 1, 0, 1], name="cubeB"
+            self.scene,
+            half_size=0.02,
+            color=[0, 1, 0, 1],
+            name="cubeB",
+            initial_pose=sapien.Pose(p=[-1, 0, self.cube_half_size]),
         )
         self.goal_region = actors.build_red_white_target(
             self.scene,
@@ -100,6 +111,7 @@ def _load_scene(self, options: dict):
             name="goal_region",
             add_collision=False,
             body_type="kinematic",
+            initial_pose=sapien.Pose(),
         )
 
     def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
diff --git a/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py b/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
index 50b70b788..0cc530152 100644
--- a/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
+++ b/mani_skill/examples/benchmarking/envs/maniskill/cartpole.py
@@ -107,6 +107,9 @@ def _default_sensor_configs(self):
     def _default_human_render_camera_configs(self):
         return dict()
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose())
+
     def _load_scene(self, options: dict):
         loader = self.scene.create_mjcf_loader()
         actor_builders = loader.parse(MJCF_FILE)["actor_builders"]

From 9f83b606198f066f43c432f4c8199e3c0a9c7e39 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 17:53:14 -0700
Subject: [PATCH 6/9] fixes

---
 .../envs/tasks/mobile_manipulation/open_cabinet_drawer.py   | 3 +++
 .../envs/tasks/mobile_manipulation/robocasa/kitchen.py      | 6 ++++++
 mani_skill/envs/tasks/quadruped/quadruped_reach.py          | 4 ++++
 mani_skill/envs/tasks/quadruped/quadruped_spin.py           | 4 ++++
 mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py       | 2 +-
 mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py      | 6 +++---
 6 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py b/mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py
index 3ae4e016c..12740a95b 100644
--- a/mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py
+++ b/mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py
@@ -88,6 +88,9 @@ def _default_human_render_camera_configs(self):
             "render_camera", pose=pose, width=512, height=512, fov=1, near=0.01, far=100
         )
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[1, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.ground = build_ground(self.scene)
         # temporarily turn off the logging as there will be big red warnings
diff --git a/mani_skill/envs/tasks/mobile_manipulation/robocasa/kitchen.py b/mani_skill/envs/tasks/mobile_manipulation/robocasa/kitchen.py
index 6b87e2895..0983d3ffd 100644
--- a/mani_skill/envs/tasks/mobile_manipulation/robocasa/kitchen.py
+++ b/mani_skill/envs/tasks/mobile_manipulation/robocasa/kitchen.py
@@ -18,6 +18,7 @@
 from mani_skill.utils.scene_builder.robocasa.utils.placement_samplers import (
     RandomizationError,
 )
+from mani_skill.utils.structs.pose import Pose
 from mani_skill.utils.structs.types import GPUMemoryConfig, SimConfig
 
 
@@ -273,6 +274,11 @@ def _default_viewer_camera_config(self):
             fov=60 * np.pi / 180,
         )
 
+    def _load_agent(self, options: dict):
+        ps = torch.zeros((self.num_envs, 3), device=self.device)
+        ps[:, 2] = 5
+        super()._load_agent(options, Pose.create_from_pq(p=ps))
+
     def _load_scene(self, options: dict):
         self.scene_builder = RoboCasaSceneBuilder(self)
         self.scene_builder.build()
diff --git a/mani_skill/envs/tasks/quadruped/quadruped_reach.py b/mani_skill/envs/tasks/quadruped/quadruped_reach.py
index ce4d27ed1..2a3ce946b 100644
--- a/mani_skill/envs/tasks/quadruped/quadruped_reach.py
+++ b/mani_skill/envs/tasks/quadruped/quadruped_reach.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, List
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.robots.anymal.anymal_c import ANYmalC
@@ -66,6 +67,9 @@ def _default_human_render_camera_configs(self):
             )
         ]
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 1]))
+
     def _load_scene(self, options: dict):
         self.ground = build_ground(self.scene, floor_width=400)
         self.goal = actors.build_sphere(
diff --git a/mani_skill/envs/tasks/quadruped/quadruped_spin.py b/mani_skill/envs/tasks/quadruped/quadruped_spin.py
index c64387762..c8dfa5bb8 100644
--- a/mani_skill/envs/tasks/quadruped/quadruped_spin.py
+++ b/mani_skill/envs/tasks/quadruped/quadruped_spin.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, List
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.robots.anymal.anymal_c import ANYmalC
@@ -65,6 +66,9 @@ def _default_human_render_camera_configs(self):
             )
         ]
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 1]))
+
     def _load_scene(self, options: dict):
         self.ground = build_ground(self.scene, floor_width=400)
 
diff --git a/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py b/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
index 17a2bb94b..b6ef7ea96 100644
--- a/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
+++ b/mani_skill/envs/tasks/tabletop/two_robot_pick_cube.py
@@ -89,7 +89,7 @@ def _load_scene(self, options: dict):
             half_size=self.cube_half_size,
             color=[1, 0, 0, 1],
             name="cube",
-            initial_pose=sapien.Pose(p=[0, 0, self.cube_half_size]),
+            initial_pose=sapien.Pose(p=[0, 0, 0.02]),
         )
         self.goal_site = actors.build_sphere(
             self.scene,
diff --git a/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py b/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
index e981d0c8c..37bd20a72 100644
--- a/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
+++ b/mani_skill/envs/tasks/tabletop/two_robot_stack_cube.py
@@ -43,7 +43,7 @@ class TwoRobotStackCube(BaseEnv):
     Visualization: TODO
     """
 
-    SUPPORTED_ROBOTS = [("panda", "panda")]
+    SUPPORTED_ROBOTS = [("panda_wristcam", "panda_wristcam")]
     agent: MultiAgent[Tuple[Panda, Panda]]
 
     goal_radius = 0.06
@@ -95,14 +95,14 @@ def _load_scene(self, options: dict):
             half_size=0.02,
             color=np.array([12, 42, 160, 255]) / 255,
             name="cubeA",
-            initial_pose=sapien.Pose(p=[1, 0, self.cube_half_size]),
+            initial_pose=sapien.Pose(p=[1, 0, 0.02]),
         )
         self.cubeB = actors.build_cube(
             self.scene,
             half_size=0.02,
             color=[0, 1, 0, 1],
             name="cubeB",
-            initial_pose=sapien.Pose(p=[-1, 0, self.cube_half_size]),
+            initial_pose=sapien.Pose(p=[-1, 0, 0.02]),
         )
         self.goal_region = actors.build_red_white_target(
             self.scene,

From 285586dcf15d92fe71a30dcca09c89d9bad2ce81 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 17:56:26 -0700
Subject: [PATCH 7/9] fixes

---
 .../envs/tasks/digital_twins/bridge_dataset_eval/base_env.py | 5 +++++
 mani_skill/envs/tasks/drawing/draw.py                        | 5 +++++
 mani_skill/envs/tasks/empty_env.py                           | 3 +++
 mani_skill/envs/tasks/humanoid/humanoid_pick_place.py        | 3 +++
 4 files changed, 16 insertions(+)

diff --git a/mani_skill/envs/tasks/digital_twins/bridge_dataset_eval/base_env.py b/mani_skill/envs/tasks/digital_twins/bridge_dataset_eval/base_env.py
index 43922c1af..0683aaec1 100644
--- a/mani_skill/envs/tasks/digital_twins/bridge_dataset_eval/base_env.py
+++ b/mani_skill/envs/tasks/digital_twins/bridge_dataset_eval/base_env.py
@@ -271,6 +271,11 @@ def _load_lighting(self, options: dict):
         self.scene.add_directional_light([-1, -0.5, -1], [0.7, 0.7, 0.7])
         self.scene.add_directional_light([1, 1, -1], [0.7, 0.7, 0.7])
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(
+            options, sapien.Pose(p=[0.127, 0.060, 0.85], q=[0, 0, 0, 1])
+        )
+
     def _load_scene(self, options: dict):
         # original SIMPLER envs always do this? except for open drawer task
         for i in range(self.num_envs):
diff --git a/mani_skill/envs/tasks/drawing/draw.py b/mani_skill/envs/tasks/drawing/draw.py
index f4214bc17..2451e7d62 100644
--- a/mani_skill/envs/tasks/drawing/draw.py
+++ b/mani_skill/envs/tasks/drawing/draw.py
@@ -87,6 +87,9 @@ def _default_human_render_camera_configs(self):
             far=100,
         )
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
+
     def _load_scene(self, options: dict):
         self.table_scene = TableSceneBuilder(self, robot_init_qpos_noise=0)
         self.table_scene.build()
@@ -118,6 +121,7 @@ def _load_scene(self, options: dict):
                         ),
                     )
                     builder.set_scene_idxs([env_idx])
+                    builder.initial_pose = sapien.Pose(p=[0, 0, 0])
                     actor = builder.build_kinematic(name=f"dot_{i}_{env_idx}")
                     actors.append(actor)
                 self.dots.append(Actor.merge(actors))
@@ -130,6 +134,7 @@ def _load_scene(self, options: dict):
                         base_color=self.BRUSH_COLORS[0]
                     ),
                 )
+                builder.initial_pose = sapien.Pose(p=[0, 0, 0])
                 actor = builder.build_kinematic(name=f"dot_{i}")
                 self.dots.append(actor)
 
diff --git a/mani_skill/envs/tasks/empty_env.py b/mani_skill/envs/tasks/empty_env.py
index 847ec9dd4..35df9d047 100644
--- a/mani_skill/envs/tasks/empty_env.py
+++ b/mani_skill/envs/tasks/empty_env.py
@@ -34,6 +34,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([1.25, -1.25, 1.5], [0.0, 0.0, 0.2])
         return CameraConfig("render_camera", pose, 2048, 2048, 1, 0.01, 100)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose())
+
     def _load_scene(self, options: dict):
         self.ground = build_ground(self.scene)
         self.ground.set_collision_group_bit(group=2, bit_idx=30, bit=1)
diff --git a/mani_skill/envs/tasks/humanoid/humanoid_pick_place.py b/mani_skill/envs/tasks/humanoid/humanoid_pick_place.py
index efb3a2ca1..ad4483ff5 100644
--- a/mani_skill/envs/tasks/humanoid/humanoid_pick_place.py
+++ b/mani_skill/envs/tasks/humanoid/humanoid_pick_place.py
@@ -43,6 +43,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose=pose, width=512, height=512, fov=1)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 1]))
+
     def _load_scene(self, options: dict):
         self.scene_builder = KitchenCounterSceneBuilder(self)
         self.kitchen_scene = self.scene_builder.build(scale=self.kitchen_scene_scale)

From 3a5c2a56970b57b8b8d3d47c41bc3c2db40a2880 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 17:59:58 -0700
Subject: [PATCH 8/9] Update utils.py

---
 tests/utils.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/tests/utils.py b/tests/utils.py
index 1a99681bd..c969880c4 100644
--- a/tests/utils.py
+++ b/tests/utils.py
@@ -7,6 +7,7 @@
 from mani_skill.utils.registration import REGISTERED_ENVS
 
 ENV_IDS = list(REGISTERED_ENVS.keys())
+ENV_IDS.remove("RoboCasaKitchen-v1")  # remove slow envs
 MULTI_AGENT_ENV_IDS = ["TwoRobotStackCube-v1", "TwoRobotPickCube-v1"]
 
 STATIONARY_ENV_IDS = [

From ec1841c5105a274763753489800db0bb67239540 Mon Sep 17 00:00:00 2001
From: Stone Tao <stonezt2019@gmail.com>
Date: Fri, 25 Oct 2024 18:04:50 -0700
Subject: [PATCH 9/9] docs

---
 mani_skill/envs/minimal_template.py | 4 ++++
 mani_skill/envs/template.py         | 7 ++++---
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/mani_skill/envs/minimal_template.py b/mani_skill/envs/minimal_template.py
index 64c3b2107..fc52d4d2b 100644
--- a/mani_skill/envs/minimal_template.py
+++ b/mani_skill/envs/minimal_template.py
@@ -1,6 +1,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.robots import Fetch, Panda
@@ -37,6 +38,9 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
         return CameraConfig("render_camera", pose=pose, width=512, height=512, fov=1)
 
+    def _load_agent(self, options: dict):
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 0]))
+
     def _load_scene(self, options: dict):
         pass
 
diff --git a/mani_skill/envs/template.py b/mani_skill/envs/template.py
index 962790b4b..9430efcfd 100644
--- a/mani_skill/envs/template.py
+++ b/mani_skill/envs/template.py
@@ -24,6 +24,7 @@
 from typing import Any, Dict, Union
 
 import numpy as np
+import sapien
 import torch
 
 from mani_skill.agents.multi_agent import MultiAgent
@@ -96,9 +97,9 @@ def _default_sim_config(self):
     """
 
     def _load_agent(self, options: dict):
-        # this code loads the agent into the current scene. You can usually ignore this function by deleting it or calling the inherited
-        # BaseEnv._load_agent function
-        super()._load_agent()
+        # this code loads the agent into the current scene. You should use it to specify the initial pose(s) of the agent(s)
+        # such that they don't collide with other objects initially
+        super()._load_agent(options, sapien.Pose(p=[0, 0, 0]))
 
     def _load_scene(self, options: dict):
         # here you add various objects like actors and articulations. If your task was to push a ball, you may add a dynamic sphere object on the ground