Merge branch 'master' into bugfix/issue153

Author: jeking04

.gitignore

@@ -1 +1,7 @@
 *.pyc
+
+# Generated by nosetest
+*.noseids
+
+# Generated by CBiRRT
+**/cmovetraj.txt

catkin.cmake

@@ -5,6 +5,5 @@ catkin_package()
 catkin_python_setup()
 
 if (CATKIN_ENABLE_TESTING)
-    #catkin_add_nosetests(tests/test_PlanningPipeline.py)
-    #catkin_add_nosetests(tests/test_DistanceFieldManager.py)
+    catkin_add_nosetests(tests)
 endif()

src/prpy/base/barretthand.py

@@ -239,8 +239,6 @@ def TareForceTorqueSensor(hand):
         """
         if not hand.ft_simulated:
             hand.ft_sensor.SendCommand('Tare')
-            # TODO: Do we still need to wait this long?
-            time.sleep(2)
 
     def _GetJointFromName(self, name):
         robot = self.manipulator.GetRobot()

src/prpy/base/manipulator.py

@@ -181,7 +181,7 @@ def _PlanWrapper(self, planning_method, args, kw_args):
             traj = CopyTrajectory(cloned_traj, env=robot.GetEnv())
 
         # Optionally execute the trajectory.
-        if 'execute' not in kw_args or kw_args['execute']:
+        if kw_args.get('execute', False):
             return self.GetRobot().ExecutePath(traj, **kw_args)
         else:
             return traj

src/prpy/base/mico.py

@@ -45,8 +45,12 @@ def __init__(self, sim,
         with env:
             dof_indices = self.GetIndices()
             accel_limits = robot.GetDOFAccelerationLimits()
-            accel_limits[dof_indices[0:3]] = 1.2
-            accel_limits[dof_indices[3:6]] = 1.0
+            accel_limits[dof_indices[0]] = 1.45
+            accel_limits[dof_indices[1]] = 1.56
+            accel_limits[dof_indices[2]] = 1.56
+            accel_limits[dof_indices[3]] = 1.5
+            accel_limits[dof_indices[4]] = 1.48
+            accel_limits[dof_indices[5]] = 1.49
             robot.SetDOFAccelerationLimits(accel_limits)
 
         # Load or_nlopt_ik as the IK solver. Unfortunately, IKFast doesn't work

src/prpy/base/robot.py

@@ -88,6 +88,8 @@ def __dir__(self):
             method_names.update(self.planner.get_planning_method_names())
         if hasattr(self, 'actions') and self.actions is not None:
             method_names.update(self.actions.get_actions())
+        if hasattr(self, 'detector') and self.detector is not None:
+            method_names.update(self.detector.get_perception_method_names())
 
         return list(method_names)
 
@@ -113,6 +115,14 @@ def wrapper_method(*args, **kw_args):
             def wrapper_method(*args, **kw_args):
                 return delegate_method(self, *args, **kw_args)
             return wrapper_method
+        elif (hasattr(self, 'detector') and self.detector is not None
+              and self.detector.has_perception_method(name)):
+            
+            delegate_method = getattr(self.detector, name)
+            @functools.wraps(delegate_method)
+            def wrapper_method(*args, **kw_args):
+                return delegate_method(self, *args, **kw_args)
+            return wrapper_method
 
         raise AttributeError('{0:s} is missing method "{1:s}".'.format(repr(self), name))
 
@@ -205,8 +215,8 @@ def PostProcessPath(self, path, defer=False, executor=None,
            curve through the waypoints (not implemented). If this curve is
            not collision free, then we fall back on...
         4. By default, we run a smoother that jointly times and smooths the
-           path via self.smoother. This algorithm can change the geometric path to
-           optimize runtime.
+           path via self.smoother. This algorithm can change the geometric path
+           to optimize runtime.
 
         The behavior in (2) and (3) can be forced by passing constrained=True
         or smooth=True. By default, the case is inferred by the tag(s) attached
@@ -411,8 +421,8 @@ def do_execute():
         else:
             raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
 
-
-    def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwargs):
+    def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
+                          **kwargs):
         """ Executes a time trajectory on the robot.
 
         This function directly executes a timed OpenRAVE trajectory on the
@@ -457,7 +467,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
         # Check that the current configuration of the robot matches the
         # initial configuration specified by the trajectory.
         if not util.IsAtTrajectoryStart(self, traj):
