diff --git a/CMakeLists.txt b/CMakeLists.txt
index cc144480fb6f115dce7e086e80d078fe16f1edbc..8a4521b7bd2294cd2af16ce6a7966de8cc8857b2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -131,6 +131,8 @@ SET(PYTHON_FILES
python/__init__.py
python/utils.py
python/robot_wrapper.py
+ python/rpy.py
+ python/explog.py
)
FOREACH(python ${PYTHON_FILES})
GET_FILENAME_COMPONENT(pythonFile ${python} NAME)
diff --git a/src/python/explog.py b/src/python/explog.py
new file mode 100644
index 0000000000000000000000000000000000000000..9f98f1bda693cea1d021a8730f5f1c3371d4fd23
--- /dev/null
+++ b/src/python/explog.py
@@ -0,0 +1,72 @@
+import libpinocchio_pywrap as se3
+import numpy as np
+import numpy.linalg as npl
+from math import sin,cos
+import math
+from utils import skew,cross,eye
+
+def exp3(v):
+ '''
+ Exp: so3 -> SO3. Return the integral of the input angular velocity during time 1.
+ '''
+ nv = npl.norm(v)
+ return se3.AngleAxis(nv,v/nv).matrix()
+
+def log3(R):
+ '''
+ Log: SO3 -> so3. Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
+ '''
+ return se3.AngleAxis(R).vector()
+
+def exp6( nu ):
+ '''
+ Exp: se3 -> SE3. Return the integral of the input spatial velocity during time 1.
+ '''
+ if isinstance(nu,se3.Motion): w = nu.angular; v = nu.linear
+ else: w = nu[3:]; v = nu[:3]
+ if npl.norm(w)>1e-15:
+ R = exp3(w)
+ t = npl.norm(w)
+ S = skew(w)
+ V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
+ p = V*v
+ return se3.SE3(R,p)
+ else:
+ return se3.SE3(eye(3),v)
+
+def log6( m ):
+ '''
+ Log: SE3 -> se3. Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
+ '''
+ if isinstance(m,se3.SE3): R = m.rotation; p = m.translation
+ else: R = m[:3,:3]; p = m[:3,3]
+ w = log3(R)
+ if npl.norm(w)>1e-15:
+ t = npl.norm(w)
+ S = skew(w)
+ V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
+ v = npl.inv(V)*p
+ else:
+ v = p
+ return se3.Motion(v,w)
+
+def exp(x):
+ if isinstance(x,se3.Motion): return exp6(x)
+ elif np.isscalar(x): return math.exp(x)
+ elif isinstance(x,np.matrix):
+ if len(x)==6: return exp6(x)
+ elif len(x)==3: return 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.'
+
+def log(x):
+ if isinstance(x,se3.SE3): return log6(x)
+ elif np.isscalar(x): return math.log(x)
+ elif isinstance(x,np.matrix):
+ if len(x)==6: return log6(x)
+ elif len(x)==3: return log3(x)
+ else: print 'Error only 3 and 6 vectors are allowed.'
+ else: print 'Error log is only defined for real, vector3, vector6 and se3.SE3 objects.'
+
+
+__all__ = [ 'exp3', 'log3', 'exp6', 'log6','exp','log' ]
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
new file mode 100644
index 0000000000000000000000000000000000000000..aa544b803bfbc6353f1b5abe1e04164b88d420ce
--- /dev/null
+++ b/src/python/robot_wrapper.py
@@ -0,0 +1,115 @@
+import numpy as np
+import libpinocchio_pywrap as se3
+import utils
+from exp import exp
+
+class RobotWrapper:
+ def __init__(self,filename):
+ self.model = se3.buildModelFromUrdf(filename,True)
+ self.data = self.model.createData()
+ self.v0 = utils.zero(self.nv)
+ self.q0 = np.matrix( [
+ 0, 0, 0.840252, 0, 0, 0, 1, # Free flyer
+ 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, # left leg
+ 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, # right leg
+ 0, # chest
+ 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # left arm
+ 0, 0, 0, 0, # head
+ 1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # right arm
+ ] ).T
+ self.opCorrespondances = { "lh": "LWristPitch",
+ "rh": "RWristPitch",
+ "rf": "RAnkleRoll",
+ "lf": "LAnkleRoll",
+ }
+ for op,name in self.opCorrespondances.items():
+ self.__dict__[op] = self.index(name)
+
+ def increment(self,q,dq):
+ M = se3.SE3( se3.Quaternion(q[6,0],q[3,0],q[4,0],q[5,0]).matrix(), q[:3])
+ dM = exp(dq[:6])
+ M = M*dM
+ q[:3] = M.translation
+ q[3:7] = se3.Quaternion(M.rotation).coeffs()
+ q[7:] += dq[6:]
+
+ def index(self,name):
+ return [ i for i,n in enumerate(self.model.names) if n == name ][0]
+
+ @property
+ def nq(self):
+ return self.model.nq
+ @property
+ def nv(self):
+ return self.model.nv
+
+ def com(self,q):
+ return se3.centerOfMass(self.model,self.data,q)
+ def Jcom(self,q):
+ return se3.centerOfMass(self.model,self.data,q)
+
+ def mass(self,q):
+ return se3.crba(self.model,self.data,q)
+ def biais(self,q,v):
+ return se3.rnea(self.model,self.data,q,v,self.v0)
+ def gravity(self,q):
+ return se3.rnea(self.model,self.data,q,self.v0,self.v0)
+
+ def position(self,q,index):
+ se3.kinematics(self.model,self.data,q,self.v0)
+ return self.data.oMi[index]
+ def velocity(self,q,v,index):
+ se3.kinematics(self.model,self.data,q,v)
+ return self.data.v[index]
+ def jacobian(self,q,index):
+ return se3.jacobian(self.model,self.data,index,q,True)
+
+
+ # --- SHORTCUTS ---
+ def Mrh(self,q):
+ return self.position(q,self.rh)
+ def Jrh(self,q):
+ return self.jacobian(q,self.rh)
+ def wJrh(self,q):
+ return se3.jacobian(self.model,self.data,self.rh,q,False)
+ def vrh(self,q,v):
+ return self.velocity(q,v,self.rh)
+
+ def Jlh(self,q):
+ return self.jacobian(q,self.lh)
+ def Mlh(self,q):
+ return self.position(q,self.lh)
+
+ def Jlf(self,q):
+ return self.jacobian(q,self.lf)
+ def Mlf(self,q):
+ return self.position(q,self.lf)
+ def Jrf(self,q):
+ return self.jacobian(q,self.rf)
+ def Mrf(self,q):
+ return self.position(q,self.rf)
+
+
+ # --- VIEWER ---
+ def initDisplay(self):
+ import robotviewer
+ try:
+ self.viewer=robotviewer.client('XML-RPC')
+ self.viewer.updateElementConfig('TEST',[0,]*6)
+ except:
+ if 'viewer' in self.__dict__: del self.viewer
+ print "Error while starting the viewer client. "
+ print "Check wheter RobotViewer is properly started (as XML-RPC server)"
+
+ def display(self,q):
+ if 'viewer' not in self.__dict__: return
+ # Update the robot geometry.
+ se3.kinematics(self.model,self.data,q,self.v0)
+ # Iteratively place the robot bodies.
+ for i in range(1,self.model.nbody):
+ xyz = self.data.oMi[i].translation
+ rpy = utils.matrixToRpy(self.data.oMi[i].rotation)
+ self.viewer.updateElementConfig('Romeo'+self.model.names[i],
+ [float(xyz[0,0]), float(xyz[1,0]), float(xyz[2,0]),
+ float(rpy[0,0]), float(rpy[1,0]), float(rpy[2,0]) ])
+