"""Utilities for Cartesian IK solving, interpolation, and path adjustment.
To generate a path that moves to a Cartesian target, the
:func:`cartesian_move_to` function is the most convenient.
To interpolate between two Cartesian task space points,
:func:`cartesian_interpolate_linear` and :func:`cartesian_interpolate_bisect`
are approximately equivalent.
To interpolate along a Cartesian task space path, the
:func:`cartesian_path_interpolate` function implements two methods for solving
for a feasible robot path. A pointwise approach moves greedily along the path,
while a roadmap approach gives a probabilistically complete solver for the
redundancy resolution.
To move an existing joint-space path by a cartesian offset, the
:func:`cartesian_bump` is used. This is useful for adapting motion primitives
to new sensor data.
All the classes in this module use a unified representation of the workspace of
one or more :class:`IKObjective` constraints, which is retrieved by
``config.getConfig(constraints)``. For example, the workspace of a position-
only constraint is its 3D world-space target coordinates. The workspace of a
fixed transform constraint is the 12D concatenation of the rotation matrix and
translation vector. See the :mod:`~klampt.model.config` module for details.
"""
from .trajectory import *
from ..robotsim import IKObjective,IKSolver
from . import ik
from . import config
from collections import deque
import math
import warnings
from typing import Union,Optional,List,Sequence,Callable
from .typing import Vector,Vector3,Matrix3,RigidTransform
[docs]def set_cartesian_constraints(
x: Vector,
constraints: Sequence[IKObjective],
solver: IKSolver
) -> None:
"""For ``x`` a workspace parameter setting (obtained via
``config.getConfig(constraints)``), a set of constraints, and a
:class:`IKSolver` object, modifies the constraints and the solver so that
the solver is setup to match the workspace parameter setting x.
"""
config.setConfig(constraints,x)
solver.clear()
for c in constraints:
solver.add(c)
[docs]def solve_cartesian(
x: Vector,
constraints: Sequence[IKObjective],
solver: IKSolver
) -> bool:
"""For ``x`` a workspace parameter setting (obtained via
``config.getConfig(constraints)``), a set of constraints, and a IKSolver
object, returns True if the solver can find a solution, starting from the
robot's current configuration). Returns True if successful.
"""
set_cartesian_constraints(x,constraints,solver)
return solver.solve()
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.link(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
[docs]def cartesian_interpolate_linear(
robot: Union[RobotModel,SubRobotModel],
a: Vector,
b: Vector,
constraints: Union[int,str,IKObjective,Sequence[int],Sequence[str],Sequence[IKObjective]],
startConfig: Union[str,Vector] = 'robot',
endConfig: Optional[Vector] = None,
delta: float = 1e-2,
solver: Optional[IKSolver] = None,
feasibilityTest: Optional[Callable[[Vector],bool]] = None,
maximize: bool = False
) -> Optional[RobotTrajectory]:
"""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].
Differs from :func:`cartesian_interpolate_bisect` in that the solver moves
incrementally along the path from a to b in linear fashion.
Args:
robot: the RobotModel or SubRobotModel.
a (list of floats): start point of the Cartesian trajectory.
Assumed derived from config.getConfig(constraints)
b (list of floats): start point of the Cartesian trajectory.
Assumed derived from config.getConfig(constraints)
constraints: one or more link indices, link names, or
:class:`IKObjective` objects specifying the Cartesian task.
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:
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():
warnings.warn("cartesian_interpolate_linear(): initial configuration cannot be solved to match initial Cartesian coordinates, residual {}".format(solver.getResidual()),RuntimeWarning)
return None
warnings.warn("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):
warnings.warn("cartesian_interpolate_linear(): initial configuration is infeasible",RuntimeWarning)
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():
warnings.warn("cartesian_interpolate_linear(): Error, end configuration does not match final Cartesian coordinates, residual {}".format(solver.getResidual()),RuntimeWarning)
return None
if feasibilityTest is not None and not feasibilityTest(startConfig):
warnings.warn("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() / max(config.distance(constraints,a,b),0.01)
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:
warnings.warn("cartesian_interpolate_linear(): Failed to take a valid step along straight line path at time {} residual {}".format(res.times[-1],solver.getResidual()),RuntimeWarning)
#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):
warnings.warn("cartesian_interpolate_linear(): Infeasibility at time {}".format(tend),RuntimeWarning)
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
class _BisectNode:
def __init__(self,a,b,ua,ub,qa,qb):
self.a,self.b = a,b
self.ua,self.ub = ua,ub
self.qa,self.qb = qa,qb
self.left,self.right = None,None
[docs]def cartesian_interpolate_bisect(
robot: Union[RobotModel,SubRobotModel],
a: Vector,
b: Vector,
constraints: Union[int,str,IKObjective,Sequence[int],Sequence[str],Sequence[IKObjective]],
startConfig: Union[str,Vector] = 'robot',
endConfig: Optional[Vector] = None,
delta: float = 1e-2,
solver: Optional[IKSolver] = None,
feasibilityTest: Optional[Callable[[Vector],bool]] = None,
maximize: bool = False,
growthTol: int = 10
) -> Optional[RobotTrajectory]:
"""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, and has time domain [0,1].
Differs from :func:`cartesian_interpolate_linear` in that the solver
creates the path from a to b using bisection. This function may be more
capable, but doesn't accept the ``maximize`` argument in case the query
doesn't have a solution.
Args:
robot (RobotModel or SubRobotModel): the robot.
a (list of floats): start point of the Cartesian trajectory.
Assumed derived from config.getConfig(constraints)
b (list of floats): start point of the Cartesian trajectory.
Assumed derived from config.getConfig(constraints)
constraints: one or more link indices, link names, or
:class:`IKObjective` objects specifying the Cartesian task space.
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:
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):
warnings.warn("cartesian_interpolate_bisect(): 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():
warnings.warn("cartesian_interpolate_bisect(): initial configuration cannot be solved to match initial Cartesian coordinates, residual {}".format(solver.getResidual()),RuntimeWarning)
return None
warnings.warn("cartesian_interpolate_bisect(): 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():
warnings.warn("cartesian_interpolate_bisect(): final configuration cannot be solved to match final Cartesian coordinates, residual {}".format(solver.getResidual()),RuntimeWarning)
return None
warnings.warn("cartesian_interpolate_bisect(): final configuration does not match final Cartesian coordinates, solving")
endConfig = robot.getConfig()
if feasibilityTest is not None and not feasibilityTest(startConfig):
warnings.warn("cartesian_interpolate_bisect(): initial configuration is infeasible",RuntimeWarning)
return None
if feasibilityTest is not None and not feasibilityTest(endConfig):
warnings.warn("cartesian_interpolate_bisect(): Error: final configuration is infeasible",RuntimeWarning)
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([])
warnings.warn("cartesian_interpolate_bisect(): Failed to solve at point {}".format(um),RuntimeWarning)
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):
warnings.warn("cartesian_interpolate_bisect(): Excessive growth condition reached {} {} {} at point {}".format(d0,d1,d2,um),RuntimeWarning)
#print(n.qa)
#print(qm)
#print(n.qb)
return None
if feasibilityTest and not feasibilityTest(qm):
warnings.warn("cartesian_interpolate_bisect(): Violation of feasibility test at point {}".format(um),RuntimeWarning)
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
[docs]def cartesian_path_interpolate(
robot: Union[RobotModel,SubRobotModel],
path: Union[Trajectory,Sequence[Vector]],
constraints: Union[int,str,IKObjective,Sequence[int],Sequence[str],Sequence[IKObjective]],
startConfig: Union[str,Vector] = 'robot',
endConfig: Optional[Vector] = None,
delta: float = 1e-2,
method: str = 'any',
solver: Optional[IKSolver] = None,
feasibilityTest: Optional[Callable[[Vector],bool]] = None,
numSamples: int = 1000,
maximize: bool = False
) -> Optional[RobotTrajectory]:
"""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.
.. note::
Only compatible with :class:`Trajectory`, not
:class:`HermiteTrajectory`. If a single link is provided, an
:class:`SE3Trajectory` can be provided (but not
:class:`SE3HermiteTrajectory`.)
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 or
:class:`IKObjective` objects specifying the Cartesian task space.
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:
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(list(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():
warnings.warn("cartesian_path_interpolate(): initial configuration cannot be solved to match initial Cartesian coordinates")
return None
warnings.warn("cartesian_path_interpolate(): Warning, initial configuration does not match initial Cartesian coordinates, solving",RuntimeWarning)
startConfig = robot.getConfig()
if endConfig:
robot.setConfig(endConfig)
set_cartesian_constraints(path.milestones[-1],constraints,solver)
if not solver.isSolved():
if not solver.solve():
warnings.warn("cartesian_path_interpolate(): final configuration cannot be solved to match final Cartesian coordinates")
return None
warnings.warn("cartesian_path_interpolate(): final configuration does not match final Cartesian coordinates, solving",RuntimeWarning)
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:
warnings.warn("cartesian_path_interpolate(): 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 range(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:
warnings.warn("cartesian_path_interpolate(): Found infeasible cartesian interpolation segment at time {}".format(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 range(numdivs)]
oldseg = 0
oldu = 0
times = [0]
milestones = [path.milestones[0]]
for t in divpts:
s,u = path.getSegment(t)
if s < 0:
s = 0
if s+1 >= len(path.milestones):
s = len(path.milestones)-2
u = 1
if s == oldseg:
if u != oldu:
assert t > times[-1]
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):
assert path.times[i] > times[-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
for i in range(len(times)-1):
assert times[i] < times[i+1]
path = path.constructor()(times,milestones)
import random
#mark whether we need to sample the end or start
pathIndices = list(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:
warnings.warn("cartesian_path_interpolate(): Exhausted all samples, perhaps endpoints are unreachable",RuntimeWarning)
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:
warnings.warn("cartesian_path_interpolate(): Found a path using roadmap after {} samples".format(samp),RuntimeWarning)
#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)]
warnings.warn(' '.join(str(nodes[n]) for n in npath),RuntimeWarning)
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)
warnings.warn("cartesian_path_interpolate(): Path to depth {} could not be found".format(depth),RuntimeWarning)
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 range(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))
#warnings.warn("cartesian_path_interpolate(): Sampled at time point {} checking {} potential connections".format(irand,k),RuntimeWarning)
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:
#warnings.warn(" Failed edge {} -> {}".format(nodes[ni],nodes[nj]),RuntimeWarning)
continue
#t.times = [path.times[i] + v*(path.times[j]-path.times[i]) for v in t.times]
#warnings.warn(" Added edge {} -> {}".format(nodes[ni],nodes[nj]),RuntimeWarning)
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?
warnings.warn("cartesian_path_interpolate(): Isolated node, removing...",RuntimeWarning)
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
warnings.warn("cartesian_path_interpolate(): Connected components:",RuntimeWarning)
for n,cc in zip(nodes,ccs):
warnings.warn(" {}:{}".format(n,cc),RuntimeWarning)
warnings.warn("cartesian_path_interpolate(): Got to depth {}".format(maxdepth),RuntimeWarning)
return findpath(maxdepth)
warnings.warn("cartesian_path_interpolate(): Unable to find a feasible path within {} iterations".format(numSamples),RuntimeWarning)
warnings.warn("cartesian_path_interpolate(): Number of feasible samples per time instance:")
return None
return None
[docs]def cartesian_bump(
robot: Union[RobotModel,SubRobotModel],
js_path: Union[Trajectory,RobotTrajectory],
constraints: Union[int,str,IKObjective,Sequence[int],Sequence[str],Sequence[IKObjective]],
bump_paths: Union[RigidTransform,SE3Trajectory,Sequence[RigidTransform],Sequence[SE3Trajectory]],
delta: float = 1e-2,
solver: Optional[IKSolver] = None,
ee_relative: bool = False,
maximize: bool = False,
closest: bool = False,
maxDeviation: Optional[List[float]] = None
) -> Optional[RobotTrajectory]:
"""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
:class:`IKObjective` objects specifying the Cartesian task space.
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:
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 delta <= 0:
raise ValueError("Invalid delta, must be positive")
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.link(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 range(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 range(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():
warnings.warn("cartesian_bump(): Unable to perform Cartesian solve on milestone at time {}".format(t),RuntimeWarning)
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:
solver.minimize() #make sure to properly minimize the residual rather than relying on solve()
closeIntervals.add(i)
#otherwise soldier on with an imperfect solution
else:
numsolved += 1
res.milestones.append(robot.getConfig())
warnings.warn("cartesian_bump(): Solved %d/%d milestone configurations along path, now interpolating paths..."%(numsolved,len(res.milestones)),RuntimeWarning)
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:
warnings.warn("cartesian_bump(): Unable to complete bump while subdividing segment at time {}".format(ta),RuntimeWarning)
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
warnings.warn("cartesian_bump(): Resolved %d/%d bumped edges"%(numResolved,numTotalEdges),RuntimeWarning)
return res
[docs]def cartesian_move_to(
robot:Union[RobotModel,SubRobotModel],
constraints: Union[int,str,IKObjective,Sequence[int],Sequence[str],Sequence[IKObjective]],
delta: float = 1e-2,
solver: Optional[IKSolver] = None,
feasibilityTest: Optional[Callable[[Vector],bool]] = None,
maximize: bool = False
) -> Optional[RobotTrajectory]:
"""A convenience function that generates a path that performs a linear
cartesian interpolation starting from the robot's current configuration
and ending at the desired IK constraints.
This is a bit more convenient than :func:`cartesian_interpolate_linear`
since you only need to pass in the target objective, rather than the
start and end Cartesian parameters as well.
Usage::
path = cartesian_move_to(robot,goal)
Other arguments are equivalent to those in cartesian_interpolate_linear.
"""
if not hasattr(constraints,'__iter__'):
constraints = [constraints]
for c in constraints:
assert isinstance(c,IKObjective)
#extract the task space coordinates of the constraints
taskEnd = config.getConfig(constraints)
#extract the task space coordinates of the current robot
for c in constraints:
xforml = robot.link(c.link()).getTransform()
xformr = robot.link(c.destLink()).getTransform() if c.destLink() >= 0 else se3.identity()
c.matchDestination(*se3.mul(se3.inv(xformr),xforml))
taskStart = config.getConfig(constraints)
#just call the solver
return cartesian_interpolate_linear(robot,taskStart,taskEnd,constraints,
delta=delta,solver=solver,feasibilityTest=feasibilityTest,
maximize=maximize)