How to use curobo to use the voxel map generated by nvblox as a voxel collision reference in MPC cases?

[KKXiaoKang]

@balakumar-s

• I have now imported my own robot, which is described in the voxel of MotionGen
• But in the MPC case, this voxel map was compressed into a small area
• 

---

• This is a voxel map generated by MotionGen during normal motion generation
• 
• what should i do to correct it？

[balakumar-s]

Do you have a minimal example of how you queried the voxel map?

[KKXiaoKang]

@balakumar-s

Thank you for your reply

• The current known situation is as follows

My code architecture is as follows：

• My code architecture is as follows. The way I update voxels is consistent with the way I use the MotionGen module to track obstacle avoidance robots in isaac_ros_cumotion
• The MotionGen module performs obstacle avoidance robot trajectories in a consistent manner

（1）Declare MPC configuration and collision world model

mpc_config = MpcSolverConfig.load_from_robot_config(
    robot_dict,
    world_file,
    collision_cache={
        'obb': collision_cache_cuboid, # 20
        'mesh': collision_cache_mesh, # 20
    },
    collision_checker_type=CollisionCheckerType.VOXEL,
    use_cuda_graph=True,
    use_cuda_graph_metrics=True,
    use_cuda_graph_full_step=False,
    self_collision_check=True,
    use_mppi=True,
    use_lbfgs=False,
    use_es=False,
    store_rollouts=True,
    step_dt=0.02,
)
mpc = MpcSolver(mpc_config)

self.mpc = mpc
self.__tensor_args = self.mpc.tensor_args
self.__world_collision = mpc.world_coll_checker

（2）Update voxel data update-voxel_data

def update_voxel_grid(self):
    global origin_esdf_grid
    global IF_PROCESS_VOXEL_MAP_SATUS
    global tsdf_response
    origin_esdf_grid = self.get_esdf_voxel_grid(tsdf_response)
    IF_PROCESS_VOXEL_MAP_SATUS = True   
    rospy.loginfo('Calling ESDF service')
    esdf_grid = origin_esdf_grid  
    if torch.max(esdf_grid.feature_tensor) <= (-1000.0 + 0.5 * self.__voxel_size + 1e-5):
        rospy.logerr('ESDF data is empty, try again after few seconds.')
        return False
    self.__world_collision.update_voxel_data(esdf_grid)
    rospy.loginfo('Updated ESDF grid')
    IF_PROCESS_VOXEL_MAP_SATUS = False
    return True

（3）Retrieve voxels from ESDF maps

def get_esdf_voxel_grid(self, esdf_data):
    esdf_array = esdf_data.esdf_and_gradients 
    array_shape = [ 
        esdf_array.layout.dim[0].size,
        esdf_array.layout.dim[1].size,
        esdf_array.layout.dim[2].size,
    ]
    array_data = np.array(esdf_array.data) 
    array_data = self.__tensor_args.to_device(array_data) 
    array_data = array_data.view(array_shape[0], array_shape[1], array_shape[2]).contiguous() 
    array_data = array_data.reshape(-1, 1) 
    # nvblox uses negative distance inside obstacles, cuRobo needs the opposite:
    array_data = -1 * array_data 
    # nvblox assigns a value of -1000.0 for unobserved voxels, making
    array_data[array_data >= 1000.0] = -1000.0  
    # nvblox distance are at origin of each voxel, cuRobo's esdf needs it to be at faces
    array_data = array_data + 0.5 * self.__voxel_size 
    esdf_grid = CuVoxelGrid(
        name='world_voxel',
        dims=self.__voxel_dims,
        pose=[
            self.grid_position[0],
            self.grid_position[1],
            self.grid_position[2],
            1,
            0.0,
            0.0,
            0,
        ],  # x, y, z, qw, qx, qy, qz
        voxel_size=self.__voxel_size,
        feature_dtype=torch.float32,
        feature_tensor=array_data,
    )
    return esdf_grid 

I compared the code

• The update-voxel_data function used by MotionGen and MpcSolverConfig to call the collision world is consistent at the underlying level, but it is unclear why there are these two different voxel effects. Obviously, MotionGen is correct, but MpcSolverConfig's description is incorrect.

[KKXiaoKang]

@balakumar-s

• In cuRobo, when the get_esdf_voxel_grid function is called, will there be any difference between the motionGen module and the MpcSolver module in transferring the ESDF map data to the target device (such as GPU)?