Merge pull request #463 from proyan/topic/robotwrapper

[Python] Minor. Remove duplicate methods. minor rearrangement

bindings/python/scripts/robot_wrapper.py

@@ -110,6 +110,11 @@ class RobotWrapper(object):
             se3.forwardKinematics(self.model, self.data, q, v)
         return self.data.v[index]
 
+    def acceleration(self, q, v, a, index, update_kinematics=True):
+        if update_kinematics:
+            se3.forwardKinematics(self.model, self.data, q, v, a)
+        return self.data.a[index]
+
     def framePosition(self, q, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q)
@@ -124,11 +129,6 @@ class RobotWrapper(object):
         parentJointVel = self.data.v[frame.parent]
         return frame.placement.actInv(parentJointVel)
 
-    def acceleration(self, q, v, a, index, update_kinematics=True):
-        if update_kinematics:
-            se3.forwardKinematics(self.model, self.data, q, v, a)
-        return self.data.a[index]
-
     def frameAcceleration(self, q, v, a, index, update_kinematics=True):
         if update_kinematics:
             se3.forwardKinematics(self.model, self.data, q, v, a)
@@ -136,6 +136,15 @@ class RobotWrapper(object):
         parentJointAcc = self.data.a[frame.parent]
         return frame.placement.actInv(parentJointAcc)
 
+    def frameClassicAcceleration(self, q, v, a, index, update_kinematics=True):
+        if update_kinematics:
+            se3.forwardKinematics(self.model, self.data, q, v, a)      
+        f = self.model.frames[index]
+        af = f.placement.actInv(self.data.a[f.parent])
+        vf = f.placement.actInv(self.data.v[f.parent])
+        af.linear += np.cross(vf.angular.T, vf.linear.T).T
+        return af;
+
     def jacobian(self, q, index, update_kinematics=True, local_frame=True):
         return se3.jacobian(self.model, self.data, q, index, local_frame, update_kinematics)
 
@@ -158,27 +167,7 @@ class RobotWrapper(object):
 
     def framesKinematics(self, q): 
         se3.framesKinematics(self.model, self.data, q)
-    
-    def framePosition(self, index):
-        f = self.model.frames[index]
-        return self.data.oMi[f.parent].act(f.placement)
-
-    def frameVelocity(self, index):
-        f = self.model.frames[index]
-        return f.placement.actInv(self.data.v[f.parent])
-    
-    ''' Return the spatial acceleration of the specified frame. '''
-    def frameAcceleration(self, index):
-        f = self.model.frames[index]
-        return f.placement.actInv(self.data.a[f.parent])
-    
-    def frameClassicAcceleration(self, index):
-        f = self.model.frames[index]
-        a = f.placement.actInv(self.data.a[f.parent])
-        v = f.placement.actInv(self.data.v[f.parent])
-        a.linear += np.cross(v.angular.T, v.linear.T).T
-        return a;
-
+   
     ''' Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first.
     Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true, 
     the function computes all the jacobians of the model. It is therefore outrageously costly wrt a