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