python: IVIGIT explog, rpy.

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)

src/python/explog.py

+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' ]

src/python/robot_wrapper.py

+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]) ])
+