from .cspace import *
from .cspaceutils import *
from . import robotcspace
from ..model import collide
from ..robotsim import IKObjective
[docs]def preferredPlanOptions(robot,movingSubset=None,optimizing=False):
"""Returns some options that might be good for your given robot, and
whether you want a feasible or just an optimal plan.
TODO: base this off of info about the robot, such as dimensionality,
joint ranges, etc.
"""
if optimizing:
return { 'type':"rrt", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
else:
return { 'type':"sbl", 'perturbationRadius':0.5, 'randomizeFrequency':1000, shortcut:1 }
[docs]def makeSpace(world,robot,
edgeCheckResolution=1e-2,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None):
"""Creates a standard CSpace instance for the robot moving in the given world.
Args:
world (WorldModel): the world in which the robot lives, including
obstacles.
robot (RobotModel): the moving robot
edgeCheckResolution (float, optional): the resolution at which edges in
the path are checked for feasibility
extraConstraints (list, optional): possible extra constraint functions,
each of which needs to return True if satisfied.
.. note::
Don't put cartesian constraints here! Instead place your
function in equalityConstraints.
equalityConstraints (list, optional): a list of IKObjectives or
equality constraints f(x)=0 that must be satisfied during the
motion. Equality constraints may return a float or a list of
floats. In the latter case, this is interpreted as a vector
function, in which all entries of the vector must be 0.
equalityTolerance (float, optional): a tolerance to which all the
equality constraints must be satisfied.
ignoreCollisions (list): a list of ignored collisions. Each element
may be a body in the world, or a pair (a,b) where a, b are bodies
in the world.
movingSubset (optional): if None, 'all', or 'auto' (default), all
joints will be allowed to move. If this is a list, then only
these joint indices will be allowed to move.
Returns:
CSpace: a C-space instance that describes the robot's feasible space.
This can be used for planning by creating a :class:`cspace.MotionPlan`
object. Note that if an EmbeddedCSpace is returned, you should
create a SubsetMotionPlan for greater convenience.
"""
subset = []
if movingSubset == 'auto' or movingSubset == 'all' or movingSubset == None:
subset = None
else:
subset = movingSubset
collider = collide.WorldCollider(world,ignore=ignoreCollisions)
implicitManifold = []
for c in equalityConstraints:
if not isinstance(c,IKObjective):
implicitManifold.append(c)
if len(equalityConstraints)==0:
space = robotcspace.RobotCSpace(robot,collider)
else:
if len(implicitManifold) > 0:
raise NotImplementedError("General inequality constraints")
else:
space = robotcspace.ClosedLoopRobotCSpace(robot,equalityConstraints,collider)
space.tol = equalityTolerance
if subset is not None and len(subset) < robot.numLinks():
space.setIKActiveDofs(subset)
space.eps = edgeCheckResolution
for c in extraConstraints:
space.addConstraint(c)
if subset is not None and len(subset) < robot.numLinks():
#choose a subset
sspace = robotcspace.EmbeddedRobotCSpace(space,subset,xinit=robot.getConfig())
sspace.disableInactiveCollisions()
space = sspace
space.setup()
return space
[docs]def planToConfig(world,robot,target,
edgeCheckResolution=1e-2,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset='auto',
verbose=True,
**planOptions):
"""Creates a MotionPlan object that can be called to solve a standard motion
planning problem for a robot in a world. The plan starts from the robot's
current configuration and ends in a target configuration.
Args:
world (WorldModel): the world in which the robot lives, including
obstacles
robot (RobotModel): the moving robot. The plan start configuration
is the robot's current configuration `robot.getConfig()`.
target (list of float): the desired final configuration of the robot.
edgeCheckResolution (float, optional): the resolution at which edges
in the path are checked for feasibility
extraConstraints (list, optional): possible extra constraint functions,
each of which needs to return True if satisfied.
.. note::
Don't put cartesian constraints here! Instead place your
function in equalityConstraints.
equalityConstraints (list, optional): a list of IKObjectives or
equality constraints f(x)=0 that must be satisfied during the
motion. Equality constraints may return a float or a list of
floats. In the latter case, this is interpreted as a vector
function, in which all entries of the vector must be 0.
equalityTolerance (float, optional): a tolerance to which all the
equality constraints must be satisfied.
ignoreCollisions (list): a list of ignored collisions. Each element may be
a body in the world, or a pair (a,b) where a, b are bodies in the world.
movingSubset (optional): if 'auto' (default), only the links that are
different between the robot's current config and target config will
be allowed to move. Otherwise, if this is None or 'all', all joints
will be allowed to move. If this is a list, then only these joint
indices will be allowed to move.
planOptions (keywords): keyword options that will be sent to the planner. See
the documentation for MotionPlan.setOptions for more details.
Returns:
MotionPlan: a planner instance that can be called to get a
kinematically-feasible plan. (see :meth:`MotionPlan.planMore`)
The underlying configuration space (a RobotCSpace, ClosedLoopRobotCSpace, or
EmbeddedRobotCSpace) can be retrieved using the "space" attribute of the
resulting MotionPlan object.
"""
q0 = robot.getConfig()
assert(len(q0)==len(target)),"target configuration must be of correct size for robot"
if movingSubset == 'auto':
subset = []
for i,(a,b) in enumerate(zip(q0,target)):
if a != b:
subset.append(i)
elif movingSubset == 'all' or movingSubset == None:
subset = list(range(len(q0)))
else:
subset = movingSubset
for i in range(len(q0)):
if i not in subset:
if q0[i] != target[i]:
raise ValueError("Error: target configuration value differs from start configuration along a fixed DOF: %s (link %d): %g vs %g"%(robot.link(i).getName(),i,q0[i],target[i]))
space = makeSpace(world=world,robot=robot,
edgeCheckResolution=edgeCheckResolution,
extraConstraints=extraConstraints,
equalityConstraints=equalityConstraints,
equalityTolerance=equalityTolerance,
ignoreCollisions=ignoreCollisions,
movingSubset=subset)
plan = SubsetMotionPlan(space,subset,q0,**planOptions)
try:
plan.setEndpoints(q0,target)
except RuntimeError:
#one of the endpoints is infeasible, print it out
if space.cspace==None: space.setup()
sfailures = space.cspace.feasibilityFailures([q0[s] for s in subset])
gfailures = space.cspace.feasibilityFailures([target[s] for s in subset])
print("Start configuration fails",sfailures)
print("Goal configuration fails",gfailures)
return None
return plan
[docs]def planToSet(world,robot,target,
edgeCheckResolution=1e-2,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None,
**planOptions):
"""
Creates a MotionPlan object that can be called to solve a standard motion
planning problem for a robot in a world. The plan starts from the robot's
current configuration and ends in a target set.
Args:
world (WorldModel): the world in which the robot lives, including
obstacles
robot (RobotModel): the moving robot. The plan starts from
robot.getConfig()
target (function or CSpace): a function f(q) returning a bool which is
True if the configuration q is a goal, OR an instance of a CSpace
subclass where sample() generates a sample in the target set and
feasible(x) tests whether a sample is in the target set.
.. note::
The function should accept vectors of the same dimensionality
as the robot, not the moving subset. Similarly, the CSpace
should have the same dimensionality as the robot.
edgeCheckResolution (float, optional): the resolution at which edges in the path are
checked for feasibility
extraConstraints (list, optional): possible extra constraint functions, each
of which needs to return True if satisfied.
.. note::
Don't put cartesian constraints here! Instead place your function in equalityConstraints.
equalityConstraints (list, optional): a list of IKObjectives or equality
constraints f(x)=0 that must be satisfied during the motion. Equality
constraints may return a float or a list of floats. In the latter case, this
is interpreted as a vector function, in which all entries of the vector must be 0.
equalityTolerance (float, optional): a tolerance to which all the equality constraints
must be satisfied.
ignoreCollisions (list): a list of ignored collisions. Each element may be
a body in the world, or a pair (a,b) where a, b are bodies in the world.
movingSubset (optional): if 'auto', 'all', or None (default), all joints
will be allowed to move. If this is a list, then only these joint
indices will be allowed to move.
planOptions (keywords): keyword options that will be sent to the planner. See
the documentation for MotionPlan.setOptions for more details.
Returns:
MotionPlan: a planner instance that can be called to get a
kinematically-feasible plan. (see :meth:`MotionPlan.planMore` )
The underlying configuration space (a RobotCSpace, ClosedLoopRobotCSpace, or
EmbeddedRobotCSpace) can be retrieved using the "space" attribute of the
resulting MotionPlan object.
"""
q0 = robot.getConfig()
subset = []
if movingSubset == 'auto' or movingSubset == 'all' or movingSubset == None:
subset = list(range(len(q0)))
else:
subset = movingSubset
space = makeSpace(world=world,robot=robot,
edgeCheckResolution=edgeCheckResolution,
extraConstraints=extraConstraints,
equalityConstraints=equalityConstraints,
equalityTolerance=equalityTolerance,
ignoreCollisions=ignoreCollisions,
movingSubset=subset)
plan = SubsetMotionPlan(space,subset,q0,**planOptions)
#convert target to a (test,sample) pair if it's a cspace
if isinstance(target,CSpace):
goal = [(lambda x:target.feasible(x)),(lambda : target.sample())]
else:
if not callable(target):
if not isinstance(target,(tuple,list)) or len(target)!=2 or not callable(target[0]) or not callable(target[1]):
raise TypeError("target must be a predicate function or CSpace object")
goal = target
try:
plan.setEndpoints(q0,goal)
except RuntimeError:
#the start configuration is infeasible, print it out
if space.cspace==None: space.setup()
sfailures = space.cspace.feasibilityFailures([q0[s] for s in subset])
print("Start configuration fails",sfailures)
raise
return plan
[docs]def planToCartesianObjective(world,robot,iktargets,iktolerance=1e-3,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None,
**planOptions):
"""
Plans a path to reach one or more IK targets.
Args:
world (WorldModel): same as planToConfig
iktargets (list of :class:`IKObjective`): a list of IKObjective
instances (see the ik module)
iktolerance (float): a tolerance to which the ik objectives must be
satisfied
Returns:
MotionPlan: a planner instance that can be called to get a
kinematically-feasible plan. (see :meth:`MotionPlan.planMore` )
The underlying configuration space (a RobotCSpace, ClosedLoopRobotCSpace, or
EmbeddedRobotCSpace) can be retrieved using the "space" attribute of the
resulting MotionPlan object.
"""
#TODO: only subselect those links that are affected by the IK target
goalset = robotcspace.ClosedLoopRobotCSpace(robot,iktargets,None)
return planToSet(world,robot,goalset,
extraConstraints=extraConstraints,
equalityConstraints=equalityConstraints,
equalityTolerance=equalityTolerance,
ignoreCollisions=ignoreCollisions,
movingSubset=movingSubset,
**planOptions)
class _WizardGUI:
def __init__(self):
self.world = None
self.movingObject = None
self.cspace = None
self.plannerSettings = preferredPlanOptions(None)
self.startConfig = None
self.goalConfig = None
self.goalIKTargets = None
self.goalSetTest = None
self.goalSetSampler = None
self.extraConstraints = []
self.equalityConstraints = []
self.movingSubset = None
#these are temporary objects
self.activeCSpace = None
self.planner = None
self.activeMovingSubset = None
self.currentRoadmap = None
self.currentSolution = None
#visualization settings
self.draw_end_effectors = None
self.draw_infeasible = False
self.debug_plan_time = 30
def makePlanner(self,use_active_space=False):
if self.startConfig is None:
start = self.movingObject.getConfig()
else:
start = self.startConfig
if self.goalConfig is not None:
goal = self.goalConfig
elif self.goalIKTargets is not None:
goal = robotcspace.ClosedLoopRobotCSpace(robot,self.goalIKTargets,None)
elif self.goalSetSampler is not None:
goal = (self.goalSetTest,self.goalSetSampler)
elif self.goalSetTest is not None:
goal = self.goalSetTest
else:
#no goal, can't create the planner
return None
if use_active_space:
plan = MotionPlan(self.activeCSpace,**self.plannerSettings)
else:
plan = MotionPlan(self.cspace,**self.plannerSettings)
plan.setEndpoints(start,goal)
return plan
[docs]def wizard(world_or_space_or_plan,moving_object=None,
draw_end_effectors=None,draw_infeasible=False,
debug_plan_time=30):
"""Launches a "wizard" to help set up or debug a planner. The wizard
will allow you to configure the planner, including the group of moving
joints, start and terminal sets, and collision detection settings.
The return value is a configured MotionPlan object, ready to be launched.
The wizard will also allow you to get a Python string that sets up the
space and/or invokes the planner.
Arguments:
world_or_space_or_plan (WorldModel, CSpace, or MotionPlan): the
world containing the moving robot, or the currently configured
CSpace or MotionPlan.
moving_object (RobotModel or RigidObjectModel, optional): if
world_or_space_or_plan is a WorldModel, this is the moving object
for which you'd like to plan. By default, robot 0 is moving.
draw_end_effectors (list, optional): if provided, initializes the
links to be drawn in the motion plan debugger.
draw_infeasible (bool, optional): initializes whether the motion plan
debugger will show infeasible configurations
debug_plan_time (float, optional): initializes the planning time in
the motion plan debugger.
Returns:
MotionPlan: a properly configured MotionPlan object that can be called
to get a motion plan. (see :meth:`MotionPlan.planMore`).
"""
gui = _WizardGUI()
if isinstance(world_or_space_or_plan,WorldModel):
gui.world = world_or_space_or_plan
if moving_object is None:
if gui.world.numRobots() == 0:
if gui.world.numRigidObjects() == 0:
raise ValueError("World has no robots or rigid objects")
moving_object = gui.world.rigidObject(0)
else:
moving_object = gui.world.robot(0)
if not isinstance(moving_object,(RobotModel,RigidObjectModel)):
raise TypeError("Invalid type of moving_object")
gui.cspace = makeSpace(gui.world,moving_object)
elif isinstance(world_or_space_or_plan,CSpace):
gui.cspace = world_or_space_or_plan
elif isinstance(world_or_space_or_plan,MotionPlan):
plan = world_or_space_or_plan
gui.cspace = world_or_space_or_plan.space
if isinstance(gui.cspace,EmbeddedCSpace):
gui.activeMovingSubset = gui.cspace.mapping
if plan is not None:
import json
gui.plannerSettings = json.loads(plan.planOptions)
start,goal = plan.planner.getEndpoints()
gui.startConfig = start
if hasattr(goal,'__iter__'):
if len(goal)==2 and callable(goal[0]):
gui.goalSetTest,gui.goalSetSampler = goal
else:
gui.endConfig = goal
else:
assert callable(goal)
gui.goalSetTest = goal
#TODO: parse IK targets