def PlanToEndEffectorPose(self,
robot,
goal_pose,
lambda_=100.0,
n_iter=100,
goal_tolerance=0.01,
**kw_args):
"""
Plan to a desired end-effector pose using GSCHOMP
@param robot
@param goal_pose desired end-effector pose
@param lambda_ step size
@param n_iter number of iterations
@param goal_tolerance tolerance in meters
@return traj
"""
self.distance_fields.sync(robot)
# CHOMP only supports start sets. Instead, we plan backwards from the
# goal TSR to the starting configuration. Afterwards, we reverse the
# trajectory.
manipulator_index = robot.GetActiveManipulatorIndex()
goal_tsr = prpy.tsr.TSR(T0_w=goal_pose, manip=manipulator_index)
start_config = robot.GetActiveDOFValues()
try:
traj = self.module.runchomp(robot=robot,
adofgoal=start_config,
start_tsr=goal_tsr,
lambda_=lambda_,
n_iter=n_iter,
goal_tolerance=goal_tolerance,
releasegil=True,
**kw_args)
traj = openravepy.planningutils.ReverseTrajectory(traj)
except RuntimeError as e:
raise PlanningError(str(e))
except openravepy.openrave_exception as e:
raise PlanningError(str(e))
# Verify that CHOMP didn't converge to the wrong goal. This is a
# workaround for a bug in GSCHOMP where the constraint projection
# fails because of joint limits.
config_spec = traj.GetConfigurationSpecification()
last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1)
final_config = config_spec.ExtractJointValues(
last_waypoint, robot, robot.GetActiveDOFIndices())
robot.SetActiveDOFValues(final_config)
final_pose = robot.GetActiveManipulator().GetEndEffectorTransform()
# TODO: Also check the orientation.
goal_distance = numpy.linalg.norm(final_pose[0:3, 3] -
goal_pose[0:3, 3])
if goal_distance > goal_tolerance:
raise PlanningError(
'CHOMP deviated from the goal pose by {0:f} meters.'.format(
goal_distance))
SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
return traj
def _Plan(self,
robot,
goals,
maxiter=500,
continue_planner=False,
or_args=None,
**kw_args):
env = robot.GetEnv()
planner = openravepy.RaveCreatePlanner(env, self.algorithm)
if planner is None:
raise UnsupportedPlanningError(
'Unable to create {:s} module.'.format(str(self)))
# Get rid of default postprocessing
extraParams = ('<_postprocessing planner="">'
'<_nmaxiterations>0</_nmaxiterations>'
'</_postprocessing>')
# Maximum planner iterations
extraParams += (
'<_nmaxiterations>{:d}</_nmaxiterations>'.format(maxiter))
if or_args is not None:
for key, value in or_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key),
v=str(value))
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(goals)
params.SetExtraParameters(extraParams)
traj = openravepy.RaveCreateTrajectory(env, 'GenericTrajectory')
try:
env.Lock()
with robot.CreateRobotStateSaver(
openravepy.Robot.SaveParameters.LinkTransformation):
# Plan.
if (not continue_planner) or not self.setup:
planner.InitPlan(robot, params)
self.setup = True
status = planner.PlanPath(traj, releasegil=True)
from openravepy import PlannerStatus
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError(
'Planner returned with status {:s}.'.format(
str(status)))
except Exception as e:
raise PlanningError('Planning failed with error: {:s}'.format(e))
finally:
env.Unlock()
return traj
def ShortcutPath(self, robot, path, timeout=1., **kwargs):
# The planner operates in-place, so we need to copy the input path. We
# also need to copy the trajectory into the planning environment.
output_path = CopyTrajectory(path, env=self.env)
extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetExtraParameters(extraParams)
# TODO: It would be nice to call planningutils.SmoothTrajectory here,
# but we can't because it passes a NULL robot to InitPlan. This is an
# issue that needs to be fixed in or_ompl.
self.planner.InitPlan(robot, params)
status = self.planner.PlanPath(output_path, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError(
'Simplifier returned with status {0:s}.'.format(str(status)))
SetTrajectoryTags(output_path, {Tags.SMOOTH: True}, append=True)
return output_path
def PlanToConfiguration(self,
robot,
goal,
lambda_=100.0,
n_iter=15,
**kw_args):
"""
Plan to a single configuration with single-goal CHOMP.
@param robot
@param goal goal configuration
@param lambda_ step size
@param n_iter number of iterations
"""
self.distance_fields.sync(robot)
try:
traj = self.module.runchomp(robot=robot,
adofgoal=goal,
lambda_=lambda_,
n_iter=n_iter,
releasegil=True,
**kw_args)
except Exception as e:
raise PlanningError(str(e))
SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
return traj
def OptimizeTrajectory(self,
robot,
traj,
lambda_=100.0,
n_iter=50,
**kw_args):
self.distance_fields.sync(robot)
cspec = traj.GetConfigurationSpecification()
cspec.AddDeltaTimeGroup()
openravepy.planningutils.ConvertTrajectorySpecification(traj, cspec)
for i in xrange(traj.GetNumWaypoints()):
waypoint = traj.GetWaypoint(i)
cspec.InsertDeltaTime(waypoint, .1)
traj.Insert(i, waypoint, True)
try:
traj = self.module.runchomp(robot=robot,
starttraj=traj,
lambda_=lambda_,
n_iter=n_iter,
**kw_args)
except Exception as e:
raise PlanningError(str(e))
SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
return traj
def PlanToConfiguration(self,
robot,
goal,
lambda_=100.0,
n_iter=15,
**kw_args):
"""
Plan to a single configuration with single-goal MULTIGRID-CHOMP.
@param robot
@param goal goal configuration
@param lambda_ step size
@param n_iter number of iterations
"""
if not self.initialized:
raise UnsupportedPlanningError('CHOMP requires a distance field.')
try:
return self.module.runchomp(robot=robot,
adofgoal=goal,
lambda_=lambda_,
n_iter=n_iter,
releasegil=True,
**kw_args)
except RuntimeError as e:
raise PlanningError(str(e))
def RetimeTrajectory(self, robot, path, options=None, **kw_args):
from openravepy import CollisionOptions, CollisionOptionsStateSaver
from copy import deepcopy
# Validate the input path.
cspec = path.GetConfigurationSpecification()
joint_values_group = cspec.GetGroupFromName('joint_values')
if joint_values_group is None:
raise ValueError('Trajectory is missing the "joint_values" group.')
elif HasAffineDOFs(cspec):
raise UnsupportedPlanningError(
'OpenRAVERetimer does not support affine DOFs.')
elif joint_values_group.interpolation != 'linear':
logger.warning(
'Path has interpolation of type "%s"; only "linear"'
' interpolation is supported.',
joint_values_group.interpolation)
# Set parameters.
all_options = deepcopy(self.default_options)
if options is not None:
all_options.update(options)
params = Planner.PlannerParameters()
params.SetConfigurationSpecification(
self.env, cspec.GetTimeDerivativeSpecification(0))
params_str = CreatePlannerParametersString(all_options, params)
# Copy the input trajectory into the planning environment. This is
# necessary for two reasons: (1) the input trajectory may be in another
# environment and/or (2) the retimer modifies the trajectory in-place.
output_traj = CopyTrajectory(path, env=self.env)
# Remove co-linear waypoints. Some of the default OpenRAVE retimers do
# not perform this check internally (e.g. ParabolicTrajectoryRetimer).
if not IsTimedTrajectory(output_traj):
output_traj = SimplifyTrajectory(output_traj, robot)
# Only collision check the active DOFs.
dof_indices, _ = cspec.ExtractUsedIndices(robot)
robot.SetActiveDOFs(dof_indices)
# Compute the timing. This happens in-place.
self.planner.InitPlan(None, params_str)
with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs):
status = self.planner.PlanPath(output_traj, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Retimer returned with status {:s}.'.format(
str(status)))
return output_traj
def RetimeTrajectory(self, robot, path):
# Copy the input trajectory into the planning environment. This is
# necessary for two reasons: (1) the input trajectory may be in another
# environment and/or (2) the retimer modifies the trajectory in-place.
output_traj = CopyTrajectory(path, env=self.env)
# Remove co-linear waypoints.
if not IsTimedTrajectory(output_traj):
output_traj = SimplifyTrajectory(output_traj, robot)
# Blend the piecewise-linear input trajectory. The blender outputs a
# collision-free path, consisting of piecewise-linear segments and
# quintic blends through waypoints.
try:
params = Planner.PlannerParameters()
self.blender.InitPlan(robot, params)
status = self.blender.PlanPath(output_traj)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Blending trajectory failed.')
except openrave_exception as e:
raise PlanningError('Blending trajectory failed: ' + str(e))
# Find the time-optimal trajectory that follows the blended path
# subject to joint velocity and acceleration limits.
try:
params = Planner.PlannerParameters()
self.retimer.InitPlan(robot, params)
status = self.retimer.PlanPath(output_traj)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Timing trajectory failed.')
except openrave_exception as e:
raise PlanningError('Timing trajectory failed: ' + str(e))
SetTrajectoryTags(output_traj, {Tags.SMOOTH: True}, append=True)
return output_traj
def PlanToNamedConfiguration(self, robot, name, **kw_args):
""" Plan to a named configuration.
The configuration is looked up by name in robot.configurations. Any
DOFs not specified in the named configuration are ignored. Planning is
performed by PlanToConfiguration using a delegate planner. If not
specified, this defaults to robot.planner.
@param name name of a saved configuration
@param **kw_args optional arguments passed to PlanToConfiguration
@returns traj trajectory
"""
try:
configurations = robot.configurations
except AttributeError:
raise PlanningError('{:s} does not have a table of named'
' configurations.'.format(robot))
try:
saved_dof_indices, saved_dof_values = robot.configurations.get_configuration(
name)
except KeyError:
raise PlanningError(
'{0:s} does not have named configuration "{1:s}".'.format(
robot, name))
arm_dof_indices = robot.GetActiveDOFIndices()
arm_dof_values = robot.GetActiveDOFValues()
# Extract the active DOF values from the configuration.
for arm_dof_index, arm_dof_value in zip(saved_dof_indices,
saved_dof_values):
if arm_dof_index in arm_dof_indices:
i = list(arm_dof_indices).index(arm_dof_index)
arm_dof_values[i] = arm_dof_value
# Delegate planning to another planner.
planner = self.delegate_planner or robot.planner
return planner.PlanToConfiguration(robot, arm_dof_values, **kw_args)
def RetimeTrajectory(self, robot, path, **kw_args):
RetimeTrajectory = openravepy.planningutils.RetimeAffineTrajectory
cspec = path.GetConfigurationSpecification()
# Check for anything other than affine dofs in the traj
# TODO: Better way to do this?
affine_groups = [
'affine_transform', 'affine_velocities', 'affine_accelerations'
]
for group in cspec.GetGroups():
found = False
for group_name in affine_groups:
if group_name in group.name: # group.name contains more stuff than just the name
found = True
break
if not found:
raise UnsupportedPlanningError(
'OpenRAVEAffineRetimer only supports untimed paths with affine DOFs. Found group: %s'
% group.name)
# Copy the input trajectory into the planning environment. This is
# necessary for two reasons: (1) the input trajectory may be in another
# environment and/or (2) the retimer modifies the trajectory in-place.
output_traj = CopyTrajectory(path, env=self.env)
# Compute the timing. This happens in-place.
max_velocities = [
robot.GetAffineTranslationMaxVels()[0],
robot.GetAffineTranslationMaxVels()[1],
robot.GetAffineRotationAxisMaxVels()[2]
]
max_accelerations = [3. * v for v in max_velocities]
status = RetimeTrajectory(output_traj, max_velocities,
max_accelerations, False)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Retimer returned with status {0:s}.'.format(
str(status)))
return output_traj
def _Plan(self,
robot,
goal=None,
timeout=30.,
shortcut_timeout=5.,
continue_planner=False,
ompl_args=None,
formatted_extra_params=None,
**kw_args):
extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)
if ompl_args is not None:
for key, value in ompl_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key),
v=str(value))
if formatted_extra_params is not None:
extraParams += formatted_extra_params
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
if goal is not None:
params.SetGoalConfig(goal)
params.SetExtraParameters(extraParams)
traj = openravepy.RaveCreateTrajectory(self.env, 'GenericTrajectory')
# Plan.
with self.env:
if (not continue_planner) or (not self.setup):
self.planner.InitPlan(robot, params)
self.setup = True
status = self.planner.PlanPath(traj, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError(
'Planner returned with status {0:s}.'.format(str(status)))
return traj
def PlanToEndEffectorPose(self,
robot,
goal_pose,
lambda_=100.0,
n_iter=100,
goal_tolerance=0.01,
**kw_args):
"""
Plan to a desired end-effector pose using GSCHOMP
@param robot
@param goal_pose desired end-effector pose
@param lambda_ step size
@param n_iter number of iterations
@param goal_tolerance tolerance in meters
@return traj
"""
# CHOMP only supports start sets. Instead, we plan backwards from the
# goal TSR to the starting configuration. Afterwards, we reverse the
# trajectory.
# TODO: Replace this with a proper goalset CHOMP implementation.
manipulator_index = robot.GetActiveManipulatorIndex()
goal_tsr = prpy.tsr.TSR(T0_w=goal_pose, manip=manipulator_index)
start_config = robot.GetActiveDOFValues()
if not self.initialized:
raise UnsupportedPlanningError('CHOMP requires a distance field.')
try:
traj = self.module.runchomp(robot=robot,
adofgoal=start_config,
start_tsr=goal_tsr,
lambda_=lambda_,
n_iter=n_iter,
goal_tolerance=goal_tolerance,
releasegil=True,
**kw_args)
traj = openravepy.planningutils.ReverseTrajectory(traj)
except RuntimeError, e:
raise PlanningError(str(e))
def _Plan(self, robot, sampling_func=VanDerCorputSampleGenerator, **kwargs):
is_deterministic = not kwargs.get('use_hmc', False)
try:
# Disable collision checking since we will perform them below.
traj = self.module.runchomp(robot=robot, no_collision_check=True,
releasegil=True, **kwargs)
except Exception as e:
raise PlanningError(str(e), deterministic=is_deterministic)
# Strip the extra groups added by CHOMP and change the trajectory to be
# linearly interpolated, as required by GetLinearCollisionCheckPts.
cspec = robot.GetActiveConfigurationSpecification('linear')
openravepy.planningutils.ConvertTrajectorySpecification(traj, cspec)
# Collision check the trajectory at DOF resolution. We do this in
# Python, instead of using CHOMP's native support for this, so we have
# access to the CollisionReport object.
checks = GetLinearCollisionCheckPts(robot, traj, norm_order=2,
sampling_func=sampling_func)
with self.robot_checker_factory(robot) as robot_checker:
for t, q in checks:
robot.SetActiveDOFValues(q)
robot_checker.VerifyCollisionFree() # Throws on collision.
# Tag the trajectory as non-determistic since CBiRRT is a randomized
# planner. Additionally tag the goal as non-deterministic if CBiRRT
# chose from a set of more than one goal configuration.
SetTrajectoryTags(traj, {
Tags.SMOOTH: True,
Tags.DETERMINISTIC_TRAJECTORY: is_deterministic,
Tags.DETERMINISTIC_ENDPOINT: True
}, append=True)
return traj
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
def _PlanToIK(self,
robot,
goal_pose,
ranker=None,
num_attempts=1,
**kw_args):
from openravepy import (IkFilterOptions, IkParameterization,
IkParameterizationType)
manipulator = robot.GetActiveManipulator()
if ranker is None:
ranker = ik_ranking.NominalConfiguration(
manipulator.GetArmDOFValues())
# Find an unordered list of IK solutions.
with robot.GetEnv():
ik_param = IkParameterization(goal_pose,
IkParameterizationType.Transform6D)
ik_solutions = manipulator.FindIKSolutions(
ik_param,
IkFilterOptions.CheckEnvCollisions,
ikreturn=False,
releasegil=True)
if ik_solutions.shape[0] == 0:
raise PlanningError('There is no IK solution at the goal pose.',
deterministic=True)
# Sort the IK solutions in ascending order by the costs returned by the
# ranker. Lower cost solutions are better and infinite cost solutions
# are assumed to be infeasible.
scores = ranker(robot, ik_solutions)
ranked_indices = numpy.argsort(scores)
ranked_indices = ranked_indices[~numpy.isposinf(scores)]
ranked_ik_solutions = ik_solutions[ranked_indices, :]
if ranked_ik_solutions.shape[0] == 0:
raise PlanningError('All IK solutions have infinite cost.',
deterministic=True)
# Sequentially plan to the solutions in descending order of cost.
planner = self.delegate_planner or robot.planner
p = openravepy.KinBody.SaveParameters
all_deterministic = True
with robot.CreateRobotStateSaver(p.ActiveDOF):
robot.SetActiveDOFs(manipulator.GetArmIndices())
num_attempts = min(ranked_ik_solutions.shape[0], num_attempts)
for i, ik_sol in enumerate(ranked_ik_solutions[0:num_attempts, :]):
try:
traj = planner.PlanToConfiguration(robot, ik_sol)
logger.info('Planned to IK solution %d of %d.', i + 1,
num_attempts)
return traj
except PlanningError as e:
logger.warning(
'Planning to IK solution %d of %d failed: %s', i + 1,
num_attempts, e)
if e.deterministic is None:
all_deterministic = False
logger.warning(
'Planner %s raised a PlanningError without the'
' "deterministic" flag set. Assuming the result'
' is non-deterministic.', planner)
elif not e.deterministic:
all_deterministic = False
raise PlanningError(
'Planning to the top {:d} of {:d} IK solutions failed.'.format(
num_attempts, ranked_ik_solutions.shape[0]),
deterministic=all_deterministic)
def PlanToEndEffectorOffset(self,
robot,
direction,
distance,
max_distance=None,
nullspace=JointLimitAvoidance,
timelimit=5.0,
step_size=0.001,
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 stepsize step size in meters for the Jacobian pseudoinverse controller
@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 step_size <= 0:
raise ValueError('Step size must be positive.')
elif position_tolerance < 0:
raise ValueError('Position tolerance must be non-negative.')
elif angular_tolerance < 0:
raise ValueError('Angular tolerance must be non-negative.')
# 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)
# 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
with robot:
manip = robot.GetActiveManipulator()
traj = openravepy.RaveCreateTrajectory(self.env, '')
traj.Init(manip.GetArmConfigurationSpecification())
active_dof_indices = manip.GetArmIndices()
limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices)
initial_pose = manip.GetEndEffectorTransform()
q = robot.GetDOFValues(active_dof_indices)
traj.Insert(0, q)
start_time = time.time()
current_distance = 0.0
sign_flipper = 1
last_rot_error = 9999999999.0
try:
while current_distance < max_distance:
# Check for a timeout.
current_time = time.time()
if timelimit is not None and current_time - start_time > timelimit:
raise PlanningError('Reached time limit.')
# Compute joint velocities using the Jacobian pseudoinverse.
q_dot = self.GetStraightVelocity(manip,
direction,
initial_pose,
nullspace,
step_size,
sign_flipper=sign_flipper)
q += q_dot
robot.SetDOFValues(q, active_dof_indices)
# Check for collisions.
#if self.env.CheckCollision(robot):
for body in active_bodies:
if self.env.CheckCollision(robot, body):
raise PlanningError('Encountered collision.')
if robot.CheckSelfCollision():
raise PlanningError('Encountered self-collision.')
# Check for joint limits.
elif not (limits_lower <
q).all() or not (q < limits_upper).all():
raise PlanningError(
'Encountered joint limit during Jacobian move.')
# Check our distance from the constraint.
current_pose = manip.GetEndEffectorTransform()
a = initial_pose[0:3, 3]
p = current_pose[0:3, 3]
orthogonal_proj = (
a - p) - numpy.dot(a - p, direction) * direction
if numpy.linalg.norm(orthogonal_proj) > position_tolerance:
raise PlanningError(
'Deviated from a straight line constraint.')
# Check our orientation against the constraint.
offset_pose = numpy.dot(numpy.linalg.inv(current_pose),
initial_pose)
offset_angle = openravepy.axisAngleFromRotationMatrix(
offset_pose)
offset_angle_norm = numpy.linalg.norm(offset_angle)
if offset_angle_norm > last_rot_error + 0.0005:
sign_flipper *= -1
last_rot_error = offset_angle_norm
if offset_angle_norm > angular_tolerance:
raise PlanningError(
'Deviated from orientation constraint.')
traj.Insert(traj.GetNumWaypoints(), q)
# Check if we've exceeded the maximum distance by projecting our
# displacement along the direction.
hand_pose = manip.GetEndEffectorTransform()
displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3]
current_distance = numpy.dot(displacement, direction)
except PlanningError as e:
# Throw an error if we haven't reached the minimum distance.
if current_distance < distance:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at distance %f < %f: %s',
current_distance, max_distance, e.message)
SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True)
return traj
def PlanToTSR(self,
robot,
tsrchains,
tsr_timeout=0.5,
num_attempts=3,
chunk_size=1,
num_candidates=50,
ranker=None,
max_deviation=2 * numpy.pi,
**kw_args):
"""
Plan to a desired TSR set using a-priori goal sampling. This planner
samples a fixed number of goals from the specified TSRs up-front, then
uses another planner's PlanToConfiguration (chunk_size = 1) or
PlanToConfigurations (chunk_size > 1) to plan to the resulting affine
transformations.
This planner will return failure if the provided TSR chains require
any constraint other than goal sampling.
@param robot the robot whose active manipulator will be used
@param tsrchains a list of TSR chains that define a goal set
@param num_attempts the maximum number of planning attempts to make
@param num_candidates the number of candidate IK solutions to rank
@param chunk_size the number of sampled goals to use per planning call
@param tsr_timeout the maximum time to spend sampling goal TSR chains
@param ranker an IK ranking function to use over the IK solutions
@param max_deviation the maximum per-joint deviation from current pose
that can be considered a valid sample.
@return traj a trajectory that satisfies the specified TSR chains
"""
# Delegate to robot.planner by default.
delegate_planner = self.delegate_planner or robot.planner
# Plan using the active manipulator.
manipulator = robot.GetActiveManipulator()
# Distance from current configuration is default ranking.
if ranker is None:
from ..ik_ranking import NominalConfiguration
ranker = NominalConfiguration(manipulator.GetArmDOFValues(),
max_deviation=max_deviation)
# Test for tsrchains that cannot be handled.
for tsrchain in tsrchains:
if tsrchain.sample_start or tsrchain.constrain:
raise UnsupportedPlanningError(
'Cannot handle start or trajectory-wide TSR constraints.')
tsrchains = [t for t in tsrchains if t.sample_goal]
def compute_ik_solutions(tsrchain):
pose = tsrchain.sample()
ik_param = IkParameterization(pose,
IkParameterizationType.Transform6D)
ik_solutions = manipulator.FindIKSolutions(
ik_param,
IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
statistics['num_tsr_samples'] += 1
statistics['num_ik_solutions'] += ik_solutions.shape[0]
return ik_solutions
def is_configuration_valid(ik_solution):
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.LinkTransformation):
robot.SetActiveDOFValues(ik_solution)
return not robot_checker.CheckCollision()
def is_time_available(*args):
# time_start and time_expired are defined below.
return time.time() - time_start + time_expired < tsr_timeout
time_expired = 0.
statistics = {'num_tsr_samples': 0, 'num_ik_solutions': 0}
configuration_generator = itertools.chain.from_iterable(
itertools.ifilter(
lambda configurations: configurations.shape[0] > 0,
itertools.imap(
compute_ik_solutions,
itertools.takewhile(is_time_available,
itertools.cycle(tsrchains)))))
for iattempt in xrange(num_attempts):
configurations_chunk = []
time_start = time.time()
# Set ActiveDOFs for IK collision checking. We intentionally
# restore the original collision checking options before calling
# the planner to give it a pristine environment.
with self.robot_checker_factory(robot) as robot_checker, \
robot.CreateRobotStateSaver(Robot.SaveParameters.ActiveDOF |
Robot.SaveParameters.LinkTransformation):
# We assume the active manipulator's DOFs are active when computing IK,
# calling the delegate planners, and collision checking with the
# ActiveDOFs option set.
robot.SetActiveDOFs(manipulator.GetArmIndices())
while is_time_available(
) and len(configurations_chunk) < chunk_size:
# Generate num_candidates candidates and rank them using the
# user-supplied IK ranker.
candidates = list(
itertools.islice(configuration_generator,
num_candidates))
if not candidates:
break
candidates_scores = ranker(robot, numpy.array(candidates))
candidates_scored = zip(candidates_scores, candidates)
candidates_scored.sort(key=lambda (score, _): score)
candidates_ranked = [q for _, q in candidates_scored]
# Select valid IK solutions from the chunk.
candidates_valid = itertools.islice(
itertools.ifilter(
is_configuration_valid,
itertools.takewhile(is_time_available,
candidates_ranked)),
chunk_size - len(configurations_chunk))
configurations_chunk.extend(candidates_valid)
time_expired += time.time() - time_start
if len(configurations_chunk) == 0:
raise PlanningError(
'Reached TSR sampling timelimit on attempt {:d} of {:d}: Failed'
' to generate any collision free IK solutions after attempting'
' {:d} TSR samples with {:d} candidate IK solutions.'.
format(iattempt + 1, num_attempts,
statistics['num_tsr_samples'],
statistics['num_ik_solutions']))
elif len(configurations_chunk) < chunk_size:
logger.warning(
'Reached TSR sampling timelimit on attempt %d of %d: got %d'
' of %d IK solutions.', iattempt + 1, num_attempts,
len(configurations_chunk), chunk_size)
try:
logger.info(
'Planning attempt %d of %d to a set of %d IK solution(s).',
iattempt + 1, num_attempts, len(configurations_chunk))
if chunk_size == 1:
traj = delegate_planner.PlanToConfiguration(
robot, configurations_chunk[0], **kw_args)
else:
traj = delegate_planner.PlanToConfigurations(
robot, configurations_chunk, **kw_args)
SetTrajectoryTags(traj, {
Tags.DETERMINISTIC_TRAJECTORY: False,
Tags.DETERMINISTIC_ENDPOINT: False,
},
append=True)
return traj
except PlanningError as e:
logger.warning('Planning attempt %d of %d failed: %s',
iattempt + 1, num_attempts, e)
raise PlanningError(
'Failed to find a solution in {:d} attempts.'.format(iattempt + 1))
def PlanToBasePose(self,
robot,
goal_pose,
timelimit=60.0,
return_first=False,
**kw_args):
"""
Plan to a base pose using SBPL
@param robot
@param goal_pose desired base pose
@param timelimit timeout in seconds
@param return_first return the first path found (if true, the planner will run until a path is found, ignoring the time limit)
"""
params = openravepy.Planner.PlannerParameters()
from openravepy import DOFAffine
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
params.SetRobotActiveJoints(robot)
config_spec = openravepy.RaveGetAffineConfigurationSpecification(
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, robot)
#params.SetConfigurationSpecification(self.env, config_spec) # This breaks
goal_config = openravepy.RaveGetAffineDOFValuesFromTransform(
goal_pose, DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
params.SetGoalConfig(goal_config)
# Setup default extra parameters
extra_params = self.planner_params
limits = robot.GetAffineTranslationLimits()
extents = [limits[0][0], limits[1][0], limits[0][1], limits[1][1]]
extra_params["extents"] = extents
extra_params["timelimit"] = timelimit
if return_first:
extra_params["return_first"] = 1
else:
extra_params["return_first"] = 0
extra_params["initial_eps"] = 1.0
for key, value in kw_args.iteritems():
extra_params[key] = value
params.SetExtraParameters(str(extra_params))
traj = openravepy.RaveCreateTrajectory(self.env, '')
try:
self.planner.InitPlan(robot, params)
status = self.planner.PlanPath(traj, releasegil=True)
except Exception as e:
raise PlanningError('Planning failed with error: {0:s}'.format(e))
from openravepy import PlannerStatus
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Planner returned with status {0:s}.'.format(
str(status)))
return traj
class CHOMPPlanner(BasePlanner):
def __init__(self):
super(CHOMPPlanner, self).__init__()
try:
from orchomp import orchomp
self.module = openravepy.RaveCreateModule(self.env, 'orchomp')
except ImportError:
raise UnsupportedPlanningError('Unable to import orchomp.')
except openravepy.openrave_exception as e:
raise UnsupportedPlanningError(
'Unable to create orchomp module: %s' % e)
if self.module is None:
raise UnsupportedPlanningError('Failed loading module.')
self.initialized = False
orchomp.bind(self.module)
def __str__(self):
return 'MULTI-CHOMP'
@PlanningMethod
def PlanToConfiguration(self,
robot,
goal,
lambda_=100.0,
n_iter=15,
**kw_args):
"""
Plan to a single configuration with single-goal MULTIGRID-CHOMP.
@param robot
@param goal goal configuration
@param lambda_ step size
@param n_iter number of iterations
"""
if not self.initialized:
raise UnsupportedPlanningError('CHOMP requires a distance field.')
try:
return self.module.runchomp(robot=robot,
adofgoal=goal,
lambda_=lambda_,
n_iter=n_iter,
releasegil=True,
**kw_args)
except RuntimeError as e:
raise PlanningError(str(e))
@PlanningMethod
def PlanToEndEffectorPose(self,
robot,
goal_pose,
lambda_=100.0,
n_iter=100,
goal_tolerance=0.01,
**kw_args):
"""
Plan to a desired end-effector pose using GSCHOMP
@param robot
@param goal_pose desired end-effector pose
@param lambda_ step size
@param n_iter number of iterations
@param goal_tolerance tolerance in meters
@return traj
"""
# CHOMP only supports start sets. Instead, we plan backwards from the
# goal TSR to the starting configuration. Afterwards, we reverse the
# trajectory.
# TODO: Replace this with a proper goalset CHOMP implementation.
manipulator_index = robot.GetActiveManipulatorIndex()
goal_tsr = prpy.tsr.TSR(T0_w=goal_pose, manip=manipulator_index)
start_config = robot.GetActiveDOFValues()
if not self.initialized:
raise UnsupportedPlanningError('CHOMP requires a distance field.')
try:
traj = self.module.runchomp(robot=robot,
adofgoal=start_config,
start_tsr=goal_tsr,
lambda_=lambda_,
n_iter=n_iter,
goal_tolerance=goal_tolerance,
releasegil=True,
**kw_args)
traj = openravepy.planningutils.ReverseTrajectory(traj)
except RuntimeError, e:
raise PlanningError(str(e))
# Verify that CHOMP didn't converge to the wrong goal. This is a
# workaround for a bug in GSCHOMP where the constraint projection
# fails because of joint limits.
config_spec = traj.GetConfigurationSpecification()
last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1)
final_config = config_spec.ExtractJointValues(
last_waypoint, robot, robot.GetActiveDOFIndices())
robot.SetActiveDOFValues(final_config)
final_pose = robot.GetActiveManipulator().GetEndEffectorTransform()
# TODO: Also check the orientation.
goal_distance = numpy.linalg.norm(final_pose[0:3, 3] -
goal_pose[0:3, 3])
if goal_distance > goal_tolerance:
raise PlanningError(
'CHOMP deviated from the goal pose by {0:f} meters.'.format(
goal_distance))
return traj
def PlanWorkspacePath(self,
robot,
traj,
timelimit=5.0,
min_waypoint_index=None,
**kw_args):
"""
Plan a configuration space path given a workspace path.
All timing information is ignored.
@param robot
@param traj workspace trajectory
represented as OpenRAVE AffineTrajectory
@param min_waypoint_index minimum waypoint index to reach
@param timelimit timeout in seconds
@return qtraj configuration space path
"""
from .exceptions import (TimeoutPlanningError, CollisionPlanningError,
SelfCollisionPlanningError)
from openravepy import CollisionReport
p = openravepy.KinBody.SaveParameters
with robot:
manip = robot.GetActiveManipulator()
robot.SetActiveDOFs(manip.GetArmIndices())
# Create a new trajectory starting at current robot location.
qtraj = openravepy.RaveCreateTrajectory(self.env, '')
qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
qtraj.Insert(0, robot.GetActiveDOFValues())
# Initial search for workspace path timing: one huge step.
t = 0.
dt = traj.GetDuration()
# Smallest CSpace step at which to give up
min_step = min(robot.GetActiveDOFResolutions()) / 100.
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6
try:
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
current_time = time.time()
if (timelimit is not None
and current_time - start_time > timelimit):
raise TimeoutPlanningError(timelimit)
# Hypothesize new configuration as closest IK to current
qcurr = robot.GetActiveDOFValues() # Configuration at t.
qnew = manip.FindIKSolution(openravepy.matrixFromPose(
traj.Sample(t + dt)[0:7]),
ik_options,
ikreturn=False,
releasegil=True)
# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
step = abs(qnew - qcurr)
if (max(step) < min_step) and qtraj:
raise PlanningError('Not making progress.')
infeasible_step = \
any(step > robot.GetActiveDOFResolutions())
if infeasible_step:
# Backtrack and try half the step
dt = dt / 2.0
else:
# Move forward to new trajectory time.
robot.SetActiveDOFValues(qnew)
qtraj.Insert(qtraj.GetNumWaypoints(), qnew)
t = min(t + dt, traj.GetDuration())
dt = dt * 2.0
except PlanningError as e:
# Compute the min acceptable time from the min waypoint index.
if min_waypoint_index is None:
min_waypoint_index = traj.GetNumWaypoints() - 1
cspec = traj.GetConfigurationSpecification()
wpts = [
traj.GetWaypoint(i) for i in range(min_waypoint_index + 1)
]
dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts]
min_time = numpy.sum(dts)
# Throw an error if we haven't reached the minimum waypoint.
if t < min_time:
# FindIKSolutions is slower than FindIKSolution, so call
# this only to identify error when there is no solution.
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(
traj.Sample(t + dt * 2.0)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
collision_error = None
# update collision_error to contain collision info.
with robot.CreateRobotStateSaver(p.LinkTransformation):
for q in ik_solutions:
robot.SetActiveDOFValues(q)
cr = CollisionReport()
if self.env.CheckCollision(robot, report=cr):
collision_error = \
CollisionPlanningError.FromReport(cr)
elif robot.CheckSelfCollision(report=cr):
collision_error = \
SelfCollisionPlanningError.FromReport(cr)
else:
collision_error = None
if collision_error is not None:
raise collision_error
else:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at time %f < %f: %s', t,
traj.GetDuration(), str(e))
# Return as much of the trajectory as we have solved.
SetTrajectoryTags(qtraj, {Tags.CONSTRAINED: True}, append=True)
return qtraj
def PlanWorkspacePath(self,
robot,
traj,
timelimit=5.0,
min_waypoint_index=None,
norm_order=2,
**kw_args):
"""
Plan a configuration space path given a workspace path.
All timing information is ignored.
@param robot
@param traj workspace trajectory
represented as OpenRAVE AffineTrajectory
@param min_waypoint_index minimum waypoint index to reach
@param timelimit timeout in seconds
@param norm_order: 1 ==> The L1 norm
2 ==> The L2 norm
inf ==> The L_infinity norm
Used to determine the resolution of collision checked waypoints
in the trajectory
@return qtraj configuration space path
"""
env = robot.GetEnv()
from .exceptions import (TimeoutPlanningError, CollisionPlanningError,
SelfCollisionPlanningError)
from openravepy import (CollisionOptions, CollisionOptionsStateSaver,
CollisionReport)
p = openravepy.KinBody.SaveParameters
with robot, CollisionOptionsStateSaver(env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs), \
robot.CreateRobotStateSaver(p.ActiveDOF | p.LinkTransformation):
manip = robot.GetActiveManipulator()
robot.SetActiveDOFs(manip.GetArmIndices())
# Create a new trajectory starting at current robot location.
qtraj = openravepy.RaveCreateTrajectory(env, '')
qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
qtraj.Insert(0, robot.GetActiveDOFValues())
# Initial search for workspace path timing: one huge step.
t = 0.
dt = traj.GetDuration()
q_resolutions = robot.GetActiveDOFResolutions()
# Smallest CSpace step at which to give up
min_step = numpy.linalg.norm(robot.GetActiveDOFResolutions() /
100.,
ord=norm_order)
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6
try:
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
# TODO: This is not really deterministic because we do not
# have control over CPU time. However, it is exceedingly
# unlikely that running the query again will change the
# outcome unless there is a significant change in CPU load.
current_time = time.time()
if (timelimit is not None
and current_time - start_time > timelimit):
raise TimeoutPlanningError(timelimit,
deterministic=True)
# Hypothesize new configuration as closest IK to current
qcurr = robot.GetActiveDOFValues() # Configuration at t.
qnew = manip.FindIKSolution(openravepy.matrixFromPose(
traj.Sample(t + dt)[0:7]),
ik_options,
ikreturn=False,
releasegil=True)
# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
steps = abs(qnew - qcurr) / q_resolutions
norm = numpy.linalg.norm(steps, ord=norm_order)
if (norm < min_step) and qtraj:
raise PlanningError('Not making progress.')
infeasible_step = norm > 1.0
if infeasible_step:
# Backtrack and try half the step
dt = dt / 2.0
else:
# Move forward to new trajectory time.
robot.SetActiveDOFValues(qnew)
qtraj.Insert(qtraj.GetNumWaypoints(), qnew)
t = min(t + dt, traj.GetDuration())
dt = dt * 2.0
except PlanningError as e:
# Compute the min acceptable time from the min waypoint index.
if min_waypoint_index is None:
min_waypoint_index = traj.GetNumWaypoints() - 1
cspec = traj.GetConfigurationSpecification()
wpts = [
traj.GetWaypoint(i) for i in range(min_waypoint_index + 1)
]
dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts]
min_time = numpy.sum(dts)
# Throw an error if we haven't reached the minimum waypoint.
if t < min_time:
# FindIKSolutions is slower than FindIKSolution, so call
# this only to identify error when there is no solution.
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(
traj.Sample(t + dt * 2.0)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
collision_error = None
# update collision_error to contain collision info.
with robot.CreateRobotStateSaver(p.LinkTransformation):
for q in ik_solutions:
robot.SetActiveDOFValues(q)
cr = CollisionReport()
if env.CheckCollision(robot, report=cr):
collision_error = \
CollisionPlanningError.FromReport(
cr, deterministic=True)
elif robot.CheckSelfCollision(report=cr):
collision_error = \
SelfCollisionPlanningError.FromReport(
cr, deterministic=True)
else:
collision_error = None
if collision_error is not None:
raise collision_error
else:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at time %f < %f: %s', t,
traj.GetDuration(), str(e))
SetTrajectoryTags(qtraj, {
Tags.CONSTRAINED: True,
Tags.DETERMINISTIC_TRAJECTORY: True,
Tags.DETERMINISTIC_ENDPOINT: True,
},
append=True)
return qtraj
def Plan(self,
robot,
smoothingitrs=None,
timelimit=None,
allowlimadj=0,
jointstarts=None,
jointgoals=None,
psample=None,
tsr_chains=None,
extra_args=None,
**kw_args):
from openravepy import CollisionOptions, CollisionOptionsStateSaver
# TODO We may need this work-around because CBiRRT doesn't like it
# when an IK solver other than GeneralIK is loaded (e.g. nlopt_ik).
# self.ClearIkSolver(robot.GetActiveManipulator())
self.env.LoadProblem(self.problem, robot.GetName())
args = ['RunCBiRRT']
if extra_args is not None:
args += extra_args
if smoothingitrs is not None:
if smoothingitrs < 0:
raise ValueError(
'Invalid number of smoothing iterations. '
'Value must be non-negative; got {:d}.'.format(
smoothingitrs))
args += ['smoothingitrs', str(smoothingitrs)]
if timelimit is not None:
if not (timelimit > 0):
raise ValueError('Invalid value for "timelimit". Limit must be'
' non-negative; got {:f}.'.format(timelimit))
args += ['timelimit', str(timelimit)]
if allowlimadj is not None:
args += ['allowlimadj', str(int(allowlimadj))]
if psample is not None:
if not (0 <= psample <= 1):
raise ValueError(
'Invalid value for "psample". Value must be '
'in the range [0, 1]; got {:f}.'.format(psample))
args += ['psample', str(psample)]
if jointstarts is not None:
for start_config in jointstarts:
if len(start_config) != robot.GetActiveDOF():
raise ValueError(
'Incorrect number of DOFs in start configuration;'
' expected {:d}, got {:d}'.format(
robot.GetActiveDOF(), len(start_config)))
args += (['jointstarts'] +
self.serialize_dof_values(start_config))
if jointgoals is not None:
for goal_config in jointgoals:
if len(goal_config) != robot.GetActiveDOF():
raise ValueError(
'Incorrect number of DOFs in goal configuration;'
' expected {:d}, got {:d}'.format(
robot.GetActiveDOF(), len(goal_config)))
args += ['jointgoals'] + self.serialize_dof_values(goal_config)
if tsr_chains is not None:
for tsr_chain in tsr_chains:
args += ['TSRChain', SerializeTSRChain(tsr_chain)]
# FIXME: Why can't we write to anything other than cmovetraj.txt or
# /tmp/cmovetraj.txt with CBiRRT?
traj_path = 'cmovetraj.txt'
args += ['filename', traj_path]
args_str = ' '.join(args)
with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs):
response = self.problem.SendCommand(args_str, True)
if not response.strip().startswith('1'):
raise PlanningError('Unknown error: ' + response)
# Construct the output trajectory.
with open(traj_path, 'rb') as traj_file:
traj_xml = traj_file.read()
traj = openravepy.RaveCreateTrajectory(self.env,
'GenericTrajectory')
traj.deserialize(traj_xml)
# Tag the trajectory as constrained if a constraint TSR is present.
if (tsr_chains is not None
and any(tsr_chain.constrain for tsr_chain in tsr_chains)):
SetTrajectoryTags(traj, {Tags.CONSTRAINED: True}, append=True)
# Strip extraneous groups from the output trajectory.
# TODO: Where are these groups coming from!?
cspec = robot.GetActiveConfigurationSpecification('linear')
openravepy.planningutils.ConvertTrajectorySpecification(traj, cspec)
return traj