[Python] make .py scripts compatible with python 2 & 3

bindings/python/scripts/__init__.py

@@ -17,10 +17,10 @@
 import numpy as np
 from pinocchio.robot_wrapper import RobotWrapper
 
-import libpinocchio_pywrap as se3
-import utils
-from explog import exp, log
-from libpinocchio_pywrap import *
+from . import libpinocchio_pywrap as se3
+from . import utils
+from .explog import exp, log
+from .libpinocchio_pywrap import *
 
 se3.AngleAxis.__repr__ = lambda s: 'AngleAxis(%s)' % s.vector()
 

bindings/python/scripts/derivative/dcrba.py

@@ -14,6 +14,8 @@
 # Pinocchio If not, see
 # <http://www.gnu.org/licenses/>.
 
+from __future__ import print_function
+
 import pinocchio as se3
 from pinocchio.robot_wrapper import RobotWrapper
 from pinocchio.utils import *
@@ -379,7 +381,7 @@ if __name__ == '__main__':
                     se3.computeAllTerms(robot.model,robot.data,q,vq1)
                     Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
     
-    print 'Check hessian = \t\t', norm(H-Htrue)    
+    print('Check hessian = \t\t', norm(H-Htrue))    
 
     # --- dCRBA ---
     # --- dCRBA ---
@@ -405,7 +407,7 @@ if __name__ == '__main__':
     
     dM /= eps
     
-    print 'Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ])
+    print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ]))
     
     
     # --- vRNEA ---
@@ -431,18 +433,18 @@ if __name__ == '__main__':
             vq1[i] = vq1[j] = 1.0
             C[:,i,j] = (rnea0(q,vq1).T-C[:,i,i]-C[:,j,j]) /2
 
