[Python] Update robot wrapper to handle acceleration and com algo

src/python/robot_wrapper.py

@@ -18,6 +18,7 @@ import numpy as np
 import libpinocchio_pywrap as se3
 import utils
 from explog import exp
+import time
 
 class RobotWrapper:
     def __init__(self,filename):
@@ -47,8 +48,8 @@ class RobotWrapper:
             q = args[0]
             v = args[1]
             a = args[2]
-            se3.centerOfMass(self.model,self.data,q,v,a)
-            return se3.data.com[0], se3.data.vcom[0], se3.data.acom[0]
+            se3.centerOfMassAcceleration(self.model,self.data,q,v,a)
+            return self.data.com_pos(0), self.data.com_vel(0), self.data.com_acc(0)
         return se3.centerOfMass(self.model,self.data,args[0])
 
     def Jcom(self,q):
@@ -78,6 +79,10 @@ class RobotWrapper:
             se3.kinematics(self.model,self.data,q,v)
 
         return self.data.v[index]
+    def acceleration(self,q,v,a,index, update_acceleration = True):
+        if update_acceleration:
+          se3.dynamics(self.model,self.data,q,v,a)
+        return self.data.a[index]
     def jacobian(self,q,index, update_geometry = True):
         return se3.jacobian(self.model,self.data,q,index,True,update_geometry)
     def computeJacobians(self,q):
@@ -153,4 +158,15 @@ class RobotWrapper:
                 self.viewer.gui.applyConfiguration(self.viewerFixedNodeNames(i),viewerConf)
         self.viewer.gui.refresh()
 
+    def play(self,q_trajectory,dt):
+        _,N = q_trajectory.shape
+
+        for k in range(N):
+            t0 = time.time()
+            self.display(q_trajectory[:,k])
+            t1 = time.time()
+            elapsed_time = t1-t0
+            if elapsed_time < dt:
+              time.sleep(dt - elapsed_time)
+
 __all__ = [ 'RobotWrapper' ]