Basic Collision detection and modeling with impact calculation using PyBullet
Collision detection and avoidance is a complex task when it comes to robot motion planning and it gets much more abstruse in robotic arms because of the kino-dynamic constraints that they bring into the system. I would like to discuss and elaborate on a method of collision detection and Impact calculation method using a UR5e robot in the Bullet physics engine.
Here I have elaborated and implemented my idea of impact calculation for the following scenario. We have a Ur5e robotic arm with 6 revolute joints contributing to its 6 Degrees of freedom. It is assumed that the robot's workspace has a static obstacle and it becomes vital to constantly check the collision of the robot with itself and with the obstacle while planning and moving along a given path. I have implemented it as a real-time collision detection module in which the joint angles of the robot are continuously read and fed to the robot model such that continuous collision queries are made to check for the collision in real-time.
1) Modeling the environment:-
It is quite very much important to be able to model and keep track of the robotic arm and its environment for performing collision detection. The detailing and design of the environment directly co-relates to its accuracy and efficiency. Better the modeling, the better the detection of collisions.
a) Modeling the Robot and collisions
The bullet engine comes up with great capabilities to design and load robot and environment models. It facilitates the developer to load the URDF file of a robot into the environment directly. The URDF is a file format in which almost all contemporary robots are modeled for many simulation and visualization purposes. The URDF file in the back end can make use of other stereolithography based file-formats like .stl, .dae, etc for the exact geometric representation of the robot that is represented.
Bringing in and loading the URDF file inside the Bullet Engine is quite very easy.
#Code example to load a URDF file
Robot = p.loadURDF("<Path-to-URDF file>", [0, 0, 0], useFixedBase=True,flags=p.URDF_USE_SELF_COLLISION)Here, [0,0,0] states the position at which the Model has to load, and the useFixedBase parameter specifies whether the robot model has to be fixed to the point where it is loaded, setting it to false will let the model react and respond to the physical interactions it deals with (Including gravity and interaction with other collision objects). In order to check the collision of the robotic arm with itself, here I have enabled the robot self-collision flag by flags=p.URDF_USE_SELF_COLLISION.
p.loadURDF("Support/Collsions_Objects/Collision1.urdf",pos,orient)
p.createConstraint(CollisionLoadList[count], -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],parentFramePosition=[0,0,0],childFramePosition=pos,childFrameOrientation=orient)Likewise, even the other collision objects can be modeled. But the other thing to be taken into account while dealing with collision models is the constraints at which they have to be fixed in the environment. Constraints in PyBullet are elaborated here in their official documentation here. As shown below, I have used the Fixed constraint to make the collision object stick to a particular point in space.
b) Modeling the robot movement
As mentioned earlier, I have implemented this module in a real-time setup to make repeated contact and impact queries for collision detection. So it becomes critical to replicate the movement of the robot seamlessly inside the environment of the physics engine so that there is not much space for movement disparity and latency. Irrespective of whether the system is real-time or pre-emptive validations etc. One can make use of the setJointMotorControlArray API for modeling the movement of the robot’s URDF in PyBullet as shown below:-
p.setJointMotorControlArray(bodyIndex=Robot,
controlMode=p.POSITION_CONTROL,
targetPositions=Pose,
jointIndices= list(range(1,7,1)),positionGains=[.1,.1,.1,.1,.1,.1])Specifying the Joint indices correctly would ensure that movement disparity between the real and simulated robot is completely avoided. For that, one must have a complete idea of the correspondence between the links and joints of the robot in the real world and the URDF. Maybe the getNumJoints and getJointInfo APIs mentioned here might be of great use.
2) Collision Detection
In this scenario, we have two types of collisions
- Robot self-collision:- The robot arm collides with itself when two links come in contact with itself.
- Robot—object Collision:- It is self-explanatory as the title goes this is when the robot and the modeled obstacle come in contact.
As previously mentioned, while loading the robot URDF it is important to set the URDF_USE_SELF_COLLISION and apply collision filter masks appropriately so that the subsequent links that are joined with each other shouldn’t be detected as collisions. Following is an example that applies a collision filter mask in such a way that we only account for collision among links that have a possibility to collide and disable all others that does not. By doing this we eliminate the chance for two subsequent links being detected as self-collision.
def Init_SelfCollision():
collisionFilterGroup =0
collisionFilterMask = 0
p.setCollisionFilterGroupMask(Robot, 9, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 8, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 7, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 6, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 5, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 4, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 3, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 2, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 1, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 0, collisionFilterGroup, collisionFilterMask)once the detailing is perfected we can then move on to making collision detection queries in PyBullet. An example code is given below,
cont_pts1 =p.getContactPoints(Robot,Robot)
if(len(cont_pts1) > 0):
self_col_flag = 1
print("Self-Collision")
else:
# print("no Self-collision")
self_col_flag = 0We use the getContactPoints() API to get the list of all points that collide right at the site of the collision. Here, as we are checking for self-collision so both the colliding objects are given as Robot. And these queries are in-line with the Collision Filter masks as set previously.
for count in range(0,len(CollisionLoadList),1):
cont_pts =p.getContactPoints(Robot,CollisionLoadList[count])
if(len(cont_pts) > 0):
# print("Collision")
col_flag = 1
curr_pos = p.getBasePositionAndOrientation(CollisionLoadList[count])
act_pos = CollisionList_init[count]
print(math.dist(curr_pos[0],act_pos[0:3]))
if(math.dist(curr_pos[0],act_pos[0:3]) > 0.02):
print("High Impact")
impact_flag = 1
else:
print("no collision")
col_flag = 0The same API is implemented in a different way to hold for multiple different collision objects. All the collision objects are defined, loaded, and stored in a list and it is later on retrieved and used while making collision queries. Thus by making use of these APIs we can decide whether a collision is occurring or not.
3) Impact calculation:-
I have just used a very basic concept of calculating displacements between two objects to calculate the impact caused by collision. As the collision objects stick to a defined point in space, colliding with other models will cause them to move from their original position to another position. Here the euclidean distance between those two positions is measured and if it crosses a certain threshold (say 5 cm) we categorize it as a collision with high impact. Else we just say it is a collision. It is even possible to take the time factor into consideration and equate it with the displacement caused to get the force exerted on the object and make impact decisions based on that. But as of now here is the code that I have implemented:-
curr_pos = p.getBasePositionAndOrientation(CollisionLoadList[count])
act_pos = CollisionList_init[count]
print(math.dist(curr_pos[0],act_pos[0:3]))
if(math.dist(curr_pos[0],act_pos[0:3]) > 0.02):
print("High Impact")
impact_flag = 1Putting all these things together we can come up with a real-time module to make collision queries and check for interaction and collision between different objects in a modeled environment. Here is the complete code for that,
import pybullet as p
from Utils.Ur5e_Robot_State import *
import pybullet_data
import numpy as np
import math
import pybullet_utils.bullet_client as bc
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True)
Robot = p.loadURDF("Support/Ur5e_Urdf/urdf/UR_SSR.urdf", [0, 0, 0.5], useFixedBase=True,flags=p.URDF_USE_SELF_COLLISION)
p.resetBasePositionAndOrientation(Robot, [0, 0, 0], [0, 0, 0, 1])
CollisionList_init = [[0.370,0.05,0,1.5,3.5,1,0]] #,[0.600,0,0,1.5,3.5,1,0],[0.700,0,0,1.5,3.5,1,0]]
ConstraintLoadList = []
CollisionLoadList = []
# for i in range(p.getNumJoints(Robot)):
# jointInfo = p.getJointInfo(Robot, i)
# # print(jointInfo,list(range(1,7,1)))
def Init_SelfCollision():
collisionFilterGroup =0
collisionFilterMask = 0
p.setCollisionFilterGroupMask(Robot, 9, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 8, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 7, collisionFilterGroup, collisionFilterMask)
p.setCollisionFilterGroupMask(Robot, 6, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 5, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 4, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 3, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 2, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 1, collisionFilterGroup, collisionFilterMask)
#p.setCollisionFilterGroupMask(robotID, 0, collisionFilterGroup, collisionFilterMask)
def init_collision(CollisionList):
for count in range(0,len(CollisionList),1):
temp_CollisionList = CollisionList[count]
pos = temp_CollisionList[0:3]
orient = temp_CollisionList[3:7]
CollisionLoadList.append(p.loadURDF("Support/Collsions_Objects/Collision1.urdf",pos,orient))
ConstraintLoadList.append(p.createConstraint(CollisionLoadList[count], -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],parentFramePosition=[0,0,0],childFramePosition=pos,childFrameOrientation=orient))
init_collision(CollisionList_init)
Init_SelfCollision()
while 1:
Pose = get_joint_vals()
p.stepSimulation()
# Pose = [0,np.deg2rad(-90),np.deg2rad(190),0,0,0]
impact_flag = 0
p.setJointMotorControlArray(bodyIndex=Robot,
controlMode=p.POSITION_CONTROL,
targetPositions=Pose,
jointIndices= list(range(1,7,1)),positionGains=[.1,.1,.1,.1,.1,.1])
for count in range(0,len(CollisionLoadList),1):
cont_pts =p.getContactPoints(Robot,CollisionLoadList[count])
if(len(cont_pts) > 0):
# print("Collision")
col_flag = 1
curr_pos = p.getBasePositionAndOrientation(CollisionLoadList[count])
act_pos = CollisionList_init[count]
print(math.dist(curr_pos[0],act_pos[0:3]))
if(math.dist(curr_pos[0],act_pos[0:3]) > 0.02):
print("High Impact")
impact_flag = 1
else:
print("no collision")
col_flag = 0
cont_pts1 =p.getContactPoints(Robot,Robot)
if(len(cont_pts1) > 0):
self_col_flag = 1
print("Self-Collision")
else:
print("no Self-collision")
self_col_flag = 0
if(col_flag == 1 or self_col_flag == 1):
if(impact_flag == 1):
p.addUserDebugText(" Collision with high impact",[0.0,0.05,0],[1,0,0],3,0.1)
else:
p.addUserDebugText(" Collision",[0.0,0.05,0],[1,0,0],3,0.1)
elif(col_flag == 0 and self_col_flag == 0):
p.addUserDebugText(" No Collision",[0.0,0.05,0],[0,1,0],3,0.1)If you are looking for a complete implementation please feel free to check out and leave issues on my repo at GitHub. Hope you found this article useful.
References:-
2. https://github.com/bulletphysics/bullet3
3.https://github.com/aswathgovind/PyBullet_Contact_Impact_Calculator.git
