Commit 850f6ec0 authored by Guilhem Saurel's avatar Guilhem Saurel

Merge branch 'devel' into 'devel'

switch to numpy arrays

See merge request gepetto/example-robot-data!27
parents e6db50ce 2c8caa12
Pipeline #7004 failed with stage
in 2 minutes and 23 seconds
......@@ -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}")
EXAMPLE_ROBOT_DATA_MODEL_DIR = "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/robots"
......@@ -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())
......
Markdown is supported
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