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
loco-3d
sot-talos-balance
Commits
857e3945
Commit
857e3945
authored
Mar 30, 2020
by
Olivier Stasse
Browse files
[tests/python] Remove unnecessary call to rospack
parent
f348e81e
Pipeline
#8889
failed with stage
in 35 minutes and 2 seconds
Changes
9
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
tests/python/test_com_estimation.py
View file @
857e3945
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.base_estimator_conf
as
base_estimator_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
(
DcmEstimator
,
EulerToQuat
,
TalosBaseEstimator
,
create_parameter_server
,
plug
)
from
sot_talos_balance.create_entities_utils
\
import
(
DcmEstimator
,
TalosBaseEstimator
,
create_parameter_server
,
plug
)
from
sot_talos_balance.euler_to_quat
import
EulerToQuat
pin
.
switchToNumpyMatrix
()
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -62,9 +67,8 @@ q = np.matrix(halfSitting).T
print
(
"q:"
)
print
(
q
.
flatten
().
tolist
()[
0
])
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
@@ -96,8 +100,9 @@ tauy = -fz * lever
wrenchLeft
=
forceLeft
+
[
0.0
,
tauy
,
0.0
]
wrenchRight
=
forceRight
+
[
0.0
,
tauy
,
0.0
]
centerTranslation
=
(
data
.
oMf
[
rightId
].
translation
+
data
.
oMf
[
leftId
].
translation
)
/
2
+
np
.
matrix
(
param_server_conf
.
rightFootSoleXYZ
).
T
centerTranslation
=
(
data
.
oMf
[
rightId
].
translation
+
data
.
oMf
[
leftId
].
translation
)
/
2
+
\
np
.
matrix
(
param_server_conf
.
rightFootSoleXYZ
).
T
centerPos
=
pin
.
SE3
(
rightPos
.
rotation
,
centerTranslation
)
print
(
"Center of feet:"
)
...
...
tests/python/test_dcm_controller_talos.py
View file @
857e3945
...
...
@@ -3,7 +3,6 @@ from math import sqrt
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
DcmController
,
create_parameter_server
...
...
@@ -62,9 +61,8 @@ q = np.matrix(halfSitting).T
print
(
"q:"
)
print
(
q
.
flatten
().
tolist
()[
0
])
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
tests/python/test_dcm_distribute.py
View file @
857e3945
...
...
@@ -3,14 +3,17 @@ from math import sqrt
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.distribute_conf
as
distribute_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
(
DcmController
,
create_distribute_wrench
,
create_parameter_server
,
plug
)
from
sot_talos_balance.create_entities_utils
import
\
(
DcmController
,
create_distribute_wrench
,
create_parameter_server
,
plug
)
pin
.
switchToNumpyMatrix
()
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -64,9 +67,8 @@ q = np.matrix(halfSitting).T
print
(
"q:"
)
print
(
q
.
flatten
().
tolist
()[
0
])
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
tests/python/test_dcm_estimator.py
View file @
857e3945
...
...
@@ -3,20 +3,18 @@ from __future__ import print_function
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
import
sot_talos_balance.talos.parameter_server_conf
as
parameter_server_conf
from
sot_talos_balance.create_entities_utils
import
Bunch
,
DcmEstimator
,
ParameterServer
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
Bunch
,
DcmEstimator
,
\
ParameterServer
dt
=
0.001
conf
=
Bunch
()
robot_name
=
'robot'
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
model
.
lowerPositionLimit
=
np
.
vstack
((
np
.
matrix
([
-
1.
]
*
7
).
T
,
model
.
lowerPositionLimit
[
7
:]))
...
...
@@ -30,12 +28,11 @@ print("Expected:")
print
(
"CoM position value: {0}"
.
format
(
tuple
(
data
.
com
[
0
].
flat
)))
print
(
"CoM velocity value: {0}"
.
format
(
tuple
(
data
.
vcom
[
0
].
flat
)))
conf
.
param_server
=
parameter_server_conf
param_server
=
ParameterServer
(
"param_server"
)
param_server
.
init
(
dt
,
conf
.
param_server
.
urdfFileName
,
robot_name
)
param_server
.
setJointsUrdfToSot
(
conf
.
param_server
.
urdftosot
)
param_server
.
setRightFootForceSensorXYZ
(
conf
.
param_server
.
rightFootSensorXYZ
)
param_server
.
setRightFootSoleXYZ
(
conf
.
param_server
.
rightFootSoleXYZ
)
param_server
.
init
(
dt
,
param_server
_conf
.
urdfFileName
,
robot_name
)
param_server
.
setJointsUrdfToSot
(
param_server
_conf
.
urdftosot
)
param_server
.
setRightFootForceSensorXYZ
(
param_server
_conf
.
rightFootSensorXYZ
)
param_server
.
setRightFootSoleXYZ
(
param_server
_conf
.
rightFootSoleXYZ
)
dcm_estimator
=
DcmEstimator
(
'dcm_estimator'
)
dcm_estimator
.
q
.
value
=
list
(
q
.
flat
)
...
...
tests/python/test_distribute.py
View file @
857e3945
from
__future__
import
print_function
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.distribute_conf
as
distribute_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
create_distribute_wrench
,
create_parameter_server
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -60,9 +60,8 @@ halfSitting = [
q
=
np
.
matrix
(
halfSitting
).
T
print
(
"q: %s
\n
"
%
str
(
q
.
flatten
().
tolist
()[
0
]))
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
tests/python/test_dummy_wpgen.py
View file @
857e3945
...
...
@@ -3,13 +3,14 @@ from math import sqrt
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
(
DummyWalkingPatternGenerator
,
SimpleReferenceFrame
,
create_parameter_server
,
plug
)
from
sot_talos_balance.create_entities_utils
import
\
(
DummyWalkingPatternGenerator
,
SimpleReferenceFrame
,
create_parameter_server
,
plug
)
pin
.
switchToNumpyMatrix
()
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -62,9 +63,8 @@ q = np.matrix(halfSitting).T
print
(
"q:"
)
print
(
q
.
flatten
().
tolist
()[
0
])
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
tests/python/test_round_double_to_int.py
View file @
857e3945
...
...
@@ -4,8 +4,6 @@ import numpy as np
from
sot_talos_balance.round_double_to_int
import
RoundDoubleToInt
import
eigenpy
eigenpy
.
switchToNumpyMatrix
()
rd
=
RoundDoubleToInt
(
'round'
)
time
=
0
...
...
tests/python/test_simple_distribute.py
View file @
857e3945
...
...
@@ -8,6 +8,8 @@ from rospkg import RosPack
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
create_parameter_server
,
create_simple_distribute_wrench
pin
.
switchToNumpyMatrix
()
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -59,9 +61,8 @@ halfSitting = [
q
=
np
.
matrix
(
halfSitting
).
T
print
(
"q: %s
\n
"
%
str
(
q
.
flatten
().
tolist
()[
0
]))
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
tests/python/test_state_transformation.py
View file @
857e3945
import
eigenpy
import
numpy
as
np
import
pinocchio
as
pin
from
numpy.testing
import
assert_almost_equal
as
assertApprox
from
rospkg
import
RosPack
pin
.
switchToNumpyMatrix
()
import
sot_talos_balance.talos.base_estimator_conf
as
base_estimator_conf
import
sot_talos_balance.talos.parameter_server_conf
as
param_server_conf
from
sot_talos_balance.create_entities_utils
import
(
DcmEstimator
,
SimpleReferenceFrame
,
StateTransformation
,
TalosBaseEstimator
,
create_parameter_server
,
plug
)
from
sot_talos_balance.euler_to_quat
import
EulerToQuat
from
sot_talos_balance.create_entities_utils
\
import
(
DcmEstimator
,
SimpleReferenceFrame
,
StateTransformation
,
TalosBaseEstimator
,
create_parameter_server
,
plug
)
eigenpy
.
switchToNumpyMatrix
()
from
sot_talos_balance.euler_to_quat
import
EulerToQuat
# --- General ---
print
(
"--- General ---"
)
...
...
@@ -64,9 +64,8 @@ q = np.matrix(halfSitting).T
print
(
"q:"
)
print
(
q
.
flatten
().
tolist
()[
0
])
rospack
=
RosPack
()
urdfPath
=
rospack
.
get_path
(
'talos_data'
)
+
"/urdf/talos_reduced.urdf"
urdfDir
=
[
rospack
.
get_path
(
'talos_data'
)
+
"/../"
]
urdfPath
=
param_server_conf
.
urdfFileName
urdfDir
=
param_server_conf
.
model_path
model
=
pin
.
buildModelFromUrdf
(
urdfPath
,
pin
.
JointModelFreeFlyer
())
data
=
model
.
createData
()
...
...
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