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
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
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
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
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
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
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[:]
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[:]
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
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
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
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