[Question] Objects not spawning in the correct terrain environment

[YannickP99]

Question

I wanted to change from a plane terrain to a TerrainImporter terrain for better sim2real capabilities. In each of the environments I am spawning a robot and one RigidObject. Before everything worked fine but now I get the problem that the robots spawn correctly in each environment but the rigid objects are spawning in the global coordinate system (see screenshot).

I am using a DirectRLEnv, havent updated yet (isaacsim==4.2.0.2) because of some other bugs in the new version.
Code:

@configclass
class Go2EnvCfg(DirectRLEnvCfg):
    events: EventCfg = EventCfg()
    #* Simulation
    sim: SimulationCfg = SimulationCfg(
        dt=1 / 200,
        render_interval=decimation,
        disable_contact_processing=True,
        physics_material=sim_utils.RigidBodyMaterialCfg(
            friction_combine_mode="multiply",
            restitution_combine_mode="multiply",
            static_friction=1.0,
            dynamic_friction=1.0,
            restitution=0.0,
        ),
    )

    #* Terrain
    terrain_gen = TerrainGeneratorCfg(
                    size=(8.0, 8.0),
                    num_cols=16,
                    num_rows=1,
                    sub_terrains={
                        "random": HfRandomUniformTerrainCfg(
                        proportion=0.2, noise_range=(0.02, 0.05), noise_step=0.01
                        ),
    },
        
    )
    terrain = TerrainImporterCfg(
        prim_path="/World/ground",
        terrain_type="generator",
        terrain_generator=terrain_gen,
        max_init_terrain_level=0,
        collision_group=-1,
        physics_material=sim_utils.RigidBodyMaterialCfg(
            friction_combine_mode="multiply",
            restitution_combine_mode="multiply",
            static_friction=1.0,
            dynamic_friction=1.0,
        ),
        debug_vis=False,
    )

    #* Scene
    scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=16, env_spacing=5.0, replicate_physics=False)

    #* Robot UNITREE_GO2_CFG
    # robot: ArticulationCfg = UNITREE_GO2_CFG.replace(prim_path="/World/envs/env_.*/Robot")     # type: ignore
    robot = ArticulationCfg(
        spawn=sim_utils.UrdfFileCfg(
            asset_path=f"source/thesis/resources/go2_manip_description/urdf/go2_manip_comb.urdf",    # go2_manip_reduced
            usd_dir=f"source/thesis/resources/go2_manip_description/usd",
            usd_file_name="go2_manip_exported",
            merge_fixed_joints=False,
            fix_base=False,
            force_usd_conversion=True,
            activate_contact_sensors=True,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False,
                retain_accelerations=False,
                linear_damping=0.0,
                angular_damping=0.0,
                max_linear_velocity=1000.0,
                max_angular_velocity=1000.0,
                max_depenetration_velocity=1.0,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(0.0, 0.0, 0.4),
            joint_pos={
                ".*L_hip_joint": 0.1,
                ".*R_hip_joint": -0.1,
                "F[L,R]_thigh_joint": 0.8,
                "R[L,R]_thigh_joint": 1.0,
                ".*_calf_joint": -1.5,
                "joint_connector_gripper": 0.0,
            },
            joint_vel={".*": 0.0},
        ),
        soft_joint_pos_limit_factor=0.9,
        actuators={
            "base_legs": DCMotorCfg(
                joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"],
                effort_limit=23.5,
                saturation_effort=23.5,
                velocity_limit=30.0,
                stiffness=25.0,
                damping=0.5,
                friction=0.0,
            ),
            "base_arm": DCMotorCfg(
                joint_names_expr=["joint_connector_gripper"],
                effort_limit=5.0,
                saturation_effort=5.0,
                velocity_limit=10.0,
                stiffness=3.0,
                damping=0.5,
                friction=0.0,
            ),
        },
    ).replace(prim_path="/World/envs/env_.*/Robot")     # type: ignore
    
    # * Bin
    radius = bin_radius_min
    height = bin_height_min
    print(f"> Bin type 0: height={height}, radius={radius}")
    bin_0 = RigidObjectCfg(
        prim_path="/World/envs/env_.*/bin_0",
        spawn=sim_utils.CylinderCfg(
            radius=radius,
            height=height,
            axis='Z',
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                                                max_depenetration_velocity=1.0, 
                                                disable_gravity=False,
                                                kinematic_enabled=True,    # fix bin position
            ),
            collision_props=sim_utils.schemas.CollisionPropertiesCfg(collision_enabled=True),
            mass_props=sim_utils.MassPropertiesCfg(mass=0.1),
            physics_material=sim_utils.RigidBodyMaterialCfg(friction_combine_mode="multiply",
                                                            restitution_combine_mode="multiply",
                                                            static_friction=1.0,
                                                            dynamic_friction=1.0,
                                                            restitution=0.0,
                                                            ),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)),
        )
    )

