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_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_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)