Merge branch 'master' of https://github.com/personalrobotics/prpy into feature/bodypoint_acceleration_helper_functions

Conflicts:
	src/prpy/util.py

Author: psigen

.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/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

@@ -340,7 +340,13 @@ def do_postprocess():
                             cloned_robot, shortcut_path, defer=False,
                             **smoothing_options)
 
-                return CopyTrajectory(traj, env=self.GetEnv())
+                # Copy the trajectory into the output environment.
+                output_traj = CopyTrajectory(traj, env=self.GetEnv()) 
+
+                # Copy meta-data from the path to the output trajectory.
+                output_traj.SetDescription(path.GetDescription())
+
+                return output_traj
 
         if defer is True:
             from trollius.executor import get_default_executor

src/prpy/base/wam.py

@@ -28,13 +28,17 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
+import logging
 import numpy
 import openravepy
+import warnings
 from manipulator import Manipulator
 from prpy.clone import Clone
 from .. import util
 from .. import exceptions
 
+logger = logging.getLogger('wam')
+
 class WAM(Manipulator):
     def __init__(self, sim, owd_namespace,
                  iktype=openravepy.IkParameterization.Type.Transform6D):
@@ -55,7 +59,7 @@ def __init__(self, sim, owd_namespace,
         if sim:
             from prpy.simulation import ServoSimulator
             self.servo_simulator = ServoSimulator(
-                    self, rate=20, watchdog_timeout=0.1)
+                self, rate=20, watchdog_timeout=0.1)
 
     def CloneBindings(self, parent):
         Manipulator.CloneBindings(self, parent)
@@ -90,6 +94,30 @@ def SetStiffness(manipulator, stiffness):
         if not manipulator.simulated:
             manipulator.controller.SendCommand('SetStiffness {0:f}'.format(stiffness))
 
+    def SetTrajectoryExecutionOptions(self, traj, stop_on_stall=False,
+            stop_on_ft=False, force_magnitude=None, force_direction=None,
+            torque=None):
+        """Set OWD's trajectory execution options on trajectory.
+        @param stop_on_stall aborts the trajectory if the arm stalls
+        @param stop_on_ft aborts the trajectory if the force/torque
+                          sensor reports a force or torque that exceeds
+                          threshold specified by the force_magnitude,
+                          force_direction, and torque options
+        @param force_magnitude force threshold value, in Newtons
+        @param force_direction unit vector in the force/torque coordinate
+                               frame, which is int he same orientation as the
+                               hand frame.
+        @param torque vector of the three torques
+        """
+        util.SetTrajectoryTags(traj, {
+            'owd_options': {
+                'stop_on_stall': bool(stop_on_stall),
+                'stop_on_ft': bool(stop_on_ft),
+                'force_magnitude': float(force_magnitude),
+                'force_direction': list(force_direction),
+                'torque': list(torque),
+        }}, append=True)
+
     def Servo(manipulator, velocities):
         """Servo with a vector of instantaneous joint velocities.
         @param velocities joint velocities, in radians per second
@@ -137,7 +165,7 @@ def ServoTo(manipulator, target, duration, timeStep=0.05, collisionChecking=True
         else:
             return False
 
-    def GetVelocityLimits(self, openrave=True, owd=True):
+    def GetVelocityLimits(self, openrave=None, owd=None):
         """Get the OpenRAVE and OWD joint velocity limits.
         This function checks both the OpenRAVE and OWD joint velocity limits.
         If they do not match, a warning is printed and the minimum value is
@@ -146,37 +174,13 @@ def GetVelocityLimits(self, openrave=True, owd=True):
         @param owd flag to set the OWD velocity limits
         @return list of velocity limits, in radians per second
         """
-        # Update the OpenRAVE limits.
-        if openrave:
-            or_velocity_limits = Manipulator.GetVelocityLimits(self)
-            if self.simulated or not owd:
-                return or_velocity_limits
-
-        # Update the OWD limits.
-        if owd and not self.simulated:
-            args  = [ 'GetSpeed' ]
-            args_str = ' '.join(args)
-            owd_speed_limits_all = self.controller.SendCommand(args_str)
-            #if we get nothing back, e.g. if the arm isn't running, return openrave lims
-            if owd_speed_limits_all is None:
-                return or_velocity_limits
-
-            owd_speed_limits = map(float, owd_speed_limits_all.split(','));
-            #first 7 numbers are velocity limits
-            owd_velocity_limits = numpy.array(owd_speed_limits[0:len(self.GetIndices())])
-
-            diff_arr = numpy.subtract(or_velocity_limits, owd_velocity_limits)
-            max_diff = max(abs(diff_arr))
-
-            if max_diff > 0.01:
-                # TODO: Change this to use the logging framework.
-                print('GetVelocityLimits Error: openrave and owd limits very different')
-                print('\tOpenrave limits:\t' + str(or_velocity_limits))
-                print('\tOWD limits:\t\t' + str(owd_velocity_limits))
+        if openrave is not None or owd is not None:
+            warnings.warn(
+                'The "openrave" and "owd" flags are deprecated in'
+                ' GetVelocityLimits and will be removed in a future version.',
+                DeprecationWarning)
 
-            return numpy.minimum(or_velocity_limits, owd_velocity_limits)
-
-        return or_velocity_limits
+        return Manipulator.GetVelocityLimits(self)
 
     def SetVelocityLimits(self, velocity_limits, min_accel_time,
                           openrave=True, owd=True):
@@ -218,106 +222,130 @@ def ClearTrajectoryStatus(manipulator):
         if not manipulator.simulated:
             manipulator.controller.SendCommand('ClearStatus')
 
-    def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
+    def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
                        max_force=5.0, max_torque=None, ignore_collisions=None, **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
         end-effector moves at least distance. In simulation, a move-until-touch
         action proceeds until the end-effector collids with the environment.
+
         @param direction unit vector for the direction of motion in the world frame
         @param distance minimum distance in meters
         @param max_distance maximum distance in meters
         @param max_force maximum force in Newtons
         @param max_torque maximum torque in Newton-Meters
-        @param execute optionally execute the trajectory
-        @param ignore_collisions collisions with these objects are ignored in simulation
+        @param ignore_collisions collisions with these objects are ignored when planning the path, e.g. the object you think you will touch
         @param **kw_args planner parameters
         @return felt_force flag indicating whether we felt a force.
         """