-            raise exceptions.TrajectoryAborted(
+            raise exceptions.TrajectoryNotExecutable(
                 'Trajectory started from different configuration than robot.')
 
         # If there was only one waypoint, at this point we are done!

src/prpy/base/wam.py

@@ -157,6 +157,7 @@ def ServoTo(manipulator, target, duration, timeStep=0.05, collisionChecking=True
 
         if not inCollision:
             for i in range(1,steps):
+                import time
                 manipulator.Servo(velocities)
                 time.sleep(timeStep)
             manipulator.Servo([0] * len(manipulator.GetArmIndices()))
@@ -223,7 +224,8 @@ def ClearTrajectoryStatus(manipulator):
             manipulator.controller.SendCommand('ClearStatus')
 
     def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
-                       max_force=5.0, max_torque=None, ignore_collisions=None, **kw_args):
+                       max_force=5.0, max_torque=None, ignore_collisions=None,
+                       velocity_limit_scale=0.25, **kw_args):
         """Execute a straight move-until-touch action.
         This action stops when a sufficient force is is felt or the manipulator
         moves the maximum distance. The motion is considered successful if the
@@ -235,100 +237,123 @@ def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
         @param max_distance maximum distance in meters
         @param max_force maximum force in Newtons
         @param max_torque maximum torque in Newton-Meters
-        @param ignore_collisions collisions with these objects are ignored when planning the path, e.g. the object you think you will touch
+        @param ignore_collisions collisions with these objects are ignored when 
+        planning the path, e.g. the object you think you will touch
+        @param velocity_limit_scale A multiplier to use to scale velocity limits 
+        when executing MoveUntilTouch ( < 1 in most cases).           
         @param **kw_args planner parameters
         @return felt_force flag indicating whether we felt a force.
         """
+        from contextlib import nested
+        from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
+        from ..planning.exceptions import CollisionPlanningError
 
+        delta_t = 0.01
+
+        robot = manipulator.GetRobot()
+        env = robot.GetEnv()
+        dof_indices = manipulator.GetArmIndices()
+
+        direction = numpy.array(direction, dtype='float')
+
+        # Default argument values.
         if max_distance is None:
             max_distance = 1.
             warnings.warn(
                 'MoveUntilTouch now requires the "max_distance" argument.'
                 ' This will be an error in the future.',
                 DeprecationWarning)
 
-        # TODO: Is ignore_collisions a list of names or KinBody pointers?
         if max_torque is None:
-            max_torque = numpy.array([100.0, 100.0, 100.0 ])
-        
-        ignore_col_obj_oldstate = []
+            max_torque = numpy.array([100.0, 100.0, 100.0])
+
         if ignore_collisions is None:
             ignore_collisions = []
 
-        for ignore_col_with in ignore_collisions:
-            ignore_col_obj_oldstate.append(ignore_col_with.IsEnabled())
-            ignore_col_with.Enable(False)
-
-        with manipulator.GetRobot().GetEnv():
-            manipulator.GetRobot().GetController().SimulationStep(0)
-
+        with env:
             # Compute the expected force direction in the hand frame.
-            direction = numpy.array(direction)
             hand_pose = manipulator.GetEndEffectorTransform()
             force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
 
-            with manipulator.GetRobot():
-                old_active_manipulator = manipulator.GetRobot().GetActiveManipulator()
-                manipulator.SetActive()
-                traj = manipulator.PlanToEndEffectorOffset(direction, distance, max_distance=max_distance,
-                                                           execute=False, **kw_args)
+            # Disable the KinBodies listed in ignore_collisions. We backup the
+            # "enabled" state of all KinBodies so we can restore them later.
+            body_savers = [
+                body.CreateKinBodyStateSaver() for body in ignore_collisions]
+            robot_saver = robot.CreateRobotStateSaver(
+                  Robot.SaveParameters.ActiveDOF
+                | Robot.SaveParameters.ActiveManipulator
+                | Robot.SaveParameters.LinkTransformation)
 
-                collided_with_obj = False
-        try:
-            if not manipulator.simulated:
-                manipulator.SetTrajectoryExecutionOptions(traj, stop_on_ft=True,
-                    force_direction=force_direction, force_magnitude=max_force,
-                    torque=max_torque)
+            with robot_saver, nested(*body_savers) as f:
+                manipulator.SetActive()
+                robot_cspec = robot.GetActiveConfigurationSpecification()
 
-                manipulator.hand.TareForceTorqueSensor()
-                manipulator.GetRobot().ExecutePath(traj)
+                for body in ignore_collisions:
+                    body.Enable(False)
 
-                for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
-                    ignore_col_with.Enable(oldstate)
-            else:
+                path = robot.PlanToEndEffectorOffset(direction=direction,
+                    distance=distance, max_distance=max_distance, **kw_args)
 