@configclass
class EventCfg():
    """Configuration for randomization."""

    def __init__(self, bin_height_min:float , bin_height_max:float, num_difficulties: int):

        self.physics_material = EventTermCfg(
            func=mdp.randomize_rigid_body_material,         # type: ignore
            mode="startup",
            params={
                "asset_cfg": SceneEntityCfg("robot", body_names=".*"),
                "static_friction_range": (0.8, 0.8),
                "dynamic_friction_range": (0.6, 0.6),
                "restitution_range": (0.0, 0.0),
                "num_buckets": 64,
            },
        )

        self.add_base_mass = EventTermCfg(
            func=mdp.randomize_rigid_body_mass,
            mode="startup",
            params={
                "asset_cfg": SceneEntityCfg("robot", body_names="base"),
                "mass_distribution_params": (-1.0, 3.0),
                "operation": "add",
            },
        )

        # Make sure that robot spawns with orientation towards the target location
        self.reset_robot_base = EventTermCfg(
            func=mdp.reset_root_state_uniform,
            mode="reset",
            params={
                "pose_range": {"x": (-0.5, 0.0), "y": (-0.5, -0.5), "yaw": (0,0)},
                "velocity_range": {
                    "x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0),
                    "roll": (0.0, 0.0), "pitch": (0.0, 0.0), "yaw": (0.0, 0.0),
                },
            },
        )

        self.reset_robot_joints = EventTermCfg(
            func=mdp.reset_joints_by_scale,
            mode="reset",
            params={
                "position_range": (0.8, 1.2),
                "velocity_range": (0.0, 0.0),
            },
        )

        height = bin_height_min/2
        self.reset_bin_0 = EventTermCfg(
            func=mdp.reset_root_state_uniform,
            mode="reset",
            params={
                "pose_range": {"x": (2.0, 5.0), "y": (-2.0, 2.0), "z": (height, height) },
                "velocity_range": {
                    "x": (0.0, 0.0), "y": (0.0, 0.0), "z": (0.0, 0.0),
                    "roll": (0.0, 0.0), "pitch": (0.0, 0.0), "yaw": (0.0, 0.0),
                },
                "asset_cfg": SceneEntityCfg("bin_0")
            },
        )

class Go2Env(DirectRLEnv):
    cfg: Go2EnvCfg

    def _setup_scene(self):
        #* Terrain
        self.cfg.terrain.num_envs = self.scene.cfg.num_envs
        self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing
        self._terrain = self.cfg.terrain.class_type(self.cfg.terrain)
        #* Robot
        self._robot = Articulation(self.cfg.robot)
        self.scene.articulations["robot"] = self._robot
        #* Bins
        self._bin_0 = RigidObject(self.cfg.bin_0)
        self.scene.rigid_objects["bin_0"] = self._bin_0
        #* Sensors
        self._contact_sensor = ContactSensor(self.cfg.contact_sensor)
        self.scene.sensors["contact_sensor"] = self._contact_sensor
        #* Lights
        light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75))
        light_cfg.func("/World/Light", light_cfg)
        #* Markers
        self._marker = VisualizationMarkers(self.cfg.markers_bin)
        # self._markers_bin_big = VisualizationMarkers(self.cfg.markers_bin_big)

        #* Clone, filter, and replicate
        self.scene.clone_environments(copy_from_source=True)
        self.scene.filter_collisions(global_prim_paths=[self._terrain.cfg.prim_path])

[RandomOakForest]

Thanks for posting this question. This requires a closer look from our side. Thanks for your patience.

[YannickP99]

@RandomOakForest a quick followup question. Is it possible to spawn one big rough environment like the standard plane where all robots spawn on the same plane instead of every robot spawning in an own environment like the method I tried before?

[RandomOakForest]

Following up, there is a _reset_idx method you need to define, could you share this part of your code? To answer your question, the recommended way is to use environments as you are doing.

[YannickP99]

Hey,
yes I found out about that. You mean similar to this, but for my objects?

      default_root_state = self._robot.data.default_root_state[env_ids]
      default_root_state[:, :3] += self._terrain.env_origins[env_ids]
      self._robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids)
      self._robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids)
      self._robot.write_joint_state_to_sim(
                              position=self._robot.data.default_joint_pos[env_ids],
                              velocity=self._robot.data.default_joint_vel[env_ids], 
                              joint_ids=None, env_ids=env_ids
                  )

