Questions Regarding Collision Detection and the performCollisionDetection API not available for python

[cmuell89]

I am trying to use PyBullet for performing motion planning research.

Essentially I would like to disable actual physical collisions that occur during simulation (since this can bump collision objects out of place) to test a variety of robot configurations for collision free validity. However, I have an issue in that I cannot seem to get robot object collision detection queries to work well.

Questions:
I've read in the documentation that getClosestPoints or getContactPoints should be called after calling performCollisionDetection however there is no available performCollisionDetection() function in the Python API. How should I go about calling this function from the Python API? Is it even necessary to call if I use p.resetJointState to a test configuration and then call getClosestPoints() ?"

Does anyone have advice on how to perform collision queries for arbitrary arrangements of the robot and with static simulation objects?

Will using the p.setCollisionFilterGroupMask disable simulated collisions? Will it completely disable any form of collision detection? I don't want to disable collision detection but I do want to disable collisions actually having physical consequences to my environment.

Here is the gist of how I am performing collision validity queries. Please note that some of the functions/objects are wrappers around PyBullet core functions such that I've created a little bit of a higher-level interface for my purposes:

I first create a Python Context Manager to disable self collisions of the robot and between the robot and any simulation objects so that when I reset the robot joint state, the arm does not slam into objects in the environment and displace them from their validity position for planning.

class DisabledCollisionsContext(): 

    """
    Python Context Manager that disables collisions. Includes all self collisions, and collisions between SimObjects and Robots. 
    
    This does not disable collision checking but enables the robot to be repositioned into a configuration and then checked for collision without causing any physical simulation effects to occur.
    
    Attributes:
        simulator (simulator.Simulator): The Simulator singleton instance.
    """
    
    def __init__(self, simulator, excluded_bodies=[], excluded_body_link_pairs=[]): 
        """
        
        Args:
        simulator (simulator.Simulator): The Simulator singleton instance.  
        excluded_bodies (list): A list of PyBullet body IDs to excluded, generally reserved for SimObjects with only one link.
        excluded_body_link_pairs (list): A list tuples where the first element is the PyBullet Body ID and the second elemend is the link index. 
        """
        self.simulator = simulator
        self.excluded_bodies = excluded_bodies
        self.excluded_body_link_pairs = excluded_body_link_pairs
          
    def __enter__(self):
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #makes loading faster
        self._disable_robot_collisions()
        self._disable_simobj_collisions()
        self.state_id = p.saveState()
      
    def __exit__(self, exc_type, exc_value, exc_traceback): 
        self._enable_robot_collisions()
        self._enable_simobj_collisions()
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) #re-enable rendering 
        p.restoreState(self.state_id)

    def _disable_robot_collisions(self):
        """
        Disables all self collisions for all robots.
        """
        for robot in self.simulator._robots.keys():
            if robot not in self.excluded_bodies:
                links = [idx for idx in range(-1, p.getNumJoints(robot))]
                for link in links:
                    if (robot, link) not in self.excluded_body_link_pairs:
                        p.setCollisionFilterGroupMask(robot, link, 0, 0)

    def _disable_simobj_collisions(self):
        """
        Disables all collisions between simobjects and robots.
        """
        for robot in self.simulator._robots.keys():
            for sim_obj in self.simulator._objects.keys():
                if sim_obj not in self.excluded_bodies:
                    p.setCollisionFilterGroupMask(robot, sim_obj, 0, 0)

    def _enable_robot_collisions(self):
        """
        Enables all self collisions for all robots.
        """
        for robot in self.simulator._robots.keys():
            links = [idx for idx in range(-1, p.getNumJoints(robot))]
            for link in links:
                p.setCollisionFilterGroupMask(robot, link, 0, 1)

    def _enable_simobj_collisions(self):
        """
        Enables all collisions between simobjects and robots.
        """
        for robot in self.simulator._robots.keys():
            for sim_obj in self.simulator._objects.keys():
                if sim_obj not in self.excluded_bodies:
                    p.setCollisionFilterGroupMask(robot, sim_obj, 0, 1)

I then perform self collision and robot object collisions with the following functions:

I have wrapper function called get_closest_points that adapts to the types of sim id's / links passed in as arguments:

def get_closest_points(client_id, body1, body2, link1=None, link2=None, max_distance=0.):
    """
    Test for the collision between link1 of body1 and link2 of body2. This method relies on p.GetClosestPoints.
    Using a max_distinace of 0 will test for exact collision given overlap of bounding boxes. 
    
    Links are optional and the method will adapt to just using the entire body.
    
    Args:
        body1 (int): PyBullet body ID.
        link1 (int): Link/joint index.
        body2 (int): PyBullet body ID.
        link2 (int): Link/joint index.
        max_distance (int, optional): Range within which to test for collision.
    
    Returns:
        CollisionInfo: Named Tuple of Collision information.
    """
    if (link1 is None) and (link2 is None):
        results = p.getClosestPoints(bodyA=body1, bodyB=body2, distance=max_distance, physicsClientId=client_id)
        print(body1, body2)
        print(results)
    elif link2 is None:
        results = p.getClosestPoints(bodyA=body1, bodyB=body2, linkIndexA=link1,
                                     distance=max_distance, physicsClientId=client_id)
    elif link1 is None:
        results = p.getClosestPoints(bodyA=body1, bodyB=body2, linkIndexB=link2,
                                     distance=max_distance, physicsClientId=client_id)
    else:
        results = p.getClosestPoints(bodyA=body1, bodyB=body2, linkIndexA=link1, linkIndexB=link2,
                                     distance=max_distance, physicsClientId=client_id)
    return [CollisionInfo(*info) for info in results]

The above function is then used for self-collision checking via the following function:

def self_collision_test(joint_configuration, robot, link_pairs, client_id=0):
    """
    Tests whether a given joint configuration will result in self collision. 

    It sets the robot state to the test configuration. Every link pair of the robot is checked for collision.
    If every link pair is collision free, the function returns true, else if there is a single collision, the function returns false.
    
    Args:
        joint_configuration (list): The joint configuration to test for self-collision
        robot (cairo_simulator Robot): The robot tested for self-collision.
        link_pairs (list): 2D pairs of pybullet body links, in this case the pairs of potential in-self-collision pairs. Assumes the first element is a robot link and second element is another robot link.
    
    
    Returns:
        bool: True if no self-collision, else False.
    """
    robot_id = robot.get_simulator_id()

    # Set new configuration
    for i, idx in enumerate(robot._arm_dof_indices):
        p.resetJointState(robot_id, idx, targetValue=joint_configuration[i], targetVelocity=0, physicsClientId=0)


    self_collisions = []
    for link1, link2 in link_pairs:
        if link1 != link2:
            if len(get_closest_points(client_id=client_id, body1=robot_id, body2=robot_id, max_distance=0,
                          link1=link1, link2=link2)) == 0:
                self_collisions.append(True)
            else:
                self_collisions.append(False)
    if all(self_collisions):
        return True
    else:
        return False

And within a robot-object collision checking function:

def robot_body_collision_test(joint_configuration, robot, object_body_id, client_id=0, max_distance=0.025):
    """
    Tests whether a given joint configuration will result in collision with an object. 

     It sets the robot state to the test configuration. There are no links test, we're testing whole body ids.

    Args:
        joint_configuration (list): The joint configuration to test for self-collision
        robot (int): PyBullet body ID.
        # link_pairs: 2D pairs of pybullet body links, in this case the pairs of potential in-self-collision pairs. 
        client_id (int): the physics server client ID.
    Returns:
        bool: True if no self-collision, else False.
    """
    robot_id = robot.get_simulator_id()
     # Set new configuration and get link states
    for i, idx in enumerate(robot._arm_dof_indices):
        p.resetJointState(robot._simulator_id, idx, targetValue=joint_configuration[i], targetVelocity=0, physicsClientId=client_id)
    if len(get_closest_points(client_id=client_id, body1=robot_id, body2=object_body_id, max_distance=max_distance)) == 0:
        return True
    else:
        return False

[cadop]

If this is actually a question and not issue, it may be better for you to post it in the discussion as a Q&A category.

https://github.com/bulletphysics/bullet3/discussions

Just my two-cents, since erwin did create it I think as part of keeping the issues clean, which I appreciated.

[cmuell89]

@cadop That is fair. And i noticed very recently that he has included the performCollisionDetection call in a PR. I will close and move this to the discussion board. Thank you.

[erwincoumans]

performCollisionDetection would be a replacement for stepSimulation, to provide contact points, only for getContactPoints.
It is not needed for getClosestPoints.
The easiest is to keep a copy of a world, and synchronize their transforms and perform collision queries in the separate world. Have you considered that?

