pre-check collisions in place poses before sending them to moveit

[oscar-lima]

add FCL support to Grasplan, to discard place poses that are in collision with MoveIt planning scene objects.

The assumption is that FCL can check unfeasible solutions early on given objects in the planning scene and the sampled place pose. The current brute force sampling approach puts too much stress on MoveIt place pipeline by checking for an entire manipulation plan from scratch, maybe they already do that which would be smart but I don't know...

[oscar-lima]

fcl snippet that can help:

import numpy as np
import fcl

class BoxCollisionChecker:
    def __init__(self):
        pass

    def create_box(self, size, pos, quat=None):
        if quat is None:
            quat = [1, 0, 0, 0]  # Default orientation (no rotation), rosq_order : qx, qy, qz, qw. same for fcl?
        box = fcl.Box(size[0], size[1], size[2])
        t = fcl.Transform(quat, pos)
        obj = fcl.CollisionObject(box, t)
        return obj

    def check_collision(self, obj1, obj2):
        request = fcl.CollisionRequest()
        result = fcl.CollisionResult()
        fcl.collide(obj1, obj2, request, result)
        return result.is_collision

if __name__ == "__main__":
    checker = BoxCollisionChecker()
    # Generate two random boxes
    # box1 = checker.generate_random_box()
    # box2 = checker.generate_random_box()
    # Define custom size, position, and orientation for a new box
    custom_size = np.array([1, 1, 1])
    custom_pos_1 = np.array([-0.51, 0, 0])
    custom_pos_2 = np.array([0.5, 0, 0])
    custom_quat_1 = np.array([0.924, 0, 0.383, 0.0])  # Example quaternion for 90 degrees rotation around the Z-axis
    custom_quat_2 = np.array([0, 0, 0, 1.0])  # Example quaternion for 90 degrees rotation around the Z-axis
    # custom_quat /= np.linalg.norm(custom_quat)  # Normalize quaternion
    box1 = checker.create_box(custom_size, custom_pos_1, custom_quat_1)
    box2 = checker.create_box(custom_size, custom_pos_2, custom_quat_2)
    # Check collision between boxes
    is_collision = checker.check_collision(box1, box2)
    print(f"Collision Detected between box1 and box2: {is_collision}")

[mintar]

I am 90% certain that MoveIt already checks for collisions first. MoveIt also uses FCL internally. I would be very surprised if reimplementing it outside of MoveIt would speed up the whole process. And if it does, we should figure out how to configure MoveIt instead of doing MoveIts job for it. And even if that weren't possible for some reason, we could probably save some code by using the MoveIt PlanningScene API and letting MoveIt do it instead of re-creating the planning scene on our end and reimplementing the collision checking.

[oscar-lima]

That makes sense! unfortunately I don't have time now to make sure this is the case. However, I will assume for now that this is the case and close the issue as this seems then unnecessary.