def test_speed_sign(self, f1):
f1_s = spw.Symbol('f1')
expr = spw.diffable_sign(f1_s)
llvm = spw.speed_up(spw.Matrix([expr]), list(expr.free_symbols))
kwargs = {'f1': f1}
r1_llvm = llvm(**kwargs)
r1 = float(spw.diffable_sign(f1))
self.assertTrue(np.isclose(r1, r1_llvm), msg='spw.sign({})={} != np.sign({})={}'.format(f1, r1, f1, r1_llvm))
def test_speed_up_max(self, f1, f2):
f1_s = spw.Symbol('f1')
f2_s = spw.Symbol('f2')
expr = spw.diffable_max_fast(f1_s, f2_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
kwargs = {'f1': f1, 'f2': f2}
r1_llvm = llvm(**kwargs)[0]
# r1_expr = np.float(expr.subs())
r1 = np.float(spw.diffable_max_fast(f1, f2))
self.assertTrue(np.isclose(r1, r1_llvm), msg='max({},{})={}, max_expr({},{})={}'.format(f1, f2, r1,
f1, f2, r1_llvm))
def speed_up_and_execute(f, params):
symbols = []
input = []
class next_symbol(object):
symbol_counter = 0
def __call__(self):
self.symbol_counter += 1
return spw.Symbol('a{}'.format(self.symbol_counter))
ns = next_symbol()
symbol_params = []
for i, param in enumerate(params):
if isinstance(param, np.ndarray):
l2 = []
for j in range(param.shape[0]):
l1 = []
if len(param.shape) == 2:
for k in range(param.shape[1]):
s = ns()
symbols.append(s)
input.append(param[j, k])
l1.append(s)
l2.append(l1)
else:
s = ns()
symbols.append(s)
input.append(param[j])
l2.append(s)
p = spw.Matrix(l2)
symbol_params.append(p)
else:
s = ns()
symbols.append(s)
input.append(param)
symbol_params.append(s)
try:
slow_f = spw.Matrix([f(*symbol_params)])
except TypeError:
slow_f = spw.Matrix(f(*symbol_params))
fast_f = spw.speed_up(slow_f, symbols)
subs = {str(symbols[i]): input[i] for i in range(len(symbols))}
# slow_f.subs()
result = fast_f(**subs).T
if result.shape[1] == 1:
return result.T[0]
else:
return result[0]
def test_speed_up_matrix_from_axis_angle(self, axis, angle):
axis_s = spw.var('x y z')
angle_s = spw.Symbol('angle')
kwargs = {'x': axis[0],
'y': axis[1],
'z': axis[2],
'angle': angle}
expr = spw.rotation_matrix_from_axis_angle(spw.Matrix(axis_s), angle_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
r1_llvm = llvm(**kwargs)
r1 = np.array(spw.rotation_matrix_from_axis_angle(axis, angle)).astype(float)
self.assertTrue(np.isclose(r1, r1_llvm).all(), msg='{} {}\n{} != \n{}'.format(axis, angle, r1, r1_llvm))
def test_speed_up_if_greater_zero(self, condition, if_result, else_result):
condition_s = spw.Symbol('condition')
if_s = spw.Symbol('if')
else_s = spw.Symbol('else')
expr = spw.diffable_if_greater_zero(condition_s, if_s, else_s)
llvm = spw.speed_up(spw.Matrix([expr]), expr.free_symbols)
kwargs = {'condition': condition,
'if': if_result,
'else': else_result}
# r1_expr = float(expr.subs(kwargs))
r1_llvm = llvm(**kwargs)[0][0]
r1 = float(spw.diffable_if_greater_zero(condition, if_result, else_result))
self.assertTrue(np.isclose(r1, r1_llvm), msg='{} if {} > 0 else {} => {} != {}'.format(if_result, condition,
else_result,
r1_llvm, r1))
def test_speed_up_slerp(self, q1, q2, t):
q1_s = spw.var('q1x q1y q1z q1w')
q2_s = spw.var('q2x q2y q2z q2w')
t_s = spw.Symbol('t')
q1_expr = spw.Matrix(q1_s)
q2_expr = spw.Matrix(q2_s)
expr = spw.diffable_slerp(q1_expr, q2_expr, t_s)
slerp = spw.speed_up(expr, expr.free_symbols)
kwargs = {'q1x': q1[0], 'q1y': q1[1], 'q1z': q1[2], 'q1w': q1[3],
'q2x': q2[0], 'q2y': q2[1], 'q2z': q2[2], 'q2w': q2[3],
't': t}
r1 = slerp(**kwargs).T[0]
r2 = np.array([float(x.evalf(real=True)) for x in spw.diffable_slerp(spw.Matrix(q1), spw.Matrix(q2), t)])
self.assertTrue(np.isclose(r1, r2, atol=1e-3).all() or
np.isclose(r1, -r2, atol=1e-3).all(),
msg='q1={} q2={} t={}\n{} != {}'.format(q1, q2, t, r1, r2))
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 make_np_frames(self):
self.fast_frames = []
for f, expression in self.frames.items():
self.fast_frames.append((f, spw.speed_up(expression, list(expression.free_symbols))))
self.fast_frames = OrderedDict(self.fast_frames)
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())
