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

Author: psigen

src/prpy/planning/__init__.py

@@ -40,3 +40,4 @@
 from openrave import BiRRTPlanner
 from tsr import TSRPlanner
 from workspace import GreedyIKPlanner
+from vectorfield import VectorFieldPlanner

src/prpy/planning/vectorfield.py

@@ -34,87 +34,220 @@
 import time
 from base import BasePlanner, PlanningError, PlanningMethod
 import prpy.util
+from enum import Enum
 
 logger = logging.getLogger('planning')
 
 
-class VectorfieldPlanner(BasePlanner):
+class Status(Enum):
+    '''
+    TERMINATE - stop, exit gracefully, and return current trajectory
+    CACHE_AND_CONTINUE - save the current trajectory and CONTINUE.
+                         return the saved trajectory if Exception.
+    CONTINUE - keep going
+    '''
+    TERMINATE = -1
+    CACHE_AND_CONTINUE = 0
+    CONTINUE = 1
+
+
+class VectorFieldPlanner(BasePlanner):
     def __init__(self):
-        super(VectorfieldPlanner, self).__init__()
+        super(VectorFieldPlanner, self).__init__()
 
     def __str__(self):
-        return 'VectorfieldPlanner'
-
-    def _geodesictwist(t1, t2):
-        '''
-        Computes the twist in global coordinates that corresponds
-        to the gradient of the geodesic distance between two transforms.
-
-        @param t1 current transform
-        @param t2 goal transform
-        @return twist in se(3)
-        '''
-        trel = numpy.dot(numpy.linalg.inv(t1), t2)
-        trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
-        omega = numpy.dot(t1[0:3, 0:3],
-                          openravepy.axisAngleFromRotationMatrix(
-                            trel[0:3, 0:3]))
-        return numpy.hstack((trans, omega))
+        return 'VectorFieldPlanner'
 
     @PlanningMethod
     def PlanToEndEffectorPose(self, robot, goal_pose, timelimit=5.0,
-                              dq_tol=0.0001, **kw_args):
+                              pose_error_tol=0.01, **kw_args):
         """
         Plan to an end effector pose by following a geodesic loss function
         in SE(3) via an optimized Jacobian.
 
         @param robot
         @param goal_pose desired end-effector pose
         @param timelimit time limit before giving up
-        @param dq_tol velocity tolerance for termination
+        @param pose_error_tol in meters
         @return traj
         """
+        manip = robot.GetActiveManipulator()
 
-        start_time = time.time()
+        def vf_geodesic():
+            twist = prpy.util.GeodesicTwist(manip.GetEndEffectorTransform(),
+                                            goal_pose)
+            dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
+                                robot, twist)
+            # Go as fast as possible
+            dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
 
-        # save all active bodies so we only check collision with those
-        active_bodies = []
-        for body in self.env.GetBodies():
-            if body.IsEnabled():
-                active_bodies.append(body)
-
-        with robot:
-            manip = robot.GetActiveManipulator()
-            robot.SetActiveDOFs(manip.GetArmIndices())
-            qtraj = openravepy.RaveCreateTrajectory(self.env, '')
-            qtraj.Init(manip.GetArmConfigurationSpecification())
-
-            dt = min(robot.GetDOFResolutions()/robot.GetDOFVelocityLimits())
-            while True:
-                # Check for a timeout.
-                current_time = time.time()
-                if (timelimit is not None and
-                        current_time - start_time > timelimit):
-                    raise PlanningError('Reached time limit.')
-
-                q_curr = robot.GetActiveDOFValues()
-                # Check for collisions.
-                for body in active_bodies:
-                    if self.env.CheckCollision(robot, body):
-                        raise PlanningError('Encountered collision.')
-                if robot.CheckSelfCollision():
-                    raise PlanningError('Encountered self-collision.')
+            return dqout
 
-                # Add to trajectory
-                qtraj.Insert(qtraj.GetNumWaypoints(), q_curr)
+        def CloseEnough():
+            pose_error = prpy.util.GeodesicDistance(
+                        manip.GetEndEffectorTransform(),
+                        goal_pose)
+            if pose_error < pose_error_tol:
+                return Status.TERMINATE
+            return Status.CONTINUE
 
