diff --git a/tests/python/test_Constraint.py b/tests/python/test_Constraint.py index 89c567e5a267989ed07fdc26bffae50f4336d5a0..db6dcac59e6b78f6084706eb68b4ba11748f289b 100644 --- a/tests/python/test_Constraint.py +++ b/tests/python/test_Constraint.py @@ -7,12 +7,12 @@ print("") print("Test Constraint Bound") print("") -se3.switchToNumpyMatrix() +#se3.switchToNumpyMatrix() tol = 1e-5 n = 5 -lb = np.matrix(-1.0 * np.ones(n)).transpose() -ub = np.matrix(np.ones(n)).transpose() +lb = -1.0 * np.ones(n) +ub = np.ones(n) ConstBound = tsid.ConstraintBound("bounds", lb, ub) assert ConstBound.isBound @@ -41,8 +41,8 @@ print("Test Constraint Equality") print("") n = 5 m = 2 -A = np.matrix(np.ones((m, n))) -b = np.matrix(np.ones(m)).transpose() +A = np.ones((m, n)) +b = np.ones(m) equality = tsid.ConstraintEquality("equality", A, b) assert not equality.isBound @@ -71,9 +71,9 @@ print("") n = 5 m = 2 -A = np.matrix(np.ones((m, n))) -lb = np.matrix(-np.ones(m)).transpose() -ub = np.matrix(np.ones(m)).transpose() +A = np.ones((m, n)) +lb = -np.ones(m) +ub = np.ones(m) inequality = tsid.ConstraintInequality("inequality", A, lb, ub) assert not inequality.isBound diff --git a/tests/python/test_Solvers.py b/tests/python/test_Solvers.py index 50668d77aafae982ba193fb44e4532e3a15e69de..60fdc2e91dc4f39b7e00caa6f3e2626cfe81884b 100644 --- a/tests/python/test_Solvers.py +++ b/tests/python/test_Solvers.py @@ -1,16 +1,10 @@ -import copy - import numpy as np -import pinocchio as se3 - import tsid print("") print("Test Solvers") print("") -se3.switchToNumpyMatrix() - EPS = 1e-3 nTest = 100 n = 60 @@ -29,15 +23,15 @@ solver = tsid.SolverHQuadProg("qp solver") solver.resize(n, neq, nin) HQPData = tsid.HQPData() -A1 = np.matrix(np.random.rand(n, n)) + 0.001 * np.matrix(np.eye(n)) -b1 = np.matrix(np.random.rand(n)).transpose() +A1 = np.random.rand(n, n) + 0.001 * np.eye(n) +b1 = np.random.rand(n) cost = tsid.ConstraintEquality("c1", A1, b1) -x = np.linalg.inv(A1) * b1 -A_in = np.matrix(np.random.rand(nin, n)) -A_lb = np.matrix(np.random.rand(nin)).transpose() * NORMAL_DISTR_VAR -A_ub = np.matrix(np.random.rand(nin)).transpose() * NORMAL_DISTR_VAR -constrVal = A_in * x +x = np.linalg.inv(A1) @ b1 +A_in = np.random.rand(nin, n) +A_lb = np.random.rand(nin) * NORMAL_DISTR_VAR +A_ub = np.random.rand(nin) * NORMAL_DISTR_VAR +constrVal = A_in @ x for i in range(0, nin): if A_ub[i] <= A_lb[i]: @@ -50,8 +44,8 @@ for i in range(0, nin): A_lb[i] = constrVal[i] - MARGIN_PERC * np.abs(constrVal[i]) in_const = tsid.ConstraintInequality("ini1", A_in, A_lb, A_ub) -A_eq = np.matrix(np.random.rand(neq, n)) -b_eq = A_eq * x +A_eq = np.random.rand(neq, n) +b_eq = A_eq @ x eq_const = tsid.ConstraintEquality("eq1", A_eq, b_eq) const1 = tsid.ConstraintLevel() @@ -73,8 +67,8 @@ HQPData.print_all() gradientPerturbations = [] hessianPerturbations = [] for i in range(0, nTest): - gradientPerturbations.append(np.matrix(np.random.rand(n) * GRADIENT_PERTURBATION_VARIANCE).transpose()) - hessianPerturbations.append(np.matrix(np.random.rand(n, n) * HESSIAN_PERTURBATION_VARIANCE)) + gradientPerturbations.append(np.random.rand(n) * GRADIENT_PERTURBATION_VARIANCE) + hessianPerturbations.append(np.random.rand(n, n) * HESSIAN_PERTURBATION_VARIANCE) for i in range(0, nTest): cost.setMatrix(cost.matrix + hessianPerturbations[i]) @@ -82,6 +76,6 @@ for i in range(0, nTest): HQPoutput = solver.solve(HQPData) - assert np.linalg.norm(A_eq * HQPoutput.x - b_eq, 2) < EPS - # assert (A_in * HQPoutput.x <= A_ub + EPS).all() - # assert (A_in * HQPoutput.x > A_lb - EPS).all() + assert np.linalg.norm(A_eq @ HQPoutput.x - b_eq, 2) < EPS + # assert (A_in @ HQPoutput.x <= A_ub + EPS).all() + # assert (A_in @ HQPoutput.x > A_lb - EPS).all()