Interestingly I do not need this for the simple plane-environment. After I implemented a similar addition like default_root_state[:, :3] += self._terrain.env_origins[env_ids] for my objects, I partially fixed my problem. So my objects are now spawning correctly, but I ran into the following thing:
I defined a terrain with 10 rows and 1 column and used 10 environments. So I thought that in each environment one robot would spawn. But some environments had multiple robots and some had none. Also in these environments the robot were spawning on the same position (also I defined a reset-event to randomize the robot position). So I wasnt sure how this could affect the training so I stopped and used the normal plane environment again
Thank you very much for your help!

[StrainFlow]

It seems like you're trying to create a single procedural ground mesh and then spawn your 10 environments, each in the correct position on that mesh. to get a really good quadruped motion policy you're going to want to create more like 4,000 environments and this method probably won't scale very well.

that's why it's probably better to use the _reset_idx function. In this one, you would write code that generates each environment randomly so that you can generate as many random environments as you need. That said, I haven't done this myself just yet so I'm not sure if you can add additional meshes and rigid bodies in this function or if you can only write to the articulation's state.

I've got a sample project (the leatherback I talk about in the Isaac lab office hours) And spawning rigid bodies upon reset is one of the next tasks I'm going to try to tackle.

[YannickP99]

@StrainFlow Thanks for your reply. I am aware of the number of environments. I have only been using 10 to visualize the environments and check if everything looks good, normally I am using 4096. When looking at the locomotion examples the ground meshes are generated in the EnvCfg-class and I think generating the ground mesh once is enough for my use case. Generating a new mesh on every reset sounds computationally expensive =)
I only do not understand how the robots are spawned in these environments. Maybe the robots are spawned in random environments intentionally.

[Mayankm96]

Could you add a clone operation here to make sure the tensor is getting copied? Something like:

      default_root_state = self._robot.data.default_root_state[env_ids].clone()
      default_root_state[:, :3] += self._terrain.env_origins[env_ids]
      self._robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids)
      self._robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids)
      self._robot.write_joint_state_to_sim(
                              position=self._robot.data.default_joint_pos[env_ids],
                              velocity=self._robot.data.default_joint_vel[env_ids], 
                              joint_ids=None, env_ids=env_ids
                  )

Also to check env_origins, you can enable debug_vis flag inside the TerrainImporterCfg.

[YannickP99]

I have tried out a few more things

1. In my code I am executing

   default_root_state = self._robot.data.default_root_state[env_ids].clone()
   default_root_state[:, :3] += self._terrain.env_origins[env_ids]
   

   but I dont do the same operation for my RigidObject-class. For the plane environment everything looks fine.
   

   When switching to the terrain the robots spawn as expected in the different environments (still some environments are empty but num robots = num environments) and the objects spawn around the global origin.
   

2. When I add the same operation for the RigidObject-class, the environments looks different for the original plane environment.
   

   Also for the terrain it does not work correctly. Some objects are spawning with the defined env_spacing as offset
   

[StrainFlow]

Can you share your _reset_idx function?

[YannickP99]

def _reset_idx(self, env_ids: torch.Tensor | None):
        if env_ids is None or len(env_ids) == self.num_envs:
            env_ids = self._robot._ALL_INDICES      # type: ignore

        self._robot.reset(env_ids)  
        super()._reset_idx(env_ids)                 # type: ignore

        if len(env_ids) == self.num_envs:                   # type: ignore
            # Spread out the resets to avoid spikes in training when many environments reset at a similar time
            self.episode_length_buf[:] = torch.randint_like(self.episode_length_buf, high=int(self.max_episode_length))
        self._actions[env_ids] = 0.0
        self._prev_actions[env_ids] = 0.0
        self._prev_prev_actions[env_ids] = 0.0     
        
        #* Reset robot state
        default_root_state = self._robot.data.default_root_state[env_ids]
        default_root_state[:, :3] += self._terrain.env_origins[env_ids]
        self._robot.write_root_link_pose_to_sim(default_root_state[:, :7], env_ids)
        self._robot.write_root_com_velocity_to_sim(default_root_state[:, 7:], env_ids)
        self._robot.write_joint_state_to_sim(
                                position=self._robot.data.default_joint_pos[env_ids],
                                velocity=self._robot.data.default_joint_vel[env_ids], 
                                joint_ids=None, env_ids=env_ids

That is the relevant part, after that I do some task reset and logging

[RandomOakForest]

Thank you for following up. I will move this post to our Discussions section for the team to continue. Let us know if you have made any inroads. Thanks.