示例#1
0
    def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
                          integration_time_interval=10.0, timelimit=5.0,
                          sampling_func=util.SampleTimeGenerator,
                          norm_order=2, **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 integration_time_interval The time interval to integrate
                                         over.
        @param timelimit time limit before giving up
        @param sampling_func sample generator to compute validity checks
           Note: Function will terminate as soon as invalid configuration is 
                 encountered. No more samples will be requested from the 
                 sampling_func after this occurs.
        @param norm_order order of norm to use for collision checking
        @param kw_args keyword arguments to be passed to fn_vectorfield
        @return traj
        """
        from .exceptions import (
            CollisionPlanningError,
            SelfCollisionPlanningError,
        )
        from openravepy import CollisionReport, RaveCreateTrajectory
        from ..util import GetLinearCollisionCheckPts
        import time
        import scipy.integrate

        CheckLimitsAction = openravepy.KinBody.CheckLimitsAction

        # This is a workaround to emulate 'nonlocal' in Python 2.
        nonlocals = {
            'exception': None,
            't_cache': None,
            # Must be negative so we check the start of the trajectory.
            't_check': -1.,
        }

        env = robot.GetEnv()
        active_indices = robot.GetActiveDOFIndices()

        # Create a new trajectory matching the current
        # robot's joint configuration specification
        cspec = robot.GetActiveConfigurationSpecification('linear')
        cspec.AddDeltaTimeGroup()
        cspec.ResetGroupOffsets()
        path = RaveCreateTrajectory(env, '')
        path.Init(cspec)

        time_start = time.time()

        def fn_wrapper(t, q):
            """
            The integrator will try to solve this equation
            at each time step.
            Note: t is the integration time and is non-monotonic.
            """
            # Set the joint values, without checking the joint limits
            robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing)

            return fn_vectorfield()

        def fn_status_callback(t, q):
            """
            Check joint-limits and collisions for a specific joint
            configuration. This is called multiple times at DOF
            resolution in order to check along the entire length of the
            trajectory.
            Note: This is called by fn_callback, which is currently
            called after each integration time step, which means we are
            doing more checks than required.
            """
            if time.time() - time_start >= timelimit:
                raise TimeLimitError()

            # Check joint position limits.
            # We do this before setting the joint angles.
            util.CheckJointLimits(robot, q)

            robot.SetActiveDOFValues(q)

            # Check collision (throws an exception on collision)
            robot_checker.VerifyCollisionFree()

            # Check the termination condition.
            status = fn_terminate()

            if Status.DoesCache(status):
                nonlocals['t_cache'] = t

            if Status.DoesTerminate(status):
                raise TerminationError()

        def fn_callback(t, q):
            """
            This is called at every successful integration step.
            """
            try:
                # Add the waypoint to the trajectory.
                waypoint = numpy.zeros(cspec.GetDOF())
                cspec.InsertDeltaTime(waypoint, t - path.GetDuration())
                cspec.InsertJointValues(waypoint, q, robot, active_indices, 0)
                path.Insert(path.GetNumWaypoints(), waypoint)

                # Run constraint checks at DOF resolution.
                if path.GetNumWaypoints() == 1:
                    checks = [(t, q)]
                else:
                    # This returns collision checks for the entire trajectory,
                    # including the part that has already been checked. These
                    # duplicate checks will be filtered out below.
                    checks = GetLinearCollisionCheckPts(robot, path,
                                                        norm_order=norm_order,
                                                        sampling_func=sampling_func)

                for t_check, q_check in checks:
                    # TODO: It would be more efficient to only generate checks
                    # for the new part of the trajectory.
                    if t_check <= nonlocals['t_check']:
                        continue

                    # Record the time of this check so we continue checking
                    # from where we left off. We do this first, just in case
                    # fn_status_callback raises an exception.
                    nonlocals['t_check'] = t_check

                    fn_status_callback(t_check, q_check)

                return 0  # Keep going.
            except PlanningError as e:
                nonlocals['exception'] = e
                return -1  # Stop.

        with self.robot_checker_factory(robot) as robot_checker, \
            robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
            # Integrate the vector field to get a configuration space path.
            #
            # TODO: Tune the integrator parameters.
            #
            # Integrator: 'dopri5'
            # DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs.
            # It is a member of the Runge-Kutta family of solvers.
            integrator = scipy.integrate.ode(f=fn_wrapper)
            integrator.set_integrator(name='dopri5',
                                      first_step=0.1,
                                      atol=1e-3,
                                      rtol=1e-3)
            # Set function to be called at every successful integration step.
            integrator.set_solout(fn_callback)
            integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)

            integrator.integrate(t=integration_time_interval)

        t_cache = nonlocals['t_cache']
        exception = nonlocals['exception']

        if t_cache is None:
            raise exception or PlanningError(
                'An unknown error has occurred.', deterministic=True)
        elif exception:
            logger.warning('Terminated early: %s', str(exception))

        # Remove any parts of the trajectory that are not cached. This also
        # strips the (potentially infeasible) timing information.
        output_cspec = robot.GetActiveConfigurationSpecification('linear')
        output_path = RaveCreateTrajectory(env, '')
        output_path.Init(output_cspec)

        # Add all waypoints before the last integration step. GetWaypoints does
        # not include the upper bound, so this is safe.
        cached_index = path.GetFirstWaypointIndexAfterTime(t_cache)
        output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec)

        # Add a segment for the feasible part of the last integration step.
        output_path.Insert(output_path.GetNumWaypoints(),
                           path.Sample(t_cache),
                           cspec)

        util.SetTrajectoryTags(output_path, {
            Tags.SMOOTH: True,
            Tags.CONSTRAINED: True,
            Tags.DETERMINISTIC_TRAJECTORY: True,
            Tags.DETERMINISTIC_ENDPOINT: True,
        }, append=True)

        return output_path
示例#2
0
文件: vectorfield.py 项目: beiju/prpy
    def FollowVectorField(self,
                          robot,
                          fn_vectorfield,
                          fn_terminate,
                          integration_time_interval=10.0,
                          timelimit=5.0,
                          **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 integration_time_interval The time interval to integrate
                                         over.
        @param timelimit time limit before giving up
        @param kw_args keyword arguments to be passed to fn_vectorfield
        @return traj
        """
        from .exceptions import (
            CollisionPlanningError,
            SelfCollisionPlanningError,
        )
        from openravepy import CollisionReport, RaveCreateTrajectory
        from ..util import GetCollisionCheckPts
        import time
        import scipy.integrate

        CheckLimitsAction = openravepy.KinBody.CheckLimitsAction

        # This is a workaround to emulate 'nonlocal' in Python 2.
        nonlocals = {
            'exception': None,
            't_cache': None,
            't_check': 0.,
        }

        env = robot.GetEnv()
        active_indices = robot.GetActiveDOFIndices()

        # Create a new trajectory matching the current
        # robot's joint configuration specification
        cspec = robot.GetActiveConfigurationSpecification('linear')
        cspec.AddDeltaTimeGroup()
        cspec.ResetGroupOffsets()
        path = RaveCreateTrajectory(env, '')
        path.Init(cspec)

        time_start = time.time()

        def fn_wrapper(t, q):
            """
            The integrator will try to solve this equation
            at each time step.
            Note: t is the integration time and is non-monotonic.
            """
            # Set the joint values, without checking the joint limits
            robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing)

            return fn_vectorfield()

        def fn_status_callback(t, q):
            """
            Check joint-limits and collisions for a specific joint
            configuration. This is called multiple times at DOF
            resolution in order to check along the entire length of the
            trajectory.
            Note: This is called by fn_callback, which is currently
            called after each integration time step, which means we are
            doing more checks than required.
            """
            if time.time() - time_start >= timelimit:
                raise TimeLimitError()

            # Check joint position limits.
            # We do this before setting the joint angles.
            util.CheckJointLimits(robot, q)

            robot.SetActiveDOFValues(q)

            # Check collision.
            report = CollisionReport()
            if env.CheckCollision(robot, report=report):
                raise CollisionPlanningError.FromReport(report)
            elif robot.CheckSelfCollision(report=report):
                raise SelfCollisionPlanningError.FromReport(report)

            # Check the termination condition.
            status = fn_terminate()

            if Status.DoesCache(status):
                nonlocals['t_cache'] = t

            if Status.DoesTerminate(status):
                raise TerminationError()

        def fn_callback(t, q):
            """
            This is called at every successful integration step.
            """
            try:
                # Add the waypoint to the trajectory.
                waypoint = numpy.zeros(cspec.GetDOF())
                cspec.InsertDeltaTime(waypoint, t - path.GetDuration())
                cspec.InsertJointValues(waypoint, q, robot, active_indices, 0)
                path.Insert(path.GetNumWaypoints(), waypoint)

                # Run constraint checks at DOF resolution.
                if path.GetNumWaypoints() == 1:
                    checks = [(t, q)]
                else:
                    # TODO: This should start at t_check. Unfortunately, a bug
                    # in GetCollisionCheckPts causes this to enter an infinite
                    # loop.
                    checks = GetCollisionCheckPts(robot,
                                                  path,
                                                  include_start=False)
                    # start_time=nonlocals['t_check'])

                for t_check, q_check in checks:
                    fn_status_callback(t_check, q_check)

                    # Record the time of this check so we continue checking at
                    # DOF resolution the next time the integrator takes a step.
                    nonlocals['t_check'] = t_check

                return 0  # Keep going.
            except PlanningError as e:
                nonlocals['exception'] = e
                return -1  # Stop.

        # Integrate the vector field to get a configuration space path.
        #
        # TODO: Tune the integrator parameters.
        #
        # Integrator: 'dopri5'
        # DOPRI (Dormand & Prince 1980) is an explicit method for solving ODEs.
        # It is a member of the Runge-Kutta family of solvers.
        integrator = scipy.integrate.ode(f=fn_wrapper)
        integrator.set_integrator(name='dopri5',
                                  first_step=0.1,
                                  atol=1e-3,
                                  rtol=1e-3)
        # Set function to be called at every successful integration step.
        integrator.set_solout(fn_callback)
        # Initial conditions
        integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)
        integrator.integrate(t=integration_time_interval)

        t_cache = nonlocals['t_cache']
        exception = nonlocals['exception']

        if t_cache is None:
            raise exception or PlanningError('An unknown error has occurred.')
        elif exception:
            logger.warning('Terminated early: %s', str(exception))

        # Remove any parts of the trajectory that are not cached. This also
        # strips the (potentially infeasible) timing information.
        output_cspec = robot.GetActiveConfigurationSpecification('linear')
        output_path = RaveCreateTrajectory(env, '')
        output_path.Init(output_cspec)

        # Add all waypoints before the last integration step. GetWaypoints does
        # not include the upper bound, so this is safe.
        cached_index = path.GetFirstWaypointIndexAfterTime(t_cache)
        output_path.Insert(0, path.GetWaypoints(0, cached_index), cspec)

        # Add a segment for the feasible part of the last integration step.
        output_path.Insert(output_path.GetNumWaypoints(), path.Sample(t_cache),
                           cspec)

        # Flag this trajectory as constrained.
        util.SetTrajectoryTags(output_path, {
            Tags.CONSTRAINED: 'true',
            Tags.SMOOTH: 'true'
        },
                               append=True)
        return output_path