full migration to ros_control, minus head

Author: ClintLiddick

src/herbpy/herbpantilt.py

@@ -95,6 +95,7 @@ def MoveTo(self, target_dof_values, execute=True, **kw_args):
         @param **kw_args keyword arguments passed to \p robot.ExecuteTrajectory
         @return pantilt trajectory
         """
+        raise NotImplementedError('The head is currently disabled under ros_control.')
         # Update the controllers to get new joint values.
         robot = self.GetRobot()
         with robot.GetEnv():

src/herbpy/herbrobot.py

@@ -1,15 +1,19 @@
 PACKAGE = 'herbpy'
 import logging
 import openravepy
+import numbers
+import numpy
 import prpy
 import prpy.rave, prpy.util
 from prpy.base.barretthand import BarrettHand
 from prpy.base.wam import WAM
 from prpy.base.robot import Robot
-from prpy.exceptions import PrPyException
+from prpy.controllers import RewdOrTrajectoryController
+from prpy.exceptions import PrPyException, TrajectoryNotExecutable
 from prpy.planning.base import UnsupportedPlanningError
 from herbbase import HerbBase
 from herbpantilt import HERBPantilt
+from ros_control_client_py import ControllerManagerClient
 
 logger = logging.getLogger('herbpy')
 
@@ -30,6 +34,31 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
 
         Robot.__init__(self, robot_name='herb')
 
+
+        # Controller setup
+        # TODO load controller names from same config as ros_control?
+        self.controller_manager = None  # TODO verify never used in sim
+        self.controllers_always_on = []
+        controllers_manip = []
+
+        # TODO how to deal with sim on controller manager?
+        self.full_controller_sim = (left_arm_sim and right_arm_sim and
+                                    left_ft_sim and right_ft_sim and
+                                    left_hand_sim and right_hand_sim and
+                                    head_sim)
+        if not self.full_controller_sim:
+            # any non-simulation requires ros and the ros_control stack
+            print 'CLINT: something not sim'
+            import rospy
+            if not rospy.core.is_initialized():
+                rospy.init_node('herbpy')
+
+            self.controller_manager = ControllerManagerClient()
+            self.controllers_always_on.append('joint_state_controller')
+        else:
+            print 'CLINT: all sim'
+
+
         # Convenience attributes for accessing self components.
         self.left_arm = self.GetManipulator('left')
         self.right_arm = self.GetManipulator('right')
@@ -38,14 +67,14 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         self.right_arm.hand = self.right_arm.GetEndEffector()
         self.left_hand = self.left_arm.hand
         self.right_hand = self.right_arm.hand
-        self.manipulators = [ self.left_arm, self.right_arm, self.head ]
+        self.manipulators = [self.left_arm, self.right_arm, self.head]
 
         # Dynamically switch to self-specific subclasses.
         prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left')
         prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right')
         prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd')
         prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm,
-                           bhd_namespace='/left', ft_sim=right_ft_sim)
+                           bhd_namespace='/left', ft_sim=left_ft_sim)
         prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm,
                            bhd_namespace='/right', ft_sim=right_ft_sim)
         self.base = HerbBase(sim=segway_sim, robot=self)
@@ -56,7 +85,54 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF()
         accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF()
         self.SetDOFAccelerationLimits(accel_limits)
-        
+
+
+        # Determine always-on controllers
+
+        # hand controllers
+        if not left_hand_sim:
+            self.controllers_always_on.append('left_hand_controller')
+
+        if not right_hand_sim:
+            self.controllers_always_on.append('right_hand_controller')
+
+        # force/torque controllers
+        if not left_ft_sim or not right_ft_sim:
+            self.controllers_always_on.append('force_torque_sensor_controller')
+
+        if not left_ft_sim:
+            self.controllers_always_on.append('left_tare_controller')
+
+        if not right_ft_sim:
+            self.controllers_always_on.append('right_tare_controller')
+
+        # Set default manipulator controllers
+        # NOTE: head is ignored until TODO new Schunk head integrated
+        if not left_arm_sim:
+            controllers_manip.append(
+                'left_gravity_compensation_controller')
+        else:
+            self.left_arm.sim_controller = self.AttachController(name=self.left_arm.GetName(),
+                                                                 args='IdealController',
+                                                                 dof_indices=self.left_arm.GetArmIndices(),
+                                                                 affine_dofs=0,
+                                                                 simulated=True)
+
+        if not right_arm_sim:
+            controllers_manip.append(
+                'right_gravity_compensation_controller')
+        else:
+            self.right_arm.sim_controller = self.AttachController(name=self.right_arm.GetName(),
+                                                                  args='IdealController',
+                                                                  dof_indices=self.right_arm.GetArmIndices(),
+                                                                  affine_dofs=0,
+                                                                  simulated=True)
+
+        # load and activate initial controllers
+        if self.controller_manager is not None:
+            self.controller_manager.request(self.controllers_always_on +
+                                            controllers_manip).switch()
+
         # Support for named configurations.
         import os.path
         self.configurations.add_group('left_arm', self.left_arm.GetArmIndices())
@@ -66,7 +142,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         self.configurations.add_group('right_hand', self.right_hand.GetIndices())
 
         configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml')
-        
+
         try:
             self.configurations.load_yaml(configurations_path)
         except IOError as e:
@@ -78,7 +154,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         for hand in [ self.left_hand, self.right_hand ]:
             hand.configurations = ConfigurationLibrary()
             hand.configurations.add_group('hand', hand.GetIndices())
-            
+
             if isinstance(hand, BarrettHand):
                 hand_configs_path = FindCatkinResource('herbpy', 'config/barrett_preshapes.yaml')
                 try:
@@ -88,7 +164,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
                         hand_configs_path))
             else:
                 logger.warn('Unrecognized hand class. Not loading named configurations.')
-        
+
         # Initialize a default planning pipeline.
         from prpy.planning import (
             FirstSupported,
@@ -140,7 +216,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
         if not (self.trajopt_planner or self.chomp_planner):
             raise PrPyException('Unable to load both CHOMP and TrajOpt. At'
                                 ' least one of these packages is required.')
-        
+
         actual_planner = Sequence(
             # First, try the straight-line trajectory.
             self.snap_planner,
@@ -151,13 +227,13 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             self.trajopt_planner or self.chomp_planner
         )
         self.planner = FirstSupported(
-            Sequence(actual_planner, 
+            Sequence(actual_planner,
                      TSRPlanner(delegate_planner=actual_planner),
                      self.cbirrt_planner),
             # Special purpose meta-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,
@@ -220,14 +296,13 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
             # Initialize herbpy ROS Node
             import rospy
             if not rospy.core.is_initialized():
-                rospy.init_node('herbpy', anonymous=True)
+                rospy.init_node('herbpy')
                 logger.debug('Started ROS node with name "%s".', rospy.get_name())
 
             import talker.msg
             from actionlib import SimpleActionClient
             self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
 
-
     def CloneBindings(self, parent):
         from prpy import Cloned
         super(HERBRobot, self).CloneBindings(parent)
@@ -242,35 +317,179 @@ def CloneBindings(self, parent):
         self.planner = parent.planner
         self.base_planner = parent.base_planner
 
-    def ExecuteTrajectory(self, traj, *args, **kwargs):
-        from prpy.exceptions import TrajectoryAborted
+    def _ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
+                           **kwargs):
+        # Don't execute trajectories that don't have at least one waypoint.
+        if traj.GetNumWaypoints() <= 0:
+            raise ValueError('Trajectory must contain at least one waypoint.')
+
+        # Check if this trajectory contains both affine and joint DOFs
+        cspec = traj.GetConfigurationSpecification()
+        needs_base = prpy.util.HasAffineDOFs(cspec)
+        needs_joints = prpy.util.HasJointDOFs(cspec)
+        if needs_base and needs_joints:
+            raise ValueError('Trajectories with affine and joint DOFs are not supported')
+
+        # Check that the current configuration of the robot matches the
+        # initial configuration specified by the trajectory.
+        if not prpy.util.IsAtTrajectoryStart(self, traj):
+            raise TrajectoryNotExecutable(
+                'Trajectory started from different configuration than robot.')
+
+        # If there was only one waypoint, at this point we are done!
+        if traj.GetNumWaypoints() == 1:
+            if defer is True:
+                import trollius
+                future = trollius.Future()
+                future.set_result(traj)
+                return future
+            else:
+                return traj
+
+        # Verify that the trajectory is timed by checking whether the first
+        # waypoint has a valid deltatime value.
+        if not prpy.util.IsTimedTrajectory(traj):
+            raise ValueError('Trajectory cannot be executed, it is not timed.')
+
+        # Verify that the trajectory has non-zero duration.
+        if traj.GetDuration() <= 0.0:
+            import warnings
+            warnings.warn('Executing zero-length trajectory. Please update the'
+                          ' function that produced this trajectory to return a'
+                          ' single-waypoint trajectory.', FutureWarning)
+
+        traj_manipulators = self.GetTrajectoryManipulators(traj)
+        controllers_manip = []
+        active_controllers = []
+        if self.head in traj_manipulators:
+            # TODO head after Schunk integration
+            import warnings
+            warnings.warn('The head is currently disabled under ros_control.')
+
+        if (self.right_arm in traj_manipulators and
+                self.left_arm in traj_manipulators):
+            controllers_manip.append('dual_arm_trajectory_controller')
+            joints = []
+            joints.extend(self.right_arm.GetJointNames())
+            joints.extend(self.left_arm.GetJointNames())
+            active_controllers.append(
+                RewdOrTrajectoryController(self, '',
+                                           'dual_arm_trajectory_controller',
+                                           joints))
+        else:
+            if self.right_arm in traj_manipulators:
+                if not self.right_arm.IsSimulated():
+                    controllers_manip.append('right_trajectory_controller')
+                    joints = []
+                    joints.extend(self.right_arm.GetJointNames())
+                    active_controllers.append(
+                        RewdOrTrajectoryController(self, '',
+                                                'right_trajectory_controller',
+                                                joints))
+                else:
+                    active_controllers.append(self.right_arm.sim_controller)
+
+            if self.left_arm in traj_manipulators:
+                if not self.left_arm.IsSimulated():
+                    controllers_manip.append('left_trajectory_controller')
+                    joints = []
+                    joints.extend(self.left_arm.GetJointNames())
+                    active_controllers.append(
+                        RewdOrTrajectoryController(self, '',
+                                                'left_trajectory_controller',
+                                                joints))
+                else:
+                    active_controllers.append(self.left_arm.sim_controller)
+
+        # load and activate controllers
+        if not self.full_controller_sim:
+            self.controller_manager.request(self.controllers_always_on +
+                                            controllers_manip).switch()
+
+        if needs_base:
+            if (hasattr(self, 'base') and hasattr(self.base, 'controller') and
+                    self.base.controller is not None):
+                active_controllers.append(self.base.controller)
+            else:
+                logger.warning(
+                    'Trajectory includes the base, but no base controller is'
+                    ' available. Is self.base.controller set?')
 
-        active_manipulators = self.GetTrajectoryManipulators(traj)
+        for controller in active_controllers:
+            controller.SetPath(traj)
 
-        for manipulator in active_manipulators:
-            manipulator.ClearTrajectoryStatus()
+        if defer is True:
+            import time
+            import trollius
 
-        value = super(HERBRobot, self).ExecuteTrajectory(traj, *args, **kwargs)
+            @trollius.coroutine
+            def do_poll():
+                time_stop = time.time() + (timeout if timeout else numpy.inf)
 
-        for manipulator in active_manipulators:
-            status = manipulator.GetTrajectoryStatus()
-            if status == 'aborted':
-                raise TrajectoryAborted()
+                while time.time() <= time_stop:
+                    is_done = all(controller.IsDone()
+                                  for controller in active_controllers)
+                    if is_done:
+                        raise trollius.Return(traj)
+
+                    yield trollius.From(trollius.sleep(period))
+
+                raise trollius.Return(None)
+
+            return trollius.async(do_poll())
+        elif defer is False:
+            prpy.util.WaitForControllers(active_controllers, timeout=timeout)
+            return traj
+        else:
+            raise ValueError('Received unexpected value "{:s}" for defer.'
+                             .format(str(defer)))
+
+    def ExecuteTrajectory(self, traj, *args, **kwargs):
+        # from prpy.exceptions import TrajectoryAborted
+
+        value = self._ExecuteTrajectory(traj, *args, **kwargs)
+
+        # TODO meaningful to do this check here?
+        # if so can be done on value future
+        #
+        # for manipulator in active_manipulators:
+        #     status = manipulator.GetTrajectoryStatus()
+        #     if status == 'aborted':
+        #         raise TrajectoryAborted()
 
         return value
 
     # Inherit docstring from the parent class.
     ExecuteTrajectory.__doc__ = Robot.ExecuteTrajectory.__doc__
 
-    def SetStiffness(self, stiffness):
+    def SetStiffness(self, stiffness, manip=None):
         """Set the stiffness of HERB's arms and head.