-                t_curr = manip.GetEndEffectorTransform()
-                twist = self._geodesictwist(t_curr, goal_pose)
-                dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
-                                robot, twist)
-                if (numpy.linalg.norm(dqout) < dq_tol):
-                    break
-                qnew = q_curr + dqout*dt
-                robot.SetActiveDOFValues(qnew)
+        return self.FollowVectorField(robot, vf_geodesic,
+                                      CloseEnough, timelimit)
+
+    @PlanningMethod
+    def PlanToEndEffectorOffset(self, robot, direction, distance,
+                                max_distance=None, timelimit=5.0,
+                                position_tolerance=0.01,
+                                angular_tolerance=0.15,
+                                **kw_args):
+        """
+        Plan to a desired end-effector offset with move-hand-straight
+        constraint. movement less than distance will return failure. The motion
+        will not move further than max_distance.
+        @param robot
+        @param direction unit vector in the direction of motion
+        @param distance minimum distance in meters
+        @param max_distance maximum distance in meters
+        @param timelimit timeout in seconds
+        @param position_tolerance constraint tolerance in meters
+        @param angular_tolerance constraint tolerance in radians
+        @return traj
+        """
+        if distance < 0:
+            raise ValueError('Distance must be non-negative.')
+        elif numpy.linalg.norm(direction) == 0:
+            raise ValueError('Direction must be non-zero')
+        elif max_distance is not None and max_distance < distance:
+            raise ValueError('Max distance is less than minimum distance.')
+        elif position_tolerance < 0:
+            raise ValueError('Position tolerance must be non-negative.')
+        elif angular_tolerance < 0:
+            raise ValueError('Angular tolerance must be non-negative.')
+
+        # Normalize the direction vector.
+        direction = numpy.array(direction, dtype='float')
+        direction /= numpy.linalg.norm(direction)
+
+        # Default to moving an exact distance.
+        if max_distance is None:
+            max_distance = distance
+
+        manip = robot.GetActiveManipulator()
+        Tstart = manip.GetEndEffectorTransform()
+
+        def vf_straightline():
+            twist = prpy.util.GeodesicTwist(manip.GetEndEffectorTransform(),
+                                            Tstart)
+            twist[0:3] = direction
+            dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
+                    robot, twist)
+
+            # Go as fast as possible
+            dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
+            return dqout
+
+        def TerminateMove():
+            '''
+            Fail if deviation larger than position and angular tolerance.
+            Succeed if distance moved is larger than max_distance.
+            Cache and continue if distance moved is larger than distance.
+            '''
+            Tnow = manip.GetEndEffectorTransform()
+            error = prpy.util.GeodesicError(Tstart, Tnow)
+            if numpy.fabs(error[3]) > angular_tolerance:
+                raise PlanningError('Deviated from orientation constraint.')
+            distance_moved = numpy.dot(error[0:3], direction)
+            position_deviation = numpy.linalg.norm(error[0:3] -
+                                                   distance_moved*direction)
+            if position_deviation > position_tolerance:
+                raise PlanningError('Deviated from straight line constraint.')
+
+            if distance_moved > max_distance:
+                return Status.TERMINATE
+
+            if distance_moved > distance:
+                return Status.CACHE_AND_CONTINUE
+
+            return Status.CONTINUE
+
+        return self.FollowVectorField(robot, vf_straightline,
+                                      TerminateMove, timelimit)
+
+    @PlanningMethod
+    def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
+                          timelimit=5.0, dq_tol=0.0001, **kw_args):
+        """
+        Follow a joint space vectorfield to termination.
+
+        @param robot
+        @param fn_vectorfield a vectorfield of joint velocities
+        @param fn_terminate custom termination condition
+        @param timelimit time limit before giving up
+        @param dq_tol velocity tolerance for termination
+        @param kw_args keyword arguments to be passed to fn_vectorfield
+        @return traj
+        """
+        start_time = time.time()
+
+        try:
+            with robot:
+                manip = robot.GetActiveManipulator()
+                robot.SetActiveDOFs(manip.GetArmIndices())
+                # Populate joint positions and joint velocities
+                cspec = manip.GetArmConfigurationSpecification('quadratic')
+                cspec.AddDerivativeGroups(1, False)
+                cspec.AddDeltaTimeGroup()
+                cspec.ResetGroupOffsets()
+                qtraj = openravepy.RaveCreateTrajectory(self.env,
+                                                        'GenericTrajectory')
+                qtraj.Init(cspec)
+                cached_traj = None
+
+                dqout = robot.GetActiveDOFVelocities()
+                dt = min(robot.GetDOFResolutions() /
+                         robot.GetDOFVelocityLimits())
+                while True:
+                    # Check for a timeout.
+                    current_time = time.time()
+                    if (timelimit is not None and
+                            current_time - start_time > timelimit):
+                        raise PlanningError('Reached time limit.')
+
+                    # Check for collisions.
+                    if self.env.CheckCollision(robot):
+                        raise PlanningError('Encountered collision.')
+                    if robot.CheckSelfCollision():
+                        raise PlanningError('Encountered self-collision.')
+
+                    # Add to trajectory
+                    waypoint = []
+                    q_curr = robot.GetActiveDOFValues()
+                    waypoint.append(q_curr)  # joint position
+                    waypoint.append(dqout)   # joint velocity
+                    waypoint.append([dt])    # delta time
+                    waypoint = numpy.concatenate(waypoint)
+                    qtraj.Insert(qtraj.GetNumWaypoints(), waypoint)
+                    dqout = fn_vectorfield()
+                    if (numpy.linalg.norm(dqout) < dq_tol):
+                        raise PlanningError('Local minimum, \
+                                             unable to progress')
+
+                    status = fn_terminate()
+
+                    if status == Status.CACHE_AND_CONTINUE:
+                        cached_traj = prpy.util.CopyTrajectory(qtraj)
+
+                    if status == Status.TERMINATE:
+                        break
+
+                    qnew = q_curr + dqout*dt
+                    robot.SetActiveDOFValues(qnew)
+
+        except PlanningError as e:
+            if cached_traj is not None:
+                logger.warning('Terminated early: %s', e.message)
+                return cached_traj
+            else:
+                raise
 
         return qtraj

