Commit ec38684d authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python][Test] Format

parent b632989c
......@@ -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")
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))
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment