Merge pull request #133 from nim65s/devel

[Python] Some propositions about Format & Tests

.travis.yml

-language: cpp
+language: python
+python:
+- "2.7"
 sudo: required
 compiler:
 - gcc
@@ -28,6 +30,9 @@ script:
   - export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE}"
   - sudo free -m -t
   - travis_wait ./.travis/run ../travis_custom/custom_build
+  - export PYTHONPATH=/tmp/_ci/install/lib/python2.7/site-packages
+  - python ./python/bindings.py
+  - python ./python/test_model.py
 after_success: ./.travis/run after_success
 after_failure: ./.travis/run after_failure
 before_install: ./travis_custom/custom_before_install

lab/python/exp_integration.py

@@ -5,11 +5,12 @@ interpolating SE(3) movements.
 '''
 
 import robotviewer
-viewer=robotviewer.client('XML-RPC')
+viewer = robotviewer.client('XML-RPC')
 try:
-    viewer.updateElementConfig('RomeoTrunkYaw',[1,0,0,0,0,0])
-except: 
-    viewer.updateElementConfig = lambda a,b: a
+    viewer.updateElementConfig('RomeoTrunkYaw', [1, 0, 0, 0, 0, 0])
+except:
+    viewer.updateElementConfig = lambda a, b: a
+
 
 def se3ToRpy(m):
     return M.translation.T.tolist()[0] + matrixToRpy(M.rotation).T.tolist()[0]
@@ -23,62 +24,63 @@ N = 1000
 M = se3.SE3.Identity()
 
 # Integrate a constant body velocity.
-v = zero(3); v[2] = 1.0 / N
-w = zero(3); w[1] = 1.0 / N
-nu = se3.Motion( v, w)
+v = zero(3)
+v[2] = 1.0 / N
+w = zero(3)
+w[1] = 1.0 / N
+nu = se3.Motion(v, w)
 
 for i in range(N):
-    M = se3.exp(nu)*M
+    M = se3.exp(nu) * M
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
 time.sleep(1)
 
 # Integrate a velocity of the body that is constant in the world frame.
 for i in range(N):
-    Mc = se3.SE3(M.rotation,zero(3))
-    M  = M*se3.exp(Mc.actInv(nu))
+    Mc = se3.SE3(M.rotation, zero(3))
+    M = M * se3.exp(Mc.actInv(nu))
     viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
 time.sleep(1)
 
 # Integrate a constant "log" velocity in body frame.
-ME = se3.SE3( se3.Quaternion(0.7, -0.6,  0.1,  0.4).normalized().matrix(), 
-              np.matrix([1,-1,2],np.double).T )
-nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
+ME = se3.SE3(se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
+             np.matrix([1, -1, 2], np.double).T)
+nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
 for i in range(N):
-    M = M*se3.exp(nu)
+    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.
-ME = se3.SE3( se3.Quaternion(0.3, -0.2,  0.6,  0.5).normalized().matrix(), 
-              np.matrix([-1,1,0.6],np.double).T )
-nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
+ME = se3.SE3(se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
+             np.matrix([-1, 1, 0.6], np.double).T)
+nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
 for i in range(N):
-    M = M*se3.exp(nu)
+    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.
-ME = se3.SE3( se3.Quaternion(0.9, -0.1,  0.1,  0.1).normalized().matrix(), 
-              np.matrix([1,0.2,1.9],np.double).T )
+ME = se3.SE3(se3.Quaternion(0.9, -0.1, 0.1, 0.1).normalized().matrix(),
+             np.matrix([1, 0.2, 1.9], np.double).T)
 for i in range(N):
-    nu = se3.log(M.inverse()*ME).vector() * 1e-2
-    M = M*se3.exp(nu)
+    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.
-ME = se3.SE3( se3.Quaternion(0.1, -0.6,  0.6,  0.1).normalized().matrix(), 
-              np.matrix([.1,-1.2,0.3],np.double).T )
+ME = se3.SE3(se3.Quaternion(0.1, -0.6, 0.6, 0.1).normalized().matrix(),
+             np.matrix([.1, -1.2, 0.3], np.double).T)
 for i in range(N):
-    ERR = ME.inverse()*M
-    v = ERR.rotation.T*ERR.translation * -9e-3
-    w = ERR.rotation.T*se3.log(ERR.rotation) * -9e-3
-    nu = se3.Motion(v,w)
-    M = M*se3.exp(nu)
+    ERR = ME.inverse() * M
+    v = ERR.rotation.T * ERR.translation * -9e-3
+    w = ERR.rotation.T * se3.log(ERR.rotation) * -9e-3
+    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,7 +4,8 @@ abscissia), dimension 3 (x,y,z coordinates), dimension 6 (SE3 exp integration)
 and dimension 6 (R3xSO3 integration, straightline in the cartesian space).
 '''
 
-def kineinv_dim1(q0,xdes,N=100):
+
+def kineinv_dim1(q0, xdes, N=100):
     '''
     Bring the robot hand to X=xdes, using a gradient descent (which is also a
     Gauss-Newton descent in this trivial case).
@@ -13,17 +14,18 @@ def kineinv_dim1(q0,xdes,N=100):
     for i in range(N):
         Mrh = robot.Mrh(q)
 
-        e = Mrh.translation[0,0] - xdes
-        J = Mrh.rotation * robot.Jrh(q)[:3,:]
-        J = J[0,:]
+        e = Mrh.translation[0, 0] - xdes
+        J = Mrh.rotation * robot.Jrh(q)[:3, :]
+        J = J[0, :]
+
+        qdot = -J.T * e
 
-        qdot = -J.T*e
-    
-        robot.increment(q,qdot*5e-2)
+        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):
+
+def kineinv_dim3(q0, xdes, N=100):
     '''
     Bring the robot hand to [x,y,z]=xdes using Gauss-Newton descent. Arg q0 is
     the initial robot position (np.matrix Nx1); xdes is the desired position
@@ -33,16 +35,17 @@ def kineinv_dim3(q0,xdes,N=100):
     for i in range(N):
         Mrh = robot.Mrh(q)
 
-        e = Mrh.translation[0:3,0] - xdes
-        J = Mrh.rotation * robot.Jrh(q)[:3,:]
+        e = Mrh.translation[:3, 0] - xdes
+        J = Mrh.rotation * robot.Jrh(q)[:3, :]
+
+        qdot = -npl.pinv(J) * e
 
