Segment-wise collision detection

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
tomjur
Posts: 2
Joined: Tue Oct 15, 2019 6:56 am

Segment-wise collision detection

Post by tomjur »

Hi,

I am working on a problem that requires collision tests for (small) linear motions between two arbitrary joint configurations, from configuration c_1 to configuration c_2.
This method should return not only if the motion is collision free, but also if c_1 is free.
I also want this method to be consistent -- whenever the method is called I want it to have the same result regardless of the queries that came before. To achieve that, the robot should start the motion without any velocities or accelerations, and if possible also end the motion with no velocity or acceleration.

My current implementation is divided into two parts (1) starting at c_1 and (2) trying to move towards c_2:
[1] Starting at c_1:
Using resetJointState on every joint j to set the robot to c_1[j], and use the parameter targetVelocity=0.0 to make set it in a resting position.
Then I use stepSimulation and setJointMotorControlArray with c_1 as target repeatedly until the result of getJointStates is close enough to c_1 or getContactPoints indicates a collision.
After collision or X steps of not reaching close enough to c_1 the method returns that the start is not valid and the segment is in collision, otherwise, I check the motion towards c_2

[2] Moving towards c_2:
Repeating the logic in steps 2 and 3 above with 2 differences (1) the motion is towards c_2 instead of c_1 and (2) instead of allowing X number of steps I limit the distance in configuration space of the current configuration to c_1 to be at most 1.5 the distance between c_1 and c_2.
If the robot reaches close enough to c_2 without collision the segment is considered valid (I do not check for no velocity or acceleration since I don’t know how to do that).

This implementation is very slow, and I would like to improve it.

I understand that that there is no one function to test a segment as free as pybullet is step-based by design. However, I have the following questions:
1. Is there a better way to check the validity of c_1 and change the robot state to c_1 at rest?
2. What is the best practice to check if the path to c_2 is collision free, given I am close enough to c_1?

Thanks for the help!
Tom
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: Segment-wise collision detection

Post by Erwin Coumans »

You could use getClosestPoints to get a better conservative answer how far you can safely move, and then use bigger steps along your trajectory.
Post Reply