コード例 #1
0
ファイル: contact.py プロジェクト: smeng9/Klampt
def contactMapHolds(contactmap):
    """Given a contact map, computes a set of non-conflicting
    Holds that enforce all simultaneous contact constraints.  Usually called in conjunction with contactMap
    with the following sequence::

        objectives = contactMapHolds(contactMap(contacts,lambda x:x==None or isinstance(x,TerrainModel)))

    """
    holds = []
    for ((o1, o2), clist) in contactmap.iteritems():
        assert o1 != None

        if not isinstance(o1, RobotModelLink):
            raise ValueError(
                "Cannot retrieve Holds for contact map not containing robot links"
            )
        h = Hold()
        h.link = o1.index
        h.contacts = clist
        x1loc = [o1.getLocalPosition(c.x) for c in clist]
        if o2 is not None and not isinstance(o2, TerrainModel):
            x2loc = [o2.getLocalPosition(c.x) for c in clist]
            h.ikConstraint = ik.objective(o1, o2, local=x1loc, world=x2loc)
        else:
            x2 = [c.x for c in clist]
            h.ikConstraint = ik.objective(o1, local=x1loc, world=x2)
        holds.append(h)
    return holds
コード例 #2
0
ファイル: contact.py プロジェクト: krishauser/Klampt
def contactMapHolds(contactmap):
    """Given a contact map, computes a set of non-conflicting
    Holds that enforce all simultaneous contact constraints.  Usually called in conjunction with contactMap
    with the following sequence:

    objectives = contactMapHolds(contactMap(contacts,lambda x:x==None or isinstance(x,TerrainModel)))
    """
    holds = []
    for ((o1,o2),clist) in contactmap.iteritems():
        assert o1 != None
        
        if not isinstance(o1,RobotModelLink):
            raise ValueError("Cannot retrieve Holds for contact map not containing robot links")
        h = Hold()
        h.link = o1.index
        h.contacts = clist
        x1loc = [o1.getLocalPosition(c.x) for c in clist]
        if o2 is not None and not isinstance(o2,TerrainModel):
            x2loc = [o2.getLocalPosition(c.x) for c in clist]
            h.ikConstraint = ik.objective(o1,o2,local=x1loc,world=x2loc)
        else:
            x2 = [c.x for c in clist]
            h.ikConstraint = ik.objective(o1,local=x1loc,world=x2)
        holds.append(h)
    return holds
コード例 #3
0
def _make_canonical(robot, constraints, startConfig, endConfig, solver):
    if not hasattr(constraints, '__iter__'):
        constraints = [constraints]
    for c in constraints:
        if isinstance(c, (int, str)):
            newconstraints = []
            for d in constraints:
                if isinstance(d, (int, str)):
                    newconstraints.append(
                        ik.objective(robot, d, R=so3.identity(), t=[0, 0, 0]))
                else:
                    assert isinstance(d, IKObjective)
                    newconstraints.append(d)
            if solver:
                newSolver = IKSolver(solver)
                newSolver.clear()
                for d in newconstraints:
                    newSolver.add(d)
            else:
                newSolver = None
            constraints = newconstraints
            solver = newSolver
            break
    if solver is None:
        solver = ik.solver(constraints)
    if startConfig == 'robot':
        startConfig = robot.getConfig()
    if endConfig == 'robot':
        endConfig = robot.getConfig()
    return constraints, startConfig, endConfig, solver
コード例 #4
0
def _make_canonical(robot,constraints,startConfig,endConfig,solver):
    if not hasattr(constraints,'__iter__'):
        constraints = [constraints]
    for c in constraints:
        if isinstance(c,(int,str)):
            newconstraints = []
            for d in constraints:
                if isinstance(d,(int,str)):
                    newconstraints.append(ik.objective(robot,d,R=so3.identity(),t=[0,0,0]))
                else:
                    assert isinstance(d,IKObjective)
                    newconstraints.append(d)
            if solver:
                newSolver = IKSolver(solver)
                newSolver.clear()
                for d in newconstraints:
                    newSolver.add(d)
            else:
                newSolver = None
            constraints = newconstraints
            solver = newSolver
            break
    if solver is None:
        solver = ik.solver(constraints)
    if startConfig=='robot':
        startConfig = robot.getConfig()
    if endConfig=='robot':
        endConfig = robot.getConfig()
    return constraints,startConfig,endConfig,solver
