Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-centroidal-dynamics
Commits
ec38684d
Commit
ec38684d
authored
Sep 24, 2019
by
Guilhem Saurel
Browse files
[Python][Test] Format
parent
b632989c
Changes
2
Hide whitespace changes
Inline
Side-by-side
python/CMakeLists.txt
View file @
ec38684d
...
...
@@ -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
View file @
ec38684d
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
))
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment