def test_simple_problem():
H = np.eye(2) * 2
A = np.ones((1, 2))
g = np.zeros(2)
lba = np.array([10.])
lb = np.array([-10., -10.])
ub = np.array([10., 10.])
qp = QPSolver()
x = qp.solve(H, g, A, lb, ub, lba, lba)
np.testing.assert_array_almost_equal(x, np.array([5, 5]), decimal=4)
def test_non_diagonal_H():
H = np.array([[1, -0.5], [-1, -1]])
A = np.ones((1, 2))
g = np.zeros(2)
lba = np.array([-10.])
lb = np.array([-10., -10.])
ub = np.array([10., 10.])
qp = QPSolver()
x = qp.solve(H, g, A, lb, ub, lba, lba)
print(x)
print(qp.qpProblem.getObjVal())
class QProblemBuilder(object):
"""
Wraps around QPOases. Builds the required matrices from constraints.
"""
def __init__(self,
joint_constraints_dict,
hard_constraints_dict,
soft_constraints_dict,
controlled_joint_symbols,
path_to_functions=''):
"""
:type joint_constraints_dict: dict
:type hard_constraints_dict: dict
:type soft_constraints_dict: dict
:type controlled_joint_symbols: list
:param path_to_functions: location where the compiled functions can be safed.
:type path_to_functions: str
"""
assert (
not len(controlled_joint_symbols) > len(joint_constraints_dict))
assert (
not len(controlled_joint_symbols) < len(joint_constraints_dict))
assert (len(hard_constraints_dict) <= len(controlled_joint_symbols))
self.path_to_functions = path_to_functions
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints = controlled_joint_symbols
self.construct_big_ass_M()
self.compile_big_ass_M()
self.shape1 = len(self.hard_constraints_dict) + len(
self.soft_constraints_dict)
self.shape2 = len(self.joint_constraints_dict) + len(
self.soft_constraints_dict)
self.num_hard_constraints = len(self.hard_constraints_dict)
self.num_joint_constraints = len(self.joint_constraints_dict)
self.num_soft_constraints = len(self.soft_constraints_dict)
self.qp_solver = QPSolver()
self.lbAs = None # for debugging purposes
def get_expr(self):
return self.compiled_big_ass_M.str_params
@profile
def construct_big_ass_M(self):
# TODO cpu intensive
weights = []
lb = []
ub = []
lbA = []
ubA = []
linear_weight = []
soft_expressions = []
hard_expressions = []
for constraint_name, constraint in self.joint_constraints_dict.items():
weights.append(constraint.weight)
lb.append(constraint.lower)
ub.append(constraint.upper)
linear_weight.append(constraint.linear_weight)
for constraint_name, constraint in self.hard_constraints_dict.items():
lbA.append(constraint.lower)
ubA.append(constraint.upper)
hard_expressions.append(constraint.expression)
for constraint_name, constraint in self.soft_constraints_dict.items(
): # type: (str, SoftConstraint)
weights.append(constraint.weight)
lbA.append(constraint.lbA)
ubA.append(constraint.ubA)
lb.append(constraint.lower_slack_limit)
ub.append(constraint.upper_slack_limit)
linear_weight.append(constraint.linear_weight)
assert not w.is_matrix(
constraint.expression
), u'Matrices are not allowed as soft constraint expression'
soft_expressions.append(constraint.expression)
self.np_g = np.zeros(len(weights))
logging.loginfo(
u'constructing new controller with {} soft constraints...'.format(
len(soft_expressions)))
self.h = len(self.hard_constraints_dict)
self.s = len(self.soft_constraints_dict)
self.j = len(self.joint_constraints_dict)
self.init_big_ass_M()
self.set_weights(weights)
self.construct_A_hard(hard_expressions)
self.construct_A_soft(soft_expressions)
self.set_lbA(w.Matrix(lbA))
self.set_ubA(w.Matrix(ubA))
self.set_lb(w.Matrix(lb))
self.set_ub(w.Matrix(ub))
self.set_linear_weights(w.Matrix(linear_weight))
@profile
def compile_big_ass_M(self):
t = time()
self.free_symbols = w.free_symbols(self.big_ass_M)
self.compiled_big_ass_M = w.speed_up(self.big_ass_M, self.free_symbols)
logging.loginfo(
u'compiled symbolic expressions in {:.5f}s'.format(time() - t))
def init_big_ass_M(self):
"""
# j s 1 1 1
# |--------------------------------------|
# h | A hard | 0 | | | |
# | -------------------| lbA | ubA | 0 |
# s | A soft |identity| | | |
# |--------------------------------------|
# j+s| H | lb | ub | g |
# | -------------------------------------|
"""
self.big_ass_M = w.zeros(self.h + self.s * 2 + self.j,
self.j + self.s + 3)
def construct_A_hard(self, hard_expressions):
A_hard = w.Matrix(hard_expressions)
A_hard = w.jacobian(A_hard, self.controlled_joints)
self.set_A_hard(A_hard)
def set_A_hard(self, A_hard):
self.big_ass_M[:self.h, :self.j] = A_hard
def construct_A_soft(self, soft_expressions):
A_soft = w.zeros(self.s, self.j + self.s)
t = time()
A_soft[:, :self.j] = w.jacobian(w.Matrix(soft_expressions),
self.controlled_joints)
logging.loginfo(u'computed Jacobian in {:.5f}s'.format(time() - t))
A_soft[:, self.j:] = w.eye(self.s)
self.set_A_soft(A_soft)
def set_A_soft(self, A_soft):
self.big_ass_M[self.h:self.h + self.s, :self.j + self.s] = A_soft
def set_lbA(self, lbA):
self.big_ass_M[:self.h + self.s, self.j + self.s] = lbA
def set_ubA(self, ubA):
self.big_ass_M[:self.h + self.s, self.j + self.s + 1] = ubA
def set_lb(self, lb):
self.big_ass_M[self.h + self.s:, self.j + self.s] = lb
def set_ub(self, ub):
self.big_ass_M[self.h + self.s:, self.j + self.s + 1] = ub
def set_linear_weights(self, linear_weights):
self.big_ass_M[self.h + self.s:, self.j + self.s + 2] = linear_weights
def set_weights(self, weights):
self.big_ass_M[self.h + self.s:, :-3] = w.diag(*weights)
def debug_print(self,
unfiltered_H,
A,
lb,
ub,
lbA,
ubA,
g,
xdot_full=None,
actually_print=False):
import pandas as pd
bA_mask, b_mask = make_filter_masks(unfiltered_H,
self.num_joint_constraints,
self.num_hard_constraints)
b_names = []
bA_names = []
for iJ, k in enumerate(self.joint_constraints_dict.keys()):
key = 'j -- ' + str(k)
b_names.append(key)
for iH, k in enumerate(self.hard_constraints_dict.keys()):
key = 'h -- ' + str(k)
bA_names.append(key)
for iS, k in enumerate(self.soft_constraints_dict.keys()):
key = 's -- ' + str(k)
bA_names.append(key)
b_names.append(key)
b_names = np.array(b_names)
filtered_b_names = b_names[b_mask]
filtered_bA_names = np.array(bA_names)[bA_mask]
filtered_H = unfiltered_H[b_mask][:, b_mask]
p_lb = pd.DataFrame(lb, filtered_b_names, [u'data'],
dtype=float).sort_index()
p_ub = pd.DataFrame(ub, filtered_b_names, [u'data'],
dtype=float).sort_index()
p_g = pd.DataFrame(g, filtered_b_names, [u'data'],
dtype=float).sort_index()
p_lbA = pd.DataFrame(lbA, filtered_bA_names, [u'data'],
dtype=float).sort_index()
p_ubA = pd.DataFrame(ubA, filtered_bA_names, [u'data'],
dtype=float).sort_index()
p_weights = pd.DataFrame(unfiltered_H.dot(
np.ones(unfiltered_H.shape[0])),
b_names, [u'data'],
dtype=float).sort_index()
if xdot_full is not None:
p_xdot = pd.DataFrame(xdot_full,
filtered_b_names, [u'data'],
dtype=float).sort_index()
Ax = np.dot(A, xdot_full)
p_Ax = pd.DataFrame(Ax, filtered_bA_names, [u'data'],
dtype=float).sort_index()
xH = np.dot((xdot_full**2).T, filtered_H)
p_xH = pd.DataFrame(xH, filtered_b_names, [u'data'],
dtype=float).sort_index()
p_xg = p_g * p_xdot
xHx = np.dot(np.dot(xdot_full.T, filtered_H), xdot_full)
x_soft = xdot_full[len(xdot_full) - len(lbA):]
p_lbA_minus_x = pd.DataFrame(lbA - x_soft,
filtered_bA_names, [u'data'],
dtype=float).sort_index()
p_ubA_minus_x = pd.DataFrame(ubA - x_soft,
filtered_bA_names, [u'data'],
dtype=float).sort_index()
else:
p_xdot = None
p_A = pd.DataFrame(A, filtered_bA_names, filtered_b_names,
dtype=float).sort_index(1).sort_index(0)
# if self.lbAs is None:
# self.lbAs = p_lbA
# else:
# self.lbAs = self.lbAs.T.append(p_lbA.T, ignore_index=True).T
# self.lbAs.T[[c for c in self.lbAs.T.columns if 'dist' in c]].plot()
# self.save_all(p_weights, p_A, p_lbA, p_ubA, p_lb, p_ub, p_xdot)
return p_weights, p_A, p_lbA, p_ubA, p_lb, p_ub
def are_joint_limits_violated(self, p_lb, p_ub):
violations = (p_ub - p_lb)[p_lb.data > p_ub.data]
if len(violations) > 0:
logging.logerr(
u'The following joints are outside of their limits: \n {}'.
format(violations))
return True
logging.loginfo(u'All joints are within limits')
return False
def save_all(self, weights, A, lbA, ubA, lb, ub, xdot=None):
if xdot is not None:
print_pd_dfs([weights, A, lbA, ubA, lb, ub, xdot],
['weights', 'A', 'lbA', 'ubA', 'lb', 'ub', 'xdot'])
else:
print_pd_dfs([weights, A, lbA, ubA, lb, ub],
['weights', 'A', 'lbA', 'ubA', 'lb', 'ub'])
def is_nan_in_array(self, name, p_array):
p_filtered = p_array.apply(
lambda x: zip(x.index[x.isnull()].tolist(), x[x.isnull()]), 1)
p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
if len(p_filtered) > 0:
logging.logerr(u'{} has the following nans:'.format(name))
self.print_pandas_array(p_filtered)
return True
logging.loginfo(u'{} has no nans'.format(name))
return False
def print_pandas_array(self, array):
import pandas as pd
if len(array) > 0:
with pd.option_context('display.max_rows', None,
'display.max_columns', None):
print(array)
def filter_zero_weight_constraints(self, H, A, lb, ub, lbA, ubA, g):
bA_mask, b_mask = make_filter_masks(H, self.num_joint_constraints,
self.num_hard_constraints)
A = A[bA_mask][:, b_mask].copy()
lbA = lbA[bA_mask]
ubA = ubA[bA_mask]
lb = lb[b_mask]
ub = ub[b_mask]
g = g[b_mask]
H = H[b_mask][:, b_mask]
return H, A, lb, ub, lbA, ubA, g
@profile
def get_cmd(self, substitutions, nWSR=None):
"""
Uses substitutions for each symbol to compute the next commands for each joint.
:param substitutions:
:type substitutions: list
:return: joint name -> joint command
:rtype: dict
"""
np_big_ass_M = self.compiled_big_ass_M.call2(substitutions)
np_H = np_big_ass_M[self.shape1:, :-3].copy()
np_A = np_big_ass_M[:self.shape1, :self.shape2].copy()
np_lb = np_big_ass_M[self.shape1:, -3].copy()
np_ub = np_big_ass_M[self.shape1:, -2].copy()
np_g = np_big_ass_M[self.shape1:, -1].copy()
np_lbA = np_big_ass_M[:self.shape1, -3].copy()
np_ubA = np_big_ass_M[:self.shape1, -2].copy()
H, A, lb, ub, lbA, ubA, g = self.filter_zero_weight_constraints(
np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, np_g)
# self.debug_print(np_H, A, lb, ub, lbA, ubA)
try:
xdot_full = self.qp_solver.solve(H, g, A, lb, ub, lbA, ubA, nWSR)
except QPSolverException as e:
p_weights, p_A, p_lbA, p_ubA, p_lb, p_ub = self.debug_print(
np_H, A, lb, ub, lbA, ubA, g, actually_print=True)
if isinstance(e, InfeasibleException):
if self.are_joint_limits_violated(p_lb, p_ub):
raise OutOfJointLimitsException(e)
raise HardConstraintsViolatedException(e)
if isinstance(e, QPSolverException):
arrays = [(p_weights, u'H'), (p_A, u'A'), (p_lbA, u'lbA'),
(p_ubA, u'ubA'), (p_lb, u'lb'), (p_ub, u'ub')]
any_nan = False
for a, name in arrays:
any_nan |= self.is_nan_in_array(name, a)
if any_nan:
raise e
raise e
if xdot_full is None:
return None
# TODO enable debug print in an elegant way, preferably without slowing anything down
self.debug_print(np_H, A, lb, ub, lbA, ubA, g, xdot_full)
return OrderedDict((observable, xdot_full[i]) for i, observable in enumerate(self.controlled_joints)), \
np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full
class QProblemBuilder(object):
BACKEND = 'numpy'
def __init__(self, joint_constraints_dict, hard_constraints_dict,
soft_constraints_dict):
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints_strs = list(self.joint_constraints_dict.keys())
self.controlled_joints = sp.sympify(self.controlled_joints_strs)
self.make_sympy_matrices()
self.qp_solver = QPSolver(self.H.shape[0], len(self.lbA))
# @profile
def make_sympy_matrices(self):
weights = []
lb = []
ub = []
lbA = []
ubA = []
soft_expressions = []
hard_expressions = []
for jn in self.controlled_joints:
c = self.joint_constraints_dict[str(jn)]
weights.append(c.weight)
lb.append(c.lower)
ub.append(c.upper)
for c in self.hard_constraints_dict.values():
lbA.append(c.lower)
ubA.append(c.upper)
hard_expressions.append(c.expression)
for c in self.soft_constraints_dict.values():
weights.append(c.weight)
lbA.append(c.lower)
ubA.append(c.upper)
lb.append(-1e9)
ub.append(1e9)
soft_expressions.append(c.expression)
self.H = sp.diag(*weights)
self.np_g = np.zeros(len(weights))
self.lb = sp.Matrix(lb)
self.ub = sp.Matrix(ub)
# make A
# hard part
M_controlled_joints = sp.Matrix(self.controlled_joints)
A_hard = sp.Matrix(hard_expressions)
A_hard = A_hard.jacobian(M_controlled_joints)
zerosHxS = sp.zeros(A_hard.shape[0], len(soft_expressions))
A_hard = A_hard.row_join(zerosHxS)
# soft part
A_soft = sp.Matrix(soft_expressions)
A_soft = A_soft.jacobian(M_controlled_joints)
identity3x3 = sp.eye(A_soft.shape[0])
A_soft = A_soft.row_join(identity3x3)
# final A
self.A = A_soft.row_insert(0, A_hard)
self.lbA = sp.Matrix(lbA)
self.ubA = sp.Matrix(ubA)
self.cython_H = lambdify(list(self.H.free_symbols),
self.H,
self.BACKEND,
dummify=False)
self.H_symbols = [str(x) for x in self.H.free_symbols]
self.cython_A = lambdify(list(self.A.free_symbols),
self.A,
self.BACKEND,
dummify=False)
self.A_symbols = [str(x) for x in self.A.free_symbols]
self.cython_lb = lambdify(list(self.lb.free_symbols),
self.lb,
self.BACKEND,
dummify=False)
self.lb_symbols = [str(x) for x in self.lb.free_symbols]
self.cython_ub = lambdify(list(self.ub.free_symbols),
self.ub,
self.BACKEND,
dummify=False)
self.ub_symbols = [str(x) for x in self.ub.free_symbols]
self.cython_lbA = lambdify(list(self.lbA.free_symbols),
self.lbA,
self.BACKEND,
dummify=False)
self.lbA_symbols = [str(x) for x in self.lbA.free_symbols]
self.cython_ubA = lambdify(list(self.ubA.free_symbols),
self.ubA,
self.BACKEND,
dummify=False)
self.ubA_symbols = [str(x) for x in self.ubA.free_symbols]
def filter_observables(self, argument_names, observables_update):
return {str(k): observables_update[k] for k in argument_names}
# @profile
def update_observables(self, observables_update):
self.np_H = self.update_expression_matrix(self.cython_H,
self.H_symbols,
observables_update)
self.np_A = self.update_expression_matrix(self.cython_A,
self.A_symbols,
observables_update)
# for i in range(5):
# print(self.A[i,:])
self.np_lb = self.update_expression_vector(self.cython_lb,
self.lb_symbols,
observables_update)
self.np_ub = self.update_expression_vector(self.cython_ub,
self.ub_symbols,
observables_update)
self.np_lbA = self.update_expression_vector(self.cython_lbA,
self.lbA_symbols,
observables_update)
self.np_ubA = self.update_expression_vector(self.cython_ubA,
self.ubA_symbols,
observables_update)
xdot_full = self.qp_solver.solve(self.np_H, self.np_g, self.np_A,
self.np_lb, self.np_ub, self.np_lbA,
self.np_ubA)
if xdot_full is None:
return None
return OrderedDict(
(observable, xdot_full[i])
for i, observable in enumerate(self.controlled_joints_strs))
# @profile
def update_expression_matrix(self, matrix, argument_names, updates_dict):
args = self.filter_observables(argument_names, updates_dict)
if len(args) == 1:
return matrix(args.values()[0])
return matrix(**args).astype(float)
def update_expression_vector(self, vector, argument_names, updates_dict):
np_v = self.update_expression_matrix(vector, argument_names,
updates_dict)
return np_v.reshape(len(np_v))
class QProblemBuilder(object):
"""
Wraps around QPOases. Builds the required matrices from constraints.
"""
def __init__(self,
joint_constraints_dict,
hard_constraints_dict,
soft_constraints_dict,
controlled_joint_symbols,
free_symbols=None,
path_to_functions=''):
"""
:type joint_constraints_dict: dict
:type hard_constraints_dict: dict
:type soft_constraints_dict: dict
:type controlled_joint_symbols: set
:type free_symbols: set
:param path_to_functions: location where the compiled functions can be safed.
:type path_to_functions: str
"""
assert (
not len(controlled_joint_symbols) > len(joint_constraints_dict))
assert (
not len(controlled_joint_symbols) < len(joint_constraints_dict))
assert (len(hard_constraints_dict) <= len(controlled_joint_symbols))
self.path_to_functions = path_to_functions
self.free_symbols = free_symbols
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints = controlled_joint_symbols
self.make_matrices()
self.shape1 = len(self.hard_constraints_dict) + len(
self.soft_constraints_dict)
self.shape2 = len(self.joint_constraints_dict) + len(
self.soft_constraints_dict)
self.qp_solver = QPSolver(
len(self.joint_constraints_dict) + len(self.soft_constraints_dict),
len(self.hard_constraints_dict) + len(self.soft_constraints_dict))
# @profile
def make_matrices(self):
"""
Turns constrains into a function that computes the matrices needed for QPOases.
"""
# TODO split this into smaller functions to increase readability
t_total = time()
# TODO cpu intensive
weights = []
lb = []
ub = []
lbA = []
ubA = []
soft_expressions = []
hard_expressions = []
for k, c in self.joint_constraints_dict.items():
weights.append(c.weight)
lb.append(c.lower)
ub.append(c.upper)
for k, c in self.hard_constraints_dict.items():
lbA.append(c.lower)
ubA.append(c.upper)
hard_expressions.append(c.expression)
for k, c in self.soft_constraints_dict.items():
weights.append(c.weight)
lbA.append(c.lower)
ubA.append(c.upper)
lb.append(-BIG_NUMBER)
ub.append(BIG_NUMBER)
assert not isinstance(
c.expression, spw.Matrix
), u'Matrices are not allowed as soft constraint expression'
soft_expressions.append(c.expression)
self.cython_big_ass_M = load_compiled_function(self.path_to_functions)
self.np_g = np.zeros(len(weights))
if self.cython_big_ass_M is None:
print(u'new controller requested; compiling')
self.H = spw.diag(*weights)
self.lb = spw.Matrix(lb)
self.ub = spw.Matrix(ub)
# make A
# hard part
M_controlled_joints = spw.Matrix(self.controlled_joints)
A_hard = spw.Matrix(hard_expressions)
A_hard = A_hard.jacobian(M_controlled_joints)
zerosHxS = spw.zeros(A_hard.shape[0], len(soft_expressions))
A_hard = A_hard.row_join(zerosHxS)
# soft part
A_soft = spw.Matrix(soft_expressions)
t = time()
A_soft = A_soft.jacobian(M_controlled_joints)
print(u'jacobian took {}'.format(time() - t))
identity = spw.eye(A_soft.shape[0])
A_soft = A_soft.row_join(identity)
# final A
self.A = A_hard.col_join(A_soft)
self.lbA = spw.Matrix(lbA)
self.ubA = spw.Matrix(ubA)
big_ass_M_A = self.A.row_join(self.lbA).row_join(self.ubA)
big_ass_M_H = self.H.row_join(self.lb).row_join(self.ub)
# putting everything into one big matrix to take full advantage of cse in speed_up()
self.big_ass_M = big_ass_M_A.col_join(big_ass_M_H)
t = time()
if self.free_symbols is None:
self.free_symbols = self.big_ass_M.free_symbols
self.cython_big_ass_M = spw.speed_up(self.big_ass_M,
self.free_symbols,
backend=BACKEND)
if self.path_to_functions is not None:
safe_compiled_function(self.cython_big_ass_M,
self.path_to_functions)
print(u'autowrap took {}'.format(time() - t))
else:
print(u'controller loaded {}'.format(self.path_to_functions))
print(u'controller ready {}s'.format(time() - t_total))
def save_pickle(self, hash, f):
with open(u'/tmp/{}'.format(hash), u'w') as file:
pickle.dump(f, file)
def load_pickle(self, hash):
return pickle.load(hash)
def debug_print(self,
np_H,
np_A,
np_lb,
np_ub,
np_lbA,
np_ubA,
xdot_full=None):
import pandas as pd
lb = []
ub = []
lbA = []
ubA = []
weights = []
xdot = []
for iJ, k in enumerate(self.joint_constraints_dict.keys()):
key = 'j -- ' + str(k)
lb.append(key)
ub.append(key)
weights.append(key)
xdot.append(key)
for iH, k in enumerate(self.hard_constraints_dict.keys()):
key = 'h -- ' + str(k)
lbA.append(key)
ubA.append(key)
for iS, k in enumerate(self.soft_constraints_dict.keys()):
key = 's -- ' + str(k)
lbA.append(key)
ubA.append(key)
weights.append(key)
xdot.append(key)
p_lb = pd.DataFrame(np_lb[:-len(self.soft_constraints_dict)], lb)
p_ub = pd.DataFrame(np_ub[:-len(self.soft_constraints_dict)], ub)
p_lbA = pd.DataFrame(np_lbA, lbA)
p_ubA = pd.DataFrame(np_ubA, ubA)
p_weights = pd.DataFrame(np_H.dot(np.ones(np_H.shape[0])), weights)
if xdot_full is not None:
p_xdot = pd.DataFrame(xdot_full, xdot)
p_A = pd.DataFrame(np_A, lbA, weights)
pass
def get_cmd(self, substitutions, nWSR=None):
"""
Uses substitutions for each symbol to compute the next commands for each joint.
:param substitutions: symbol -> value
:type substitutions: dict
:return: joint name -> joint command
:rtype: dict
"""
np_big_ass_M = self.cython_big_ass_M(**substitutions)
# TODO create functions to extract the different matrices.
np_H = np.array(np_big_ass_M[self.shape1:, :-2])
np_A = np.array(np_big_ass_M[:self.shape1, :self.shape2])
np_lb = np.array(np_big_ass_M[self.shape1:, -2])
np_ub = np.array(np_big_ass_M[self.shape1:, -1])
np_lbA = np.array(np_big_ass_M[:self.shape1, -2])
np_ubA = np.array(np_big_ass_M[:self.shape1, -1])
# self.debug_print(np_H, np_A, np_lb, np_ub, np_lbA, np_ubA)
xdot_full = self.qp_solver.solve(np_H, self.np_g, np_A, np_lb, np_ub,
np_lbA, np_ubA, nWSR)
if xdot_full is None:
return None
# TODO enable debug print in an elegant way, preferably without slowing anything down
# self.debug_print(np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full)
return OrderedDict(
(observable, xdot_full[i])
for i, observable in enumerate(self.controlled_joints))
class QProblemBuilder(object):
"""
Wraps around QPOases. Builds the required matrices from constraints.
"""
def __init__(self, joint_constraints_dict, hard_constraints_dict, soft_constraints_dict, controlled_joint_symbols,
free_symbols=None, path_to_functions=''):
"""
:type joint_constraints_dict: dict
:type hard_constraints_dict: dict
:type soft_constraints_dict: dict
:type controlled_joint_symbols: set
:type free_symbols: set
:param path_to_functions: location where the compiled functions can be safed.
:type path_to_functions: str
"""
if free_symbols is not None:
warnings.warn(u'use of free_symbols deprecated', DeprecationWarning)
assert (not len(controlled_joint_symbols) > len(joint_constraints_dict))
assert (not len(controlled_joint_symbols) < len(joint_constraints_dict))
assert (len(hard_constraints_dict) <= len(controlled_joint_symbols))
self.path_to_functions = path_to_functions
self.free_symbols = free_symbols
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints = controlled_joint_symbols
self.make_matrices()
self.shape1 = len(self.hard_constraints_dict) + len(self.soft_constraints_dict)
self.shape2 = len(self.joint_constraints_dict) + len(self.soft_constraints_dict)
self.qp_solver = QPSolver(len(self.hard_constraints_dict),
len(self.joint_constraints_dict),
len(self.soft_constraints_dict))
self.lbAs = None # for debugging purposes
def get_expr(self):
return self.cython_big_ass_M.str_params
def make_matrices(self):
"""
Turns constrains into a function that computes the matrices needed for QPOases.
"""
# TODO split this into smaller functions to increase readability
t_total = time()
# TODO cpu intensive
weights = []
lb = []
ub = []
lbA = []
ubA = []
soft_expressions = []
hard_expressions = []
for k, c in self.joint_constraints_dict.items():
weights.append(c.weight)
lb.append(c.lower)
ub.append(c.upper)
for k, c in self.hard_constraints_dict.items():
lbA.append(c.lower)
ubA.append(c.upper)
hard_expressions.append(c.expression)
for k, c in self.soft_constraints_dict.items():
weights.append(c.weight)
lbA.append(c.lower)
ubA.append(c.upper)
lb.append(-BIG_NUMBER)
ub.append(BIG_NUMBER)
assert not w.is_matrix(c.expression), u'Matrices are not allowed as soft constraint expression'
soft_expressions.append(c.expression)
self.cython_big_ass_M = w.load_compiled_function(self.path_to_functions)
self.np_g = np.zeros(len(weights))
if self.cython_big_ass_M is None:
logging.loginfo(u'new controller with {} constraints requested; compiling'.format(len(soft_expressions)))
h = len(self.hard_constraints_dict)
s = len(self.soft_constraints_dict)
c = len(self.joint_constraints_dict)
# c s 1 1
# |----------------------------------
# h | A hard | 0 | |
# | -------------------| lbA | ubA
# s | A soft |identity| |
# |-----------------------------------
#c+s| H | lb | ub
# | ----------------------------------
self.big_ass_M = w.zeros(h+s+s+c, c+s+2)
self.big_ass_M[h+s:,:-2] = w.diag(*weights)
self.lb = w.Matrix(lb)
self.ub = w.Matrix(ub)
# make A
# hard part
A_hard = w.Matrix(hard_expressions)
A_hard = w.jacobian(A_hard, self.controlled_joints)
self.big_ass_M[:h, :c] = A_hard
# soft part
A_soft = w.Matrix(soft_expressions)
t = time()
A_soft = w.jacobian(A_soft, self.controlled_joints)
logging.loginfo(u'jacobian took {}'.format(time() - t))
identity = w.eye(A_soft.shape[0])
self.big_ass_M[h:h+s, :c] = A_soft
self.big_ass_M[h:h+s, c:c+s] = identity
self.lbA = w.Matrix(lbA)
self.ubA = w.Matrix(ubA)
self.big_ass_M[:h+s, c+s] = self.lbA
self.big_ass_M[:h+s, c+s+1] = self.ubA
self.big_ass_M[h+s:, c+s] = self.lb
self.big_ass_M[h+s:, c+s+1] = self.ub
t = time()
if self.free_symbols is None:
self.free_symbols = w.free_symbols(self.big_ass_M)
self.cython_big_ass_M = w.speed_up(self.big_ass_M,
self.free_symbols)
if self.path_to_functions is not None:
# safe_compiled_function(self.cython_big_ass_M, self.path_to_functions)
logging.loginfo(u'autowrap took {}'.format(time() - t))
else:
logging.loginfo(u'controller loaded {}'.format(self.path_to_functions))
logging.loginfo(u'controller ready {}s'.format(time() - t_total))
def save_pickle(self, hash, f):
with open(u'/tmp/{}'.format(hash), u'w') as file:
pickle.dump(f, file)
def load_pickle(self, hash):
return pickle.load(hash)
def debug_print(self, np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full=None):
import pandas as pd
lb = []
lbA = []
weights = []
xdot = []
# if xdot_full is not None:
# A_dot_x = np_A.dot(xdot_full)
for iJ, k in enumerate(self.joint_constraints_dict.keys()):
key = 'j -- ' + str(k)
lb.append(key)
weights.append(key)
xdot.append(key)
for iH, k in enumerate(self.hard_constraints_dict.keys()):
key = 'h -- ' + str(k)
lbA.append(key)
upper_bound = np_ubA[iH]
lower_bound = np_lbA[iH]
if np.sign(upper_bound) == np.sign(lower_bound):
logging.logwarn(u'{} out of bounds'.format(k))
if upper_bound > 0:
logging.logwarn(u'{} value below lower bound by {}'.format(k, lower_bound))
vel = np_ub[iH]
if abs(vel) < abs(lower_bound):
logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel))
else:
logging.logwarn(u'{} value above upper bound by {}'.format(k, abs(upper_bound)))
vel = np_lb[iH]
if abs(vel) < abs(lower_bound):
logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel))
for iS, k in enumerate(self.soft_constraints_dict.keys()):
key = 's -- ' + str(k)
lbA.append(key)
weights.append(key)
# xdot.append(key)
p_lb = pd.DataFrame(np_lb[:-len(self.soft_constraints_dict)], lb).sort_index()
p_ub = pd.DataFrame(np_ub[:-len(self.soft_constraints_dict)], lb).sort_index()
p_lbA = pd.DataFrame(np_lbA, lbA).sort_index()
# if xdot_full is not None:
# p_A_dot_x = pd.DataFrame(A_dot_x, lbA).sort_index()
p_ubA = pd.DataFrame(np_ubA, lbA).sort_index()
p_weights = pd.DataFrame(np_H.dot(np.ones(np_H.shape[0])), weights).sort_index()
if xdot_full is not None:
p_xdot = pd.DataFrame(xdot_full[:len(xdot)], xdot).sort_index()
p_A = pd.DataFrame(np_A, lbA, weights).sort_index(1).sort_index(0)
if self.lbAs is None:
self.lbAs = p_lbA
else:
self.lbAs = self.lbAs.T.append(p_lbA.T, ignore_index=True).T
# self.lbAs.T[[c for c in self.lbAs.T.columns if 'dist' in c]].plot()
# arrays = [(p_weights, u'H'),
# (p_A, u'A'),
# (p_lbA, u'lbA'),
# (p_ubA, u'ubA'),
# (p_lb, u'lb'),
# (p_ub, u'ub')]
# for a, name in arrays:
# self.check_for_nan(name, a)
# self.check_for_big_numbers(name, a)
pass
def check_for_nan(self, name, p_array):
p_filtered = p_array.apply(lambda x: zip(x.index[x.isnull()].tolist(), x[x.isnull()]), 1)
p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
if len(p_filtered) > 0:
logging.logwarn(u'{} has the following nans:'.format(name))
self.print_pandas_array(p_filtered)
else:
logging.loginfo(u'no nans')
def check_for_big_numbers(self, name, p_array, big=1e5):
# FIXME fails if condition is true on first entry
p_filtered = p_array.apply(lambda x: zip(x.index[abs(x) > big].tolist(), x[x > big]), 1)
p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
if len(p_filtered) > 0:
logging.logwarn(u'{} has the following big numbers:'.format(name))
self.print_pandas_array(p_filtered)
else:
logging.loginfo(u'no big numbers')
def print_pandas_array(self, array):
import pandas as pd
if len(array) > 0:
with pd.option_context('display.max_rows', None, 'display.max_columns', None):
print(array)
def get_cmd(self, substitutions, nWSR=None):
"""
Uses substitutions for each symbol to compute the next commands for each joint.
:param substitutions: symbol -> value
:type substitutions: dict
:return: joint name -> joint command
:rtype: dict
"""
np_big_ass_M = self.cython_big_ass_M.call2(substitutions)
np_H = np_big_ass_M[self.shape1:, :-2].copy()
np_A = np_big_ass_M[:self.shape1, :self.shape2].copy()
np_lb = np_big_ass_M[self.shape1:, -2].copy()
np_ub = np_big_ass_M[self.shape1:, -1].copy()
np_lbA = np_big_ass_M[:self.shape1, -2].copy()
np_ubA = np_big_ass_M[:self.shape1, -1].copy()
# self.debug_print(np_H, np_A, np_lb, np_ub, np_lbA, np_ubA)
try:
xdot_full = self.qp_solver.solve(np_H, self.np_g, np_A, np_lb, np_ub, np_lbA, np_ubA, nWSR)
except QPSolverException as e:
self.debug_print(np_H, np_A, np_lb, np_ub, np_lbA, np_ubA)
raise e
if xdot_full is None:
return None
# TODO enable debug print in an elegant way, preferably without slowing anything down
# self.debug_print(np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full)
return OrderedDict((observable, xdot_full[i]) for i, observable in enumerate(self.controlled_joints)), \
np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full
def test_simple_problem_split():
cw1 = 10.
cw2 = 1000.
H = np.diag([1, 1, cw1, cw2])
A = np.array([[1., 1, 1, 0], [1, 1, 0, 1]])
g = np.zeros(4)
lba = np.array([10., 5.])
lb = np.array([-10., -10., -1e9, -1e9])
ub = np.array([10., 10., 1e9, 1e9])
qp = QPSolver()
x1 = qp.solve(H, g, A, lb, ub, lba, lba)
print(x1)
H = np.diag([1, 1, cw1])
A = np.array([[1., 1, 1]])
g = np.zeros(3)
lba = np.array([10.])
lb = np.array([-10., -10., -1e9])
ub = np.array([10., 10., 1e9])
qp = QPSolver()
x2 = qp.solve(H, g, A, lb, ub, lba, lba)
print(x2)
H = np.diag([1, 1, cw2])
A = np.array([[1., 1, 1]])
g = np.zeros(3)
lba = np.array([5.])
lb = np.array([-10., -10., -1e9])
ub = np.array([10., 10., 1e9])
qp = QPSolver()
x3 = qp.solve(H, g, A, lb, ub, lba, lba)
print(x3)
H = np.diag([1., 1, cw1, cw1, cw2, cw2])
A = np.array([[1., 0, 1, 0, 0, 0], [0., 1, 0, 1, 0, 0],
[1., 0, 0, 0, 1, 0], [0., 1, 0, 0, 0, 1]])
g = np.zeros(H.shape[0])
lba = np.array([
x2[0],
x2[1],
x3[0],
x3[1],
])
lb = np.array([-10., -10., -1e9, -1e9, -1e9, -1e9])
ub = np.array([10., 10., 1e9, 1e9, 1e9, 1e9])
qp = QPSolver()
x4 = qp.solve(H, g, A, lb, ub, lba, lba)
print(x4)
H = np.diag([1., 1, cw1, cw2, cw1, cw2])
A = np.array([[1., 0, 1, 0, 0, 0], [0., 1, 1, 0, 0, 0],
[1., 0, 0, 1, 0, 0], [0., 1, 0, 1, 0, 0]])
g = np.zeros(H.shape[0])
lba = np.array([
x2[0],
x2[1],
x3[0],
x3[1],
])
lb = np.array([-10., -10., -1e9, -1e9, -1e9, -1e9])
ub = np.array([10., 10., 1e9, 1e9, 1e9, 1e9])
qp = QPSolver()
x5 = qp.solve(H, g, A, lb, ub, lba, lba)
print(x5)
print((x2[:2] + x3[:2]) / 2)
print(np.sqrt((x1[:2]**2 + x2[:2]**2)) / 2)
class QProblemBuilder(object):
def __init__(self,
joint_constraints_dict,
hard_constraints_dict,
soft_constraints_dict,
backend=None,
logging=print_wrapper):
self.backend = backend
self.logging = logging
self.joint_constraints_dict = joint_constraints_dict
self.hard_constraints_dict = hard_constraints_dict
self.soft_constraints_dict = soft_constraints_dict
self.controlled_joints_strs = list(self.joint_constraints_dict.keys())
self.controlled_joints = [
spw.Symbol(n) for n in self.controlled_joints_strs
]
self.soft_constraint_indices = {}
self.make_sympy_matrices()
self.qp_solver = QPSolver(self.H.shape[0], len(self.lbA))
# @profile
def make_sympy_matrices(self):
weights = []
lb = []
ub = []
lbA = []
ubA = []
soft_expressions = []
hard_expressions = []
for jn in self.controlled_joints:
c = self.joint_constraints_dict[str(jn)]
weights.append(c.weight)
lb.append(c.lower)
ub.append(c.upper)
for c in self.hard_constraints_dict.values():
lbA.append(c.lower)
ubA.append(c.upper)
hard_expressions.append(c.expression)
for scname, c in self.soft_constraints_dict.items():
self.soft_constraint_indices[scname] = len(lbA)
weights.append(c.weight)
lbA.append(c.lower)
ubA.append(c.upper)
lb.append(-BIG_NUMBER)
ub.append(BIG_NUMBER)
soft_expressions.append(c.expression)
self.H = spw.diag(*weights)
self.np_g = np.zeros(len(weights))
self.lb = spw.Matrix(lb)
self.ub = spw.Matrix(ub)
# make A
# hard part
M_controlled_joints = spw.Matrix(self.controlled_joints)
A_hard = spw.Matrix(hard_expressions)
A_hard = A_hard.jacobian(M_controlled_joints)
zerosHxS = spw.zeros(A_hard.shape[0], len(soft_expressions))
A_hard = A_hard.row_join(zerosHxS)
# soft part
A_soft = spw.Matrix(soft_expressions)
t = time()
A_soft = A_soft.jacobian(M_controlled_joints)
self.logging('jacobian took {}'.format(time() - t))
identity = spw.eye(A_soft.shape[0])
A_soft = A_soft.row_join(identity)
# final A
self.A = A_hard.col_join(A_soft)
self.lbA = spw.Matrix(lbA)
self.ubA = spw.Matrix(ubA)
t = time()
try:
self.cython_H = spw.speed_up(self.H, self.H.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping weight matrix! Error: {}\n'.format(e))
try:
self.cython_A = spw.speed_up(self.A, self.A.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping jacobian! Error: {}\n'.format(e))
try:
self.cython_lb = spw.speed_up(self.lb, self.lb.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping lower bounds! Error: {}\n'.format(e))
try:
self.cython_ub = spw.speed_up(self.ub, self.ub.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping upper bounds! Error: {}\n'.format(e))
try:
self.cython_lbA = spw.speed_up(self.lbA, self.lbA.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping jacobian lower bounds! Error: {}\n'.
format(e))
try:
self.cython_ubA = spw.speed_up(self.ubA, self.ubA.free_symbols)
except Exception as e:
raise Exception(
'Error while wrapping jacobian upper bounds! Error: {}\n'.
format(e))
self.logging('autowrap took {}'.format(time() - t))
# Strings for printing
col_names = self.controlled_joints_strs + ['slack'
] * len(soft_expressions)
row_names = self.hard_constraints_dict.keys(
) + self.soft_constraints_dict.keys()
self.str_A = pretty_matrix_format_str(col_names, row_names)
self.str_b = pretty_matrix_format_str(
['lb', 'ub'],
self.controlled_joints_strs + self.soft_constraints_dict.keys())
self.str_bA = pretty_matrix_format_str(
['lbA', 'ubA'],
self.hard_constraints_dict.keys() +
self.soft_constraints_dict.keys())
# @profile
def update_observables(self, observables_update):
# print('Evaluating H')
self.np_H = self.cython_H(**observables_update)
# print('Evaluating A')
self.np_A = self.cython_A(**observables_update)
# print('Evaluating ctrl lb')
self.np_lb = self.cython_lb(**observables_update).reshape(
self.lb.shape[0])
#print('Evaluating ctrl ub')
self.np_ub = self.cython_ub(**observables_update).reshape(
self.ub.shape[0])
#print('Evaluating A lb')
self.np_lbA = self.cython_lbA(**observables_update).reshape(
self.lbA.shape[0])
#print('Evaluating A ub')
self.np_ubA = self.cython_ubA(**observables_update).reshape(
self.ubA.shape[0])
xdot_full = self.qp_solver.solve(self.np_H, self.np_g, self.np_A,
self.np_lb, self.np_ub, self.np_lbA,
self.np_ubA)
if xdot_full is None:
return None
return OrderedDict(
(observable, xdot_full[i])
for i, observable in enumerate(self.controlled_joints_strs))
def constraints_met(self, lbThreshold=0.01, ubThreshold=-0.01, names=None):
if names == None:
for x in range(len(self.np_lbA)):
if self.np_lbA[x] > lbThreshold or self.np_ubA[x] < ubThreshold:
return False
else:
for name in names:
x = self.soft_constraint_indices[name]
if self.np_lbA[x] > lbThreshold or self.np_ubA[x] < ubThreshold:
return False
return True
def str_jacobian(self):
return format_matrix(self.np_A, self.str_A)
def str_lb_ub(self):
return format_matrix(
np.concatenate((self.np_lb.reshape(len(
self.np_lb), 1), self.np_ub.reshape(len(self.np_ub), 1)),
axis=1), self.str_b)
def str_lbA_ubA(self):
return format_matrix(
np.concatenate((self.np_lbA.reshape(len(
self.np_lbA), 1), self.np_ubA.reshape(len(self.np_lbA), 1)),
axis=1), self.str_bA)
def log_jacobian(self):
self.logging('Matrix A: \n{}'.format(self.str_jacobian()))
def log_lb_ub(self):
self.logging('Matrix lb, ub: \n{}'.format(self.str_lb_ub()))
def log_lbA_ubA(self):
self.logging('Matrix lbA, ubA: \n{}'.format(self.str_lbA_ubA()))
def reset_solver(self):
self.qp_solver = QPSolver(self.H.shape[0], len(self.lbA))