-        qdot = -npl.pinv(J)*e
-    
-        robot.increment(q,qdot*5e-2)
+        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):
+
+def kineinv_dim6(q0, Mdes, straight=True, N=100):
     '''
     Bring the robot hand to the reference placement (position+orientation) Mdes
     using Gauss-Newton descent. Arg q0 is the initial robot position (np.matrix
@@ -55,30 +58,31 @@ def kineinv_dim6(q0,Mdes,straight=True,N=100):
     q = np.copy(q0)
     for i in range(100):
         Mrh = robot.Mrh(q)
-        
+
         if straight:
-            ERR = Mdes.inverse()*Mrh
-            v = ERR.rotation.T*ERR.translation * -1
-            w = ERR.rotation.T*se3.log(ERR.rotation) * -1
-            nu = se3.Motion(v,w)
+            ERR = Mdes.inverse() * Mrh
+            v = -ERR.rotation.T * ERR.translation
+            w = -ERR.rotation.T * se3.log(ERR.rotation)
+            nu = se3.Motion(v, w)
         else:
-            nu = se3.log( Mrh.inverse()*Mdes )
+            nu = se3.log(Mrh.inverse() * Mdes)
 
         Jrh = robot.Jrh(q)
-        qdot = npl.pinv(Jrh)*nu.vector()
-        
-        robot.increment(q,qdot*5e-2)
+        qdot = npl.pinv(Jrh) * nu.vector()
+
+        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 ------------------------------------------------------
-import pinocchio as se3
-from pinocchio.utils import *
+if __name__ == '__main__':
+    import pinocchio as se3
+    from pinocchio.utils import *
 
-robot = se3.RobotWrapper('../../models/romeo.urdf')
-robot.initDisplay()
+    robot = se3.RobotWrapper('../../models/romeo.urdf')
+    robot.initDisplay()
 
-kineinv_dim1(robot.q0, 2.0)
-kineinv_dim3(robot.q0, np.matrix([2.0,0.5,1.0]).T)
-kineinv_dim6(robot.q0, se3.SE3(eye(3),np.matrix([2.0,0.5,1.0]).T),straight=False)
-kineinv_dim6(robot.q0, se3.SE3(eye(3),np.matrix([2.0,0.5,1.0]).T),straight=True)
+    kineinv_dim1(robot.q0, 2.0)
+    kineinv_dim3(robot.q0, np.matrix([2.0, 0.5, 1.0]).T)
+    kineinv_dim6(robot.q0, se3.SE3(eye(3), np.matrix([2.0, 0.5, 1.0]).T), straight=False)
+    kineinv_dim6(robot.q0, se3.SE3(eye(3), np.matrix([2.0, 0.5, 1.0]).T), straight=True)

python/bindings.py

+#!/usr/bin/env python
+
 import pinocchio as se3
-from pinocchio.utils import *
+from pinocchio.utils import isapprox, np, npl, rand, skew, zero
 
 # --- SE3
-R = rand([3,3]); [R,tmp,tmp] = npl.svd(R)
+R = rand([3, 3])
+[R, tmp, tmp] = npl.svd(R)
 p = rand(3)
-m = se3.SE3(R,p)
-X = np.vstack( [ np.hstack([ R, skew(p)*R ]), np.hstack([ zero([3,3]), R ]) ])
-assert( isapprox(m.action(),X))
-M = np.vstack( [ np.hstack([R,p]), np.matrix('0 0 0 1',np.double) ] )
-assert( isapprox(m.homogeneous(),M) )
+m = se3.SE3(R, p)
+X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
+assert isapprox(m.action, X)
+M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
+assert isapprox(m.homogeneous, M)
 m2 = se3.SE3.Random()
-assert(isapprox( (m*m2).homogeneous(),m.homogeneous()*m2.homogeneous() ))
-assert(isapprox( (~m).homogeneous(),npl.inv(m.homogeneous()) ))
+assert isapprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
+assert isapprox((~m).homogeneous, npl.inv(m.homogeneous))
 
 p = rand(3)
-assert(isapprox(m*p,m.rotation*p+m.translation))
-assert(isapprox(m.actInv(p),m.rotation.T*p-m.rotation.T*m.translation))
+assert isapprox(m * p, m.rotation * p + m.translation)
+assert isapprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
 
-p = np.vstack([p,1])
-assert(isapprox(m*p,m.homogeneous()*p))
-assert(isapprox(m.actInv(p),npl.inv(m.homogeneous())*p))
+p = np.vstack([p, 1])
+assert isapprox(m * p, m.homogeneous * p)
+assert isapprox(m.actInv(p), npl.inv(m.homogeneous) * p)
 
 p = rand(6)
-assert(isapprox(m*p,m.action()*p))
-assert(isapprox(m.actInv(p),npl.inv(m.action())*p))
+assert isapprox(m * p, m.action * p)
+assert isapprox(m.actInv(p), npl.inv(m.action) * p)
 
 # --- Motion
-assert(isapprox(se3.Motion.Zero().vector(),zero(6)))
+assert isapprox(se3.Motion.Zero().vector, zero(6))
 v = se3.Motion.Random()
-assert(isapprox((m*v).vector(),m.action()*v.vector()))
-assert(isapprox((m.actInv(v)).vector(),npl.inv(m.action())*v.vector()))
-vv = v.linear; vw = v.angular
-assert(isapprox( v.vector(),np.vstack([vv,vw]) ))
-assert(isapprox((v**v).vector(),zero(6)))
+assert isapprox((m * v).vector, m.action * v.vector)
+assert isapprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
+vv = v.linear
+vw = v.angular
+assert isapprox(v.vector, np.vstack([vv, vw]))
+assert isapprox((v ** v).vector, zero(6))
 
 # --- Force ---
-assert(isapprox(se3.Force.Zero().vector(),zero(6)))
+assert isapprox(se3.Force.Zero().vector, zero(6))
 f = se3.Force.Random()
-ff = f.linear; ft = f.angular
-assert(isapprox( f.vector(),np.vstack([ff,ft]) ))
+ff = f.linear
+ft = f.angular
+assert isapprox(f.vector, np.vstack([ff, ft]))
 
-assert(isapprox((m*f).vector(),npl.inv(m.action().T)*f.vector()))
-assert(isapprox((m.actInv(f)).vector(),m.action().T*f.vector()))
-f = se3.Force( np.vstack([ v.vector()[3:], v.vector()[:3] ]) )
-assert(isapprox( (v**f).vector(),zero(6) ))
+assert isapprox((m * f).vector, npl.inv(m.action.T) * f.vector)
+assert isapprox((m.actInv(f)).vector, m.action.T * f.vector)
+f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
+assert isapprox((v ** f).vector, zero(6))
 
 # --- Inertia ---
 Y1 = se3.Inertia.Random()
 Y2 = se3.Inertia.Random()
-Y=Y1+Y2
-assert(isapprox(Y1.matrix()+Y2.matrix(),Y.matrix()))
-assert(isapprox((Y*v).vector(),Y.matrix()*v.vector()))
-assert(isapprox( (m*Y).matrix(),m.inverse().action().T*Y.matrix()*m.inverse().action())) 
-assert(isapprox( (m.actInv(Y)).matrix(),m.action().T*Y.matrix()*m.action())) 
+Y = Y1 + Y2
+assert isapprox(Y1.matrix() + Y2.matrix(), Y.matrix())
+assert isapprox((Y * v).vector, Y.matrix() * v.vector)
+assert isapprox((m * Y).matrix(), m.inverse().action.T * Y.matrix() * m.inverse().action)
+assert isapprox((m.actInv(Y)).matrix(), m.action.T * Y.matrix() * m.action)

python/test_model.py

+#!/usr/bin/env python
+
 import pinocchio as se3
-from pinocchio.utils import *
+from pinocchio.utils import isapprox, np, zero
 
 model = se3.Model.BuildEmptyModel()
-assert(model.nbody==1 and model.nq==0 and model.nv==0)
+assert model.nbody == 1 and model.nq == 0 and model.nv == 0
 
 model = se3.Model.BuildHumanoidSimple()
-nb=28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
-assert(model.nbody==nb and model.nq==nb-1+6 and model.nv==nb-1+5)
+nb = 28  # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
+assert model.nbody == nb and model.nq == nb - 1 + 6 and model.nv == nb - 1 + 5
 model.inertias[1] = model.inertias[2]
-assert( isapprox(model.inertias[1].np,model.inertias[2].np) )
+assert isapprox(model.inertias[1].np, model.inertias[2].np)
 model.jointPlacements[1] = model.jointPlacements[2]
-assert( isapprox(model.jointPlacements[1].np,model.jointPlacements[2].np) )
-assert(model.parents[0]==0 and model.parents[1] == 0)
+assert isapprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
+assert model.parents[0] == 0 and model.parents[1] == 0
 model.parents[2] = model.parents[1]
-assert( model.parents[2] == model.parents[1] )
-assert(model.names[0] == "universe" )
-assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )
+assert model.parents[2] == model.parents[1]
+assert model.names[0] == "universe"
+assert isapprox(model.gravity.np, np.matrix('0 0 -9.81 0 0 0').T)
 
 data = model.createData()
 
@@ -23,10 +25,11 @@ data = model.createData()
 q = zero(model.nq)
 qdot = zero(model.nv)
 qddot = zero(model.nv)
-for i in range(model.nbody): data.a[i] = se3.Motion.Zero()
+for i in range(model.nbody):
+    data.a[i] = se3.Motion.Zero()
 
-se3.rnea(model,data,q,qdot,qddot)
+se3.rnea(model, data, q, qdot, qddot)
 for i in range(model.nbody):
-    assert( isapprox(data.v[i].np,zero(6)) )
-assert( isapprox(data.a[0].np,-model.gravity.np) )
-assert( isapprox(data.f[-1],model.inertias[-1]*data.a[-1]) )
+    assert isapprox(data.v[i].np, zero(6))
+assert isapprox(data.a_gf[0].np, -model.gravity.np)
+assert isapprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])

src/python/__init__.py

@@ -15,61 +15,63 @@
 # <http://www.gnu.org/licenses/>.
 
 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 *
 
 se3.SE3.__repr__ = se3.SE3.__str__
 se3.Motion.__repr__ = se3.Motion.__str__
-se3.AngleAxis.__repr__ = lambda s: 'AngleAxis('+s.vector().__str__()+')'
+se3.AngleAxis.__repr__ = lambda s: 'AngleAxis(%s)' % s.vector()
+
 
 # --- SE3 action ---
-def SE3act(m,x):
-    assert(isinstance(m,se3.SE3))
-    if isinstance(x,np.ndarray):
-        if x.shape[0]==3:
-            return m.rotation*x+m.translation
-        elif x.shape[0] == 4:
-            return m.homogeneous()*x
-        elif x.shape[0] == 6:
-            return m.action()*x
-        else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
-    elif 'se3Action' in x.__class__.__dict__:
+def SE3act(m, x):
+    assert isinstance(m, se3.SE3)
+    if isinstance(x, np.ndarray):
+        if x.shape[0] == 3:
+            return m.rotation * x + m.translation
+        if x.shape[0] == 4:
+            return m.homogeneous * x
+        if x.shape[0] == 6:
+            return m.action * x
+        raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
+    if 'se3Action' in x.__class__.__dict__:
         return x.se3Action(m)
-    else:
-        #print 'Error: SE3 cannot act on the given object'
-        return m.oldmult(x)
-setattr(se3.SE3,'oldmult',se3.SE3.__mul__)
-setattr(se3.SE3,'__mul__',SE3act)
-setattr(se3.SE3,'act',SE3act)
+    return m.oldmult(x)
+
+setattr(se3.SE3, 'oldmult', se3.SE3.__mul__)
+setattr(se3.SE3, '__mul__', SE3act)
+setattr(se3.SE3, 'act', SE3act)
+
 
-def SE3actinv(m,x):
-    assert(isinstance(m,se3.SE3))
-    if isinstance(x,np.ndarray):
-        if x.shape[0]==3:
-            return m.rotation.T*x-m.rotation.T*m.translation
-        elif x.shape[0] == 4:
-            return m.inverse().homogeneous()*x
-        elif x.shape[0] == 6:
-            return m.inverse().action()*x
-        else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
-    elif 'se3Action' in x.__class__.__dict__:
+def SE3actinv(m, x):
+    assert isinstance(m, se3.SE3)
+    if isinstance(x, np.ndarray):
+        if x.shape[0] == 3:
+            return m.rotation.T * x - m.rotation.T * m.translation
+        if x.shape[0] == 4:
+            return m.inverse().homogeneous * x
+        if x.shape[0] == 6:
+            return m.inverse().action * x
+        raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
+    if 'se3Action' in x.__class__.__dict__:
         return x.se3ActionInverse(m)
-    else:
-        print 'Error: SE3 cannot act on the given object'
-setattr(se3.SE3,'actInv',SE3actinv)
+    raise ValueError('Error: SE3 cannot act on the given object')
+
+setattr(se3.SE3, 'actInv', SE3actinv)
+
 
 # --- M6/F6 cross product --
-def SE3cross(self,y):
-    assert(isinstance(self,se3.Motion))
-    if isinstance(y,se3.Motion):
+def SE3cross(self, y):
+    assert isinstance(self, se3.Motion)
+    if isinstance(y, se3.Motion):
         return self.cross_motion(y)
-    elif isinstance(y,se3.Force):
+    if isinstance(y, se3.Force):
         return self.cross_force(y)
-    else: raise Exception('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
-setattr(se3.Motion,'__pow__',SE3cross)
-setattr(se3.Motion,'cross',SE3cross)
-
-from libpinocchio_pywrap import *
-from pinocchio.robot_wrapper import RobotWrapper
-from explog import exp,log
+    raise ValueException('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
 
+setattr(se3.Motion, '__pow__', SE3cross)
+setattr(se3.Motion, 'cross', SE3cross)

src/python/explog.py

@@ -19,23 +19,32 @@ import libpinocchio_pywrap as se3
 import numpy as np
 import math
 
+
 def exp(x):
-    if isinstance(x,se3.Motion): return se3.exp6FromMotion(x)
-    elif np.isscalar(x): return math.exp(x)
-    elif isinstance(x,np.matrix):
-        if len(x)==6: return se3.exp6FromVector(x)
-        elif len(x)==3: return se3.exp3(x)
-        else: print 'Error only 3 and 6 vectors are allowed.'
-    else: print 'Error exp is only defined for real, vector3, vector6 and se3.Motion objects.'
+    if isinstance(x, se3.Motion):
+        return se3.exp6FromMotion(x)
+    if np.isscalar(x):
+        return math.exp(x)
+    if isinstance(x, np.matrix):
+        if len(x) == 6:
+            return se3.exp6FromVector(x)
+        if len(x) == 3:
+            return se3.exp3(x)
+        raise ValueError('Error only 3 and 6 vectors are allowed.')
+    raise ValueError('Error exp is only defined for real, vector3, vector6 and se3.Motion objects.')
 
-def log(x):
-    if isinstance(x,se3.SE3): return se3.log6FromSE3(x)
-    elif np.isscalar(x): return math.log(x)
-    elif isinstance(x,np.matrix):
-        if len(x)==4: return se3.log6FromMatrix(x)
-        elif len(x)==3: return se3.log3(x)
-        else: print 'Error only 3 and 4 matrices are allowed.'
-    else: print 'Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.'
 
+def log(x):
+    if isinstance(x, se3.SE3):
+        return se3.log6FromSE3(x)
+    if np.isscalar(x):
+        return math.log(x)
+    if isinstance(x, np.matrix):
+        if len(x) == 4:
+            return se3.log6FromMatrix(x)
+        if len(x) == 3:
+            return se3.log3(x)
+        raise ValueError('Error only 3 and 4 matrices are allowed.')
+    raise ValueError('Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.')
 
-__all__ = [ 'exp','log' ]
+__all__ = ['exp', 'log']

src/python/robot_wrapper.py

@@ -14,43 +14,44 @@
 # Pinocchio If not, see
 # <http://www.gnu.org/licenses/>.
 
-import numpy as np
+import time
+
 import libpinocchio_pywrap as se3
 import utils
 from explog import exp
-import time
 
-class RobotWrapper:
 
-    def __init__(self, filename, package_dirs = None, root_joint = None):
-        if(root_joint is None):