Enabled baked collision checking by default

src/herbpy/herb.py

@@ -2,11 +2,16 @@
 import os
 import prpy
 import prpy.dependency_manager
+from prpy.collision import (
+    BakedRobotCollisionCheckerFactory,
+    SimpleRobotCollisionCheckerFactory,
+)
 from openravepy import (
     Environment,
     RaveCreateModule,
     RaveCreateCollisionChecker,
     RaveInitialize,
+    openrave_exception,
 )
 from .herbbase import HerbBase
 from .herbrobot import HERBRobot
@@ -28,7 +33,8 @@ def initialize(robot_xml=None, env_path=None, attach_viewer=False,
     env = Environment()
     if env_path is not None:
         if not env.Load(env_path):
-            raise Exception('Unable to load environment frompath %s' % env_path)
+            raise ValueError(
+                'Unable to load environment from path {:s}'.format(env_path))
 
     # Load the URDF file into OpenRAVE.
     urdf_module = RaveCreateModule(env, 'urdf')
@@ -49,6 +55,31 @@ def initialize(robot_xml=None, env_path=None, attach_viewer=False,
         raise ValueError('Unable to find robot with name "{:s}".'.format(
                          herb_name))
 
+    # Default to FCL.
+    collision_checker = RaveCreateCollisionChecker(env, 'fcl')
+    if collision_checker is not None:
+        env.SetCollisionChecker(collision_checker)
+    else:
+        collision_checker = env.GetCollisionChecker()
+        logger.warning(
+            'Failed creating "fcl", defaulting to the default OpenRAVE'
+            ' collision checker. Did you install or_fcl?')
+
+    # Enable baking if it is supported.
+    try:
+        result = collision_checker.SendCommand('BakeGetType')
+        is_baking_suported = (result is not None)
+    except openrave_exception:
+        is_baking_suported = False
+
+    if is_baking_suported:
+        robot_checker_factory = BakedRobotCollisionCheckerFactory()
+    else:
+        robot_checker_factory = SimpleRobotCollisionCheckerFactory()
+        logger.warning(
+            'Collision checker does not support baking. Defaulting to'
+            ' the slower SimpleRobotCollisionCheckerFactory.')
+
     # Default arguments.
     keys = [ 'left_arm_sim', 'left_hand_sim', 'left_ft_sim',
              'right_arm_sim', 'right_hand_sim', 'right_ft_sim',
@@ -57,7 +88,8 @@ def initialize(robot_xml=None, env_path=None, attach_viewer=False,
         if key not in kw_args:
             kw_args[key] = sim
 
-    prpy.bind_subclass(robot, HERBRobot, **kw_args)
+    prpy.bind_subclass(robot, HERBRobot,
+        robot_checker_factory=robot_checker_factory, **kw_args)
 
     if sim:
         dof_indices, dof_values \
@@ -82,13 +114,6 @@ def initialize(robot_xml=None, env_path=None, attach_viewer=False,
             raise Exception('Failed creating viewer of type "{0:s}".'.format(
                             attach_viewer))
 
-    # Default to FCL.
-    collision_checker = RaveCreateCollisionChecker(env, 'fcl')
-    if collision_checker is not None:
-        env.SetCollisionChecker(collision_checker)
-    else:
-        logger.warning('Failed creating "fcl". Did you install or_fcl?')
-
     # Remove the ROS logging handler again. It might have been added when we
     # loaded or_rviz.
     prpy.logger.remove_ros_logger()

src/herbpy/herbrobot.py

@@ -1,18 +1,42 @@
 PACKAGE = 'herbpy'
 import logging
-import openravepy
 import numbers
 import numpy
+import openravepy
 import prpy
-import prpy.rave, prpy.util
+import prpy.rave
+import prpy.util
+import yaml
+import subprocess
+from .barretthand import BarrettHand
+from .herbbase import HerbBase
+from .herbpantilt import HERBPantilt
+from .wam import WAM
+from prpy import Cloned
+from prpy.action import ActionLibrary
 from prpy.base.robot import Robot
 from prpy.controllers import RewdOrTrajectoryController
 from prpy.exceptions import PrPyException, TrajectoryNotExecutable
+from prpy.named_config import ConfigurationLibrary
+from prpy.planning import (
+    CBiRRTPlanner,
+    CHOMPPlanner,
+    FirstSupported,
+    GreedyIKPlanner,
+    IKPlanner,
+    MethodMask,
+    NamedPlanner,
+    Ranked,
+    SBPLPlanner,
+    Sequence,
+    SnapPlanner,
+    TSRPlanner,
+    VectorFieldPlanner,
+)
 from prpy.planning.base import UnsupportedPlanningError
-from barretthand import BarrettHand
-from herbbase import HerbBase
-from herbpantilt import HERBPantilt
-from wam import WAM
+from prpy.planning.retimer import HauserParabolicSmoother
+from prpy.util import FindCatkinResource
+
 
 logger = logging.getLogger('herbpy')
 
@@ -28,11 +52,10 @@ def try_and_warn(fn, exception_type, message, default_value=None):
 class HERBRobot(Robot):
     def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
                        left_hand_sim, right_hand_sim, left_ft_sim,
-                       head_sim, talker_sim, segway_sim, perception_sim):
-        from prpy.util import FindCatkinResource
-
+                       head_sim, talker_sim, segway_sim, perception_sim,
+                       robot_checker_factory):
         Robot.__init__(self, robot_name='herb')
-
+        self.robot_checker_factory = robot_checker_factory
 
         # Controller setup
         self.controller_manager = None
@@ -45,8 +68,11 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         if not self.full_controller_sim:
             # any non-simulation requires ros and the ros_control stack
             import rospy
-            from ros_control_client_py import (ControllerManagerClient,
-                                               JointStateClient)
+            from ros_control_client_py import (
+                ControllerManagerClient,
+                JointStateClient,
+            )
+
             if not rospy.core.is_initialized():
                 raise RuntimeError('rospy not initialized. '
                                    'Must call rospy.init_node()')
@@ -58,7 +84,6 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             self.controller_manager = ControllerManagerClient()
             self.controllers_always_on.append('joint_state_controller')
 
-
         # Convenience attributes for accessing self components.
         self.left_arm = self.GetManipulator('left')
         self.right_arm = self.GetManipulator('right')
@@ -144,7 +169,6 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
                 configurations_path))
 
         # Hand configurations
-        from prpy.named_config import ConfigurationLibrary
         for hand in [ self.left_hand, self.right_hand ]:
             hand.configurations = ConfigurationLibrary()
             hand.configurations.add_group('hand', hand.GetIndices())
@@ -159,48 +183,35 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             else:
                 logger.warning('Unrecognized hand class. Not loading named configurations.')
 
-        # Initialize a default planning pipeline.
-        from prpy.planning import (
-            FirstSupported,
-            MethodMask,
-            Ranked,
-            Sequence,
-        )
-        from prpy.planning import (
-            CBiRRTPlanner,
-            CHOMPPlanner,
-            GreedyIKPlanner,
-            IKPlanner,
-            NamedPlanner,
-            SBPLPlanner,
-            SnapPlanner,
-            TSRPlanner,
-            VectorFieldPlanner
-        )
-
         # TODO: These should be meta-planners.
         self.named_planner = NamedPlanner()
         self.ik_planner = IKPlanner()
 
         # Special-purpose planners.
-        self.snap_planner = SnapPlanner()
-        self.vectorfield_planner = VectorFieldPlanner()
+        self.snap_planner = SnapPlanner(
+            robot_checker_factory=robot_checker_factory)
+        self.vectorfield_planner = VectorFieldPlanner(
+            robot_checker_factory=robot_checker_factory)
+        # TODO: GreedyIKPlanner doesn't support robot_checker_factory.
         self.greedyik_planner = GreedyIKPlanner()
 
         # General-purpose planners.
-        self.cbirrt_planner = CBiRRTPlanner()
+        self.cbirrt_planner = CBiRRTPlanner(
+            robot_checker_factory=robot_checker_factory)
 
         # Trajectory optimizer.
         try:
             from or_trajopt import TrajoptPlanner
-            self.trajopt_planner = TrajoptPlanner()  
+            self.trajopt_planner = TrajoptPlanner(
+                robot_checker_factory=robot_checker_factory)
         except ImportError:
             self.trajopt_planner = None
             logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
                            ' package in your workspace and built?')
 
         try:
