[Python] Update the binding features

src/python/algorithms.hpp

@@ -78,23 +78,32 @@ namespace se3
 
       static Eigen::MatrixXd jacobian_proxy( const ModelHandler& model, 
                DataHandler & data,
-               Model::Index jointId,
                const VectorXd_fx & q,
-               bool local )
+               Model::Index jointId,
+               bool local,
+							 bool update_geometry )
       {
   Eigen::MatrixXd J( 6,model->nv ); J.setZero();
-  computeJacobians( *model,*data,q );
+	if (update_geometry)
+  	computeJacobians( *model,*data,q );
   if(local) getJacobian<true> (*model, *data, jointId, J);
   else getJacobian<false> (*model, *data, jointId, J);
   return J;
       }
+      
+      static void compute_jacobians_proxy(const ModelHandler& model, 
+               DataHandler & data,
+               const VectorXd_fx & q)
+      {
+        computeJacobians( *model,*data,q );
+      }
 
       static void kinematics_proxy( const ModelHandler& model, 
             DataHandler & data,
             const VectorXd_fx & q,
             const VectorXd_fx & qdot )
       {
-  kinematics( *model,*data,q,qdot );
+        kinematics( *model,*data,q,qdot );
       }
 
       static void geometry_proxy(const ModelHandler & model,
@@ -176,12 +185,18 @@ namespace se3
     bp::args("Model","Data",
        "Configuration q (size Model::nq)",
        "Joint ID (int)",
-       "frame (true = local, false = world)"),
+       "frame (true = local, false = world)",
+       "update_geometry (true = update the value of the total jacobian)"),
     "Calling computeJacobians then getJacobian, return the result. Attention: the "
     "function computes indeed all the jacobians of the model, even if just outputing "
-    "the demanded one. It is therefore outrageously costly wrt a dedicated "
+    "the demanded one if update_geometry is set to false. It is therefore outrageously costly wrt a dedicated "
     "call. Function to be used only for prototyping.");
 
+  bp::def("computeJacobians",compute_jacobians_proxy,
+    bp::args("Model","Data",
+       "Configuration q (size Model::nq)"),
+    "Calling computeJacobians");
+
   bp::def("jointLimits",jointLimits_proxy,
     bp::args("Model","Data",
         "Configuration q (size Model::nq)"),

src/python/robot_wrapper.py

@@ -50,18 +50,29 @@ class RobotWrapper:
     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)
+        return se3.nle(self.model,self.data,q,v)
     def gravity(self,q):
         return se3.rnea(self.model,self.data,q,self.v0,self.v0)
+    
+    def geometry(self,q):
+        se3.geometry(self.model, self.data, q)
+    def kinematics(self,q,v):
+        se3.kinematics(self.model, self.data, q, v)
+
+    def position(self,q,index, update_geometry = True):
+        if update_geometry:
+            se3.geometry(self.model,self.data,q)
 
-    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)
+    def velocity(self,q,v,index, update_kinematics = True):
+        if update_kinematics:
+            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)
+    def jacobian(self,q,index, update_geometry = True):
+        return se3.jacobian(self.model,self.data,q,index,True,update_geometry)
+    def computeJacobians(self,q):
+        return se3.computeJacobians(self.model,self.data,q)
 
     # --- ACCESS TO NAMES ----
     # Return the index of the joint whose name is given in argument.
@@ -92,7 +103,7 @@ class RobotWrapper:
             print "Check wheter gepetto-viewer is properly started"
         
 
-    # Create the scene displaying the robot meshes in Gepetto-viewer.
+    # Create the scene displaying the robot meshes in gepetto-viewer
     def loadDisplayModel(self, nodeName, windowName = "pinocchio", meshDir = None):
         import os
         try:
@@ -112,7 +123,7 @@ class RobotWrapper:
     def display(self,q): 
         if 'viewer' not in self.__dict__: return
         # Update the robot geometry.
-        se3.kinematics(self.model,self.data,q,self.v0)
+        se3.geometry(self.model,self.data,q)
         # Iteratively place the moving robot bodies.
         for i in range(1,self.model.nbody):
             if self.model.hasVisual[i]:

src/python/utils.py

@@ -37,7 +37,7 @@ def se3ToXYZQUAT(M):
                 float(quat[0,0]), float(quat[1,0]), float(quat[2,0]), float(quat[3,0]) ]
     return config
 
-# Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a homogeneous matrix.
+# Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a SE3 element
 def XYZQUATToSe3(xyzq):
     if isinstance(xyzq,tuple) or isinstance(xyzq,list):
         xyzq = np.matrix(xyzq,np.float).T
@@ -45,7 +45,7 @@ def XYZQUATToSe3(xyzq):
         se3.Quaternion( xyzq[6,0],xyzq[3,0],xyzq[4,0],xyzq[5,0]).matrix(),
         xyzq[:3] )
 
-# Convert the input 7D vector [X,Y,Z,b,c,d,a] to 7D vector [X,Y,Y,a,b,c,d]
+# Convert the input 7D vector [X,Y,Z,b,c,d,a] to 7D vector [X,Y,Z,a,b,c,d]
 def XYZQUATToViewerConfiguration(xyzq):
     if isinstance(xyzq,tuple) or isinstance(xyzq,list):
         xyzq = np.matrix(xyzq,np.float).T