-                traj = manipulator.GetRobot().PostProcessPath(traj)
-
-                traj_duration = traj.GetDuration()
-                delta_t = 0.01
-
-                traj_config_spec = traj.GetConfigurationSpecification()
-                new_traj = openravepy.RaveCreateTrajectory(manipulator.GetRobot().GetEnv(), '')
-                new_traj.Init(traj_config_spec)
-
-                for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
-                    ignore_col_with.Enable(oldstate)
-                
-                with manipulator.GetRobot():
-                    manipulator.SetActive()
-                    waypoint_ind = 0
-                    for t in numpy.arange(0, traj_duration, delta_t):
-                        traj_sample = traj.Sample(t)
-
-                        waypoint = traj_config_spec.ExtractJointValues(traj_sample, manipulator.GetRobot(), manipulator.GetArmIndices())
-                        manipulator.SetDOFValues(waypoint)
-
-                        # Check collision with each body on the robot
-                        for body in manipulator.GetRobot().GetEnv().GetBodies():
-                            if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot(), body):
-                                collided_with_obj = True
-                                break
-                        if collided_with_obj:
-                            break
-                        else:
-                            #set timing on new sampled waypoint
-                            if waypoint_ind == 0:
-                                traj_config_spec.InsertDeltaTime(traj_sample, 0.)
-                            else:
-                                traj_config_spec.InsertDeltaTime(traj_sample, delta_t)
-                            
-                            new_traj.Insert(int(waypoint_ind), traj_sample)
-                            waypoint_ind += 1
-
-
-                manipulator.GetRobot().ExecuteTrajectory(new_traj)
-
-            return collided_with_obj
-        # Trajectory is aborted by OWD because we felt a force.
-        except exceptions.TrajectoryAborted:
-            return True
+        # Execute on the real robot by tagging the trajectory with options that
+        # tell the controller to stop on force/torque input.
+        if not manipulator.simulated:
+            manipulator.SetTrajectoryExecutionOptions(path, stop_on_ft=True,
+                force_direction=force_direction, force_magnitude=max_force,
+                torque=max_torque)
+
+            manipulator.hand.TareForceTorqueSensor()
+
+            try:
+                with robot.CreateRobotStateSaver(Robot.SaveParameters.JointMaxVelocityAndAcceleration):
+                    vl = robot.GetDOFVelocityLimits()
+                    manipulator.SetVelocityLimits(velocity_limit_scale*vl, 0.5)
+                    robot.ExecutePath(path)
+                return False
+            except exceptions.TrajectoryAborted as e:
+                logger.warn('MoveUntilTouch aborted: %s', str(e))
+                return True
+        # Forward-simulate the motion until it hits an object.
+        else:
+            traj = robot.PostProcessPath(path)
+            is_collision = False
+
+            traj_cspec = traj.GetConfigurationSpecification()
+            new_traj = RaveCreateTrajectory(env, '')
+            new_traj.Init(traj_cspec)
+
+            robot_saver = robot.CreateRobotStateSaver(
+                Robot.SaveParameters.LinkTransformation)
+            
+            with env, robot_saver:
+                for t in numpy.arange(0, traj.GetDuration(), delta_t):
+                    waypoint = traj.Sample(t)
+
+                    dof_values = robot_cspec.ExtractJointValues(
+                        waypoint, robot, dof_indices, 0)
+                    manipulator.SetDOFValues(dof_values)
+
+                    # Terminate if we detect collision with the environment.
+                    report = CollisionReport()
+                    if env.CheckCollision(robot, report=report):
+                        logger.info('Terminated from collision: %s',
+                            str(CollisionPlanningError.FromReport(report)))
+                        is_collision = True
+                        break
+                    elif robot.CheckSelfCollision(report=report):
+                        logger.info('Terminated from self-collision: %s',
+                            str(CollisionPlanningError.FromReport(report)))
+                        is_collision = True
+                        break
+
+                    # Build the output trajectory that stops in contact.
+                    if new_traj.GetNumWaypoints() == 0:
+                        traj_cspec.InsertDeltaTime(waypoint, 0.)
+                    else:
+                        traj_cspec.InsertDeltaTime(waypoint, delta_t)
+                    
+                    new_traj.Insert(new_traj.GetNumWaypoints(), waypoint)
+
+            if new_traj.GetNumWaypoints() > 0:
+                robot.ExecuteTrajectory(new_traj)
+
+            return is_collision

src/prpy/exceptions.py

@@ -3,26 +3,50 @@ class PrPyException(Exception):
     Generic PrPy exception.
     """
 
-class TrajectoryAborted(PrPyException):
+
+class TrajectoryException(PrPyException):
+    """
+    Trajectory failed to execute.
+    """
+
+
+class TrajectoryNotExecutable(TrajectoryException):
+    """
+    Trajectory could not begin execution.
+
+    This exception typically indicates that some precondition of trajectory
+    execution was violated, such as the robot starting at a different
+    configuration, the trajectory not being in the correct format.
+
+    This exception indicates that the trajectory was not even attempted due to
+    one of these conditions.
+    """
+
+
+class TrajectoryAborted(TrajectoryException):
     """
     Trajectory was aborted.
     """
 
-class TrajectoryStalled(PrPyException):
+
+class TrajectoryStalled(TrajectoryException):
     """
     Trajectory stalled.
     """
 
+
 class SynchronizationException(PrPyException):
     """
     Controller synchronization failed.
     """
 
+
 class SerializationException(PrPyException):
     """
     Serialization failed.
     """
 
+
 class UnsupportedTypeSerializationException(SerializationException):
     """
     Serialization failed due to an unknown type.
@@ -35,6 +59,7 @@ def __init__(self, value):
             'Serializing type "{:s}.{:s}" is not supported.'.format(
                 self.type.__module__, self.type.__name__))
 
+
 class UnsupportedTypeDeserializationException(SerializationException):
     """
     Deserialization failed due to an unknown type.