[Python][Test] Format

python/CMakeLists.txt

@@ -20,5 +20,4 @@ INSTALL(
   TARGETS ${PY_NAME}	DESTINATION ${PYTHON_SITELIB}
 )
 
-# TODO
-#ADD_PYTHON_UNIT_TEST("python-centroidal-dynamics" "python/test/binding_tests.py" "python")
+ADD_PYTHON_UNIT_TEST("python-centroidal-dynamics" "python/test/binding_tests.py" "python")

python/test/binding_tests.py

-from hpp_centroidal_dynamics import *
+from numpy import array, asmatrix, cross
 
-#testing constructors
+from hpp_centroidal_dynamics import LP_STATUS_OPTIMAL, Equilibrium, EquilibriumAlgorithm, SolverLP
+
+# testing constructors
 eq = Equilibrium("test", 54., 4)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
 eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
 eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
-eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True )
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
 
-#whether useWarmStart is enable (True by default)
+# whether useWarmStart is enable (True by default)
 previous = eq.useWarmStart()
-#enable warm start in solver (only for QPOases)
+# enable warm start in solver (only for QPOases)
 eq.setUseWarmStart(False)
-assert(previous != eq.useWarmStart())
-
-#access solver name
-assert(eq.getName() == "test")
-
+assert (previous != eq.useWarmStart())
 
-# creating contact points
-from numpy import array, asmatrix, matrix
+# access solver name
+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]]))
+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)]))
 
-#setting contact positions and normals, as well as friction coefficients
-eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
+# setting contact positions and normals, as well as friction coefficients
+eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
 
-c= asmatrix(array([0.,0.,1.])).T
+c = asmatrix(array([0., 0., 1.])).T
 
-#computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
+# computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
 status, robustness = eq.computeEquilibriumRobustness(c)
 assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
 assert (robustness > 0), "first test should be in equilibrirum"
 
-#computing robustness of a given configuration with non zero acceleration
-ddc= asmatrix(array([1000.,0.,0.])).T
-status, robustness = eq.computeEquilibriumRobustness(c,ddc)
+# computing robustness of a given configuration with non zero acceleration
+ddc = asmatrix(array([1000., 0., 0.])).T
+status, robustness = eq.computeEquilibriumRobustness(c, ddc)
 assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
 assert (robustness < 0), "first test should NOT be in equilibrirum"
 
-#now, use polytope projection algorithm
-eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
-H,h = eq.getPolytopeInequalities()
+# now, use polytope projection algorithm
+eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
+H, h = eq.getPolytopeInequalities()
 
-#~ c= asmatrix(array([0.,0.,1.])).T
+# c= asmatrix(array([0.,0.,1.])).T
 status, stable = eq.checkRobustEquilibrium(c)
 assert (status == LP_STATUS_OPTIMAL), "checkRobustEquilibrium should not fail"
 assert (stable), "lat test should be in equilibrirum"
 
 
-from numpy import array, vstack, zeros, sqrt, cross, matrix, asmatrix
-from numpy.linalg import norm
-import numpy as np
+def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
+    w1 = m * (ddc - g_vec)
+    return array(w1.tolist() + (cross(c, w1) + dL).tolist())
 
-def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
-	w1 = m * (ddc - g_vec)
-	return array(w1.tolist() + (cross(c, w1) + dL).tolist())
 
+def is_stable(H,
+              c=array([0., 0., 1.]),
+              ddc=array([0., 0., 0.]),
+              dL=array([0., 0., 0.]),
+              m=54.,
+              g_vec=array([0., 0., -9.81]),
+              robustness=0.):
+    w = compute_w(c, ddc, dL, m, g_vec)
+    return (H.dot(-w) <= -robustness).all()
 
-def is_stable(H,c=array([0.,0.,1.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
-	w = compute_w(c, ddc, dL, m, g_vec)
-	return (H.dot(-w)<=-robustness).all()
 
-assert(is_stable(H))
+assert (is_stable(H))