import unittest

import numpy as np

from constraints import (generate_constraints_function,
                         generate_constraint_gradients_function, )
from robot_arm import RobotArm


class TestConstraintFunctions(unittest.TestCase):
    def setUp(self):
        self.lengths = (3, 2, 2,)
        self.destinations = (
            (5, 4, 6, 4, 5),
            (0, 2, 0.5, -2, -1),
        )
        self.theta = (np.pi, np.pi / 2, 0,)
        self.thetas = np.ones((3 * 5,))
        self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
        self.constraints_func = generate_constraints_function(self.robot_arm)
        self.constraint_gradients_func = generate_constraint_gradients_function(self.robot_arm)

    def test_constraints_func_return_type(self):
        constraints = self.constraints_func(self.thetas)
        self.assertEqual(constraints.shape, (2 * 5,))

    def test_constraint_gradients_func_return_type(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
        # print(np.array2string(constraint_gradients, max_line_width=np.inf))

import unittest

import numpy as np

from constraints import (generate_constraints_function,
                         generate_constraint_gradients_function, )
from robot_arm import RobotArm


class TestConstraintFunctions(unittest.TestCase):
    def setUp(self):
        self.lengths = (3, 2, 2,)
        self.destinations = (
            (5, 4, 6, 4, 5),
            (0, 2, 0.5, -2, -1),
        )
        self.theta = (np.pi, np.pi / 2, 0,)
        self.thetas = np.ones((3 * 5,))
        self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
        self.constraints_func = generate_constraints_function(self.robot_arm)
        self.constraint_gradients_func = generate_constraint_gradients_function(self.robot_arm)

    def test_constraints_func_return_type(self):
        constraints = self.constraints_func(self.thetas)
        self.assertEqual(constraints.shape, (2 * 5,))

    def test_constraint_gradients_func_return_type(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
        # print(np.array2string(constraint_gradients, max_line_width=np.inf))

    def test_licq(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        rank = np.linalg.matrix_rank(constraint_gradients)
        self.assertEqual(rank, 2 * 5)

Test LICQ condition of constraint gradient
Test LICQ condition of constraint gradient

  import unittest
  
  import numpy as np
  
  from constraints import (generate_constraints_function,
                           generate_constraint_gradients_function, )
  from robot_arm import RobotArm
  
  
  class TestConstraintFunctions(unittest.TestCase):
      def setUp(self):
          self.lengths = (3, 2, 2,)
          self.destinations = (
              (5, 4, 6, 4, 5),
              (0, 2, 0.5, -2, -1),
          )
          self.theta = (np.pi, np.pi / 2, 0,)
          self.thetas = np.ones((3 * 5,))
          self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
          self.constraints_func = generate_constraints_function(self.robot_arm)
          self.constraint_gradients_func = generate_constraint_gradients_function(self.robot_arm)
  
      def test_constraints_func_return_type(self):
          constraints = self.constraints_func(self.thetas)
          self.assertEqual(constraints.shape, (2 * 5,))
  
      def test_constraint_gradients_func_return_type(self):
          constraint_gradients = self.constraint_gradients_func(self.thetas)
          self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
          # print(np.array2string(constraint_gradients, max_line_width=np.inf))
  
+     def test_licq(self):
+         constraint_gradients = self.constraint_gradients_func(self.thetas)
+         rank = np.linalg.matrix_rank(constraint_gradients)
+         self.assertEqual(rank, 2 * 5)
+ 
Test LICQ condition of constraint gradient
## Code Before:
import unittest

import numpy as np

from constraints import (generate_constraints_function,
                         generate_constraint_gradients_function, )
from robot_arm import RobotArm


class TestConstraintFunctions(unittest.TestCase):
    def setUp(self):
        self.lengths = (3, 2, 2,)
        self.destinations = (
            (5, 4, 6, 4, 5),
            (0, 2, 0.5, -2, -1),
        )
        self.theta = (np.pi, np.pi / 2, 0,)
        self.thetas = np.ones((3 * 5,))
        self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
        self.constraints_func = generate_constraints_function(self.robot_arm)
        self.constraint_gradients_func = generate_constraint_gradients_function(self.robot_arm)

    def test_constraints_func_return_type(self):
        constraints = self.constraints_func(self.thetas)
        self.assertEqual(constraints.shape, (2 * 5,))

    def test_constraint_gradients_func_return_type(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
        # print(np.array2string(constraint_gradients, max_line_width=np.inf))

## Instruction:
Test LICQ condition of constraint gradient
## Code After:
import unittest

import numpy as np

from constraints import (generate_constraints_function,
                         generate_constraint_gradients_function, )
from robot_arm import RobotArm


class TestConstraintFunctions(unittest.TestCase):
    def setUp(self):
        self.lengths = (3, 2, 2,)
        self.destinations = (
            (5, 4, 6, 4, 5),
            (0, 2, 0.5, -2, -1),
        )
        self.theta = (np.pi, np.pi / 2, 0,)
        self.thetas = np.ones((3 * 5,))
        self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
        self.constraints_func = generate_constraints_function(self.robot_arm)
        self.constraint_gradients_func = generate_constraint_gradients_function(self.robot_arm)

    def test_constraints_func_return_type(self):
        constraints = self.constraints_func(self.thetas)
        self.assertEqual(constraints.shape, (2 * 5,))

    def test_constraint_gradients_func_return_type(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
        # print(np.array2string(constraint_gradients, max_line_width=np.inf))

    def test_licq(self):
        constraint_gradients = self.constraint_gradients_func(self.thetas)
        rank = np.linalg.matrix_rank(constraint_gradients)
        self.assertEqual(rank, 2 * 5)
