Test script to reproduce bad Herb sphere locations

gistfile1.py

#!/usr/bin/env python2
import atexit
import openravepy
import orcdchomp.orcdchomp

# create openrave robot
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
e = openravepy.Environment()
atexit.register(e.Destroy)

e.SetViewer('qtcoin')

# load the robot
or_urdf = openravepy.RaveCreateModule(e, 'urdf')
or_urdf.SendCommand('load'
   + ' /home/cdellin/ws-spline/devel/share/herb_description/robots/herb.urdf'
   + ' /home/cdellin/ws-spline/devel/share/herb_description/robots/herb.srdf')
(r,) = e.GetRobots()

# home position
r.SetDOFValues(
   [ 5.5, -1.9, -0.2,  2.4, -1.6,  0.0,  0.8,    0.0, 0.0, 0.0, 0.0,
     0.6, -1.9,  0.2,  2.4, -1.6,  0.0,  0.8,    0.0, 0.0, 0.0, 0.0 ],
   [15,17,19,20,21,22,23, 14,16,18,13,
     4, 6, 8, 9,10,11,12,  3, 5, 7, 2])

# set active stuff
r.SetActiveManipulator('right')
m = r.GetActiveManipulator()
r.SetActiveDOFs(m.GetArmIndices())

# load chomp
m_chomp = openravepy.RaveCreateModule(e, 'orcdchomp')
orcdchomp.orcdchomp.bind(m_chomp)

raw_input('Press [Enter] to view spheres ...')
m_chomp.viewspheres(robot=r)
raw_input('Press [Enter] to clear spheres ...')
with e:
   for b in e.GetBodies():
      if b.GetName().startswith('orcdchomp_sphere_'):
         e.Remove(b)

raw_input('press enter to quit ...')