Collision detection in Python

edit

Here is my solution to detect collisions using moveit, you would need to publish of course your obstacle to /planning_scene topic beforehand:

#!/usr/bin/env python

import rospy
from moveit_msgs.srv import GetStateValidityRequest, GetStateValidity
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState


class StateValidity():
    def __init__(self):
        # subscribe to joint joint states
        rospy.Subscriber("joint_states", JointState, self.jointStatesCB, queue_size=1)
        # prepare service for collision check
        self.sv_srv = rospy.ServiceProxy('/check_state_validity', GetStateValidity)
        # wait for service to become available
        self.sv_srv.wait_for_service()
        rospy.loginfo('service is avaiable')
        # prepare msg to interface with moveit
        self.rs = RobotState()
        self.rs.joint_state.name = ['joint1','joint2']
        self.rs.joint_state.position = [0.0, 0.0]
        self.joint_states_received = False


    def checkCollision(self):
        '''
        check if robotis in collision
        '''
        if self.getStateValidity().valid:
            rospy.loginfo('robot not in collision, all ok!')
        else:
            rospy.logwarn('robot in collision')


    def jointStatesCB(self, msg):
        '''
        update robot state
        '''
        self.rs.joint_state.position = [msg.position[0], msg.position[1]]
        self.joint_states_received = True


    def getStateValidity(self, group_name='acrobat', constraints=None):
        '''
        Given a RobotState and a group name and an optional Constraints
        return the validity of the State
        '''
        gsvr = GetStateValidityRequest()
        gsvr.robot_state = self.rs
        gsvr.group_name = group_name
        if constraints != None:
            gsvr.constraints = constraints
        result = self.sv_srv.call(gsvr)
        return result


    def start_collision_checker(self):
        while not self.joint_states_received:
            rospy.sleep(0.1)
        rospy.loginfo('joint states received! continue')
        self.checkCollision()
        rospy.spin()


if __name__ == '__main__':
    rospy.init_node('collision_checker_node', anonymous=False)
    collision_checker_node = StateValidity()
    collision_checker_node.start_collision_checker()

There are python bindings for the Bullet Physics engine, called PyBullet. PyBullet comes with various collision checking facilities that are fast. The python bindings also have some functionality to spawn URDFs and geometric primities. For a quick script, that should do the job.

• Main Website
• Quickstart Guide
• Github

My experiences with PyBullet are:

PRO:

• It is easy to set up a simple experimental application.
• The framework is fast, considering that you are coding the top part in Python.
• The developers are responsive and helpful on github. They have quickly responded to issues, merged PRs, and even fixed bugs that we encountered.

CON:

• PyBullet has not been designed with ROS integration in mind. So as a ROS developer, at some time frustration sets in.
• The way the python bindings are done, it feels a lot like coding good-old C and not pythonic, at all.
• Not all Bullet features are wrapped for Python. So, sometimes you hit a brick wall because you're missing an essential feature.
• The architecture of the framework is complicated with a Client-Server setup and Python-C-C++-Wrappings. Hence, you need a lot of expertise to provide new features or further wrappings.