Exemplo n.º 1
0
    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])
Exemplo n.º 2
0
    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))
Exemplo n.º 3
0
 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)
Exemplo n.º 4
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)
Exemplo n.º 5
0
 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)
Exemplo n.º 6
0
 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)
Exemplo n.º 7
0
 def test_is_matrix(self):
     self.assertFalse(w.is_matrix(w.Symbol('a')))
     self.assertTrue(w.is_matrix(w.Matrix([[0, 0]])))
Exemplo n.º 8
0
 def get_expression(self):
     return w.Matrix([self.fx, self.fy, self.fz, self.tx, self.ty, self.tz])
Exemplo n.º 9
0
 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]])