Merge pull request #211 from pariterre/master

Allowed MX/SX as functions in to_casadi

Author: pariterre

binding/c/biorbd_c.cpp

@@ -104,6 +104,16 @@ void c_massMatrix(
     for (unsigned int i=0; i<nQ*nQ; ++i)
         massMatrix[i] = Mass(i);
 }
+void c_CoM(
+        biorbd::Model* model,
+        const double* q,
+        double *com) {
+    biorbd::rigidbody::GeneralizedCoordinates Q(dispatchQinput(model, q));
+
+    biorbd::utils::Vector3d CoM(model->CoM(Q));
+
+    dispatchVectorOutput(CoM, com);
+}
 
 
 // dof functions

binding/c/biorbd_c.h

@@ -51,6 +51,10 @@ extern "C" {
             biorbd::Model* model,
             const double* q,
             double* massMatrix);
+    BIORBD_API_C void c_CoM(
+            biorbd::Model* model,
+            const double* q,
+            double *com);
 
 
     // dof functions

binding/matlab/Matlab_MusclesPoints.h

@@ -7,7 +7,7 @@
 #include "class_handle.h"
 #include "processArguments.h"
 #include "Utils/Vector3d.h"
-#include "Muscles/WrappingCylinder.h"
+#include "Muscles/WrappingHalfCylinder.h"
 #include "Muscles/MuscleGroup.h"
 #include "Muscles/Muscle.h"
 #include "Muscles/PathModifiers.h"
@@ -56,8 +56,8 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[],
                     wrap_RT.push_back(wrappingObject.RT());
 
                     // Quel est sa dimension
-                    if (type == biorbd::utils::NODE_TYPE::WRAPPING_CYLINDER){
-                        const biorbd::muscles::WrappingCylinder& cylinder(dynamic_cast<const biorbd::muscles::WrappingCylinder&>(wrappingObject));
+                    if (type == biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER){
+                        const biorbd::muscles::WrappingHalfCylinder& cylinder(dynamic_cast<const biorbd::muscles::WrappingHalfCylinder&>(wrappingObject));
                         wrap_dim1.push_back(cylinder.radius());
                         wrap_dim2.push_back(cylinder.length());
                     }

binding/python3/__init__.py

@@ -4,17 +4,21 @@
 from .rigid_body import *
 
 if biorbd.currentLinearAlgebraBackend() == 1:
-    from casadi import Function, MX, horzcat
+    from casadi import Function, MX, SX, horzcat
 
     def to_casadi_func(name, func, *all_param):
-        mx_param = []
+        cx_param = []
         for p in all_param:
-            if isinstance(p, MX):
-                mx_param.append(p)
+            if isinstance(p, (MX, SX)):
+                cx_param.append(p)
+
+        if isinstance(func, (MX, SX, Function)):
+            func_evaluated = func
+        else:
+            func_evaluated = func(*all_param)
+            if isinstance(func_evaluated, (list, tuple)):
+                func_evaluated = horzcat(*[val if isinstance(val, MX) else val.to_mx() for val in func_evaluated])
+            elif not isinstance(func_evaluated, MX):
+                func_evaluated = func_evaluated.to_mx()
+        return Function(name, cx_param, [func_evaluated]).expand()
 
-        func_evaluated = func(*all_param)
-        if isinstance(func_evaluated, (list, tuple)):
-            func_evaluated = horzcat(*[val if isinstance(val, MX) else val.to_mx() for val in func_evaluated])
-        elif not isinstance(func_evaluated, MX):
-            func_evaluated = func_evaluated.to_mx()
-        return Function(name, mx_param, [func_evaluated]).expand()