Strange behavior of `update_world` not working unless the mesh names change

[tylerlum]

Problem

In many situations, we want to create a motion_gen object and warmup like so BEFORE we know the exact world:

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

After running this, at some point later we create a new world config that replaces the mesh above, eg.

NEW_world_config = {
    "mesh": {
        "base_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
motion_gen.update_world(WorldConfig.from_dict(world_config))

However, this does NOT work as desired. The collisions are not updated to use the new mesh.

Temporary Solution

I found that if I change the name of the mesh object in the world config, it works again, eg.

NEW_world_config = {
    "mesh": {
        "NEW_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
motion_gen.update_world(WorldConfig.from_dict(world_config))

Takeaway

I believe there is some behavior that makes the mesh collision scene NOT update if the object name is the same (eg. "base_scene"), but it works if we have a new name (eg. "NEW_scene"). This behavior is very unclear, gives no warnings, and I don't see any documentation on this, so I think it is very important that this issue be investigated.

Full Example Code

This bug is best recreated by downloading this NEW_SCENE.obj file and putting it into "src/curobo/content/assets/scene/nvblox/". This is just a big object that is spawned at the origin, which should make motion planning fail because it blocks everything and probably starts in collision. NEW_SCENE.zip (needs zip because doesn't allow obj for some reason)

This is identical to the Motion Generation example code in the documentation, with heavily commented sections showing where the issue is.

python base_case.py 
Creating new Mesh cache: 1
Trajectory Generated:  tensor([False], device='cuda:0')
SHOULD BE FALSE BECAUSE OBJECT IS BLOCKING

python reproduce_update_world_bug.py              
Creating new Mesh cache: 1
Trajectory Generated:  tensor([True], device='cuda:0')
SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG

python reproduce_update_world_bug_WITH_HACK_FIX.py
Creating new Mesh cache: 1
Trajectory Generated:  tensor([False], device='cuda:0')
SHOULD BE FALSE, AND IS FALSE WITH HACK FIX

Files with the above code: bug_code_examples.zip

base_case.py

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "NEW_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
######## CHANGE START ##########
    NEW_world_config,
######## CHANGE END ##########
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
######## CHANGE START ##########
# motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE BECAUSE OBJECT IS BLOCKING")
######## CHANGE END ##########

reproduce_update_world_bug.py

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "base_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
######## CHANGE START ##########
motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG")
######## CHANGE END ##########

reproduce_update_world_bug_WITH_HACK_FIX.py:

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "NEW_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
######## CHANGE START ##########
motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE, AND IS FALSE WITH HACK FIX")
######## CHANGE END ##########

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): python
2. python version: Python 3.8.19
3. Isaac Sim version (if using): Not used

---

Issue Details

[tylerlum]

Another observation: If you forgot to put NEW_SCENE.obj into the right folder, the reproduce_update_world_bug.py example does not even raise an error, so it clearly is not reading the obj file, but it is read in the other cases

python base_case.py 
Creating new Mesh cache: 1
Traceback (most recent call last):
  File "base_case.py", line 44, in <module>
    motion_gen_config = MotionGenConfig.load_from_robot_config(
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/wrap/reacher/motion_gen.py", line 638, in load_from_robot_config
    world_coll_checker = create_collision_checker(world_cfg)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/utils.py", line 32, in create_collision_checker
    return WorldMeshCollision(config)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 62, in __init__
    super().__init__(config)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world.py", line 517, in __init__
    self.load_collision_model(self.world_model)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 88, in load_collision_model
    name_list, w_mid, w_inv_pose = self._load_batch_mesh_to_warp(world_model.mesh)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 138, in _load_batch_mesh_to_warp
    m_data = self._load_mesh_into_cache(m_idx)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 126, in _load_mesh_into_cache
    self._wp_mesh_cache[mesh.name] = self._load_mesh_to_warp(mesh)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 116, in _load_mesh_to_warp
    verts, faces = mesh.get_mesh_data()
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/types.py", line 427, in get_mesh_data
    m = self.get_trimesh_mesh(process=process)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/types.py", line 386, in get_trimesh_mesh
    m = trimesh.load(self.file_path, process=process, force="mesh")
  File "/juno/u/tylerlum/miniconda3/envs/nerf_grasping_env_experimental/lib/python3.8/site-packages/trimesh/exchange/load.py", line 114, in load
    ) = _parse_file_args(file_obj=file_obj, file_type=file_type, resolver=resolver)
  File "/juno/u/tylerlum/miniconda3/envs/nerf_grasping_env_experimental/lib/python3.8/site-packages/trimesh/exchange/load.py", line 606, in _parse_file_args
    raise ValueError(f"string is not a file: {file_obj}")
ValueError: string is not a file: /juno/u/tylerlum/github_repos/curobo/src/curobo/content/assets/scene/nvblox/NEW_SCENE.obj

python reproduce_update_world_bug.py              
Creating new Mesh cache: 1
Trajectory Generated:  tensor([True], device='cuda:0')
SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG

python reproduce_update_world_bug_WITH_HACK_FIX.py 
Creating new Mesh cache: 1
Traceback (most recent call last):
  File "reproduce_update_world_bug_WITH_HACK_FIX.py", line 71, in <module>
    motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1721, in update_world
    self.world_coll_checker.load_collision_model(world, fix_cache_reference=self.use_cuda_graph)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 88, in load_collision_model
    name_list, w_mid, w_inv_pose = self._load_batch_mesh_to_warp(world_model.mesh)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 138, in _load_batch_mesh_to_warp
    m_data = self._load_mesh_into_cache(m_idx)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 126, in _load_mesh_into_cache
    self._wp_mesh_cache[mesh.name] = self._load_mesh_to_warp(mesh)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/sdf/world_mesh.py", line 116, in _load_mesh_to_warp
    verts, faces = mesh.get_mesh_data()
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/types.py", line 427, in get_mesh_data
    m = self.get_trimesh_mesh(process=process)
  File "/juno/u/tylerlum/github_repos/curobo/src/curobo/geom/types.py", line 386, in get_trimesh_mesh
    m = trimesh.load(self.file_path, process=process, force="mesh")
  File "/juno/u/tylerlum/miniconda3/envs/nerf_grasping_env_experimental/lib/python3.8/site-packages/trimesh/exchange/load.py", line 114, in load
    ) = _parse_file_args(file_obj=file_obj, file_type=file_type, resolver=resolver)
  File "/juno/u/tylerlum/miniconda3/envs/nerf_grasping_env_experimental/lib/python3.8/site-packages/trimesh/exchange/load.py", line 606, in _parse_file_args
    raise ValueError(f"string is not a file: {file_obj}")
ValueError: string is not a file: /juno/u/tylerlum/github_repos/curobo/src/curobo/content/assets/scene/nvblox/NEW_SCENE.obj

[tylerlum]

Clear diffs between base_case.py and reproduce_update_world_bug.py (only uses NEW_SCENE.obj vs. using base_scene then updating to NEW_SCENE.obj)

diff base_case.py reproduce_update_world_bug.py 
30c30
<         "NEW_scene": {
---
>         "base_scene": {
46,48c46
< ######## CHANGE START ##########
<     NEW_world_config,
< ######## CHANGE END ##########
---
>     world_config,
73c71
< # motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
---
> motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
81c79
< print("SHOULD BE FALSE BECAUSE OBJECT IS BLOCKING")
---
> print("SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG")

Diff between reproduce_update_world_bug.py and reproduce_update_world_bug_WITH_HACK_FIX.py (changes the name of the mesh in the config dict)

diff reproduce_update_world_bug.py reproduce_update_world_bug_WITH_HACK_FIX.py 
30c30
<         "base_scene": {
---
>         "NEW_scene": {
79c79
< print("SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG")
---
> print("SHOULD BE FALSE, AND IS FALSE WITH HACK FIX")

[balakumar-s]

cuRobo treats meshes as assets so that you can use the same mesh across different environments. This would mean that that cuRobo identifies a mesh by it's name and any future updates to the world will not update meshes with the same name. However, you can clear the current mesh assets by calling https://curobo.org/_api/curobo.wrap.reacher.motion_gen.html#curobo.wrap.reacher.motion_gen.MotionGen.clear_world_cache . Then new updates to world will reload the meshes.

Regarding mesh paths, you need to specify absolute paths as /home/user/code/mesh_name.stl then it will be correctly loaded. If a relative path is given, then the mesh is assumed to be in the cuRobo install directory.

[kevin-thankyou-lin]

I tried using clear_world_cache() right before the world config update, however, I get this bug which suggests that clearing the cache wouldn't do anything?

curobo/src/curobo/geom/sdf/world_mesh.py", line 626, in <listcomp>
    [None for _ in range(self.cache["mesh"])] for _ in range(self.n_envs)
TypeError: 'NoneType' object is not subscriptable

Full code:

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "base_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
# clearing cache here
motion_gen.clear_world_cache()
######## CHANGE START ##########
motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG")
######## CHANGE END ##########

[kevin-thankyou-lin]

Hacky fix in curobo/src/curobo/geom/sdf/world_mesh.py - for loop over _env_mesh_names

    def clear_cache(self):
        self._wp_mesh_cache = {}
        if self._mesh_tensor_list is not None:
            self._mesh_tensor_list[2][:] = 0
        if self._env_n_mesh is not None:
            self._env_n_mesh[:] = 0
        if self._env_mesh_names is not None:
            for i in range(self.n_envs):
                for j in range(len(self._env_mesh_names)):
                    self._env_mesh_names[i][j] = None

        super().clear_cache()

[tylerlum]

The absolute path vs. relative path makes sense.

"cuRobo treats meshes as assets so that you can use the same mesh across different environments. This would mean that that cuRobo identifies a mesh by it's name and any future updates to the world will not update meshes with the same name."

• This makes sense for performance, but the behavior is non-obvious and users who try to update_world (like myself) will think the collisions have been updated when they have not, which is a bit worrisome.
• I believe it will be a pretty common situation in which the same object name is used for a different mesh file (like what I did in my use case). Ideally there could be a big WARNING if the same object name but different mesh filepath is given
• For example, imagine a use case in which a real robot is controlled by curobo continuously over time, and there is a RGBD camera used the capture the scene and continuously updates the collision geometry of the scene. If it does this by writing to a new mesh_filepath every N seconds and then subsequently using update_world to point to the new mesh file, the user could very easily forget to change the object name for each of these new meshes, so the collisions are not updated. This could result in undesired collisions. The user would have no way to figure out why this happened unless they stumbled across this thread

[tylerlum]

Another undesired symptom of this bug. When I was first debugging this, I used motion_gen.world_coll_checker.world_model.save_world_as_mesh("world.obj") to visualize the scene and check if the scene was being updated correctly. It appeared as though the scene was being updated, but it actually was not (due to the naming bug).

Example (using same NEW_SCENE.obj as above):

base_case_and_save_mesh.py: This creates an obj file that appears like so, which has a large bottle blocking the robot, so result.sucess should be False (as desired).

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "NEW_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
######## CHANGE START ##########
    NEW_world_config,
######## CHANGE END ##########
    interpolation_dt=0.01,
)
######## CHANGE START ##########
motion_gen_config.world_coll_checker.world_model.save_world_as_mesh("base_case.obj")
######## CHANGE END ##########
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
######## CHANGE START ##########
# motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE BECAUSE OBJECT IS BLOCKING")
######## CHANGE END ##########

---

reproduce_update_world_bug_and_save_mesh.py: This creates an obj file before and after the update_world. The before update obj shows the scene without the big bottle at the center, so this causes no collisions, so in this scene the result should be success = True. The after update obj shows the scene with the big bottle at the center, so this causes collisions, so in this scene the result should be success = False. However, we get True. This means that the visualized mesh is being updated as desired by update_world but the collision geometry is not being updated (due to this naming issue), so the visualized mesh and actual collision geometry have diverged.

BEFORE:

AFTER:

import torch

# cuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

from curobo.geom.types import WorldConfig

world_config = {
    "mesh": {
        "base_scene": {
            "pose": [10.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834],
            "file_path": "scene/nvblox/srl_ur10_bins.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

######## CHANGE START ##########
# NEW SCENE
NEW_world_config = {
    "mesh": {
        "base_scene": {
            "pose": [0, 0, 0, 1, 0, 0, 0],
            "file_path": "scene/nvblox/NEW_SCENE.obj",
        },
    },
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [0.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}
######## CHANGE END ##########

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
######## CHANGE START ##########
motion_gen.world_coll_checker.world_model.save_world_as_mesh("before_world_update.obj")
######## CHANGE END ##########
motion_gen.warmup()

retract_cfg = motion_gen.get_retract_config()

state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(retract_cfg.view(1, -1))
)

goal_pose = Pose.from_list([-0.4, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0])  # x, y, z, qw, qx, qy, qz
start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(),
    joint_names=[
        "shoulder_pan_joint",
        "shoulder_lift_joint",
        "elbow_joint",
        "wrist_1_joint",
        "wrist_2_joint",
        "wrist_3_joint",
    ],
)
######## CHANGE START ##########
motion_gen.update_world(WorldConfig.from_dict(NEW_world_config))
######## CHANGE START ##########
motion_gen.world_coll_checker.world_model.save_world_as_mesh("after_world_update.obj")
######## CHANGE END ##########
######## CHANGE END ##########

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
print("Trajectory Generated: ", result.success)

######## CHANGE START ##########
print("SHOULD BE FALSE, BUT IS TRUE BECAUSE OF BUG")
######## CHANGE END ##########

IN SUMMARY: Using save_world_as_mesh as an introspection tool doesn't work. The mesh it generates is NOT the same as the current collision geometry being used (due to this object name issue above). This is very undesirable because the user no longer has visualization tools to see what collision geometry is really being used.

[balakumar-s]

To visualize the current collision world, use this https://curobo.org/_api/curobo.geom.sdf.world_mesh.html#curobo.geom.sdf.world_mesh.WorldMeshCollision.get_mesh_in_bounding_box

This will sample collisions for a grid of voxels, and generate a mesh using marching cubes.

[tylerlum]

This is very helpful thank you!

[tylerlum]

If there's any way to check if the update world is given an object with the same name but different file path, then giving a big warning, that'd be very helpful. Is that feasible?

[balakumar-s]

Good point, we could use path variable to throw a warning. We could also actually raise an error. I can look into adding a warning first.

[balakumar-s]

Added warning in latest main.