-    print "Check d/dv rnea = \t\t",norm(quad(Q,vq)-rnea0(q,vq))
-    print "Check C  = Q+Q.T = \t\t", norm((Q+Q.transpose(0,2,1))/2-C)
-    print "Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) )
-    print "Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp - 
-                                            (Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 )
+    print("Check d/dv rnea = \t\t",norm(quad(Q,vq)-rnea0(q,vq)))
+    print("Check C  = Q+Q.T = \t\t", norm((Q+Q.transpose(0,2,1))/2-C))
+    print("Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) ))
+    print("Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp - 
+                                            (Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 ))
 
     # --- CORIOLIS
     # --- CORIOLIS
     # --- CORIOLIS
     coriolis = Coriolis(robot)
     C = coriolis(q,vq)
-    print "Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq))
+    print("Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq)))
 
     # --- DRNEA
     # --- DRNEA
@@ -459,7 +461,7 @@ if __name__ == '__main__':
         dq = zero(NV); dq[i]=eps
         qdq = se3.integrate(robot.model,q,dq)
         Rd[:,i] = (se3.rnea(robot.model,robot.data,qdq,vq,aq)-r0)/eps
-    print "Check drnea    \t\t\t",norm(Rd-R)
+    print("Check drnea    \t\t\t",norm(Rd-R))
 
 
     

bindings/python/scripts/derivative/lambdas.py

@@ -26,8 +26,8 @@ def jFromIdx(idxv,robot):
             return j
 
 parent = lambda i,robot: robot.model.parents[i]
-iv = lambda i,robot: range(robot.model.joints[i].idx_v,
-                     robot.model.joints[i].idx_v+robot.model.joints[i].nv)
+iv = lambda i,robot: list(range(robot.model.joints[i].idx_v,
+                     robot.model.joints[i].idx_v+robot.model.joints[i].nv))
 ancestors = lambda j,robot,res=[]: res if j==0 else ancestors(robot.model.parents[j],robot,[j,]+res)
 
 class ancestorOf:

bindings/python/scripts/explog.py

@@ -19,7 +19,7 @@ import math
 
 import numpy as np
 
-import libpinocchio_pywrap as se3
+from . import libpinocchio_pywrap as se3
 
 
 def exp(x):

bindings/python/scripts/robot_wrapper.py

@@ -14,8 +14,8 @@
 # Pinocchio If not, see
 # <http://www.gnu.org/licenses/>.
 
-import libpinocchio_pywrap as se3
-import utils
+from . import libpinocchio_pywrap as se3
+from . import utils
 import time
 import os
 
@@ -38,7 +38,7 @@ class RobotWrapper(object):
                 self.collision_model = se3.buildGeomFromUrdf(self.model, filename, se3.GeometryType.COLLISION)
                 self.visual_model = se3.buildGeomFromUrdf(self.model, filename, se3.GeometryType.VISUAL)
             else:
-                if not all(isinstance(item, basestring) for item in package_dirs):
+                if not all(isinstance(item, str) for item in package_dirs):
                     raise Exception('The list of package directories is wrong. At least one is not a string')
                 else:
                     collision_model = se3.buildGeomFromUrdf(model, filename,
@@ -246,7 +246,7 @@ class RobotWrapper(object):
     def loadDisplayModel(self, rootNodeName="pinocchio"):
     
         def loadDisplayGeometryObject(geometry_object,geometry_type):
-            from rpy import npToTuple
+            from .rpy import npToTuple
             
             meshName = self.getViewerNodeName(geometry_object,geometry_type)
             meshPath = geometry_object.meshPath

bindings/python/scripts/romeo_wrapper.py

@@ -16,8 +16,8 @@
 
 import numpy as np
 
-import libpinocchio_pywrap as se3
-from robot_wrapper import RobotWrapper
+from . import libpinocchio_pywrap as se3
+from .robot_wrapper import RobotWrapper
 
 
 class RomeoWrapper(RobotWrapper):

bindings/python/scripts/rpy.py

@@ -18,7 +18,7 @@ from math import atan2, pi, sqrt
 
 import numpy as np
 
-import libpinocchio_pywrap as se3
+from . import libpinocchio_pywrap as se3
 
 
 def npToTTuple(M):

bindings/python/scripts/utils.py

@@ -14,15 +14,17 @@
 # Pinocchio If not, see
 # <http://www.gnu.org/licenses/>.
 
+from __future__ import print_function
+
 import sys
 
 import numpy as np
 import numpy.linalg as npl
 
-import libpinocchio_pywrap as se3
-from rpy import matrixToRpy, npToTTuple, npToTuple, rotate, rpyToMatrix
+from . import libpinocchio_pywrap as se3
+from .rpy import matrixToRpy, npToTTuple, npToTuple, rotate, rpyToMatrix
 
-from deprecation import deprecated
+from .deprecation import deprecated
 
 eye = lambda n: np.matrix(np.eye(n), np.double)
 zero = lambda n: np.matrix(np.zeros([n, 1] if isinstance(n, int) else n), np.double)
@@ -96,8 +98,8 @@ def mprint(M, name="ans",eps=1e-15):
         M = M.homogeneous
     ncol = M.shape[1]
     NC = 6
-    print name, " = "
-    print
+    print(name, " = ")
+    print()
 
     Mmin = lambda M: M.min() if np.nonzero(M)[1].shape[1]>0 else M.sum()
     Mmax = lambda M: M.max() if np.nonzero(M)[1].shape[1]>0 else M.sum()
@@ -110,15 +112,15 @@ def mprint(M, name="ans",eps=1e-15):
         cmin = i * 6
         cmax = (i + 1) * 6
         cmax = ncol if ncol < cmax else cmax
-        print "Columns %s through %s" % (cmin, cmax - 1)
-        print
+        print("Columns %s through %s" % (cmin, cmax - 1))
+        print()
         for r in range(M.shape[0]):
             sys.stdout.write("  ")
             for c in range(cmin, cmax):
                 if abs(M[r,c])>eps: sys.stdout.write(fmt % M[r,c]  + "   ")
                 else: sys.stdout.write(" 0"+" "*9)
-            print
-        print
+            print()
+        print()
 
 
 def fromListToVectorOfString(items):

lab/python/exp_integration.py

@@ -4,6 +4,8 @@ purpose of this script is to exhibit the use of the exp and log map for
 interpolating SE(3) movements.
 '''
 
+from __future__ import print_function
+
 import robotviewer
 viewer = robotviewer.client('XML-RPC')
 try:
@@ -49,7 +51,7 @@ nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
 for i in range(N):
     M = M * se3.exp(nu)
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
-print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
+print("Residuals = ", norm(se3.log(M.inverse() * ME).vector()))
 time.sleep(1)
 
 # Integrate a constant "log" velocity in reference frame.
@@ -59,7 +61,7 @@ nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
 for i in range(N):
     M = M * se3.exp(nu)
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
-print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
+print("Residuals = ", norm(se3.log(M.inverse() * ME).vector()))
 time.sleep(1)
 
 # Integrate an exponential decay vector field toward ME.
@@ -69,7 +71,7 @@ for i in range(N):
     nu = se3.log(M.inverse() * ME).vector() * 1e-2
     M = M * se3.exp(nu)
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
-print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
+print("Residuals = ", norm(se3.log(M.inverse() * ME).vector()))
 time.sleep(1)
 
 # Integrate a straight-line vector field toward ME.
@@ -82,5 +84,5 @@ for i in range(N):
     nu = se3.Motion(v, w)
     M = M * se3.exp(nu)
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
-print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
+print("Residuals = ", norm(se3.log(M.inverse() * ME).vector()))
 time.sleep(1)

lab/python/kineinv.py

@@ -4,6 +4,8 @@ abscissia), dimension 3 (x,y,z coordinates), dimension 6 (SE3 exp integration)
 and dimension 6 (R3xSO3 integration, straightline in the cartesian space).
 '''
 
+from __future__ import print_function
+
 
 def kineinv_dim1(q0, xdes, N=100):
     '''
@@ -22,7 +24,7 @@ def kineinv_dim1(q0, xdes, N=100):
 
         robot.increment(q, qdot * 5e-2)
         robot.display(q)
-    print "Residuals = ", robot.Mrh(q).translation[0, 0] - xdes
+    print("Residuals = ", robot.Mrh(q).translation[0, 0] - xdes)
 
 
 def kineinv_dim3(q0, xdes, N=100):
@@ -42,7 +44,7 @@ def kineinv_dim3(q0, xdes, N=100):
 
         robot.increment(q, qdot * 5e-2)
         robot.display(q)
-    print "Residuals = ", npl.norm(robot.Mrh(q).translation[:3] - xdes)
+    print("Residuals = ", npl.norm(robot.Mrh(q).translation[:3] - xdes))
 
 
 def kineinv_dim6(q0, Mdes, straight=True, N=100):
@@ -72,7 +74,7 @@ def kineinv_dim6(q0, Mdes, straight=True, N=100):
 
         robot.increment(q, qdot * 5e-2)
         robot.display(q)
-    print "Residuals = ", npl.norm(se3.log(robot.Mrh(q).inverse() * Mdes).vector())
+    print("Residuals = ", npl.norm(se3.log(robot.Mrh(q).inverse() * Mdes).vector()))
 
 # --- MAIN ------------------------------------------------------
 if __name__ == '__main__':

python/explog.py

@@ -13,7 +13,7 @@ class TestExpLog(TestCase):
         self.assertApprox(log(42), math.log(42))
         self.assertApprox(exp(log(42)), 42)
         self.assertApprox(log(exp(42)), 42)
-        m = np.matrix(range(1, 4), np.double).T
+        m = np.matrix(list(range(1, 4)), np.double).T
         self.assertApprox(log(exp(m)), m)
         m = se3.SE3.Random()
         self.assertApprox(exp(log(m)), m)
@@ -24,8 +24,8 @@ class TestExpLog(TestCase):
         with self.assertRaises(ValueError):
             exp(np.eye(4))
         with self.assertRaises(ValueError):
-            exp(range(3))
+            exp(list(range(3)))
         with self.assertRaises(ValueError):
-            log(range(3))
+            log(list(range(3)))
         with self.assertRaises(ValueError):
             log(np.zeros(5))

python/rpy.py

@@ -8,7 +8,7 @@ from test_case import TestCase
 
 class TestRPY(TestCase):
     def test_npToTuple(self):
-        m = np.matrix(range(9))
+        m = np.matrix(list(range(9)))
         self.assertEqual(npToTuple(m), tuple(range(9)))
         self.assertEqual(npToTuple(m.T), tuple(range(9)))
         self.assertEqual(npToTuple(np.reshape(m, (3, 3))), ((0, 1, 2), (3, 4, 5), (6, 7, 8)))
@@ -18,5 +18,5 @@ class TestRPY(TestCase):
         self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
         m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
         self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
-        rpy = np.matrix(range(3)).T * pi / 2
+        rpy = np.matrix(list(range(3))).T * pi / 2
         self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)

python/tests.py

 #!/usr/bin/env python
 
+from __future__ import print_function
+
 import unittest, sys
 
 from bindings import TestSE3  # noqa
@@ -15,6 +17,6 @@ from rpy import TestRPY  # noqa
 from utils import TestUtils  # noqa
 
 if __name__ == '__main__':
-    print "Python version"
-    print sys.version_info
+    print("Python version")
+    print(sys.version_info)
     unittest.main()