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_1(self): q = spw.Matrix([0., 0., 0.14943813, 0.98877108]) r1 = spw.rotation3_quaternion(*q) r1 = np.asarray(r1.tolist()).reshape(r1.shape) r_goal = quaternion_matrix(q) np.testing.assert_array_almost_equal(r1, r_goal)
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 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 make_constraints(self, robot): t = time() for eef in robot.end_effectors: start_pose = robot.get_eef_position()[eef] start_position = spw.pos_of(start_pose) current_pose = robot.frames[eef] current_position = spw.pos_of(current_pose) current_rotation = spw.rot_of(current_pose)[:3, :3] goal_position = self.goal_eef[eef].get_position() goal_rotation = self.goal_eef[eef].get_rotation()[:3, :3] # pos control dist = spw.norm(spw.pos_of(current_pose) - goal_position) # line x0 = current_position[:-1, :] x1 = spw.Matrix((start_position[:-1, :] + np.random.random( (3, 1)) * 0.005).astype(float).tolist()) x2 = goal_position[:-1, :] dist_to_line = spw.norm(spw.cross((x0 - x1), (x0 - x2))) / spw.norm(x2 - x1) dist_to_line = dist_to_line**2 # rot control dist_r = spw.norm( current_rotation.reshape(9, 1) - goal_rotation.reshape(9, 1)) self._soft_constraints['align {} position'.format( eef)] = SoftConstraint( lower=-dist, upper=-dist, weight=self.goal_weights[eef].get_expression(), expression=dist) self._soft_constraints['align {} rotation'.format( eef)] = SoftConstraint( lower=-dist_r, upper=-dist_r, weight=self.goal_weights[eef].get_expression(), expression=dist_r) self._soft_constraints['{} stay on line'.format( eef)] = SoftConstraint( lower=-dist_to_line, upper=-dist_to_line, weight=self.goal_weights[eef].get_expression() * 100, expression=dist_to_line) self._controllable_constraints = robot.joint_constraints self._hard_constraints = robot.hard_constraints self.update_observables( {self.goal_weights[eef].get_symbol_str(): self.weight}) self.set_goal({eef: robot.get_eef_position2()[eef]}) print('make constraints took {}'.format(time() - t))
def get_eef_position(self): eef = {} for end_effector in self.end_effectors: eef_joints = self.frames[end_effector].free_symbols eef_joint_symbols = [self.get_joint_state_input().to_str_symbol(str(x)) for x in eef_joints] js = {k: self.get_state()[k] for k in eef_joint_symbols} evaled_frame = self.fast_frames[end_effector](**js) # eef_pos = np.array(pos_of(evaled_frame).tolist(), dtype=float)[:-1].reshape(3) # eef_rot = np.array(rot_of(evaled_frame).tolist(), dtype=float) # eef_rot = quaternion_from_matrix(eef_rot) eef[end_effector] = spw.Matrix(evaled_frame.tolist()) return eef
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 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 get_expression(self): return spw.Matrix([spw.Symbol(x) for x in self._symbol_map.values()])
def test_slerp2(self, q1, q2, t): r1 = np.array([float(x.evalf(real=True)) for x in spw.diffable_slerp(spw.Matrix(q1), spw.Matrix(q2), t)]) r2 = quaternion_slerp(q1, 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 test_dot(self, vectors): u, v = vectors r1 = np.float(spw.dot(spw.Matrix(u), spw.Matrix(v))) r2 = np.dot(u, v) self.assertTrue(np.isclose(r1, r2), msg='{} * {}={} != {}'.format(u, v, r1, r2))
def test_scale(self, v, a): assume(np.linalg.norm(v) != 0) r1 = np.array(spw.scale(spw.Matrix(v), a)).astype(float).T[0] r2 = v / np.linalg.norm(v) * a self.assertTrue(np.isclose(r1, r2).all(), msg='v={} a={}\n{} != {}'.format(v, a, r1, r2))
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())