ODE HERB bug test case

ode-herb-bug.py

#!/usr/bin/env python2
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import absolute_import
from __future__ import division
import atexit
import subprocess
import openravepy

# what resource_retriever.get_filename(url, use_protocol=False) should be!
def rr_get_filename(url):
   PACKAGE_PREFIX = 'package://'
   if not url.startswith(PACKAGE_PREFIX):
      return url
   mod_url = url[len(PACKAGE_PREFIX):]
   return subprocess.check_output(['catkin_find',mod_url]).rstrip('\n')

openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
env = openravepy.Environment()
atexit.register(env.Destroy)

env.SetCollisionChecker(openravepy.RaveCreateCollisionChecker(env,'ode'))

or_urdf = openravepy.RaveCreateModule(env, 'urdf')
fn_urdf = rr_get_filename('package://herb_description/robots/herb.urdf')
fn_srdf = rr_get_filename('package://herb_description/robots/herb.srdf')
robot = env.GetRobot(or_urdf.SendCommand('load {} {}'.format(fn_urdf,fn_srdf)))

for link in robot.GetLinks():
   if link.GetName() not in ['/right/wam3','/right/wam5']:
      link.Enable(False)

config = [4.2735537841327362, 0.4740221519094111, -2.4119405981641928, 1.9935127248613886, -1.7596538407270064, -0.14357628170248468, -1.4142931202799787]

robot.SetActiveManipulator('right')
robot.SetActiveDOFs(robot.GetActiveManipulator().GetArmIndices())
robot.SetActiveDOFValues(config)

with env:
   links_colliding = env.CheckCollision(robot.GetLink('/right/wam3'), robot.GetLink('/right/wam5'))
print('links_colliding (should be False):', links_colliding)

Here's the associated issue: personalrobotics/benchmarks#7