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 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