FCL Test Case

fcl-test.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 numpy
import openravepy

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

# problem poses (or order qw qx qy qz x y z)
H = lambda pose: openravepy.matrixFromPose(pose)

# load environment obstacle
obstacle = openravepy.RaveCreateKinBody(env,'')
obstacle.SetName('obstacle')
obstacle.InitFromBoxes(numpy.array([[0.0, 0.0, 0.0, 0.193, 0.0075, 0.168125]]),True)
env.Add(obstacle)
obstacle.SetTransform([ 1.,0.,0.,0.,  0.90, 0., 0.75 ])

# load robot, in home config
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
herb_urdf = subprocess.check_output(['catkin_find','herb_description/robots/herb.urdf']).rstrip('\n')
herb_srdf = subprocess.check_output(['catkin_find','herb_description/robots/herb.srdf']).rstrip('\n')
robot = env.GetRobot(or_urdf.SendCommand('load {herb_urdf} {herb_srdf}'.format(**locals())))

# set start config (relaxed home)
armjoints = 'j1 j2 j3 j4 j5 j6 j7 j01 j11 j21 j00'.split()
herbjoints = ['/right/'+x for x in armjoints] + ['/left/'+x for x in armjoints]
robot_onetruedofs = [robot.GetJoint(jname).GetDOFIndex() for jname in herbjoints]
robot.SetActiveDOFs(robot_onetruedofs)
robot.SetActiveDOFValues([
  numpy.pi, 0., 0., 0., 0., 0., 0., 1.3, 1.3, 1.3,0., # right (hand closed)
  0.64,-1.76, 0.26, 1.96, 1.16, 0.87, 1.43, 0.0,0.0,0.0,0.  # left
])

# measure hand frame
H_ee = robot.GetManipulator('right').GetEndEffectorTransform()
p_w_fjoint = robot.GetJoint('/right/j01').GetAnchor()
p_ee_fjoint = numpy.dot(numpy.linalg.inv(H_ee), numpy.append(p_w_fjoint,1))
H_hand_ee = H([ 1., 0., 0., 0., 0., 0., -p_ee_fjoint[2] ])

# load ik for right arm
robot.SetActiveManipulator('right')
robot.SetActiveDOFs(robot.GetActiveManipulator().GetArmIndices())
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
  robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
  ikmodel.autogenerate()

# load box
box_grabbed = openravepy.RaveCreateKinBody(env,'')
box_grabbed.SetName('box_grabbed')
box_grabbed.InitFromBoxes(numpy.array([[0,0,0, 0.071,0.0105,0.102]]),True)
env.Add(box_grabbed)

# grab box
H_box_hand = H([ 0.5, 0.5, 0.5, 0.5, -0.10406809,-0.00289544,-0.00850699 ])
box_grabbed.SetTransform(reduce(numpy.dot,
  [robot.GetActiveManipulator().GetEndEffectorTransform(),
   numpy.linalg.inv(H_hand_ee),
   numpy.linalg.inv(H_box_hand)]))
robot.Grab(box_grabbed)

# ee for goal location
H_boxgoal = H([ 1., 0., 0., 0., 0.94641661, 0.14570830, 0.86834586 ])
H_eegoal = reduce(numpy.dot, [H_boxgoal, H_box_hand, H_hand_ee])

# try this call with different checkers
for checker_name in ['ode', 'fcl']:

  print('doing checking with checker={}'.format(checker_name))
   
  collision_checker = openravepy.RaveCreateCollisionChecker(env, checker_name)
  env.SetCollisionChecker(collision_checker)

  ik_goals = robot.GetActiveManipulator().FindIKSolutions(H_eegoal,
    openravepy.IkFilterOptions.CheckEnvCollisions)

  print('found {} ik_goals'.format(len(ik_goals)))
  if checker_name == 'ode':
    q = ik_goals[0]

# here's one of the configs that my ode says is good but fcl fails ...
robot.SetActiveDOFValues(q)
env.SetViewer('qtcoin')
raw_input('enter to quit ...')