-        # TODO: Is ignore_collisions a list of names or KinBody pointers?
+        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)
+
         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()
+            # 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)
+
+            with robot_saver, nested(*body_savers) as f:
                 manipulator.SetActive()
-                traj = manipulator.PlanToEndEffectorOffset(direction, distance, max_distance=max_distance,
-                                                           execute=False, **kw_args)
-
-	collided_with_obj = False
-        try:
-            if not manipulator.simulated:
-                manipulator.hand.TareForceTorqueSensor()
-                #manipulator.GetRobot().ExecuteTrajectory(traj, execute=True, retime=True, blend=False) 
-                manipulator.GetRobot().ExecuteTrajectory(traj, execute=True, retime=True, blend=True, stop_on_ft=True, 
-								force_direction=force_direction, force_magnitude=max_force, torque=max_torque)
-                for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
-                    ignore_col_with.Enable(oldstate)
-            else:
-                traj = manipulator.GetRobot().BlendTrajectory(traj)
-                traj = manipulator.GetRobot().RetimeTrajectory(traj, stop_on_ft=True, force_direction=force_direction,
-                                                               force_magnitude=max_force, torque=max_torque)
-                traj_duration = traj.GetDuration()
-                delta_t = 0.01
-
-                traj_config_spec = traj.GetConfigurationSpecification()
-                traj_angle_group = traj_config_spec.GetGroupFromName('joint_values')
-                path_config_spec = openravepy.ConfigurationSpecification()
-#                path_config_spec.AddDeltaTimeGroup()
-                path_config_spec.AddGroup(traj_angle_group.name, traj_angle_group.dof, '')
-#                path_config_spec.AddDerivativeGroups(1, False);
-#                path_config_spec.AddDerivativeGroups(2, False);
-#                path_config_spec.AddGroup('owd_blend_radius', 1, 'next')
-                path_config_spec.ResetGroupOffsets()
-
-                new_traj = openravepy.RaveCreateTrajectory(manipulator.GetRobot().GetEnv(), '')
-                new_traj.Init(path_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)
-                        #if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot()):
-                        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:
-                            #waypoint = numpy.append(waypoint,t)
-                            new_traj.Insert(int(waypoint_ind), waypoint, path_config_spec)
-                            waypoint_ind += 1
-
-                    #new_traj = manipulator.GetRobot().BlendTrajectory(new_traj)
-                    #new_traj = manipulator.GetRobot().RetimeTrajectory(new_traj)
-                manipulator.GetRobot().ExecuteTrajectory(new_traj, execute = True, retime=True, blend=True)
-
-            return collided_with_obj
-        # Trajectory is aborted by OWD because we felt a force.
-        except exceptions.TrajectoryAborted:
-            return True
+                robot_cspec = robot.GetActiveConfigurationSpecification()
+
+                for body in ignore_collisions:
+                    body.Enable(False)
+
+                path = robot.PlanToEndEffectorOffset(direction=direction,
+                    distance=distance, max_distance=max_distance, **kw_args)
+
+        # 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:
+                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/logger.py

@@ -28,7 +28,7 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import collections, logging, sys
+import collections, logging, sys, warnings
 
 class ColoredFormatter(logging.Formatter):
     def __init__(self, default):
@@ -80,6 +80,9 @@ def initialize_logging():
         spammy_logger = logging.getLogger(spammy_logger_name)
         spammy_logger.setLevel(logging.WARNING)
 
+    # Enable deprecation warnings, which are off by default in Python 2.7.
+    warnings.simplefilter('default')
+
     return base_logger
 
 def remove_ros_logger():