コード例 #5
0
ファイル: contact.py プロジェクト: victor8733/Klampt
def contactMapIKObjectives(contactmap):
    """Given a contact map, computes a set of non-conflicting
    IKObjective's or GeneralizedIKObjective's that enforce all simultaneous
    contact constraints.  Usually called in conjunction with contactMap
    with the following sequence:

    objectives = contactMapIKObjectives(contactMap(contacts,lambda x:x==None or isinstance(x,TerrainModel)))
    """
    objectives = []
    for ((o1, o2), clist) in contactmap.iteritems():
        assert o1 != None

        x1loc = [o1.getLocalPosition(c.x) for c in clist]
        if o2 is not None and not isinstance(o2, TerrainModel):
            x2loc = [o2.getLocalPosition(c.x) for c in clist]
            objectives.append(ik.objective(o1, o2, local=x1loc, world=x2loc))
        else:
            x2 = [c.x for c in clist]
            objectives.append(ik.objective(o1, local=x1loc, world=x2))
    return objectives
コード例 #6
0
ファイル: contact.py プロジェクト: krishauser/Klampt
def contactMapIKObjectives(contactmap):
    """Given a contact map, computes a set of non-conflicting
    IKObjective's or GeneralizedIKObjective's that enforce all simultaneous
    contact constraints.  Usually called in conjunction with contactMap
    with the following sequence:

    objectives = contactMapIKObjectives(contactMap(contacts,lambda x:x==None or isinstance(x,TerrainModel)))
    """
    objectives = []
    for ((o1,o2),clist) in contactmap.iteritems():
        assert o1 != None
        
        x1loc = [o1.getLocalPosition(c.x) for c in clist]
        if o2 is not None and not isinstance(o2,TerrainModel):
            x2loc = [o2.getLocalPosition(c.x) for c in clist]
            objectives.append(ik.objective(o1,o2,local=x1loc,world=x2loc))
        else:
            x2 = [c.x for c in clist]
            objectives.append(ik.objective(o1,local=x1loc,world=x2))
    return objectives
コード例 #7
0
ファイル: contact.py プロジェクト: krishauser/Klampt
    def setFixed(self,link,contacts):
        """Creates this hold such that it fixes a robot link to match a list of contacts
        (in world space) at its current transform.

        Args:
            link: a robot link or rigid object, currently contacting the environment / object at contacts
            contacts (list of ContactPoint): the fixed contact points, given in world coordinates.
        """
        assert isinstance(link,(RobotModelLink,RigidObjectModel)),"Argument must be a robot link or rigid object"
        self.link = link.index
        T = link.getTransform()
        Tinv = se3.inv(T)
        self.ikConstraint = ik.objective(link,local=[se3.apply(Tinv,c.x) for c in contacts],world=[c.x for c in contacts])
        self.contacts = contacts[:]
コード例 #8
0
ファイル: contact.py プロジェクト: victor8733/Klampt
    def setFixed(self, link, contacts):
        """Creates this hold such that it fixes a robot link to match a list of contacts
        (in world space) at its current transform.

        Args:
            link: a robot link or rigid object, currently contacting the environment / object at contacts
            contacts (list of ContactPoint): the fixed contact points, given in world coordinates.
        """
        assert isinstance(link, (
            RobotModelLink,
            RigidObjectModel)), "Argument must be a robot link or rigid object"
        self.link = link.index
        T = link.getTransform()
        Tinv = se3.inv(T)
        self.ikConstraint = ik.objective(
            link,
            local=[se3.apply(Tinv, c.x) for c in contacts],
            world=[c.x for c in contacts])
        self.contacts = contacts[:]
