diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 998d2c43c06eee5141e5470f63913b7a8b92183e..ce695288afdc4154eaf2ba452e224c850bed1bf3 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -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}") diff --git a/python/example_robot_data/path.py.in b/python/example_robot_data/path.py.in new file mode 100644 index 0000000000000000000000000000000000000000..c1b0976152319a2a6d7b3ef8b8afc5452ba38e55 --- /dev/null +++ b/python/example_robot_data/path.py.in @@ -0,0 +1 @@ +EXAMPLE_ROBOT_DATA_MODEL_DIR = "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/robots" diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index c2c8caac790be9e28dc23ce41370c07dfa4edc4c..8d36a81502d12923fee441f0ac9d593df46d74d6 100644 --- a/python/example_robot_data/robots_loader.py +++ b/python/example_robot_data/robots_loader.py @@ -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())