diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py index 767b649932288a21caf100e7622f8e5d760bb726..44e29eb24addaef40359ab7919de91fce96d29e6 100644 --- a/python/test/binding_tests.py +++ b/python/test/binding_tests.py @@ -1,8 +1,7 @@ import curves # noqa - necessary to register curves::bezier_curve import numpy as np +from numpy import array from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP -from numpy import array, asmatrix, matrix - from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData, computeCOMTraj, zeroStepCapturability) @@ -24,18 +23,18 @@ assert (previous != eq.useWarmStart()) assert (eq.getName() == "test") z = array([0., 0., 1.]) -P = asmatrix(array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]])) -N = asmatrix(array([z for _ in range(4)])) +P = array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]]) +N = array([z for _ in range(4)]) # setting contact positions and normals, as well as friction coefficients -eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) -# eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP) +eq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) +# eq.setNewContacts(P,N,0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP) # setting up optimization problem -c0 = matrix([0., 0., 1.]).T -# dc0 = matrix(np.random.uniform(-1, 1, size=3)); -dc0 = matrix([0.1, 0., 0.]).T -l0 = matrix([0., 0., 0.]).T +c0 = array([0., 0., 1.]) +# dc0 = array(np.random.uniform(-1, 1, size=3)); +dc0 = array([0.1, 0., 0.]) +l0 = array([0., 0., 0.]) T = 1.2 tstep = -1. @@ -45,7 +44,7 @@ assert (a.success) a.c_of_t(0) a.dL_of_t(T) -Kin = matrix(np.identity(3)) +Kin = np.identity(3) kin = 10 * np.ones(3) # TODO: Invalid sizes when resizing a matrix or array. # a = zeroStepCapturability(eq, c0, dc0, l0, False, T, tstep, Kin, matrix(kin)) @@ -60,11 +59,11 @@ kin = 10 * np.ones(3) # testing contactData cData = ContactData(Equilibrium("test", 54., 4)) ceq = cData.contactPhase_ -ceq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) +ceq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) assert cData.contactPhase_.getAlgorithm( ) == EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP, "modifying ceq should modify cData.contactPhase_" -Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) +Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) excep = False try: @@ -72,12 +71,12 @@ try: except RuntimeError: excep = True assert excep, "[ERROR] No kin assigned should have raised exception" -cData.setKinematicConstraints(Id, matrix([0., 0., 1.]).T) +cData.setKinematicConstraints(Id, array([0., 0., 1.])) cData.Kin_ excep = False try: - cData.setKinematicConstraints(Id, matrix([0., 0., 0., 1.]).T) + cData.setKinematicConstraints(Id, array([0., 0., 0., 1.])) except RuntimeError: excep = True assert excep, "[ERROR] Miss matching matrix and vector should raise an error" @@ -88,12 +87,12 @@ try: except RuntimeError: excep = True assert excep, "[ERROR] No Ang_ assigned should have raised exception" -cData.setAngularConstraints(Id, matrix([0., 0., 1.]).T) +cData.setAngularConstraints(Id, array([0., 0., 1.])) cData.Ang_ excep = False try: - cData.setAngularConstraints(Id, matrix([0., 0., 0., 1.]).T) + cData.setAngularConstraints(Id, array([0., 0., 0., 1.])) except RuntimeError: excep = True assert excep, "[ERROR] Missmatching matrix and vector should raise an error" @@ -117,7 +116,7 @@ assert c.reduce_h_ != old # testing problem data c = ProblemData() -nv = matrix([0., 0., 10.]).T +nv = array([0., 0., 10.]) old = c.c0_ c.c0_ = nv assert (c.c0_ != old).any() @@ -151,7 +150,7 @@ pD.constraints_.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | Cons def initContactData(pD): cData = ContactData(Equilibrium("test", 54., 4)) - cData.contactPhase_.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) + cData.contactPhase_.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) pD.addContact(cData) @@ -159,23 +158,23 @@ def initContactData(pD): pD.c0_ = c0 pD.dc0_ = dc0 -res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05) +res = computeCOMTraj(pD, array([0.4, 0.4, 0.4]), 0.05) # test glpk only if defined try: test = SOLVER_GLPK - res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_GLPK) + res = computeCOMTraj(pD, array([0.4, 0.4, 0.4]), 0.05, SOLVER_GLPK) except NameError: print("[WARNING] SOLVER_GLPK is not defined.") print("Consider installing GLPK if you are using CROC with a force formulation") -res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_QUADPROG) -# res = computeCOMTraj(pD,matrix([0.4,0.4,0.4]).T,0.05,SOLVER_QUADPROG_SPARSE) +res = computeCOMTraj(pD, array([0.4, 0.4, 0.4]), 0.05, SOLVER_QUADPROG) +# res = computeCOMTraj(pD,array([0.4,0.4,0.4]),0.05,SOLVER_QUADPROG_SPARSE) assert np.linalg.norm(res.c_of_t.derivate(1.2, 1)) < 0.00000001 # non matching time step and contact phases excep = False try: - res = computeCOMTraj(pD, matrix([0.4, 0.4]).T, 0.05) + res = computeCOMTraj(pD, array([0.4, 0.4]), 0.05) except RuntimeError: excep = True assert excep, "[ERROR] computeCOMTraj should have raised exception"