Commit 8586c58a authored by Guilhem Saurel's avatar Guilhem Saurel Committed by jcarpent
Browse files

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

parent 40a7b89f
......@@ -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()
......@@ -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))
......@@ -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:
......
......@@ -19,7 +19,7 @@ import math
import numpy as np
import libpinocchio_pywrap as se3
from . import libpinocchio_pywrap as se3
def exp(x):
......
......@@ -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
......
......@@ -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):
......
......@@ -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):
......
......@@ -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):
......
......@@ -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)
......@@ -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__':
......
......@@ -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))
......@@ -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)
#!/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()
Supports Markdown
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