-            self.chomp_planner = CHOMPPlanner()
+            self.chomp_planner = CHOMPPlanner(
+                robot_checker_factory=robot_checker_factory)
         except UnsupportedPlanningError:
             self.chomp_planner = None
             logger.warning('Failed loading the CHOMP module. Is the or_cdchomp'
@@ -219,28 +230,25 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             # Next, try a trajectory optimizer.
             self.trajopt_planner or self.chomp_planner
         )
+        tsr_planner = TSRPlanner(
+            delegate_planner=actual_planner,
+            robot_checker_factory=robot_checker_factory)
         self.planner = FirstSupported(
-            Sequence(actual_planner,
-                     TSRPlanner(delegate_planner=actual_planner),
-                     self.cbirrt_planner),
-            # Special purpose meta-planner.
+            Sequence(actual_planner, tsr_planner, self.cbirrt_planner),
             NamedPlanner(delegate_planner=actual_planner),
         )
 
-        from prpy.planning.retimer import HauserParabolicSmoother
         self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
         self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False,
             blend_iterations=5, blend_radius=0.4)
         self.simplifier = None
 
         # Base planning
-        from prpy.util import FindCatkinResource
         planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml')
 
         self.sbpl_planner = SBPLPlanner()
         try:
             with open(planner_parameters_path, 'rb') as config_file:
-                import yaml
                 params_yaml = yaml.load(config_file)
             self.sbpl_planner.SetPlannerParameters(params_yaml)
         except IOError as e:
@@ -250,7 +258,6 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         self.base_planner = self.sbpl_planner
 
         # Create action library
-        from prpy.action import ActionLibrary
         self.actions = ActionLibrary()
 
         # Register default actions and TSRs
@@ -270,9 +277,9 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         else:
             from prpy.perception import ApriltagsModule
             try:
-                kinbody_path = prpy.util.FindCatkinResource('pr_ordata',
+                kinbody_path = FindCatkinResource('pr_ordata',
                                                             'data/objects')
-                marker_data_path = prpy.util.FindCatkinResource('pr_ordata',
+                marker_data_path = FindCatkinResource('pr_ordata',
                                                                 'data/objects/tag_data.json')
                 self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array',
                                                 marker_data_path=marker_data_path,
@@ -298,7 +305,6 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
 
     def CloneBindings(self, parent):
-        from prpy import Cloned
         super(HERBRobot, self).CloneBindings(parent)
         self.left_arm = Cloned(parent.left_arm)
         self.right_arm = Cloned(parent.right_arm)
@@ -472,7 +478,6 @@ def SetStiffness(self, stiffness, manip=None):
     def Say(self, words, block=True):
         """Speak 'words' using talker action service or espeak locally in simulation"""
         if self.talker_simulated:
-            import subprocess
             try:
                 proc = subprocess.Popen(['espeak', '-s', '160', '"{0}"'.format(words)])
                 if block: