class SmoothTrajectoryTest(object):
def setUp(self):
from openravepy import RaveCreateTrajectory
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.feasible_path = RaveCreateTrajectory(self.env, '')
self.feasible_path.Init(cspec)
self.feasible_path.Insert(0, self.waypoint1)
self.feasible_path.Insert(1, self.waypoint2)
self.feasible_path.Insert(2, self.waypoint3)
def test_SmoothTrajectory_DoesNotModifyBoundaryPoints(self):
from numpy.testing import assert_allclose
# Setup/Test
traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path)
# Assert
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.assertGreaterEqual(traj.GetNumWaypoints(), 2)
first_waypoint = traj.GetWaypoint(0, cspec)
last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec)
assert_allclose(first_waypoint, self.waypoint1)
assert_allclose(last_waypoint, self.waypoint3)
class ShortcutPathTest(object):
def setUp(self):
from openravepy import RaveCreateTrajectory
self.input_path = RaveCreateTrajectory(self.env, '')
self.input_path.Init(
self.robot.GetActiveConfigurationSpecification('linear'))
self.input_path.Insert(0, self.waypoint1)
self.input_path.Insert(1, self.waypoint2)
self.input_path.Insert(2, self.waypoint3)
def test_ShortcutPath_ShortcutExists_ReducesLength(self):
from numpy.testing import assert_allclose
# Setup/Test
smoothed_path = self.planner.ShortcutPath(self.robot, self.input_path)
# Assert
self.assertEquals(smoothed_path.GetConfigurationSpecification(),
self.input_path.GetConfigurationSpecification())
self.assertGreaterEqual(smoothed_path.GetNumWaypoints(), 2)
n = smoothed_path.GetNumWaypoints()
assert_allclose(smoothed_path.GetWaypoint(0), self.waypoint1)
assert_allclose(smoothed_path.GetWaypoint(n - 1), self.waypoint3)
self.assertLess(self.ComputeArcLength(smoothed_path),
0.5 * self.ComputeArcLength(self.input_path))
def Success_impl(robot):
import numpy
from openravepy import RaveCreateTrajectory
cspec = robot.GetActiveConfigurationSpecification()
traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
traj.Init(cspec)
traj.Insert(0, numpy.zeros(cspec.GetDOF()))
traj.Insert(1, numpy.ones(cspec.GetDOF()))
return traj
class MetaPlannerTests(object):
def setUp(self):
from openravepy import Environment, RaveCreateTrajectory
self.join_timeout = 5.0
self.env = Environment()
self.env.Load('data/lab1.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
# Create a valid trajectory used to test planner successes.
cspec = self.robot.GetActiveConfigurationSpecification()
self.traj = RaveCreateTrajectory(self.env, 'GenericTrajectory')
self.traj.Init(cspec)
self.traj.Insert(0, numpy.zeros(cspec.GetDOF()))
self.traj.Insert(1, numpy.ones(cspec.GetDOF()))
def test_get_joint(self):
""" This test creates a path in openrave format and test the class method get_joint to return a known joint.
"""
path = blade_coverage.Path(self.rays)
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints[:1])):
waypoint = list(self.joints[i])
waypoint.extend(list(self.joints[i]))
waypoint.extend(list(self.joints[i]))
waypoint.extend([0])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path.data = [traj]
joint = path.get_joint(self.robot, 0, 0)
self.assertTrue(linalg.norm(joint - self.joints[0]) <= 1e-5,
msg='get_joint failed')
def get_feasible_path(robot, path):
"""
Check a path for feasibility (collision) and return a truncated path
containing the path only up to just before the first collision.
@param robot: the OpenRAVE robot
@param path: the path to check for feasiblity
@return tuple with the (possibly) truncated path, and a boolean indicating
whether or not path was truncated
"""
env = robot.GetEnv()
path_cspec = path.GetConfigurationSpecification()
dof_indices, _ = path_cspec.ExtractUsedIndices(robot)
# Create a ConfigurationSpecification containing only positions.
cspec = path.GetConfigurationSpecification()
with robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
# Check for collision. Compute a unit timing to simplify this.
if IsTimedTrajectory(path):
unit_path = path
else:
unit_path = ComputeUnitTiming(robot, path)
t_collision = None
t_prev = 0.0
for t, dofvals in GetCollisionCheckPts(robot, unit_path):
robot.SetDOFValues(dofvals, dof_indices)
if env.CheckCollision(robot) or robot.CheckSelfCollision():
t_collision = t_prev
break
t_prev = t
# Build a path (with no timing) that stops before collision.
output_path = RaveCreateTrajectory(env, '')
if t_collision is not None:
end_idx = unit_path.GetFirstWaypointIndexAfterTime(t_collision)
if end_idx > 0:
waypoints = unit_path.GetWaypoints(0, end_idx, cspec)
else:
waypoints = list()
waypoints = np.concatenate((waypoints,
unit_path.Sample(t_collision, cspec)))
output_path.Init(cspec)
output_path.Insert(0, waypoints)
else:
output_path.Clone(path, 0)
return output_path, t_collision is not None
def create_trajectory(turbine, joints, joints_vel, joints_acc, times):
""" This method creates the trajectory specification in OpenRave format and insert the waypoints:
joints - vels - accs - times
DOF - DOF - DOF - DOF - 1
Args:
turbine: (@ref Turbine) turbine object
joints: (float[n<SUB>i</SUB>][nDOF]) joints to create the trajectory
joints_vel: (float[n<SUB>i</SUB>][nDOF]) joints_vel to create the trajectory
joints_acc: (float[n<SUB>i</SUB>][nDOF]) joints_acc to create the trajectory
times: (float[n<SUB>i</SUB>]) deltatimes
Returns:
trajectory: OpenRave object.
"""
robot = turbine.robot
ind = str()
for i in range(robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + robot.GetName() + ' ' + ind,
len(robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(turbine.env, '')
traj.Init(cs)
for i in range(len(joints)):
waypoint = list(joints[i])
waypoint.extend(list(joints_vel[i]))
waypoint.extend(list(joints_acc[i]))
waypoint.extend([times[i]])
traj.Insert(traj.GetNumWaypoints(), waypoint)
return traj
def test_serialize(self):
""" This test verifies if the class can serialize a path in openrave format.
"""
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints)):
waypoint = list(self.joints[i])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path = blade_coverage.Path(self.rays)
path.data = [traj]
try:
path.serialize('test_serialize')
shutil.rmtree('test_serialize')
except:
self.assertTrue(False, msg='Data can not be serialized')
def _get_trajectory(self, robot, q_goal, dof_indices):
"""
Generates a hand trajectory that attempts to move the specified
dof_indices to the configuration indicated in q_goal. The trajectory
will move the hand until q_goal is reached or an invalid configuration
(usually collision) is detected.
@param robot: OpenRAVE robot
@param q_goal: goal configuration (dof values)
@param dof_indices: indices of the dofs specified in q_goal
@return hand trajectory
"""
def collision_callback(report, is_physics):
"""
Callback for collision detection. Add the links that are in
collision to the colliding_links set in the enclosing frame.
@param report: collision report
@param is_physics: whether collision is from physics
@return ignore collision action
"""
colliding_links.update([report.plink1, report.plink2])
return CollisionAction.Ignore
env = robot.GetEnv()
collision_checker = env.GetCollisionChecker()
dof_indices = np.array(dof_indices, dtype='int')
report = CollisionReport()
handle = env.RegisterCollisionCallback(collision_callback)
with \
robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.LinkTransformation), \
CollisionOptionsStateSaver(
collision_checker,
CollisionOptions.ActiveDOFs):
robot.SetActiveDOFs(dof_indices)
q_prev = robot.GetActiveDOFValues()
# Create the output trajectory.
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(env, '')
traj.Init(cspec)
# Velocity that each joint will be moving at while active.
qd = np.sign(q_goal - q_prev) * robot.GetActiveDOFMaxVel()
# Duration required for each joint to reach the goal.
durations = (q_goal - q_prev) / qd
durations[qd == 0.] = 0.
t_prev = 0.
events = np.concatenate((
[0.],
durations,
np.arange(0., durations.max(), 0.01),
))
mask = np.array([True] * len(q_goal), dtype='bool')
for t_curr in np.unique(events):
robot.SetActiveDOFs(dof_indices[mask])
# Disable joints that have reached the goal.
mask = np.logical_and(mask, durations >= t_curr)
# Advance the active DOFs.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
robot.SetDOFValues(q_curr, dof_indices)
# Check for collision. This only operates on the active DOFs
# because the ActiveDOFs flag is set. We use a collision
# callback to build a set of all links in collision.
colliding_links = set()
env.CheckCollision(robot, report=report)
robot.CheckSelfCollision(report=report)
# Check which DOF(s) are involved in the collision.
mask = np.logical_and(mask, [
not any(
robot.DoesAffect(dof_index, link.GetIndex())
for link in colliding_links)
for dof_index in dof_indices
])
# Revert the DOFs that are in collision.
# TODO: This doesn't seem to take the links out of collision.
q_curr = q_prev.copy()
q_curr[mask] += (t_curr - t_prev) * qd[mask]
# Add a waypoint to the output trajectory.
waypoint = np.empty(cspec.GetDOF())
cspec.InsertDeltaTime(waypoint, t_curr - t_prev)
cspec.InsertJointValues(waypoint, q_curr, robot, dof_indices,
0)
traj.Insert(traj.GetNumWaypoints(), waypoint)
t_prev = t_curr
q_prev = q_curr
# Terminate if all joints are inactive.
if not mask.any():
break
del handle
return traj
def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
max_force=5.0, max_torque=None, ignore_collisions=None,
velocity_limit_scale=0.25, **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 ignore_collisions collisions with these objects are ignored when
planning the path, e.g. the object you think you will touch
@param velocity_limit_scale A multiplier to use to scale velocity limits
when executing MoveUntilTouch ( < 1 in most cases).
@param **kw_args planner parameters
@return felt_force flag indicating whether we felt a force.
"""
from contextlib import nested
from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
from prpy.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])
if ignore_collisions is None:
ignore_collisions = []
with env:
# Compute the expected force direction in the hand frame.
hand_pose = manipulator.GetEndEffectorTransform()
force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
# 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()
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:
raise NotImplementedError('MoveUntilTouch not yet implemented under ros_control.')
# 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
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
class RetimeTrajectoryTest(object):
def setUp(self):
from openravepy import planningutils, Planner, RaveCreateTrajectory
cspec = self.robot.GetActiveConfigurationSpecification('linear')
self.feasible_path = RaveCreateTrajectory(self.env, '')
self.feasible_path.Init(cspec)
self.feasible_path.Insert(0, self.waypoint1)
self.feasible_path.Insert(1, self.waypoint2)
self.feasible_path.Insert(2, self.waypoint3)
self.dt = 0.01
self.tolerance = 0.1 # 10% error
def test_RetimeTrajectory(self):
import numpy
from numpy.testing import assert_allclose
from openravepy import planningutils, Planner
# Setup/Test
traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path)
# Assert
position_cspec = self.feasible_path.GetConfigurationSpecification()
velocity_cspec = position_cspec.ConvertToDerivativeSpecification(1)
zero_dof_values = numpy.zeros(position_cspec.GetDOF())
# Verify that the trajectory passes through the original waypoints.
waypoints = [self.waypoint1, self.waypoint2, self.waypoint3]
waypoint_indices = [None] * len(waypoints)
for iwaypoint in xrange(traj.GetNumWaypoints()):
joint_values = traj.GetWaypoint(iwaypoint, position_cspec)
# Compare the waypoint against every input waypoint.
for icandidate, candidate_waypoint in enumerate(waypoints):
if numpy.allclose(joint_values, candidate_waypoint):
self.assertIsNone(
waypoint_indices[icandidate],
'Input waypoint {} appears twice in the output'
' trajectory (indices: {} and {})'.format(
icandidate, waypoint_indices[icandidate],
iwaypoint))
waypoint_indices[icandidate] = iwaypoint
self.assertEquals(waypoint_indices[0], 0)
self.assertEquals(waypoint_indices[-1], traj.GetNumWaypoints() - 1)
for iwaypoint in waypoint_indices:
self.assertIsNotNone(iwaypoint)
# Verify that the velocity at the waypoint is zero.
joint_velocities = traj.GetWaypoint(iwaypoint, velocity_cspec)
assert_allclose(joint_velocities, zero_dof_values)
# Verify the trajectory between waypoints.
for t in numpy.arange(self.dt, traj.GetDuration(), self.dt):
iafter = traj.GetFirstWaypointIndexAfterTime(t)
ibefore = iafter - 1
joint_values = traj.Sample(t, position_cspec)
joint_values_before = traj.GetWaypoint(ibefore, position_cspec)
joint_values_after = traj.GetWaypoint(iafter, position_cspec)
distance_full = numpy.linalg.norm(joint_values_after -
joint_values_before)
distance_before = numpy.linalg.norm(joint_values -
joint_values_before)
distance_after = numpy.linalg.norm(joint_values -
joint_values_after)
deviation = distance_before + distance_after - distance_full
self.assertLess(deviation, self.tolerance * distance_full)
# Check joint limits and dynamic feasibility.
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
planningutils.VerifyTrajectory(params, traj, self.dt)
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