How to get the updated pose from isaacsim USD stage? #603
Description
If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions
Please provide the below information in addition to your issue:
- cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): isaac sim
- python version: 3.11.13
- Isaac Sim version (if using): 5.0
Issue Details
I'm trying to use Curobo as my motion planner to avoid obstacles along with isaac sim simulator.
I'm planning to use multiple environemnts so I built everything in batch with num_envs, where for Curobo, I only planned single environment at once and right now I only have one environment.
I did not have the world config file (eg. world.yml) but build and read everything from the simulator.
After I move the object using the arm, the pose is updated in the sim but not updated after I called get_obstacles_from_stage().get_collision_check_world() and motion_gen.update_world(obstacles)
Initialized Isaac Sim:
device = 'cuda'
backend = 'torch'
world = World(stage_units_in_meters=1.0, device=device, backend=backend, physics_dt=1/240, rendering_dt=1/60)
world.get_physics_context().enable_gpu_dynamics(True)
world.get_physics_context().set_broadphase_type("GPU")
world.scene.add_default_ground_plane()
cube_path = f"{base_env_path}/cube"
cube = DynamicCuboid(
prim_path=cube_path,
scale=torch.tensor([0.06, 0.06, 0.06]).to(device),
position=torch.tensor([0.6, 0.43, 0.63]).to(device),
color=torch.tensor([1.0, 0.0, 0.0]).to(device),
physics_material=cube_material,
)
cloner.clone(
source_prim_path=base_env_path,
prim_paths=target_paths,
copy_from_source=True,
)
cubes = RigidPrim(prim_paths_expr="/World/envs/env.*/cube", name="cube_view")
world.scene.add(cubes)
# same for the robot ...
Initialize Curobo:
tensor_args = TensorDeviceType()
usd_helper.load_stage(world.stage)
world_cfg = usd_helper.get_obstacles_from_stage(
reference_prim_path="/World",
ignore_substring=
[
f"{base_env_path}/robot",
"/World/defaultGroundPlane",
]
)
robot_cfg_path = get_robot_configs_path()
robot_cfg = load_yaml(join_path(robot_cfg_path, "bimanual_robotiq_flat.yml"))['robot_cfg']
robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 10}
tensor_args = TensorDeviceType()
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_cfg,
world_cfg,
tensor_args=tensor_args,
collision_checker_type=CollisionCheckerType.MESH,
num_trajopt_seeds=64,
num_graph_seeds=64,
optimize_dt=False,
interpolation_dt=0.15,
trajopt_tsteps=32,
maximum_trajectory_dt=0.3,
collision_cache={"obb": 10, "mesh": 10},
)
motion_gen = MotionGen(motion_gen_config)
print("[INFO] warming up...")
motion_gen.warmup(enable_graph=True)
plan_config = MotionGenPlanConfig(
enable_graph=True,
enable_graph_attempt=10,
max_attempts=50,
enable_finetune_trajopt=False,
time_dilation_factor=0.8,
)
obstacles = world_cfg.get_collision_check_world()
motion_gen.update_world(obstacles)
print("[INFO] CuRobo MotionGen is ready.")
Main Loop:
while simulation_app.is_running():
plan_and_execute(robots, cubes, curobo, ...)
world.step()
obstacles = usd_helper.get_obstacles_from_stage(
reference_prim_path="/World",
ignore_substring=
[
f"{base_env_path}/robot",
"/World/defaultGroundPlane",
]
).get_collision_check_world()
motion_gen.update_world(obstacles)
print("[INFO] Updated World!\n")
cube_pos, _ = cubes.get_world_poses()
print(f"[INFO] Cube position: {cube_pos}")
print(obstacles.objects)
The cube is being moved after the action and updated inside the sim, but when I printed the obstacles' pose, it is still the initial pose and the robot will have collision with the cube in the new pose as Curobo thought there's no object.
Result:
[INFO]: Setup complete...
[INFO] Picking...
[INFO] Cube position: tensor([[0.6001, 0.4299, 0.6300]], device='cuda:0')
[INFO]: Cuboid(name='/World/envs/env_0/cube', pose=[0.6000000238418579, 0.4300000071525574, 0.6299999952316284, 1.0, 0.0, 0.0, 0.0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[0.05999999865889549, 0.05999999865889549, 0.05999999865889549])
[INFO] Moving...
[INFO] Cube position: tensor([[0.5992, 0.4281, 0.7742]], device='cuda:0')
[INFO]: Cuboid(name='/World/envs/env_0/cube', pose=[0.6000000238418579, 0.4300000071525574, 0.6299999952316284, 1.0, 0.0, 0.0, 0.0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[0.05999999865889549, 0.05999999865889549, 0.05999999865889549])
[INFO] Dropping...
[INFO] Cube position: tensor([[0.6252, 0.0343, 0.6408]], device='cuda:0')
[INFO]: Cuboid(name='/World/envs/env_0/cube', pose=[0.6000000238418579, 0.4300000071525574, 0.6299999952316284, 1.0, 0.0, 0.0, 0.0], scale=None, color=None, texture_id=None, texture=None, material=Material(metallic=0.0, roughness=0.4), tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32), dims=[0.05999999865889549, 0.05999999865889549, 0.05999999865889549])
I'm not sure if this is a bug in my code regarding how to set up the scene correctly, or a communication issue with Curobo reading the USD stage.
I appreciate it if anyone knows how to solve or has the same problem.