def solve_ik(robotlink, localpos, worldpos, index):
"""
:param robotlink: {Klampt.RobotModelLink} -- the Klampt robot link model
:param localpos: {list} -- the list of points in the robot link frame
:param worldpos: {list} -- the list of points in the world frame
:param index: {int} -- the index number
:return: robot.getConfig(): {list} -- the list of the robot configuration
"""
robot = robotlink.robot()
space = robotcspace.RobotCSpace(robot)
obj = ik.objective(robotlink, local=localpos, world=worldpos)
maxIters = 100
tol = 1e-8
for i in range(1000):
s = ik.solver(obj, maxIters, tol)
res = s.solve()
if res and not space.selfCollision() and not space.envCollision():
return robot.getConfig()
else:
print(
"Couldn't solve IK problem in " + str(index) +
"th. Or the robot exists self-collision or environment collision."
)
s.sampleInitial()
def score(self,robot):
"""Returns an error score that is equal to the optimum at a feasible
solution. Evaluated at the robot's current configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
c = (0 if self.costFunction is None else self.costFunction(robot.getConfig()))
return c+vectorops.norm(solver.getResidual())
def score(self, robot):
"""Returns an error score that is equal to the optimum at a feasible
solution. Evaluated at the robot's current configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
c = (0 if self.costFunction is None else self.costFunction(
robot.getConfig()))
return c + vectorops.norm(solver.getResidual())
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def get_ik_solver(self, robot):
"""Returns a configured IK solver that will try to achieve the
specified constraints.
"""
obj = self.ik_constraint.copy()
obj.robot = robot
solver = ik.solver(obj)
q = robot.getConfig()
robot.setConfig(self.set_finger_config(q))
active = solver.getActiveDofs()
marked_active = [False] * robot.numLinks()
for a in active:
marked_active[a] = True
for l in self.finger_links:
marked_active[l] = False
active = [i for i, a in enumerate(marked_active) if a]
solver.setActiveDofs(active)
return solver
def constraintResidual(self, robot):
"""Returns a residual of the constraints at the robot's configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
return solver.getResidual()
def solve(self, robot=None, params=IKSolverParams()):
"""Globally solves the given problem. Returns the solution
configuration or None if failed."""
#set this to False if you want to run the local optimizer for each
#random restart.
postOptimize = True
t0 = time.time()
if len(self.objectives) == 0:
if self.costFunction is not None or self.feasibilityTest is not None:
raise NotImplementedError(
"Raw optimization without IK goals not done yet")
return None
if robot is None:
if not hasattr(self.objectives[0], 'robot'):
print(
"The objectives passed to IKSolver should come from ik.objective() or have their 'robot' member manually set"
)
robot = self.objectives[0].robot
else:
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
if self.activeDofs is not None:
solver.setActiveDofs(self.activeDofs)
ikActiveDofs = self.activeDofs
if self.jointLimits is not None:
solver.setJointLimits(*self.jointLimits)
qmin, qmax = solver.getJointLimits()
if self.activeDofs is None:
#need to distinguish between dofs that affect feasibility vs
ikActiveDofs = solver.getActiveDofs()
if self.costFunction is not None or self.feasibilityTest is not None:
activeDofs = [
i for i in range(len(qmin)) if qmin[i] != qmax[i]
]
nonIKDofs = [i for i in activeDofs if i not in ikActiveDofs]
ikToActive = [activeDofs.index(i) for i in ikActiveDofs]
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(activeDofs))
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(ikActiveDofs))
#sample random start point
if params.startRandom:
solver.sampleInitial()
if len(nonIKDofs) > 0:
q = robot.getConfig()
for i in nonIKDofs:
q[i] = random.uniform(qmin[i], qmax[i])
robot.setConfig(q)
if params.localMethod is not None or params.globalMethod is not None:
#set up optProblem, an instance of optimize.Problem
optProblem = optimize.Problem()
Jactive = [[0.0] * len(activeDofs)] * len(solver.getResidual())
def ikConstraint(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if NEW_KLAMPT:
def ikConstraintJac(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return solver.getJacobian()
else:
#old version of Klamp't didn't compute the jacobian w.r.t. the active DOFS
def ikConstraintJac(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
Jikdofs = solver.getJacobian()
for i in ikActiveDofs:
for j in xrange(len(Jactive)):
Jactive[j][ikToActive[i]] = Jikdofs[j][i]
return Jactive
def costFunc(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
return self.costFunction(q)
def feasFunc(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
return self.feasibilityTest(q)
optProblem.addEquality(ikConstraint, ikConstraintJac)
if len(qmax) > 0:
optProblem.setBounds([qmin[d] for d in activeDofs],
[qmax[d] for d in activeDofs])
if self.costFunction is None:
optProblem.setObjective(lambda x: 0)
else:
optProblem.setObjective(costFunc)
if self.feasibilityTest is not None:
optProblem.setFeasibilityTest(feasFunc)
#optProblem is now ready to use
if self.softObjectives:
softOptProblem = optimize.Problem()
def costFunc(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return vectorops.normSquared(solver.getResidual()) * 0.5
def costFuncGrad(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if len(qmax) > 0:
softOptProblem.setBounds([qmin[d] for d in activeDofs],
[qmax[d] for d in activeDofs])
if self.feasibilityTest is not None:
softOptProblem.setFeasibilityTest(feasFunc)
softOptProblem.setObjective(costFunc, costFuncGrad)
#softOptProblem is now ready to use
if params.globalMethod is not None:
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
if self.softObjectives:
#globally optimize the soft objective function. If 0 objective value is obtained, use equality constrained
#optProblem. If 0 objective value is not obtained, constrain the residual norm-squared to be that value
(succ, res) = global_solve(softOptProblem, params, x0)
if not succ:
print("Global soft optimize returned failure")
return None
for d, v in zip(activeDofs, res):
q[d] = v
if self.costFunction is None:
#no cost function, just return
print("Global optimize succeeded! Cost",
self.costFunction(q))
return q
x0 = res
#now modify the constraint of optProblem
robot.setConfig(q)
residual = solver.getResidual()
if max(abs(v) for v in residual) < params.tol:
#the constraint is satisfied, now just optimize cost
pass
else:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5 * vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return [
vectorops.normSquared(solver.getResidual()) * 0.5 -
threshold
]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality, inequalityGrad)
#do global optimization of the cost function and return
(succ, res) = global_solve(optProblem, params, x0)
if not succ:
print("Global optimize returned failure")
return None
for d, v in zip(activeDofs, res):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(
q):
print("Result from global optimize isn't feasible")
return None
if not softObjectives:
if max(abs(v) for v in solver.getResidual()) > params.tol:
print(
"Result from global optimize doesn't satisfy tolerance. Residual",
vectorops.norm(solver.getResidual()))
return None
#passed
print("Global optimize succeeded! Cost", self.costFunction(q))
return q
#DONT DO THIS... much faster to do newton solves first, then local optimize.
if not postOptimize and params.localMethod is not None:
if self.softObjectives:
raise RuntimeError(
"Direct local optimization of soft objectives is not done yet"
)
#random restart + local optimize
optSolver = optimize.LocalOptimizer(method=params.localMethod)
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
best = None
bestQuality = float('inf')
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
res = optSolver.solve(optProblem, params.numIters, params.tol)
if res[0]:
q = robot.getConfig()
for d, v in zip(activeDofs, res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(
q):
continue
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#random restart
solver.sampleInitial()
q = robot.getConfig()
if len(nonIKDofs) > 0:
for i in nonIKDofs:
q[i] = random.uniform(qmin[i], qmax[i])
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
else:
#random-restart newton-raphson
solver.setMaxIters(params.numIters)
solver.setTolerance(params.tol)
best = None
bestQuality = float('inf')
if self.softObjectives:
#quality is a tuple
bestQuality = bestQuality, bestQuality
quality = None
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
t0 = time.time()
res = solver.solve()
if res or self.softObjectives:
q = robot.getConfig()
#check feasibility if desired
t0 = time.time()
if self.feasibilityTest is not None and not self.feasibilityTest(
q):
if len(nonIKDofs) > 0:
u = float(restart + 0.5) / params.numRestarts
q = robot.getConfig()
#perturbation sampling
for i in nonIKDofs:
delta = u * (qmax[i] - qmin[i]) * 0.5
q[i] = random.uniform(
max(q[i] - delta, qmin[i]),
min(q[i] + delta, qmax[i]))
robot.setConfig(q)
if not self.feasibilityTest(q):
solver.sampleInitial()
continue
else:
solver.sampleInitial()
continue
if self.softObjectives:
residual = solver.getResidual()
ikerr = max(abs(v) for v in residual)
if ikerr < params.tol:
ikerr = 0
else:
#minimize squared error
ikerr = vectorops.normSquared(residual)
if self.costFunction is None:
cost = 0
if ikerr == 0:
#feasible and no cost
return q
else:
cost = self.costFunction(q)
quality = ikerr, cost
else:
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#sample a new ik seed
solver.sampleInitial()
#post-optimize using local optimizer
if postOptimize and best is not None and params.localMethod is not None:
if self.softObjectives:
robot.setConfig(best)
residual = solver.getResidual()
if max(abs(v) for v in residual) > params.tol:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5 * vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d, v in zip(activeDofs, x):
q[d] = v
robot.setConfig(q)
return [
vectorops.normSquared(solver.getResidual()) * 0.5 -
threshold
]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality, inequalityGrad)
optSolver = optimize.LocalOptimizer(method=params.localMethod)
x0 = [best[d] for d in activeDofs]
optSolver.setSeed(x0)
res = optSolver.solve(optProblem, params.numIters, params.tol)
if res[0]:
q = robot.getConfig()
for d, v in zip(activeDofs, res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(
q):
pass
elif self.costFunction is None:
#feasible
best = q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
#print "Optimization improvement",bestQuality,"->",quality
best = q
bestQuality = quality
elif quality > bestQuality + 1e-2:
print("Got worse solution by local optimizing?",
bestQuality, "->", quality)
return best
def constraintResidual(self,robot):
"""Returns a residual of the constraints at the robot's configuration."""
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
return solver.getResidual()
def solve(self,robot=None,params=IKSolverParams()):
"""Globally solves the given problem. Returns the solution
configuration or None if failed."""
#set this to False if you want to run the local optimizer for each
#random restart.
postOptimize = True
t0 = time.time()
if len(self.objectives) == 0:
if self.costFunction is not None or self.feasibilityTest is not None:
raise NotImplementedError("Raw optimization without IK goals not done yet")
return None
if robot is None:
if not hasattr(self.objectives[0],'robot'):
print "The objectives passed to IKSolver should come from ik.objective() or have their 'robot' member manually set"
robot = self.objectives[0].robot
else:
for obj in self.objectives:
obj.robot = robot
solver = ik.solver(self.objectives)
if self.activeDofs is not None:
solver.setActiveDofs(self.activeDofs)
ikActiveDofs = self.activeDofs
if self.jointLimits is not None: solver.setJointLimits(*self.jointLimits)
qmin,qmax = solver.getJointLimits()
if self.activeDofs is None:
#need to distinguish between dofs that affect feasibility vs
ikActiveDofs = solver.getActiveDofs()
if self.costFunction is not None or self.feasibilityTest is not None:
activeDofs = [i for i in range(len(qmin)) if qmin[i] != qmax[i]]
nonIKDofs = [i for i in activeDofs if i not in ikActiveDofs]
ikToActive = [activeDofs.index(i) for i in ikActiveDofs]
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(activeDofs))
else:
activeDofs = ikActiveDofs
nonIKDofs = []
ikToActive = range(len(ikActiveDofs))
#sample random start point
if params.startRandom:
solver.sampleInitial()
if len(nonIKDofs)>0:
q = robot.getConfig()
for i in nonIKDofs:
q[i] = random.uniform(qmin[i],qmax[i])
robot.setConfig(q)
if params.localMethod is not None or params.globalMethod is not None:
#set up optProblem, an instance of optimize.Problem
optProblem = optimize.Problem()
Jactive = [[0.0]*len(activeDofs)]*len(solver.getResidual())
def ikConstraint(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if NEW_KLAMPT:
def ikConstraintJac(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getJacobian()
else:
#old version of Klamp't didn't compute the jacobian w.r.t. the active DOFS
def ikConstraintJac(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
Jikdofs = solver.getJacobian()
for i in ikActiveDofs:
for j in xrange(len(Jactive)):
Jactive[j][ikToActive[i]] = Jikdofs[j][i]
return Jactive
def costFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
return self.costFunction(q)
def feasFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
return self.feasibilityTest(q)
optProblem.addEquality(ikConstraint,ikConstraintJac)
if len(qmax) > 0:
optProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs])
if self.costFunction is None:
optProblem.setObjective(lambda x:0)
else:
optProblem.setObjective(costFunc)
if self.feasibilityTest is not None:
optProblem.setFeasibilityTest(feasFunc)
#optProblem is now ready to use
if self.softObjectives:
softOptProblem = optimize.Problem()
def costFunc(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return vectorops.normSquared(solver.getResidual())*0.5
def costFuncGrad(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return solver.getResidual()
if len(qmax) > 0:
softOptProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs])
if self.feasibilityTest is not None:
softOptProblem.setFeasibilityTest(feasFunc)
softOptProblem.setObjective(costFunc,costFuncGrad)
#softOptProblem is now ready to use
if params.globalMethod is not None:
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
if self.softObjectives:
#globally optimize the soft objective function. If 0 objective value is obtained, use equality constrained
#optProblem. If 0 objective value is not obtained, constrain the residual norm-squared to be that value
(succ,res) = global_solve(softOptProblem,params,x0)
if not succ:
print "Global soft optimize returned failure"
return None
for d,v in zip(activeDofs,res):
q[d] = v
if self.costFunction is None:
#no cost function, just return
print "Global optimize succeeded! Cost",self.costFunction(q)
return q
x0 = res
#now modify the constraint of optProblem
robot.setConfig(q)
residual = solver.getResidual()
if max(abs(v) for v in residual) < params.tol:
#the constraint is satisfied, now just optimize cost
pass
else:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5*vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return [vectorops.normSquared(solver.getResidual())*0.5 - threshold]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality,inequalityGrad)
#do global optimization of the cost function and return
(succ,res) = global_solve(optProblem,params,x0)
if not succ:
print "Global optimize returned failure"
return None
for d,v in zip(activeDofs,res):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
print "Result from global optimize isn't feasible"
return None
if not softObjectives:
if max(abs(v) for v in solver.getResidual()) > params.tol:
print "Result from global optimize doesn't satisfy tolerance. Residual",vectorops.norm(solver.getResidual())
return None
#passed
print "Global optimize succeeded! Cost",self.costFunction(q)
return q
#DONT DO THIS... much faster to do newton solves first, then local optimize.
if not postOptimize and params.localMethod is not None:
if self.softObjectives:
raise RuntimeError("Direct local optimization of soft objectives is not done yet")
#random restart + local optimize
optSolver = optimize.LocalOptimizer(method=params.localMethod)
q = robot.getConfig()
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
best = None
bestQuality = float('inf')
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
res = optSolver.solve(optProblem,params.numIters,params.tol)
if res[0]:
q = robot.getConfig()
for d,v in zip(activeDofs,res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
continue
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#random restart
solver.sampleInitial()
q = robot.getConfig()
if len(nonIKDofs)>0:
for i in nonIKDofs:
q[i] = random.uniform(qmin[i],qmax[i])
x0 = [q[d] for d in activeDofs]
optSolver.setSeed(x0)
else:
#random-restart newton-raphson
solver.setMaxIters(params.numIters)
solver.setTolerance(params.tol)
best = None
bestQuality = float('inf')
if self.softObjectives:
#quality is a tuple
bestQuality = bestQuality,bestQuality
quality = None
for restart in xrange(params.numRestarts):
if time.time() - t0 > params.timeout:
return best
t0 = time.time()
res = solver.solve()
if res or self.softObjectives:
q = robot.getConfig()
#check feasibility if desired
t0 = time.time()
if self.feasibilityTest is not None and not self.feasibilityTest(q):
if len(nonIKDofs) > 0:
u = float(restart+0.5)/params.numRestarts
q = robot.getConfig()
#perturbation sampling
for i in nonIKDofs:
delta = u*(qmax[i]-qmin[i])*0.5
q[i] = random.uniform(max(q[i]-delta,qmin[i]),min(q[i]+delta,qmax[i]))
robot.setConfig(q)
if not self.feasibilityTest(q):
solver.sampleInitial()
continue
else:
solver.sampleInitial()
continue
if self.softObjectives:
residual = solver.getResidual()
ikerr = max(abs(v) for v in residual)
if ikerr < params.tol:
ikerr = 0
else:
#minimize squared error
ikerr = vectorops.normSquared(residual)
if self.costFunction is None:
cost = 0
if ikerr == 0:
#feasible and no cost
return q
else:
cost = self.costFunction(q)
quality = ikerr,cost
else:
if self.costFunction is None:
#feasible
return q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
best = q
bestQuality = quality
#sample a new ik seed
solver.sampleInitial()
#post-optimize using local optimizer
if postOptimize and best is not None and params.localMethod is not None:
if self.softObjectives:
robot.setConfig(best)
residual = solver.getResidual()
if max(abs(v) for v in residual) > params.tol:
#the constraint is not satisfied, now use the residual as the constraint
threshold = 0.5*vectorops.normSquared(residual)
def inequality(x):
q = robot.getConfig()
for d,v in zip(activeDofs,x):
q[d] = v
robot.setConfig(q)
return [vectorops.normSquared(solver.getResidual())*0.5 - threshold]
def inequalityGrad(x):
return [costFuncGrad(x)]
optProblem.equalities = []
optProblem.equalityGrads = []
optProblem.addInequality(inequality,inequalityGrad)
optSolver = optimize.LocalOptimizer(method=params.localMethod)
x0 = [best[d] for d in activeDofs]
optSolver.setSeed(x0)
res = optSolver.solve(optProblem,params.numIters,params.tol)
if res[0]:
q = robot.getConfig()
for d,v in zip(activeDofs,res[1]):
q[d] = v
#check feasibility if desired
if self.feasibilityTest is not None and not self.feasibilityTest(q):
pass
elif self.costFunction is None:
#feasible
best = q
else:
#optimize
quality = self.costFunction(q)
if quality < bestQuality:
#print "Optimization improvement",bestQuality,"->",quality
best = q
bestQuality = quality
elif quality > bestQuality + 1e-2:
print "Got worse solution by local optimizing?",bestQuality,"->",quality
return best