コード例 #9
0
ファイル: coordinates.py プロジェクト: krishauser/Klampt
def ik_objective(obj,target):
    """Returns an IK objective that attempts to fix the given
    klampt.coordinates object 'obj' at given target object 'target'. 

    Arguments:
        obj: An instance of one of the {Point,Direction,Transform,Frame} classes.
        target: If 'obj' is a Point, Direction, or  Frame objects, this
            must be an object of the same type of 'obj' denoting the target to
            which 'obj' should be fixed.  In other words, the local coordinates
            of 'obj' relative to 'target's parent frame will be equal to 'target's
            local coordinates.
            If obj is a Transform object, this element is an se3 object.

    Returns:
        (IKObjective): An IK objective to be used with the klampt.ik module.

    Since the klampt.ik module is not aware about custom frames, an
    ancestor of the object must be attached to a RobotModelLink or a
    RigidObjectModel, or else None will be returned.  The same goes for target,
    if provided.

    TODO: support lists of objects to fix.

    TODO: support Direction constraints.
    """
    body = None
    coords = None
    ref = None
    if isinstance(obj,Frame):
        assert isinstance(target,Frame),"ik_objective: target must be of same type as obj"
        body = obj
        ref = target.parent()
        coords = target.relativeCoordinates()
    elif isinstance(obj,Transform):
        if ref != None: print "ik_objective: Warning, ref argument passed with Transform object, ignoring"
        body = obj.source()
        ref = obj.destination()
        coords = target
    elif isinstance(obj,(Point,Direction)):
        assert type(target)==type(obj),"ik_objective: target must be of same type as obj"
        body = obj.frame()
        ref = target.frame()
        coords = target.localCoordinates()
    else:
        raise ValueError("Argument to ik_objective must be an object from the coordinates module")
    
    linkframe = _ancestor_with_link(body)
    if linkframe == None:
        print "Warning: object provided to ik_objective is not attached to a robot link or rigid object, returning None"
        return None
    linkbody = linkframe._data

    #find the movable frame attached to ref
    refframe = _ancestor_with_link(ref) if ref != None else None
    refbody = (refframe._data if refframe!=None else None)
    if isinstance(obj,(Frame,Transform)):
        #figure out the desired transform T[linkbody->refbody], given
        #coords = T[obj->ref], T[obj->linkbody], T[ref->refbody]
        #result = (T[ref->refbody] * coords * T[obj->linkbody]^-1)
        if linkframe != body: coords = se3.mul(coords,Transform(linkframe,body).coordinates())
        if refframe != ref: coords = se3.mul(Transform(ref,refframe).coordinates(),coords)
        return ik.objective(linkbody,ref=refbody,R=coords[0],t=coords[1])
    elif isinstance(obj,Point):
        #figure out the local and world points
        local = obj.to(linkframe).localCoordinates()
        world = target.to(refframe).localCoordinates()
        return ik.objective(linkbody,local=[local],world=[world])
    elif isinstance(obj,Direction):
        raise ValueError("Axis constraints are not yet supported in the klampt.ik module")
    return None
コード例 #10
0
ファイル: coordinates.py プロジェクト: whutddk/Klampt
def ik_objective(obj,target):
    """Returns an IK objective that attempts to fix the given
    klampt.coordinates object 'obj' at given target object 'target'. 

    Arguments:
     - obj: An instance of one of the
       {Point,Direction,Transform,Frame} classes.
     - target: If 'obj' is a Point, Direction, or  Frame objects, this
       must be an object of the same type of 'obj' denoting the target to
       which 'obj' should be fixed.  In other words, the local coordinates
       of 'obj' relative to 'target's parent frame will be equal to 'target's
       local coordinates.
       If obj is a Transform object, this element is an se3 object.
    Return value:
     - An IK objective to be used with the klampt.ik module.

    Since the klampt.ik module is not aware about custom frames, an
    ancestor of the object must be attached to a RobotModelLink or a
    RigidObjectModel, or else None will be returned.  The same goes for target,
    if provided.

    TODO: support lists of objects to fix.

    TODO: support Direction constraints.
    """
    body = None
    coords = None
    ref = None
    if isinstance(obj,Frame):
        assert isinstance(target,Frame),"ik_objective: target must be of same type as obj"
        body = obj
        ref = target.parent()
        coords = target.relativeCoordinates()
    elif isinstance(obj,Transform):
        if ref != None: print "ik_objective: Warning, ref argument passed with Transform object, ignoring"
        body = obj.source()
        ref = obj.destination()
        coords = target
    elif isinstance(obj,(Point,Direction)):
        assert type(target)==type(obj),"ik_objective: target must be of same type as obj"
        body = obj.frame()
        ref = target.frame()
        coords = target.localCoordinates()
    else:
        raise ValueError("Argument to ik_objective must be an object from the coordinates module")
    
    linkframe = _ancestor_with_link(body)
    if linkframe == None:
        print "Warning: object provided to ik_objective is not attached to a robot link or rigid object, returning None"
        return None
    linkbody = linkframe._data

    #find the movable frame attached to ref
    refframe = _ancestor_with_link(ref) if ref != None else None
    refbody = (refframe._data if refframe!=None else None)
    if isinstance(obj,(Frame,Transform)):
        #figure out the desired transform T[linkbody->refbody], given
        #coords = T[obj->ref], T[obj->linkbody], T[ref->refbody]
        #result = (T[ref->refbody] * coords * T[obj->linkbody]^-1)
        if linkframe != body: coords = se3.mul(coords,Transform(linkframe,body).coordinates())
        if refframe != ref: coords = se3.mul(Transform(ref,refframe).coordinates(),coords)
        return ik.objective(linkbody,ref=refbody,R=coords[0],t=coords[1])
    elif isinstance(obj,Point):
        #figure out the local and world points
        local = obj.to(linkframe).localCoordinates()
        world = target.to(refframe).localCoordinates()
        return ik.objective(linkbody,local=[local],world=[world])
    elif isinstance(obj,Direction):
        raise ValueError("Axis constraints are not yet supported in the klampt.ik module")
    return None
