def cartesian_interpolate_linear(robot, a, b, constraints, startConfig='robot', endConfig=None, delta=1e-2, solver=None, feasibilityTest=None, maximize=False): """Resolves a continuous robot trajectory that interpolates between two cartesian points for specified IK constraints. The output path is only a kinematic resolution, and has time domain [0,1]. Args: robot: the RobotModel or SubRobotModel. a, b (list of floats): endpoints of the Cartesian trajectory. Assumed derived from config.getConfig(constraints) constraints: one or more link indices, link names, or IKObjective's giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or None (any configuration) endConfig (optional): same type as startConfig. delta (float, optional): the maximum configuration space distance between points on the output path solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns false when a configuration q is infeasible maximize (bool, optional): if true, goes as far as possible along the path. Returns: RobotTrajectory or None: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0, "Spatial resolution must be positive" constraints, startConfig, endConfig, solver = _make_canonical( robot, constraints, startConfig, endConfig, solver) assert startConfig is not None, "Unable to cartesian interpolate without a start configuration" robot.setConfig(startConfig) set_cartesian_constraints(a, constraints, solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_interpolate_linear(): Error, initial configuration cannot be solved to match initial Cartesian coordinates, residual", solver.getResidual( ) return None print "cartesian_interpolate_linear(): Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(startConfig): print "Error: initial configuration is infeasible" return None if endConfig is not None: #doing endpoint-constrained cartesian interpolation set_cartesian_constraints(b, constraints, solver) robot.setConfig(endConfig) if not solver.isSolved(): print "cartesian_interpolate_linear(): Error, end configuration does not match final Cartesian coordinates, residual", solver.getResidual( ) return None if feasibilityTest is not None and not feasibilityTest(startConfig): print "cartesian_interpolate_linear(): Error: final configuration is infeasible" return None res = RobotTrajectory(robot) t = 0 res.times.append(t) res.milestones.append(startConfig) qmin0, qmax0 = solver.getJointLimits() tol0 = solver.getTolerance() solver.setTolerance(tol0 * 0.1) set_cartesian_constraints(a, constraints, solver) if not solver.isSolved(): solver.solve() res.times.append(t + 1e-7) res.milestones.append(robot.getConfig()) t = res.times[-1] paramStallTolerance = 0.01 * solver.getTolerance() / config.distance( constraints, a, b) stepsize = 0.1 while t < 1: tookstep = False tend = min(t + stepsize, 1) x = config.interpolate(constraints, a, b, tend) if endConfig is not None: robot.setConfig(robot.interpolate(startConfig, endConfig, tend)) solver.setBiasConfig(robot.getConfig()) q = res.milestones[-1] solver.setJointLimits( [max(vmin, v - delta) for v, vmin in zip(q, qmin0)], [min(vmax, v + delta) for v, vmax in zip(q, qmax0)]) #print "Trying step",tend-t,"time t=",tend if solve_cartesian(x, constraints, solver): #valid step, increasing step size #print "Accept and increase step" tookstep = True stepsize *= 1.5 else: #do a line search while stepsize > paramStallTolerance: stepsize *= 0.5 tend = min(t + stepsize, 1) x = config.interpolate(constraints, a, b, tend) if endConfig is not None: robot.setConfig( robot.interpolate(startConfig, endConfig, tend)) solver.setBiasConfig(robot.getConfig()) else: robot.setConfig(q) #print "Trying step",tend-t,"time t=",tend if solve_cartesian(x, constraints, solver): #print "Accept" tookstep = True break else: solver.setTolerance(tol0) if solver.isSolved(): #print "Grudgingly accepted" tookstep = True break solver.setTolerance(tol0 * 0.1) if not tookstep: print "cartesian_interpolate_linear(): Failed to take a valid step along straight line path at time", res.times[ -1], "residual", solver.getResidual() #x = config.interpolate(constraints,a,b,res.times[-1]) #set_cartesian_constraints(x,constraints,solver) #robot.setConfig(res.milestones[-1]) #print "Last residual",solver.getResidual() #x = config.interpolate(constraints,a,b,tend) #set_cartesian_constraints(x,constraints,solver) #print "Residual from last config",solver.getResidual() solver.setJointLimits(qmin0, qmax0) solver.setTolerance(tol0) if maximize: return res return None x = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(x): print "cartesian_interpolate_linear(): Infeasibility at time", tend solver.setJointLimits(qmin0, qmax0) solver.setTolerance(tol0) if maximize: return res return None #print "Distances from last:",max(abs(a-b) for (a,b) in zip(res.milestones[-1],x)) res.times.append(tend) res.milestones.append(x) t = tend solver.setJointLimits(qmin0, qmax0) solver.setTolerance(tol0) if endConfig is not None: if robot.distance(res.milestones[-2], endConfig) > delta: #hit a local minimum, couldn't reach the goal if maximize: res.times.pop(-1) res.milestones.pop(-1) return res #print "Hit a local minimum while trying to reach end configuration" return None else: #clean up the end configuration res.milestones[-1] = endConfig return res
def cartesian_path_interpolate(robot, path, constraints, startConfig='robot', endConfig=None, delta=1e-2, method='any', solver=None, feasibilityTest=None, numSamples=1000, maximize=False): """Resolves a continuous robot trajectory that follows a cartesian path for one or more links of a robot. Note: The output path is only a kinematic resolution, and may not respect the robot's velocity / acceleration limits. Args: robot (RobotModel or SubRobotModel): the robot. path (Trajectory or list of milestones): a cartesian path for the parameters of the the given constraints. If only milestones are given, the milestones are spaced 1s apart in time. constraints: one or more link indices, link names, or IKObjective's giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or None (any configuration) endConfig (optional): same type as startConfig. delta (float, optional): the maximum configuration space distance between points on the output path method (str): method used. Can be 'any', 'pointwise', or 'roadmap'. solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns False when a configuration q is infeasible numSamples (int, optional): if 'roadmap' or 'any' method is used, the # of configuration space samples that are used. maximize (bool, optional): if true, goes as far as possible along the path. Returns: RobotTrajectory or None: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0, "Spatial resolution must be positive" if hasattr(path, '__iter__'): path = Trajectory(range(len(path)), path) constraints, startConfig, endConfig, solver = _make_canonical( robot, constraints, startConfig, endConfig, solver) #correct start and goal configurations, if specified if startConfig: robot.setConfig(startConfig) set_cartesian_constraints(path.milestones[0], constraints, solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_path_interpolate(): Error, initial configuration cannot be solved to match initial Cartesian coordinates" return None print "cartesian_path_interpolate(): Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() if endConfig: robot.setConfig(endConfig) set_cartesian_constraints(path.milestones[-1], constraints, solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_path_interpolate(): Error, final configuration cannot be solved to match final Cartesian coordinates" return None print "cartesian_path_interpolate(): Warning, final configuration does not match final Cartesian coordinates, solving" endConfig = robot.getConfig() #now we're at a canonical setup if method == 'any' or method == 'pointwise': #try pointwise resolution first if startConfig is None: if ik.solve_global(constraints, solver.getMaxIters(), solver.getTolerance(), solver.getActiveDofs(), max(100, numSamples), feasibilityTest): startConfig = robot.getConfig() else: print "cartesian_path_interpolate(): Error: could not solve for start configuration" return None res = RobotTrajectory(robot) res.times.append(path.times[0]) res.milestones.append(startConfig) infeasible = False for i in xrange(len(path.milestones) - 1): if endConfig is None: segEnd = None else: u = (path.times[i + 1] - path.times[i]) / (path.times[-1] - path.times[i]) segEnd = robot.interpolate(res.milestones[-1], endConfig, u) robot.setConfig(segEnd) if solve_cartesian(path.milestones[i + 1], constraints, solver): segEnd = robot.getConfig() if segEnd is None: seg = cartesian_interpolate_linear( robot, path.milestones[i], path.milestones[i + 1], constraints, startConfig=res.milestones[-1], endConfig=segEnd, delta=delta, solver=solver, feasibilityTest=feasibilityTest) else: seg = cartesian_interpolate_bisect( robot, path.milestones[i], path.milestones[i + 1], constraints, startConfig=res.milestones[-1], endConfig=segEnd, delta=delta, solver=solver, feasibilityTest=feasibilityTest) if not seg: print "cartesian_path_interpolate(): Found infeasible cartesian interpolation segment at time", path.times[ i + 1] infeasible = True break #concatenate dt = path.times[i + 1] - path.times[i] seg.times = [t * dt for t in seg.times] res = res.concat(seg, relative=True) if not infeasible: #print "Resolved with pointwise interpolation!" return res if method == 'pointwise' and maximize: return res if method == 'any' or method == 'roadmap': #TODO: sample on continuous parameterization of path if path.duration() > 0: #manual discretization using config.interpolate numdivs = 20 divpts = [ path.startTime() + path.duration() * float(i) / (numdivs - 1) for i in xrange(numdivs) ] oldseg = 0 oldu = 0 times = [0] milestones = [path.milestones[0]] for t in divpts: s, u = path.getSegment(t) if s + 1 >= len(path.milestones): s = len(path.milestones) - 2 u = 1 if s == oldseg: if u != oldu: times.append(t) milestones.append( config.interpolate(constraints, path.milestones[s], path.milestones[s + 1], u)) else: for i in range(oldseg + 1, s + 1): times.append(path.times[i]) milestones.append(path.milestones[i]) times.append(t) print s, u milestones.append( config.interpolate(constraints, path.milestones[s], path.milestones[s + 1], u)) oldseg, oldu = s, u path = path.constructor()(times, milestones) import random #mark whether we need to sample the end or start pathIndices = range(1, len(path.milestones) - 1) if startConfig is None: pathIndices = [0] + pathIndices if endConfig is None: pathIndices = pathIndices + [len(path.milestones) - 1] samp = 0 if startConfig is None: #need to seed a start configuration while samp < numSamples: samp += 1 solver.sampleInitial() if solve_cartesian(path.milestones[0], constraints, solver): if feasibilityTest is None or feasibilityTest( robot.getConfig()): startConfig = robot.getConfig() break if endConfig is None: #need to seed an end configuration samp = 0 while samp < numSamples: samp += 1 if samp > 0: solver.sampleInitial() else: robot.setConfig(startConfig) if solve_cartesian(path.milestones[-1], constraints, solver): if feasibilityTest is None or feasibilityTest( robot.getConfig()): endConfig = robot.getConfig() break if startConfig is None or endConfig is None: print "cartesian_path_interpolate(): Exhausted all samples, perhaps endpoints are unreachable" return None selfMotionManifolds = [[] for i in path.milestones] nodes = [] configs = [] ccs = [] edges = [] def findpath(depth): #start and goal are connected! find a path through the edges list using BFS eadj = [[] for n in nodes] for (i, j, p) in edges: eadj[i].append((j, p)) q = deque() parent = [None] * len(nodes) for c in selfMotionManifolds[0]: q.append(c) #print "Adjacency list" #for i,alist in enumerate(eadj): # print nodes[i],": ",' '.join(str(nodes[j]) for (j,p) in alist) while len(q) > 0: n = q.pop() for c, p in eadj[n]: if parent[c] is not None: continue parent[c] = n if nodes[c][0] == depth: print "cartesian_path_interpolate(): Found a path using roadmap after", samp, "samples" #arrived at goal node, trace parent list back npath = [] n = c while c is not None: npath.append(c) c = parent[c] npath = [n for n in reversed(npath)] print ' '.join(str(nodes[n]) for n in npath) assert nodes[npath[0]][ 0] == 0, "Didnt end up at a start configuration?" res = RobotTrajectory(robot) res.times.append(path.times[0]) res.milestones.append(configs[npath[0]]) for i, n in enumerate(npath[:-1]): found = False for j, p in eadj[n]: if j == npath[i + 1]: #print "Suffix",p.times[0],p.times[-1] #print res.times[0],res.times[-1] res = res.concat(p, relative=False) #print "Resulting range",res.times[0],res.times[-1] found = True break assert found, "Internal error? " + str( nodes[npath[i]]) + " -> " + str( nodes[npath[i + 1]]) return res q.append(c) print "cartesian_path_interpolate(): Path to depth", depth, "could not be found" return None selfMotionManifolds[0].append(0) configs.append(startConfig) nodes.append((0, 0)) ccs.append(0) selfMotionManifolds[-1].append(1) configs.append(endConfig) nodes.append((len(path.milestones) - 1, 0)) ccs.append(1) for samp in xrange(samp, numSamples): irand = random.choice(pathIndices) solver.sampleInitial() #check for successful sample on self motion manifold, test feasibility if not solve_cartesian(path.milestones[irand], constraints, solver): continue x = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(x): continue #add to data structure nx = len(nodes) nodes.append((irand, len(selfMotionManifolds[irand]))) ccs.append(nx) assert len(ccs) == nx + 1 selfMotionManifolds[irand].append(nx) configs.append(x) #try connecting to other nodes k = int(math.log(samp + 2)) + 2 #brute force k-nearest neighbor d = [] for i, n in enumerate(nodes[:-1]): if n[0] == irand: continue dist = config.distance(constraints, path.milestones[n[0]], path.milestones[irand]) dist = robot.distance(x, configs[i]) d.append((dist, i)) k = min(k, len(d)) print "cartesian_path_interpolate(): Sampled at time point", irand, "checking", k, "potential connections" totest = [v[1] for v in sorted(d)[:k]] for n in totest: i = irand j = nodes[n][0] qi = x qj = configs[n] ni = nx nj = n if ccs[ni] == ccs[nj]: #same connected component, use visibility graph technique continue if i > j: i, j = j, i qi, qj = qj, qi ni, nj = nj, ni pij = path.constructor()(path.times[i:j + 1], path.milestones[i:j + 1]) #try connecting edges t = cartesian_path_interpolate(robot, pij, constraints, startConfig=qi, endConfig=qj, delta=delta, method='pointwise', solver=solver, feasibilityTest=feasibilityTest) #t = cartesian_interpolate_bisect(robot,path.milestones[i],path.milestones[j],constraints,qi,qj,delta=delta,solver=solver,feasibilityTest=feasibilityTest) if t is None: print " Failed edge", nodes[ni], "->", nodes[nj] continue #t.times = [path.times[i] + v*(path.times[j]-path.times[i]) for v in t.times] print " Added edge", nodes[ni], "->", nodes[nj] edges.append((ni, nj, t)) if ccs[ni] != ccs[nj]: #not in same connected component, collapse ccs src, tgt = ccs[ni], ccs[nj] if src < tgt: src, tgt = tgt, src checkgoal = False for i, cc in enumerate(ccs): if ccs[i] == src: ccs[i] = tgt if nodes[i][0] == 0 or nodes[i][0] == len( path.milestones) - 1: checkgoal = True if checkgoal: checkgoal = False for c in selfMotionManifolds[0]: for d in selfMotionManifolds[-1]: if ccs[c] == ccs[d]: checkgoal = True break if checkgoal: break if checkgoal: return findpath(len(path.milestones) - 1) if ccs[-1] != 0 and ccs[-1] != 1 and False: #didn't connect to either start or goal... delete isolated points? print "cartesian_path_interpolate(): Isolated node, removing..." edges = [(i, j, t) for (i, j, t) in edges if i != nx and j == nx] selfMotionManifolds[irand].pop(-1) nodes.pop(-1) configs.pop(-1) ccs.pop(-1) #raw_input() if maximize: #find the point furthest along the path startccs = set() for c in selfMotionManifolds[0]: startccs.add(ccs[c]) maxdepth = 0 maxnode = 0 for i, cc in enumerate(ccs): if nodes[i][0] > maxdepth and cc in startccs: maxdepth = nodes[i][0] maxnode = i print "cartesian_path_interpolate(): Connected components:" for n, cc in zip(nodes, ccs): print " ", n, ":", cc print "cartesian_path_interpolate(): Got to depth", maxdepth return findpath(maxdepth) print "cartesian_path_interpolate(): Unable to find a feasible path within", numSamples, "iterations" print "cartesian_path_interpolate(): Number of feasible samples per time instance:" return None return None
def cartesian_interpolate_bisect(robot, a, b, constraints, startConfig='robot', endConfig=None, delta=1e-2, solver=None, feasibilityTest=None, growthTol=10): """Resolves a continuous robot trajectory that interpolates between two Cartesian points for a single link of a robot. The output path is only a kinematic resolution. It has time domain [0,1]. Args: robot (RobotModel or SubRobotModel): the robot. a, b (list of floats): endpoints of the Cartesian trajectory. Assumed derived from `config.getConfig(constraints)` constraints: one or more link indices, link names, or ``IKObjective``'s giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together. startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or `None` (any configuration) endConfig (optional): same type as `startConfig`. delta (float, optional): the maximum configuration space distance between points on the output path solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns `False` when a configuration q is infeasible. growthTol (float, optional): the end path can be at most `growthTol` times the length of the length between the start and goal configurations. Returns: RobotTrajectory or None: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0, "Spatial resolution must be positive" assert growthTol > 1, "Growth parameter must be in range [1,infty]" constraints, startConfig, endConfig, solver = _make_canonical( robot, constraints, startConfig, endConfig, solver) assert startConfig is not None, "Unable to cartesian bisection interpolate without a start configuration" if endConfig is None: #find an end point robot.setConfig(startConfig) if not solve_cartesian(b, constraints, solver): print "cartesian_interpolate_bisect(): Error, could not find an end configuration to match final Cartesian coordinates" return None endConfig = robot.getConfig() robot.setConfig(startConfig) set_cartesian_constraints(a, constraints, solver) if not solver.isSolved(): if not solver.solve(): print "Error, initial configuration cannot be solved to match initial Cartesian coordinates, residual", solver.getResidual( ) return None print "Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() robot.setConfig(endConfig) set_cartesian_constraints(b, constraints, solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_interpolate_bisect(): Error, final configuration cannot be solved to match final Cartesian coordinates, residual", solver.getResidual( ) return None print "Warning, final configuration does not match final Cartesian coordinates, solving" endConfig = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(startConfig): print "cartesian_interpolate_bisect(): Error: initial configuration is infeasible" return None if feasibilityTest is not None and not feasibilityTest(endConfig): print "cartesian_interpolate_bisect(): Error: final configuration is infeasible" return None root = _BisectNode(a, b, 0, 1, startConfig, endConfig) root.d = robot.distance(startConfig, endConfig) dtotal = root.d dorig = root.d scalecond = 0.5 * (2 - 2.0 / growthTol) q = deque() q.append(root) while len(q) > 0: n = q.pop() d0 = n.d if d0 <= delta: continue m = config.interpolate(constraints, n.a, n.b, 0.5) qm = robot.interpolate(n.qa, n.qb, 0.5) um = (n.ua + n.ub) * 0.5 robot.setConfig(qm) solver.setBiasConfig(qm) if not solve_cartesian(m, constraints, solver): solver.setBiasConfig([]) print "cartesian_interpolate_bisect(): Failed to solve at point", um return None solver.setBiasConfig([]) d1 = robot.distance(n.qa, qm) d2 = robot.distance(qm, n.qb) #print d1,d2 #print qm,"->",robot.getConfig() qm = robot.getConfig() d1 = robot.distance(n.qa, qm) d2 = robot.distance(qm, n.qb) dtotal += d1 + d2 - d0 if dtotal > dorig * growthTol or (d1 > scalecond * d0) or ( d2 > scalecond * d0): print "cartesian_interpolate_bisect(): Excessive growth condition reached", d0, d1, d2, "at point", um print n.qa print qm print n.qb return None if feasibilityTest and not feasibilityTest(qm): print "cartesian_interpolate_bisect(): Violation of feasibility test", "at point", um return None n.left = _BisectNode(n.a, m, n.ua, um, n.qa, qm) n.left.d = d1 n.right = _BisectNode(m, n.b, um, n.ub, qm, n.qb) n.right.d = d2 if d1 < d2: q.append(n.left) q.append(n.right) else: q.append(n.right) q.append(n.left) #done resolving, now output path from left to right of tree res = RobotTrajectory(robot, [0], [startConfig]) q = [root] while len(q) > 0: n = q.pop(-1) if n.left is None: #leaf node res.times.append(n.ub) res.milestones.append(n.qb) else: q.append(n.right) q.append(n.left) return res
def cartesian_interpolate_linear(robot,a,b,constraints, startConfig='robot',endConfig=None, delta=1e-2, solver=None, feasibilityTest=None, maximize=False): """Resolves a continuous robot trajectory that interpolates between two cartesian points for specified IK constraints. Note that the output path is only a kinematic resolution. It has time domain [0,1]. Args: robot: the RobotModel or SubRobotModel. a, b (list of floats): endpoints of the Cartesian trajectory. Assumed derived from config.getConfig(constraints) constraints: one or more link indices, link names, or IKObjective's giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or None (any configuration) endConfig (optional): same type as startConfig. delta (float, optional): the maximum configuration space distance between points on the output path solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns false when a configuration q is infeasible maximize (bool, optional): if true, goes as far as possible along the path. Returns: RobotTrajectory: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0,"Spatial resolution must be positive" constraints,startConfig,endConfig,solver = _make_canonical(robot,constraints,startConfig,endConfig,solver) assert startConfig is not None,"Unable to cartesian interpolate without a start configuration" robot.setConfig(startConfig) set_cartesian_constraints(a,constraints,solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_interpolate_linear(): Error, initial configuration cannot be solved to match initial Cartesian coordinates, residual",solver.getResidual() return None print "cartesian_interpolate_linear(): Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(startConfig): print "Error: initial configuration is infeasible" return None if endConfig is not None: #doing endpoint-constrained cartesian interpolation set_cartesian_constraints(b,constraints,solver) robot.setConfig(endConfig) if not solver.isSolved(): print "cartesian_interpolate_linear(): Error, end configuration does not match final Cartesian coordinates, residual",solver.getResidual() return None if feasibilityTest is not None and not feasibilityTest(startConfig): print "cartesian_interpolate_linear(): Error: final configuration is infeasible" return None res = RobotTrajectory(robot) t = 0 res.times.append(t) res.milestones.append(startConfig) qmin0,qmax0 = solver.getJointLimits() tol0 = solver.getTolerance() solver.setTolerance(tol0*0.1) set_cartesian_constraints(a,constraints,solver) if not solver.isSolved(): solver.solve() res.times.append(t+1e-7) res.milestones.append(robot.getConfig()) t = res.times[-1] paramStallTolerance = 0.01*solver.getTolerance() / config.distance(constraints,a,b) stepsize = 0.1 while t < 1: tookstep = False tend = min(t+stepsize,1) x = config.interpolate(constraints,a,b,tend) if endConfig is not None: robot.setConfig(robot.interpolate(startConfig,endConfig,tend)) solver.setBiasConfig(robot.getConfig()) q = res.milestones[-1] solver.setJointLimits([max(vmin,v-delta) for v,vmin in zip(q,qmin0)],[min(vmax,v+delta) for v,vmax in zip(q,qmax0)]) #print "Trying step",tend-t,"time t=",tend if solve_cartesian(x,constraints,solver): #valid step, increasing step size #print "Accept and increase step" tookstep = True stepsize *= 1.5 else: #do a line search while stepsize > paramStallTolerance: stepsize *= 0.5 tend = min(t+stepsize,1) x = config.interpolate(constraints,a,b,tend) if endConfig is not None: robot.setConfig(robot.interpolate(startConfig,endConfig,tend)) solver.setBiasConfig(robot.getConfig()) else: robot.setConfig(q) #print "Trying step",tend-t,"time t=",tend if solve_cartesian(x,constraints,solver): #print "Accept" tookstep = True break else: solver.setTolerance(tol0) if solver.isSolved(): #print "Grudgingly accepted" tookstep = True break solver.setTolerance(tol0*0.1) if not tookstep: print "cartesian_interpolate_linear(): Failed to take a valid step along straight line path at time",res.times[-1],"residual",solver.getResidual() #x = config.interpolate(constraints,a,b,res.times[-1]) #set_cartesian_constraints(x,constraints,solver) #robot.setConfig(res.milestones[-1]) #print "Last residual",solver.getResidual() #x = config.interpolate(constraints,a,b,tend) #set_cartesian_constraints(x,constraints,solver) #print "Residual from last config",solver.getResidual() solver.setJointLimits(qmin0,qmax0) solver.setTolerance(tol0) if maximize: return res return None x = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(x): print "cartesian_interpolate_linear(): Infeasibility at time",tend solver.setJointLimits(qmin0,qmax0) solver.setTolerance(tol0) if maximize: return res return None #print "Distances from last:",max(abs(a-b) for (a,b) in zip(res.milestones[-1],x)) res.times.append(tend) res.milestones.append(x) t = tend solver.setJointLimits(qmin0,qmax0) solver.setTolerance(tol0) if endConfig is not None: if robot.distance(res.milestones[-2],endConfig) > delta: #hit a local minimum, couldn't reach the goal if maximize: res.times.pop(-1) res.milestones.pop(-1) return res #print "Hit a local minimum while trying to reach end configuration" return None else: #clean up the end configuration res.milestones[-1] = endConfig return res
def cartesian_path_interpolate(robot,path,constraints, startConfig='robot',endConfig=None, delta=1e-2, method='any', solver=None, feasibilityTest=None, numSamples=1000, maximize=False): """Resolves a continuous robot trajectory that follows a cartesian path for a single link of a robot. Note that the output path is only a kinematic resolution, and may not respect the robot's velocity / acceleration limits. Args: robot: the RobotModel or SubRobotModel. path: a list of milestones, or a Trajectory for the parameters of the given constraints. In the former case the milestones are spaced 1s apart in time. constraints: one or more link indices, link names, or IKObjective's giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or None (any configuration) endConfig (optional): same type as startConfig. delta (float, optional): the maximum configuration space distance between points on the output path method: method used. Can be 'any', 'pointwise', or 'roadmap'. solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns false when a configuration q is infeasible numSamples (int, optional): if 'roadmap' or 'any' method is used, the # of configuration space samples that are used. maximize (bool, optional): if true, goes as far as possible along the path. Returns: RobotTrajectory: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0,"Spatial resolution must be positive" if hasattr(path,'__iter__'): path = Trajectory(range(len(path)),path) constraints,startConfig,endConfig,solver = _make_canonical(robot,constraints,startConfig,endConfig,solver) #correct start and goal configurations, if specified if startConfig: robot.setConfig(startConfig) set_cartesian_constraints(path.milestones[0],constraints,solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_path_interpolate(): Error, initial configuration cannot be solved to match initial Cartesian coordinates" return None print "cartesian_path_interpolate(): Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() if endConfig: robot.setConfig(endConfig) set_cartesian_constraints(path.milestones[-1],constraints,solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_path_interpolate(): Error, final configuration cannot be solved to match final Cartesian coordinates" return None print "cartesian_path_interpolate(): Warning, final configuration does not match final Cartesian coordinates, solving" endConfig = robot.getConfig() #now we're at a canonical setup if method == 'any' or method == 'pointwise': #try pointwise resolution first if startConfig is None: if ik.solve_global(constraints,solver.getMaxIters(),solver.getTolerance(),solver.getActiveDofs(),max(100,numSamples),feasibilityTest): startConfig = robot.getConfig() else: print "cartesian_path_interpolate(): Error: could not solve for start configuration" return None res = RobotTrajectory(robot) res.times.append(path.times[0]) res.milestones.append(startConfig) infeasible = False for i in xrange(len(path.milestones)-1): if endConfig is None: segEnd = None else: u = (path.times[i+1] - path.times[i])/(path.times[-1] - path.times[i]) segEnd = robot.interpolate(res.milestones[-1],endConfig,u) robot.setConfig(segEnd) if solve_cartesian(path.milestones[i+1],constraints,solver): segEnd = robot.getConfig() if segEnd is None: seg = cartesian_interpolate_linear(robot,path.milestones[i],path.milestones[i+1],constraints, startConfig=res.milestones[-1],endConfig=segEnd,delta=delta,solver=solver,feasibilityTest=feasibilityTest) else: seg = cartesian_interpolate_bisect(robot,path.milestones[i],path.milestones[i+1],constraints, startConfig=res.milestones[-1],endConfig=segEnd,delta=delta,solver=solver,feasibilityTest=feasibilityTest) if not seg: print "cartesian_path_interpolate(): Found infeasible cartesian interpolation segment at time",path.times[i+1] infeasible = True break #concatenate dt = path.times[i+1] - path.times[i] seg.times = [t*dt for t in seg.times] res = res.concat(seg,relative=True) if not infeasible: #print "Resolved with pointwise interpolation!" return res if method == 'pointwise' and maximize: return res if method == 'any' or method == 'roadmap': #TODO: sample on continuous parameterization of path if path.duration() > 0: #manual discretization using config.interpolate numdivs = 20 divpts = [path.startTime() + path.duration()*float(i)/(numdivs-1) for i in xrange(numdivs)] oldseg = 0 oldu = 0 times = [0] milestones = [path.milestones[0]] for t in divpts: s,u = path.getSegment(t) if s+1 >= len(path.milestones): s = len(path.milestones)-2 u = 1 if s == oldseg: if u != oldu: times.append(t) milestones.append(config.interpolate(constraints,path.milestones[s],path.milestones[s+1],u)) else: for i in range(oldseg+1,s+1): times.append(path.times[i]) milestones.append(path.milestones[i]) times.append(t) print s,u milestones.append(config.interpolate(constraints,path.milestones[s],path.milestones[s+1],u)) oldseg,oldu = s,u path = path.constructor()(times,milestones) import random #mark whether we need to sample the end or start pathIndices = range(1,len(path.milestones)-1) if startConfig is None: pathIndices = [0] + pathIndices if endConfig is None: pathIndices = pathIndices + [len(path.milestones)-1] samp = 0 if startConfig is None: #need to seed a start configuration while samp < numSamples: samp += 1 solver.sampleInitial() if solve_cartesian(path.milestones[0],constraints,solver): if feasibilityTest is None or feasibilityTest(robot.getConfig()): startConfig = robot.getConfig() break if endConfig is None: #need to seed an end configuration samp = 0 while samp < numSamples: samp += 1 if samp > 0: solver.sampleInitial() else: robot.setConfig(startConfig) if solve_cartesian(path.milestones[-1],constraints,solver): if feasibilityTest is None or feasibilityTest(robot.getConfig()): endConfig = robot.getConfig() break if startConfig is None or endConfig is None: print "cartesian_path_interpolate(): Exhausted all samples, perhaps endpoints are unreachable" return None selfMotionManifolds = [[] for i in path.milestones] nodes = [] configs = [] ccs = [] edges = [] def findpath(depth): #start and goal are connected! find a path through the edges list using BFS eadj = [[] for n in nodes] for (i,j,p) in edges: eadj[i].append((j,p)) q = deque() parent = [None]*len(nodes) for c in selfMotionManifolds[0]: q.append(c) #print "Adjacency list" #for i,alist in enumerate(eadj): # print nodes[i],": ",' '.join(str(nodes[j]) for (j,p) in alist) while len(q) > 0: n = q.pop() for c,p in eadj[n]: if parent[c] is not None: continue parent[c] = n if nodes[c][0] == depth: print "cartesian_path_interpolate(): Found a path using roadmap after",samp,"samples" #arrived at goal node, trace parent list back npath = [] n = c while c is not None: npath.append(c) c = parent[c] npath = [n for n in reversed(npath)] print ' '.join(str(nodes[n]) for n in npath) assert nodes[npath[0]][0] == 0,"Didnt end up at a start configuration?" res = RobotTrajectory(robot) res.times.append(path.times[0]) res.milestones.append(configs[npath[0]]) for i,n in enumerate(npath[:-1]): found = False for j,p in eadj[n]: if j == npath[i+1]: #print "Suffix",p.times[0],p.times[-1] #print res.times[0],res.times[-1] res = res.concat(p,relative=False) #print "Resulting range",res.times[0],res.times[-1] found = True break assert found,"Internal error? "+str(nodes[npath[i]])+" -> "+str(nodes[npath[i+1]]) return res q.append(c) print "cartesian_path_interpolate(): Path to depth",depth,"could not be found" return None selfMotionManifolds[0].append(0) configs.append(startConfig) nodes.append((0,0)) ccs.append(0) selfMotionManifolds[-1].append(1) configs.append(endConfig) nodes.append((len(path.milestones)-1,0)) ccs.append(1) for samp in xrange(samp,numSamples): irand = random.choice(pathIndices) solver.sampleInitial() #check for successful sample on self motion manifold, test feasibility if not solve_cartesian(path.milestones[irand],constraints,solver): continue x = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(x): continue #add to data structure nx = len(nodes) nodes.append((irand,len(selfMotionManifolds[irand]))) ccs.append(nx) assert len(ccs) == nx+1 selfMotionManifolds[irand].append(nx) configs.append(x) #try connecting to other nodes k = int(math.log(samp+2)) + 2 #brute force k-nearest neighbor d = [] for i,n in enumerate(nodes[:-1]): if n[0] == irand: continue dist = config.distance(constraints,path.milestones[n[0]],path.milestones[irand]) dist = robot.distance(x,configs[i]) d.append((dist,i)) k = min(k,len(d)) print "cartesian_path_interpolate(): Sampled at time point",irand,"checking",k,"potential connections" totest = [v[1] for v in sorted(d)[:k]] for n in totest: i = irand j = nodes[n][0] qi = x qj = configs[n] ni = nx nj = n if ccs[ni] == ccs[nj]: #same connected component, use visibility graph technique continue if i > j: i,j = j,i qi,qj = qj,qi ni,nj = nj,ni pij = path.constructor()(path.times[i:j+1],path.milestones[i:j+1]) #try connecting edges t = cartesian_path_interpolate(robot,pij,constraints, startConfig=qi,endConfig=qj,delta=delta,method='pointwise',solver=solver,feasibilityTest=feasibilityTest) #t = cartesian_interpolate_bisect(robot,path.milestones[i],path.milestones[j],constraints,qi,qj,delta=delta,solver=solver,feasibilityTest=feasibilityTest) if t is None: print " Failed edge",nodes[ni],"->",nodes[nj] continue #t.times = [path.times[i] + v*(path.times[j]-path.times[i]) for v in t.times] print " Added edge",nodes[ni],"->",nodes[nj] edges.append((ni,nj,t)) if ccs[ni] != ccs[nj]: #not in same connected component, collapse ccs src,tgt = ccs[ni],ccs[nj] if src < tgt: src,tgt = tgt,src checkgoal = False for i,cc in enumerate(ccs): if ccs[i] == src: ccs[i] = tgt if nodes[i][0] == 0 or nodes[i][0] == len(path.milestones)-1: checkgoal=True if checkgoal: checkgoal = False for c in selfMotionManifolds[0]: for d in selfMotionManifolds[-1]: if ccs[c] == ccs[d]: checkgoal = True break if checkgoal: break if checkgoal: return findpath(len(path.milestones)-1) if ccs[-1] != 0 and ccs[-1] != 1 and False: #didn't connect to either start or goal... delete isolated points? print "cartesian_path_interpolate(): Isolated node, removing..." edges = [(i,j,t) for (i,j,t) in edges if i != nx and j == nx] selfMotionManifolds[irand].pop(-1) nodes.pop(-1) configs.pop(-1) ccs.pop(-1) #raw_input() if maximize: #find the point furthest along the path startccs = set() for c in selfMotionManifolds[0]: startccs.add(ccs[c]) maxdepth = 0 maxnode = 0 for i,cc in enumerate(ccs): if nodes[i][0] > maxdepth and cc in startccs: maxdepth = nodes[i][0] maxnode = i print "cartesian_path_interpolate(): Connected components:" for n,cc in zip(nodes,ccs): print " ",n,":",cc print "cartesian_path_interpolate(): Got to depth",maxdepth return findpath(maxdepth) print "cartesian_path_interpolate(): Unable to find a feasible path within",numSamples,"iterations" print "cartesian_path_interpolate(): Number of feasible samples per time instance:" return None return None
def cartesian_interpolate_bisect(robot,a,b,constraints, startConfig='robot',endConfig=None, delta=1e-2, solver=None, feasibilityTest=None, growthTol=10): """Resolves a continuous robot trajectory that interpolates between two cartesian points for a single link of a robot. Note that the output path is only a kinematic resolution. It has time domain [0,1]. Args: robot: the RobotModel or SubRobotModel. a, b (list of floats): endpoints of the Cartesian trajectory. Assumed derived from config.getConfig(constraints) constraints: one or more link indices, link names, or IKObjective's giving the manner in which the Cartesian space is defined. Interpreted as follows: * int or str: the specified link's entire pose is constrained * IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory. * list of int, list of str, or list of IKObjective: concatenates the specified constraints together startConfig (optional): either 'robot' (configuration taken from the robot), a configuration, or None (any configuration) endConfig (optional): same type as startConfig. delta (float, optional): the maximum configuration space distance between points on the output path solver (IKSolver, optional): if provided, an IKSolver configured with the desired parameters for IK constraint solving. feasibilityTest (function, optional): a function f(q) that returns false when a configuration q is infeasible growthTol (float, optional): the end path can be at most growthTol the times length of the length between the start and goal configurations. Returns: RobotTrajectory: a configuration space path that interpolates the Cartesian path, or None if no solution can be found. """ assert delta > 0,"Spatial resolution must be positive" assert growthTol > 1,"Growth parameter must be in range [1,infty]" constraints,startConfig,endConfig,solver = _make_canonical(robot,constraints,startConfig,endConfig,solver) assert startConfig is not None,"Unable to cartesian bisection interpolate without a start configuration" if endConfig is None: #find an end point robot.setConfig(startConfig) if not solve_cartesian(b,constraints,solver): print "cartesian_interpolate_bisect(): Error, could not find an end configuration to match final Cartesian coordinates" return None endConfig = robot.getConfig() robot.setConfig(startConfig) set_cartesian_constraints(a,constraints,solver) if not solver.isSolved(): if not solver.solve(): print "Error, initial configuration cannot be solved to match initial Cartesian coordinates, residual",solver.getResidual() return None print "Warning, initial configuration does not match initial Cartesian coordinates, solving" startConfig = robot.getConfig() robot.setConfig(endConfig) set_cartesian_constraints(b,constraints,solver) if not solver.isSolved(): if not solver.solve(): print "cartesian_interpolate_bisect(): Error, final configuration cannot be solved to match final Cartesian coordinates, residual",solver.getResidual() return None print "Warning, final configuration does not match final Cartesian coordinates, solving" endConfig = robot.getConfig() if feasibilityTest is not None and not feasibilityTest(startConfig): print "cartesian_interpolate_bisect(): Error: initial configuration is infeasible" return None if feasibilityTest is not None and not feasibilityTest(endConfig): print "cartesian_interpolate_bisect(): Error: final configuration is infeasible" return None root = BisectNode(a,b,0,1,startConfig,endConfig) root.d = robot.distance(startConfig,endConfig) dtotal = root.d dorig = root.d scalecond = 0.5*(2 - 2.0/growthTol) q = deque() q.append(root) while len(q) > 0: n = q.pop() d0 = n.d if d0 <= delta: continue m = config.interpolate(constraints,n.a,n.b,0.5) qm = robot.interpolate(n.qa,n.qb,0.5) um = (n.ua+n.ub)*0.5 robot.setConfig(qm) solver.setBiasConfig(qm) if not solve_cartesian(m,constraints,solver): solver.setBiasConfig([]) print "cartesian_interpolate_bisect(): Failed to solve at point",um return None solver.setBiasConfig([]) d1 = robot.distance(n.qa,qm) d2 = robot.distance(qm,n.qb) #print d1,d2 #print qm,"->",robot.getConfig() qm = robot.getConfig() d1 = robot.distance(n.qa,qm) d2 = robot.distance(qm,n.qb) dtotal += d1+d2 - d0 if dtotal > dorig*growthTol or (d1 > scalecond*d0) or (d2 > scalecond*d0): print "cartesian_interpolate_bisect(): Excessive growth condition reached",d0,d1,d2,"at point",um print n.qa print qm print n.qb return None if feasibilityTest and not feasibilityTest(qm): print "cartesian_interpolate_bisect(): Violation of feasibility test","at point",um return None n.left = BisectNode(n.a,m,n.ua,um,n.qa,qm) n.left.d = d1 n.right = BisectNode(m,n.b,um,n.ub,qm,n.qb) n.right.d = d2 if d1 < d2: q.append(n.left) q.append(n.right) else: q.append(n.right) q.append(n.left) #done resolving, now output path from left to right of tree res = RobotTrajectory(robot,[0],[startConfig]) q = [root] while len(q) > 0: n = q.pop(-1) if n.left is None: #leaf node res.times.append(n.ub) res.milestones.append(n.qb) else: q.append(n.right) q.append(n.left) return res