Merge branch 'devel' into 'devel'

switch to numpy arrays See merge request gepetto/example-robot-data!27
Gepetto · Nov 27, 2019 · 850f6ec · 850f6ec
2 parents e6db50c + 2c8caa1
commit 850f6ec
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 6 deletions.
diff --git 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
@@ -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
@@ -2,20 +2,30 @@
 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())
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		EXAMPLE_ROBOT_DATA_MODEL_DIR = "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/robots"