コード例 #11
0
def cartesian_bump(robot,
                   js_path,
                   constraints,
                   bump_paths,
                   delta=1e-2,
                   solver=None,
                   ee_relative=False,
                   maximize=False,
                   closest=False,
                   maxDeviation=None):
    """Given the robot and a reference joint space trajectory, "bumps" the
    trajectory in Cartesian space using a given relative transform (or
    transform path).  The movement in joint space is approximately minimized
    to follow the bumped Cartesian path.

    For example, to translate the motion of an end effector by [dx,dy,dz]
    in world coordinates,  call::

        cartesian_bump(robot,traj,ik.fixed_objective(link),se3.from_translation([dx,dy,dz]))

    Args:
        robot (RobotModel or SubRobotModel): the robot for which the bump is
            applied.  
        js_path (Trajectory or RobotTrajectory): the reference joint space
            Trajectory of the robot.
        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

        bump_paths: one or more transforms or transform paths specifying the
            world-space relative "bump" of each cartesian goal.  One bump per
            constraint must be given as input.  Each bump can either be a
            static klampt.se3 element or a SE3Trajectory.
        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.
        ee_relative (bool, optional): if False (default), bumps are given in
            world coordinates.  If True, bumps are given in end-effector local
            coordinates.
        maximize (bool, optional): if true, goes as far as possible along the
            path.
        closest (bool, optional): if not resolved and this is True, the
            function returns the robot trajectory
            whose cartesian targets get as close as possible (locally) to the
            bumped trajectory
        maxDeviation (list of floats, optional): if not None, this is a vector
            giving the max joint space distance by which each active joint is
            allowed to move from `js_path`.

    Returns:
        RobotTrajectory or None: the bumped trajectory, or None if no path can
        be found.
    """
    #make into canonical form
    if not hasattr(constraints, '__iter__'):
        constraints = [constraints]
    if not hasattr(bump_paths, '__iter__'):
        bump_paths = [bump_paths]
    assert len(constraints) == len(
        bump_paths), "Must specify one bump per constraint"
    if maxDeviation != None:
        assert len(maxDeviation) == robot.numLinks()
    for c in constraints:
        if isinstance(c, (int, str)):
            newconstraints = []
            for d in constraints:
                if isinstance(d, (int, str)):
                    newconstraints.append(
                        ik.objective(robot, d, R=so3.identity(), t=[0, 0, 0]))
                else:
                    assert isinstance(d, IKObjective)
                    newconstraints.append(d)
    is1xform = any(isinstance(p, (int, float)) for p in bump_paths)
    if is1xform:
        bump_paths = [bump_paths]
    meshpts = []
    for i, p in enumerate(bump_paths):
        #it's a static transform, map it to a path
        if isinstance(p, (list, tuple)):
            bump_paths[i] = SE3Trajectory(times=[js_path.startTime()],
                                          milestones=[p])
        else:
            assert hasattr(p,
                           'times'), "bump_paths must contain SE3Trajectory's"
            meshpts += p.times
    if solver is None:
        solver = ik.solver(constraints)
    #now preprocess the joint space so that everything is initially within delta distance
    for i in xrange(len(js_path.milestones) - 1):
        d = robot.distance(js_path.milestones[i], js_path.milestones[i + 1])
        if d > delta:
            #add in subdividing mesh points
            a, b = js_path.times[i], js_path.times[i + 1]
            numdivs = int(math.ceil(d / delta))
            for j in xrange(1, numdivs):
                meshpts.append(a + float(j) / float(numdivs) * (b - a))
    #ensure that all the movements of the SE3 trajectories are captured
    if len(meshpts) > 0:
        js_path = js_path.remesh(meshpts)[0]
    qmin0, qmax0 = solver.getJointLimits()
    #OK, now move all the cartesian points
    numsolved = 0
    res = RobotTrajectory(robot)
    res.times = js_path.times
    closeIntervals = set()
    for i, q in enumerate(js_path.milestones):
        t = js_path.times[i]
        robot.setConfig(q)
        solver.clear()
        for c, p in zip(constraints, bump_paths):
            xform0 = robot.link(c.link()).getTransform()
            xformrel = p.eval_se3(t)
            xform = (se3.mul(xform0, xformrel) if ee_relative else se3.mul(
                xformrel, xform0))
            c.matchDestination(*xform)
            solver.add(c)
        if maxDeviation != None:
            qmin = [
                max(v - d, vmin)
                for (v, d, vmin) in zip(q, maxDeviation, qmin0)
            ]
            qmax = [
                min(v + d, vmax)
                for (v, d, vmax) in zip(q, maxDeviation, qmax0)
            ]
            solver.setJointLimits(qmin, qmax)
        if not solver.solve():
            print "cartesian_bump(): Unable to perform Cartesian solve on milestone at time", t
            if not closest:
                if maximize:
                    #going as far as possible, just clip the result
                    res.times = res.times[:len(res.milestones)]
                    break
                else:
                    solver.setJointLimits(qmin0, qmax0)
                    return None
            else:
                closeIntervals.add(i)
                #otherwise soldier on with an imperfect solution
        else:
            numsolved += 1
        res.milestones.append(robot.getConfig())
    print "cartesian_bump(): Solved %d/%d milestone configurations along path, now interpolating paths..." % (
        numsolved, len(res.milestones))
    numResolved = 0
    numTotalEdges = len(res.milestones) - 1
    solver.setJointLimits(qmin0, qmax0)
    i = 0
    while i + 1 < len(res.milestones):
        q = res.milestones[i]
        qnext = res.milestones[i + 1]
        d = robot.distance(q, qnext)
        if d > delta:
            if i in closeIntervals:
                i += 1
                continue
            #resolve cartesian path
            ta = res.times[i]
            tb = res.times[i + 1]
            robot.setConfig(q)
            for c, p in zip(constraints, bump_paths):
                xform0 = robot.link(c.link()).getTransform()
                c.matchDestination(*xform0)
            xa = config.getConfig(constraints)
            robot.setConfig(qnext)
            for c, p in zip(constraints, bump_paths):
                xform0 = robot.link(c.link()).getTransform()
                c.matchDestination(*xform0)
            xb = config.getConfig(constraints)
            #TODO: add joint limits into the solver?
            newseg = cartesian_interpolate_bisect(robot,
                                                  xa,
                                                  xb,
                                                  constraints,
                                                  startConfig=q,
                                                  endConfig=qnext,
                                                  delta=delta,
                                                  solver=solver)
            if newseg == None:
                print "cartesian_bump(): Unable to complete bump while subdividing segment at time", ta
                if closest:
                    i += 1
                    continue
                if maximize:
                    res.times = res.times[:i + 1]
                    res.milestones = res.milestones[:i + 1]
                return None
            #splice in the results
            assert newseg.times[0] == 0 and newseg.times[-1] == 1
            newseg.times = [ta + float(t) * (tb - ta) for t in newseg.times]
            res.times = res.times[:i + 1] + newseg.times[1:-1] + res.times[i +
                                                                           1:]
            #print "Adding",len(newseg.milestones)-2,"intermediate milestones"
            assert res.milestones[i] == newseg.milestones[0]
            assert res.milestones[i + 1] == newseg.milestones[-1]
            res.milestones = res.milestones[:i + 1] + newseg.milestones[
                1:-1] + res.milestones[i + 1:]
            #adjust close intervals
            newclose = set()
            for c in closeIntervals:
                if c > i:
                    newclose.add(c + len(newseg.times) - 2)
            closeIntervals = newclose
            i += len(newseg.milestones) - 2
            numResolved += 1
        else:
            #print "Skipping",i
            numResolved += 1
        i += 1
    print "cartesian_bump(): Resolved %d/%d bumped edges" % (numResolved,
                                                             numTotalEdges)
    return res