[cmuell89]

But even if I were to employ your approach of using a unique client per sample, I would still want to place the robot into the sampled configuration that will be tested for collision. How would that be different than iterating over points and resetting the joint state for each?

P.S. Thank you very much for taking the time to help with this. It's been a big roadblock in moving my research forward. Cheers!

[erwincoumans]

You can run all the 1000s of samples in 1 single separate client. It would solve this

I do want to disable collisions actually having physical consequences to my environment.

[cmuell89]

Hmm. Maybe I'm not following well enough to see the advantage of this step. Wouldn't resetting the joint state potentially have physical consequences in that separate client, and therefore interfere with proper collision checking for the next sample robot configuration? So essentially I've pushed the problem to this separate client, even though my original client's environment remains preserved.

Collision checking / sampling does not require any robot movement etc. It simply asks whether a randomly sampled robot configuration / arrangement will be in collision with the environment or not. I do not need to simulate the robot doing anything in separate client except to simply change all the joint angles and test for collision.

[erwincoumans]

performCollisionDetection doesn't affect the state. A second client lets you add/remove/change object state, without worrying about affecting the original simulation in any way.

[cmuell89]

Ah I think I see my confusion here. From my perspective, I am using the physics server / client as a way to generate the motion plan. I then create a new client/server copy once the plan is produce so that it runs in with a clean starting state.

What I was meaning to say is that when testing for collision for robot configurations, if I do not disable collisions, setting the robot joint state would potentially bump into a table for example. Since the table has been moved by such a collision, then the next sample I test will not be evaluated in the same environment arrangement.

It appears my approach for checking a single collision is working. But I will look deeper into your suggestion about having multiple clients if I need to generate motion plans concurrent to a running simulation.

[erwincoumans]

BTW, this discussion belongs in the Discussions section.

[cadop]

fyi, it was recreated #3813

also if you set it as 'Q&A' instead of 'general', it can be checked off as 'answered' (basically stackoverflow), and then discussions can be sorted by unanswered. (assuming you haven't played around much with it yet, just trying to be helpful)

[cmuell89]

Thanks cadop. I am not used to the github discussions.

[erwincoumans]

I'm new to this, and not sure how much time I have for it. Too many forums, trackers and social media.

[cadop]

@cmuell89 I think its new to everyone, only been a few months as a feature. I really like it, although could be better of course. For me though, its way nicer than having to switch between forum boards

[cmuell89]

Yes. And it helps de-clutter the Issue board from being a Q&A forum as well.

[cmuell89]

From @cadop:

9 minutes ago
This conversation may help explain the use of performCollisionDetection: #2900

As I understood, the reason you need to call it (or the benefit of it being added) is the simulation step is what is needed normally to update contact points, and just updating the bounding box doesn't solve it.

[cmuell89]

I hate to unearth an old discussion but I still can't seem to achieve what I need to perform collision checking for state validity checks in a sampling-based planning system. What I want to essentially perform is the following:

For n of N-configurations:

1. Move the robot arm / object into a specific joint configuration.
2. Test whether or not there is a collision bewteen the arm, itself, and with any other simulation objects.

I don't care about violated dynamics etc since I am not using the the physics client to execute simulated behavior during this process. Thus I don't think I need to create a separate 'world' or separate physics client as Erwin mentioned above. I am simply checking if certain arm arrangements will be in a collision. For some reason, p.resetJointState does not appear to update the physics simulation's current configuration of the robot arm.

Is there a certain way I could do this?

[cadop]

Can you explain why this doesnt work?

1. move/resetjoint state
2. call performCollisionDetection (add performCollisionDetection (stepSimulation also calls this, but do… #3331)
3. check collision

[cmuell89]

For some reason, p.resetJointState() call does not appear to be reflected in the p.getGetJointStates() function call.

I thought I didn't need to call .step() or .performCollision() if I am using the two above function calls?

[cmuell89]

I guess a more specific question is why there might be a difference between the reset joint states and the current state of hte robot? Collision detection appears to be working correctly but the configuration I set should be collision-free, but upon resetting the robot configuration to this collision-free point getClosestPoints() is resulting in collision detection and the current robot state is not the expected configuration that I just set it to.

Essentially the current state of the robot is in a configuration that is indeed in collision, but is not reflected by resetJointStates nor the actual visual of the robot in the simulation GUI.