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
Gepetto
example-robot-data
Commits
850f6ec0
Commit
850f6ec0
authored
Nov 27, 2019
by
Guilhem Saurel
Browse files
Merge branch 'devel' into 'devel'
switch to numpy arrays See merge request
!27
parents
e6db50ce
2c8caa12
Pipeline
#7004
failed with stage
in 2 minutes and 23 seconds
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
python/CMakeLists.txt
View file @
850f6ec0
...
...
@@ -7,3 +7,6 @@ SET(${PROJECT_NAME}_PYTHON_FILES
FOREACH
(
python
${${
PROJECT_NAME
}
_PYTHON_FILES
}
)
PYTHON_INSTALL_ON_SITE
(
${
PY_NAME
}
${
python
}
)
ENDFOREACH
(
python
${${
PROJECT_NAME
}
_PYTHON_FILES
}
)
CONFIGURE_FILE
(
${
PY_NAME
}
/path.py.in
${
PY_NAME
}
/path.py
)
INSTALL
(
FILES
${
CMAKE_CURRENT_BINARY_DIR
}
/
${
PY_NAME
}
/path.py DESTINATION
"
${
PYTHON_SITELIB
}
/
${
PY_NAME
}
"
)
python/example_robot_data/path.py.in
0 → 100644
View file @
850f6ec0
EXAMPLE_ROBOT_DATA_MODEL_DIR = "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/robots"
python/example_robot_data/robots_loader.py
View file @
850f6ec0
...
...
@@ -2,20 +2,30 @@ import sys
from
os.path
import
dirname
,
exists
,
join
import
numpy
as
np
import
pinocchio
from
pinocchio.robot_wrapper
import
RobotWrapper
pinocchio
.
switchToNumpyArray
()
def
getModelPath
(
subpath
,
printmsg
=
False
):
base
=
'../../../share/example-robot-data/robots'
main_dir
=
dirname
(
dirname
(
dirname
(
__file__
)))
for
path
in
[
join
(
dirname
(
main_dir
),
'robots'
),
join
(
main_dir
,
'robots'
)
]
+
[
join
(
p
,
base
.
strip
(
'/'
))
for
p
in
sys
.
path
]:
paths
=
[
join
(
dirname
(
dirname
(
dirname
(
dirname
(
__file__
)))),
'robots'
),
join
(
dirname
(
dirname
(
dirname
(
__file__
))),
'robots'
)
]
try
:
from
.path
import
EXAMPLE_ROBOT_DATA_MODEL_DIR
paths
.
append
(
EXAMPLE_ROBOT_DATA_MODEL_DIR
)
except
ImportError
:
pass
paths
+=
[
join
(
p
,
'../../../share/example-robot-data/robots'
)
for
p
in
sys
.
path
]
for
path
in
paths
:
if
exists
(
join
(
path
,
subpath
.
strip
(
'/'
))):
if
printmsg
:
print
(
"using %s as modelPath"
%
path
)
return
path
raise
IOError
(
'%s not found'
%
(
subpath
)
)
raise
IOError
(
'%s not found'
%
subpath
)
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
,
has_rotor_parameters
=
True
,
referencePose
=
'half_sitting'
):
...
...
@@ -138,7 +148,7 @@ def loadTalosLegs():
robot
.
visual_data
=
pinocchio
.
GeometryData
(
g2
)
# Load SRDF file
robot
.
q0
=
np
.
matrix
(
np
.
resize
(
robot
.
q0
,
robot
.
model
.
nq
)).
T
robot
.
q0
=
np
.
array
(
np
.
resize
(
robot
.
q0
,
robot
.
model
.
nq
)).
T
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
assert
((
m2
.
armature
[:
6
]
==
0.
).
all
())
...
...
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