Using motion_gen plan single generates collision trajectories!

[exaFLOPs26]

1. Situation: Using cuRobo in IsaacLab for manipulation task
2. Robot: mobile manipulator

As I am using a mobile manipulator, I need to update the scene when I need planning. So when I stop moving the mobile base I update the scene like the following code

pose_to_gf_matrix_tensor and usd_helper.get_obstacles_from_stage_exaFLOPs are custom function I made and pretty sure they are doing what we expect; inverse transformation for all object so the robot is in 0,0,0.

When I use motion.plan_single, it gives me a trajectory that collides.

not.collision.free.mov

Any tips to make it work will help a lot. Thanks advance for your help and this wonderful research.

[balakumar-s]

The standard get_obstacles_from_stage() will find the transform for all objects in the world w.r.t. the robot. You don't need to add any additional offsets.

[balakumar-s]

You can approximate the omni-wheel motions with three kinematic joints at the base of the robot:

curobo/src/curobo/content/assets/robot/franka_description/franka_panda_mobile_xy_tz.urdf

Line 9 in ebb7170

<joint name="base_x" type="prismatic">

You can add these joints to curobo and lock them at the state given by isaaclab.

In simulation:

1. Use your omni wheels to move the robot

In curobo:

1. Read the current position and rotation of the robot base. Convert to x y theta_z for the joints and lock them.
2. Read the world using USDHelper and update in motion_gen.

[exaFLOPs26]

But isn't that the same as what I am doing?

pose_to_gf_matrix_tensor: gives the matrix of current robot pos
get_obstacles_from_stage_exaFLOPs: give the obstacles position based on the current robot pos in simulation

[exaFLOPs26]

def pose_to_gf_matrix_tensor(position: torch.Tensor, quaternion: torch.Tensor) -> Gf.Matrix4d:
        """
        position: torch.Tensor of shape (3,)
        quaternion: torch.Tensor of shape (4,) in w, x, y, z order
        """
        # Convert to numpy
        pos = position.detach().cpu().numpy().astype(np.float64)
        quat_wxyz = quaternion.detach().cpu().numpy().astype(np.float64)

        # Reorder to x, y, z, w
        quat = np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]], dtype=np.float64)
        x, y, z, w = quat

        # Compute rotation matrix
        rot = np.array([
                [1 - 2*(y*y + z*z), 2*(x*y - z*w),         2*(x*z + y*w)],
                [2*(x*y + z*w),         1 - 2*(x*x + z*z), 2*(y*z - x*w)],
                [2*(x*z - y*w),         2*(y*z + x*w),     1 - 2*(x*x + y*y)]
        ], dtype=np.float64)

        # Build 4x4 matrix
        mat = np.eye(4, dtype=np.float64)
        mat[:3, :3] = rot
        mat[3, :3] = pos

        return Gf.Matrix4d(mat.tolist())

def get_prim_world_pose_exaFLOPs(t_mat: Gf.Matrix4d, inverse: bool = False):
        if inverse:
                t_mat = t_mat.GetInverse()

        # mat = np.zeros((4,4))
        # mat[:,:] = t_mat
        translation: Gf.Vec3d = t_mat.ExtractTranslation()
        rotation: Gf.Rotation = t_mat.ExtractRotation()
        q = rotation.GetQuaternion()
        orientation = [q.GetReal()] + list(q.GetImaginary())
        t_mat = (
                Pose.from_list(list(translation) + orientation, TensorDeviceType())
                .get_matrix()
                .view(4, 4)
                .cpu()
                .numpy()
        )

        return t_mat

[balakumar-s]

I am not sure what's implemented in get_obstacles_from_stage_exaFLOPS.

One way to debug this is to query the collision checker for occupancy after updating the world and overlaying it in the current robot pose. motion_gen.world_coll_checker.get_mesh_in_bounding_box() will give you a mesh:
https://github.com/NVlabs/curobo/blob/ebb71702f3f70e767f40fd8e050674af0288abe8/src/curobo/geom/sdf/world.py#L632C9-L632C33

[exaFLOPs26]

Changed the code but I found out that it still generates collision trajectories.

The original code is above.

Will there be other common issue or tips to solve this?

[exaFLOPs26]

Fixed it. Found that the transformation was wrong.