def planToCartesianObjective(world,
robot,
iktargets,
iktolerance=1e-3,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None,
**planOptions):
"""
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` )
"""
#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)
def planToCartesianObjective(world,
robot,
iktargets,
iktolerance=1e-3,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None,
**planOptions):
"""Arguments: all the same as planToConfig, except
- iktargets: a list of IKObjective instances (see the ik module)
- iktolerance: a tolerance to which the ik objectives must be satisfied
Output: a cspace.MotionPlan instance that can be called to get a
kinematically-feasible plan. (see cspace.MotionPlan.planMore(iterations))
"""
#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)
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
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.
"""
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 = EmbeddedCSpace(space, subset, xinit=robot.getConfig())
active = [False] * robot.numLinks()
for i in subset:
active[i] = True
for i in range(robot.numLinks()):
if active[robot.link(i).getParent()]:
active[i] = True
inactive = []
for i in range(robot.numLinks()):
if not active[i]:
inactive.append(i)
#disable self-collisions for inactive objects
for i in inactive:
rindices = space.collider.robots[robot.index]
rindex = rindices[i]
if rindex < 0:
continue
newmask = set()
for j in range(robot.numLinks()):
if rindices[j] in space.collider.mask[rindex] and active[j]:
newmask.add(rindices[j])
space.collider.mask[rindex] = newmask
space = sspace
space.setup()
return space
def makeSpace(world,
robot,
edgeCheckResolution=1e-2,
extraConstraints=[],
equalityConstraints=[],
equalityTolerance=1e-3,
ignoreCollisions=[],
movingSubset=None):
"""Arguments:
- world: a WorldModel instance
- robot: a RobotModel in the world
- edgeCheckResolution: the resolution at which edges in the path are
checked for feasibility
- extraConstraints: a list of 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: 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 which case all
entries of the vector must be 0.
- equalityTolerance: a tolerance to which all the equality constraints
must be satisfied.
- ignoreCollisions: 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: 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.
Output: a CSpace instance
"""
subset = []
if movingSubset == 'auto' or movingSubset == 'all' or movingSubset == None:
subset = None
else:
subset = movingSubset
collider = robotcollide.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
space.eps = edgeCheckResolution
for c in extraConstraints:
space.addConstraint(c)
if subset is not None and len(subset) < robot.numLinks():
#choose a subset
sspace = EmbeddedCSpace(space, subset, xinit=robot.getConfig())
active = [False] * robot.numLinks()
for i in subset:
active[i] = True
for i in range(robot.numLinks()):
if active[robot.link(i).getParent()]:
active[i] = True
inactive = []
for i in range(robot.numLinks()):
if not active[i]:
inactive.append(i)
#disable self-collisions for inactive objects
for i in inactive:
rindex = space.collider.robots[robot.index][i]
space.collider.mask[rindex] = set()
space = sspace
space.setup()
return space
