def error_analysis(self):
"""
:return: None
"""
Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])
Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])
trans_error_list = []
trans_error_mat = []
rot_error_list = []
rot_error_mat = []
redraw_list = []
for i in range(len(self.Tlink_set)):
Toct_needle_est = se3.mul(
se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle)
trans_error = vectorops.sub(
se3.inv(self.trans_list[i])[1], Toct_needle_est[1])
rot_error = so3.error(
se3.inv(self.trans_list[i])[0], Toct_needle_est[0])
trans_error_list.append(vectorops.normSquared(trans_error))
trans_error_mat.append(np.absolute(trans_error))
rot_error_list.append(vectorops.normSquared(rot_error))
rot_error_mat.append(np.absolute(rot_error))
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[0])
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[1])
redraw_list.append(
create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0]))
draw_geometries(redraw_list)
trans_error_list = np.array(trans_error_list)
trans_error_mat = np.array(trans_error_mat)
rot_error_list = np.array(rot_error_list)
rot_error_mat = np.array(rot_error_mat)
rmse_trans = np.sqrt(np.mean(trans_error_list))
rmse_rot = np.sqrt(np.mean(rot_error_list))
print("The squared translation error list is:\n ",
np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans)
print("The mean error in XYZ directions is: ",
np.mean(trans_error_mat, axis=0))
print("The squared rotation error list is:\n ",
np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot)
print("The mean error in three rotation vectors is: ",
np.mean(rot_error_mat, axis=0))
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list),
# np.sqrt(rot_error_list)]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0),
# np.mean(rot_error_mat, axis=0)]), allow_pickle=True)
trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
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 config_to_opening(self,qfinger):
"""Estimates far qfinger is from closed_config to open_config.
Only meaningful if qfinger is close to being along the straight
C-space line between the two.
"""
if self.closed_config is None or self.open_config is None:
raise ValueError("Can't estimate opening width to")
assert len(qfinger) == len(self.closed_config)
b = vectorops.sub(qfinger,self.closed_config)
a = vectorops.sub(self.open_config,self.closed_config)
return min(1,max(0,vectorops.dot(a,b)/vectorops.normSquared(a)))
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 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 make(robotfile, world, tempname="temp.rob", debug=False):
"""Converts the given fixed-base robot file into a moving base robot
and loads it into the given world.
Args:
robotfile (str): the name of a fixed-base robot file to load
world (WorldModel): a world that will contain the new robot
tempname (str, optional): a name of a temporary file containing
the moving-base robot
debug (bool, optional): if True, the robot file named by
``tempname`` is not removed from disk.
Returns:
(RobotModel): the loaded robot, stored in ``world``.
"""
_template_ = """### Boilerplate kinematics of a drivable floating (translating and rotating) cube with a robot hand mounted on it
TParent 1 0 0 0 1 0 0 0 1 0 0 0 \\
1 0 0 0 1 0 0 0 1 0 0 0 \\
1 0 0 0 1 0 0 0 1 0 0 0 \\
1 0 0 0 1 0 0 0 1 0 0 0 \\
1 0 0 0 1 0 0 0 1 0 0 0 \\
1 0 0 0 1 0 0 0 1 0 0 0
parents -1 0 1 2 3 4
axis 1 0 0 0 1 0 0 0 1 0 0 1 0 1 0 1 0 0
jointtype p p p r r r
qMin -1 -1 -1 -inf -inf -inf
qMax 1 1 1 inf inf inf
q 0 0 0 0 0 0
links "tx" "ty" "tz" "rz" "ry" "rx"
geometry "" "" "" "" "" "{TriangleMesh\\nOFF\\n8 12 0\\n0 0 0\\n0 0 1\\n0 1 0\\n0 1 1\\n1 0 0\\n1 0 1\\n1 1 0\\n1 1 1\\n3 0 1 3\\n3 0 3 2\\n3 4 6 7\\n3 4 7 5\\n3 0 4 5\\n3 0 5 1\\n3 2 3 7\\n3 2 7 6\\n3 0 2 6\\n3 0 6 4\\n3 1 5 7\\n3 1 7 3\\n}"
geomscale 1 1 1 1 1 0.01
mass 0.1 0.1 0.1 0.1 0.1 0.1
com 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
inertia 0.001 0 0 0 0.001 0 0 0 0.001 \\
0.001 0 0 0 0.001 0 0 0 0.001 \\
0.001 0 0 0 0.001 0 0 0 0.001 \\
0.001 0 0 0 0.001 0 0 0 0.001 \\
0.001 0 0 0 0.001 0 0 0 0.001 \\
0.001 0 0 0 0.001 0 0 0 0.001
torqueMax 500 500 500 50 50 50
accMax 4 4 4 4 4 4 4
velMax 2 2 2 3 3 3
joint normal 0
joint normal 1
joint normal 2
joint spin 3
joint spin 4
joint spin 5
driver normal 0
driver normal 1
driver normal 2
driver normal 3
driver normal 4
driver normal 5
servoP 5000 5000 5000 500 500 500
servoI 10 10 10 .5 .5 .5
servoD 100 100 100 10 10 10
viscousFriction 50 50 50 50 50 50
dryFriction 1 1 1 1 1 1
property sensors <sensors><ForceTorqueSensor name="base_force" link="5" hasForce="1 1 1" hasTorque="1 1 1" /></sensors>
mount 5 "%s" 1 0 0 0 1 0 0 0 1 0 0 0 as "%s"
"""
robotname = os.path.splitext(os.path.basename(robotfile))[0]
f = open(tempname, 'w')
f.write(_template_ % (robotfile, robotname))
f.close()
world.loadElement(tempname)
robot = world.robot(world.numRobots() - 1)
#set torques
mass = sum(robot.link(i).getMass().mass for i in range(robot.numLinks()))
inertia = 0.0
for i in range(robot.numLinks()):
m = robot.link(i).getMass()
inertia += (vectorops.normSquared(m.com) * m.mass + max(m.inertia))
tmax = robot.getTorqueLimits()
tmax[0] = tmax[1] = tmax[2] = mass * 9.8 * 5
tmax[3] = tmax[4] = tmax[5] = inertia * 9.8 * 5
robot.setName("moving-base[" + robotname + "]")
robot.setTorqueMax(tmax)
if debug:
robot.saveFile(tempname)
else:
os.remove(tempname)
return robot
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 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 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