def test_jacobian(self):
a = w.Symbol('a')
b = w.Symbol('b')
m = w.Matrix([a+b, a**2, b**2])
jac = w.jacobian(m, [a,b])
expected = w.Matrix([[1,1],[2*a,0],[0,2*b]])
for i in range(expected.shape[0]):
for j in range(expected.shape[1]):
assert w.equivalent(jac[i,j], expected[i,j])
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))
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 test_matrix2(self, x_dim, y_dim):
data = [[(i)+(j*x_dim) for j in range(y_dim)] for i in range(x_dim)]
m = w.Matrix(data)
self.assertEqual(float(m[0,0]), 0)
self.assertEqual(float(m[x_dim-1,y_dim-1]), (x_dim*y_dim)-1)
def test_matrix(self, x_dim):
data = list(range(x_dim))
m = w.Matrix(data)
self.assertEqual(m[0], 0)
self.assertEqual(m[-1], x_dim -1)
def test_is_matrix(self):
self.assertFalse(w.is_matrix(w.Symbol('a')))
self.assertTrue(w.is_matrix(w.Matrix([[0,0]])))
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 get_expression(self):
return w.Matrix([self.fx, self.fy, self.fz, self.tx, self.ty, self.tz])
def get_frame(self):
return w.Matrix([[self.f00, self.f01, self.f02, self.f03],
[self.f10, self.f11, self.f12, self.f13],
[self.f20, self.f21, self.f22, self.f23],
[0, 0, 0, 1]])