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