-        Zero is gravity compensation, one is position control. Stifness values
-        between zero and one are experimental.
-        @param stiffness value between zero and one
+        Stiffness False/0 is gravity compensation and stiffness True/(>0) is position
+        control.
+        @param stiffness boolean or decimal value 0.0 to 1.0
         """
-        self.head.SetStiffness(stiffness)
-        self.left_arm.SetStiffness(stiffness)
-        self.right_arm.SetStiffness(stiffness)
+        if isinstance(stiffness, numbers.Number) and not (0 <= stiffness <= 1):
+            raise Exception('Stiffness must be boolean or decimal in the range [0, 1];'
+                            'got {}.'.format(stiffness))
+        # TODO head after Schunk integration
+        new_manip_controllers = []
+        if stiffness:
+            if not self.left_arm_sim and (manip is None or manip is self.left_arm):
+                new_manip_controllers.append(
+                    'left_position_controller')
+            if not self.right_arm_sim and (manip is None or manip is self.right_arm):
+                new_manip_controllers.append(
+                    'right_position_controller')
+        else:
+            if not self.left_arm_sim and (manip is None or manip is self.left_arm):
+                new_manip_controllers.append(
+                    'left_gravity_compensation_controller')
+            if not self.right_arm_sim and (manip is None or manip is self.right_arm):
+                new_manip_controllers.append(
+                    'right_gravity_compensation_controller')
+
+            self.controller_manager.request(self.controllers_always_on +
+                                            new_manip_controllers).switch()
 
     def Say(self, words, block=True):
         """Speak 'words' using talker action service or espeak locally in simulation"""