コード例 #12
0
def cartesian_bump(robot,js_path,constraints,bump_paths,
    delta=1e-2,
    solver=None,
    ee_relative=False,
    maximize=False,
    closest=False,
    maxDeviation=None):
    """Given the robot and a reference joint space trajectory, "bumps" the trajectory
    in cartesian space using a given relative transform (or transform path).  The movement in
    joint space is approximately minimized to follow the bumped cartesian path.

    For example, to translate the motion of an end effector by [dx,dy,dz] in world coordinates, 
    call::

        cartesian_bump(robot,traj,ik.fixed_objective(link),se3.from_translation([dx,dy,dz]))

    Args:
        robot (RobotModel or SubRobotModel): the robot for which the bump is applied.  
        js_path: the reference joint space Trajectory of the robot.  May be a RobotTrajectory.
        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

        bump_paths: one or more transforms or transform paths specifying the world-space relative
            "bump" of each cartesian goal.  One bump per constraint must be given as input.  Each bump can
            either be a static klampt.se3 element or a SE3Trajectory.
        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.
        ee_relative (bool, optional): if False (default), bumps are given in world coordinates.  If True,
            bumps are given in end-effector local coordinates.
        maximize (bool, optional): if true, goes as far as possible along the path.
        closest (bool, optional): if not resolved and this is True, the function returns the robot trajectory
            whose cartesian targets get as close as possible (locally) to the bumped trajectory
        maxDeviation (list of floats, optional): if not None, this is a vector giving the max joint space
            distance by which each active joint is allowed to move from js_path.

    Returns:
        RobotTrajectory: the bumped trajectory, or None if none can be found
    """
    #make into canonical form
    if not hasattr(constraints,'__iter__'):
        constraints = [constraints]
    if not hasattr(bump_paths,'__iter__'):
        bump_paths = [bump_paths]
    assert len(constraints) == len(bump_paths),"Must specify one bump per constraint"
    if maxDeviation != None:
        assert len(maxDeviation) == robot.numLinks()
    for c in constraints:
        if isinstance(c,(int,str)):
            newconstraints = []
            for d in constraints:
                if isinstance(d,(int,str)):
                    newconstraints.append(ik.objective(robot,d,R=so3.identity(),t=[0,0,0]))
                else:
                    assert isinstance(d,IKObjective)
                    newconstraints.append(d)
    is1xform = any(isinstance(p,(int,float)) for p in bump_paths)
    if is1xform:
        bump_paths = [bump_paths]
    meshpts = []
    for i,p in enumerate(bump_paths):
        #it's a static transform, map it to a path
        if isinstance(p,(list,tuple)):
            bump_paths[i] = SE3Trajectory(times=[js_path.startTime()],milestones=[p])
        else:
            assert hasattr(p,'times'),"bump_paths must contain SE3Trajectory's"
            meshpts += p.times
    if solver is None:
        solver = ik.solver(constraints)
    #now preprocess the joint space so that everything is initially within delta distance
    for i in xrange(len(js_path.milestones)-1):
        d = robot.distance(js_path.milestones[i],js_path.milestones[i+1])
        if d > delta:
            #add in subdividing mesh points
            a,b = js_path.times[i],js_path.times[i+1]
            numdivs = int(math.ceil(d/delta))
            for j in xrange(1,numdivs):
                meshpts.append(a + float(j)/float(numdivs)*(b-a))
    #ensure that all the movements of the SE3 trajectories are captured
    if len(meshpts) > 0:
        js_path = js_path.remesh(meshpts)[0]
    qmin0,qmax0 = solver.getJointLimits()
    #OK, now move all the cartesian points
    numsolved = 0
    res = RobotTrajectory(robot)
    res.times = js_path.times
    closeIntervals = set()
    for i,q in enumerate(js_path.milestones):
        t = js_path.times[i]
        robot.setConfig(q)
        solver.clear()
        for c,p in zip(constraints,bump_paths):
            xform0 = robot.link(c.link()).getTransform()
            xformrel = p.eval_se3(t)
            xform = (se3.mul(xform0,xformrel) if ee_relative else se3.mul(xformrel,xform0))
            c.matchDestination(*xform)
            solver.add(c)
        if maxDeviation != None:
            qmin = [max(v-d,vmin) for (v,d,vmin) in zip(q,maxDeviation,qmin0)]
            qmax = [min(v+d,vmax) for (v,d,vmax) in zip(q,maxDeviation,qmax0)]
            solver.setJointLimits(qmin,qmax)
        if not solver.solve():
            print "cartesian_bump(): Unable to perform Cartesian solve on milestone at time",t
            if not closest:
                if maximize:
                    #going as far as possible, just clip the result
                    res.times = res.times[:len(res.milestones)]
                    break
                else:
                    solver.setJointLimits(qmin0,qmax0)
                    return None
            else:
                closeIntervals.add(i)
                #otherwise soldier on with an imperfect solution
        else:
            numsolved += 1
        res.milestones.append(robot.getConfig())
    print "cartesian_bump(): Solved %d/%d milestone configurations along path, now interpolating paths..."%(numsolved,len(res.milestones))
    numResolved = 0
    numTotalEdges = len(res.milestones)-1
    solver.setJointLimits(qmin0,qmax0)
    i = 0
    while i+1 < len(res.milestones):
        q = res.milestones[i]
        qnext = res.milestones[i+1]
        d = robot.distance(q,qnext)
        if d > delta:
            if i in closeIntervals:
                i += 1
                continue
            #resolve cartesian path
            ta = res.times[i]
            tb = res.times[i+1]
            robot.setConfig(q)
            for c,p in zip(constraints,bump_paths):
                xform0 = robot.link(c.link()).getTransform()
                c.matchDestination(*xform0)
            xa = config.getConfig(constraints)
            robot.setConfig(qnext)
            for c,p in zip(constraints,bump_paths):
                xform0 = robot.link(c.link()).getTransform()
                c.matchDestination(*xform0)
            xb = config.getConfig(constraints)
            #TODO: add joint limits into the solver?
            newseg = cartesian_interpolate_bisect(robot,xa,xb,constraints,
                startConfig=q,endConfig=qnext,
                delta=delta,solver=solver)
            if newseg == None:
                print "cartesian_bump(): Unable to complete bump while subdividing segment at time",ta
                if closest:
                    i += 1
                    continue
                if maximize:
                    res.times = res.times[:i+1]
                    res.milestones = res.milestones[:i+1]
                return None
            #splice in the results
            assert newseg.times[0] == 0 and newseg.times[-1] == 1
            newseg.times = [ta + float(t)*(tb-ta) for t in newseg.times]
            res.times = res.times[:i+1]+newseg.times[1:-1]+res.times[i+1:]
            #print "Adding",len(newseg.milestones)-2,"intermediate milestones"
            assert res.milestones[i] == newseg.milestones[0]
            assert res.milestones[i+1] == newseg.milestones[-1]
            res.milestones = res.milestones[:i+1]+newseg.milestones[1:-1]+res.milestones[i+1:]
            #adjust close intervals
            newclose = set()
            for c in closeIntervals:
                if c > i:
                    newclose.add(c + len(newseg.times)-2)
            closeIntervals = newclose
            i += len(newseg.milestones)-2
            numResolved += 1
        else:
            #print "Skipping",i
            numResolved += 1
        i += 1
    print "cartesian_bump(): Resolved %d/%d bumped edges"%(numResolved,numTotalEdges)
    return res