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