Ask Your Question
2

Collision detection in Python

asked 2015-02-23 15:50:42 -0500

akihiko gravatar image

updated 2015-02-24 11:28:39 -0500

Hello everyone! I would like to ask a simple question.

How can we detect collisions in Python? (as simple as possible) My goal is as follows:

  • Want to check a state validity and a trajectory validity by evaluating self-collision and collision with obstacles.
  • Want to use a PR2 robot.
  • Want to add some simple primitives like a box, a cylinder, etc. to the scene.
  • Want to attach simple primitives to the robot body (assuming that the robot is grasping an object).
  • Not want to implement too much.

I found several solutions as follows, but I want simpler solutions.

  1. Using the /environment_server node through the services.
    • This is slow, and has a problem to detect collisions between cylinders as reported here.
  2. Using MoveIt!.
  3. Using FCL.
    • Only has a C++ interface.

Hence, to the best of my knowledge, I need to implement a python interface to call C++ functions (by using either the Python/C interface or the ROS communication), which sounds a bit complicated.

If anyone can suggest simpler solutions, please tell me.

Thanks in advance.

edit retag flag offensive close merge delete

Comments

As to the MoveIt option lacking a Python interface: you might want to send a message to the moveit-users list about this.

gvdhoorn gravatar imagegvdhoorn ( 2015-02-25 01:37:14 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
0

answered 2015-02-25 13:01:45 -0500

akihiko gravatar image

Good idea. I have created a topic about this.

edit flag offensive delete link more
1

answered 2018-09-12 12:09:03 -0500

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()
edit flag offensive delete link more
0

answered 2018-09-13 01:10:07 -0500

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.

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.
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-02-23 15:50:42 -0500

Seen: 2,259 times

Last updated: Sep 13 '18