src/prpy/util.py

@@ -493,3 +493,48 @@ def ComputeJointVelocityFromTwist(robot, twist,
     twist_opt = numpy.dot(jacobian, dq_opt)
 
     return dq_opt, twist_opt
+
+
+def GeodesicTwist(t1, t2):
+    '''
+    Computes the twist in global coordinates that corresponds
+    to the gradient of the geodesic distance between two transforms.
+
+    @param t1 current transform
+    @param t2 goal transform
+    @return twist in se(3)
+    '''
+    trel = numpy.dot(numpy.linalg.inv(t1), t2)
+    trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
+    omega = numpy.dot(t1[0:3, 0:3],
+                      openravepy.axisAngleFromRotationMatrix(
+                        trel[0:3, 0:3]))
+    return numpy.hstack((trans, omega))
+
+
+def GeodesicError(t1, t2):
+    '''
+    Computes the error in global coordinates between two transforms
+
+    @param t1 current transform
+    @param t2 goal transform
+    @return a 4-vector of [dx, dy, dz, solid angle]
+    '''
+    trel = numpy.dot(numpy.linalg.inv(t1), t2)
+    trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
+    omega = openravepy.axisAngleFromRotationMatrix(trel[0:3, 0:3])
+    angle = numpy.linalg.norm(omega)
+    return numpy.hstack((trans, angle))
+
+
+def GeodesicDistance(t1, t2, r=1.0):
+    '''
+    Computes the geodesic distance between two transforms
+
+    @param t1 current transform
+    @param t2 goal transform
+    @param r in units of meters/radians converts radians to meters
+    '''
+    error = GeodesicError(t1, t2)
+    error[3] = r*error[3]
+    return numpy.linalg.norm(error)