Merge pull request #204 from fbailly/master

Swig vector3d
pyomeca · Mar 29, 2021 · 9dc0589 · 9dc0589
2 parents aa5da40 + fdbcb12
commit 9dc0589
Show file tree

Hide file tree

Showing 8 changed files with 118 additions and 43 deletions.
diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in
@@ -10,6 +10,7 @@
 
 #include "BiorbdModel.h"
 #include "Utils/Matrix.h"
+#include "Utils/Vector3d.h"
 #include "RigidBody/GeneralizedCoordinates.h"
 #include "RigidBody/GeneralizedVelocity.h"
 #include "RigidBody/GeneralizedAcceleration.h"
@@ -810,15 +811,24 @@ biorbd::rigidbody::GeneralizedAcceleration * {
 }
 #endif
 
-// --- Node --- //
-#ifndef BIORBD_USE_CASADI_MATH
-%typemap(typecheck, precedence=2130) biorbd::utils::Vector3d &{
+// --- Vector3d --- //
+%typemap(typecheck, precedence=2130) 
+biorbd::utils::Vector3d &{
     void *argp1 = 0;
     if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Node,  0  | 0)) && argp1) {
         // Test if it is a pointer to SWIGTYPE_p_biorbd__utils__Node already exists
         $1 = true;
-    } else if( PyArray_Check($input) ) {
+     }
+#ifdef BIORBD_USE_CASADI_MATH
+    else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX,  0  | 0)) && argp1) {
+        // Test if it is a pointer to
+        // SWIGTYPE_p_biorbd__utils__Vector3d already exists
+        $1 = true;
+    }
+#endif
+    else if( PyArray_Check($input) ) {
         // test if it is a numpy array
+
         $1 = true;
     } else {
         $1 = false;
@@ -829,7 +839,13 @@ biorbd::rigidbody::GeneralizedAcceleration * {
     if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Node,  0  | 0)) && argp1) {
         // Recast the pointer
         $1 = reinterpret_cast< biorbd::utils::Vector3d * >(argp1);
-    } else if( PyArray_Check($input) ) {
+     }
+#ifdef BIORBD_USE_CASADI_MATH
+    else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX,  0  | 0)) && argp1) {
+        $1 = new biorbd::utils::Vector3d(biorbd::utils::Vector(*reinterpret_cast<casadi::MX*>(argp1)));
+    }
+#endif
+    else if( PyArray_Check($input) ) {
         // Get dimensions of the data
         int        ndim     = PyArray_NDIM    ((PyArrayObject*)$input);
         npy_intp*  dims     = PyArray_DIMS    ((PyArrayObject*)$input);
@@ -852,7 +868,7 @@ biorbd::rigidbody::GeneralizedAcceleration * {
         SWIG_fail;
     }
 };
-#endif
+
 %extend biorbd::utils::Vector3d{
 #ifdef BIORBD_USE_CASADI_MATH
     casadi::MX to_mx(){

diff --git a/examples/forwardDynamicsFromMusclesExample.cpp b/examples/forwardDynamicsFromMusclesExample.cpp
@@ -25,9 +25,10 @@ int main()
     Qdot.setZero();
 
     // Set all muscles to half of their maximal activation
-    std::vector<std::shared_ptr<biorbd::muscles::State>> states;
+    std::vector<std::shared_ptr<biorbd::muscles::State>> states = model.stateSet();
     for (unsigned int i=0; i<model.nbMuscles(); ++i){
-        states.push_back(std::make_shared<biorbd::muscles::State>(0, 0.5));
+        states[i]->setExcitation(0);
+        states[i]->setActivation(0.5);
     }
 
     // Proceed with the computation of joint torque from the muscles

diff --git a/include/Utils/Vector3d.h b/include/Utils/Vector3d.h
@@ -118,7 +118,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio
     ///
     Vector3d(
             const RBDLCasadiMath::MX_Xd_SubMatrix& other);
-
 #endif
 
     ///

diff --git a/src/Muscles/Geometry.cpp b/src/Muscles/Geometry.cpp
@@ -6,6 +6,7 @@
 #include "Utils/Error.h"
 #include "Utils/Matrix.h"
 #include "Utils/RotoTrans.h"
+#include "RigidBody/NodeSegment.h"
 #include "RigidBody/Joints.h"
 #include "RigidBody/GeneralizedCoordinates.h"
 #include "RigidBody/GeneralizedVelocity.h"
@@ -191,7 +192,12 @@ void biorbd::muscles::Geometry::updateKinematics(
 void biorbd::muscles::Geometry::setOrigin(
         const utils::Vector3d &position)
 {
-    *m_origin = position;
+    if (dynamic_cast<const biorbd::rigidbody::NodeSegment*>(&position)){
+        *m_origin = position;
+    } else {
+        // Preserve the Node information
+        m_origin->RigidBodyDynamics::Math::Vector3d::operator=(position);
+    }
 }
 const biorbd::utils::Vector3d& biorbd::muscles::Geometry::originInLocal() const
 {
@@ -201,7 +207,12 @@ const biorbd::utils::Vector3d& biorbd::muscles::Geometry::originInLocal() const
 void biorbd::muscles::Geometry::setInsertionInLocal(
         const utils::Vector3d &position)
 {
-    *m_insertion = position;
+    if (dynamic_cast<const biorbd::rigidbody::NodeSegment*>(&position)){
+        *m_insertion = position;
+    } else {
+        // Preserve the Node information
+        m_insertion->RigidBodyDynamics::Math::Vector3d::operator=(position);
+    }
 }
 const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInLocal() const
 {

diff --git a/src/Utils/RotoTrans.cpp b/src/Utils/RotoTrans.cpp
@@ -77,7 +77,7 @@ biorbd::utils::Vector3d biorbd::utils::RotoTrans::axe(unsigned int idx) const
 {
     biorbd::utils::Error::check(
                 idx<=2, "Axis must be between 0 and 2 included");
-    return static_cast<RigidBodyDynamics::Math::VectorNd>(rot().block(0,idx,3,1));
+    return rot().block(0,idx,3,1);
 }
 
 biorbd::utils::RotoTrans biorbd::utils::RotoTrans::transpose() const

diff --git a/src/Utils/Vector3d.cpp b/src/Utils/Vector3d.cpp
@@ -50,7 +50,6 @@ biorbd::utils::Vector3d::Vector3d(
     setType();
 }
 
-
 biorbd::utils::Vector3d::Vector3d(
         const RigidBodyDynamics::Math::VectorNd &other) :
     RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
@@ -61,7 +60,8 @@ biorbd::utils::Vector3d::Vector3d(
 
 biorbd::utils::Vector3d::Vector3d(
         const RigidBodyDynamics::Math::Vector4d &other) :
-    RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]), biorbd::utils::Node ()
+    RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
+    biorbd::utils::Node ()
 {
     setType();
 }
@@ -97,7 +97,7 @@ biorbd::utils::Vector3d biorbd::utils::Vector3d::applyRT(const biorbd::utils::Ro
     RigidBodyDynamics::Math::Vector4d v;
     v.block(0, 0, 3, 1) = *this;
     v[3] = 1;
-    return static_cast<RigidBodyDynamics::Math::VectorNd>((rt * v).block(0, 0, 3, 1));
+    return (rt * v).block(0, 0, 3, 1);
 }
 
 void biorbd::utils::Vector3d::applyRT(

diff --git a/test/binding/Python3/test_conversion.py b/test/binding/Python3/test_conversion.py
@@ -28,22 +28,42 @@ def test_np_mx_to_generalized():
 
 
 # --- Options --- #
-def imu_to_array():
+def test_imu_to_array():
     m = biorbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod")
-    q = np.zeros( (m.nbQ(), ) )
+    q = np.zeros((m.nbQ(),))
 
     if biorbd.currentLinearAlgebraBackend() == 1:
         from casadi import MX
+
         q_sym = MX.sym("q", m.nbQ(), 1)
         imu_func = biorbd.to_casadi_func("imu", m.IMU, q_sym)
         imu = imu_func(q)[:, :4]
 
     else:
         imu = m.IMU(q)[0].to_array()
 
-    np.testing.assert_almost_equal(imu, np.array([
-        [0.99003329, -0.09933467, 0.09983342, 0.26719],
-        [0.10925158, 0.98903828, -0.09933467, 0.04783],
-        [-0.08887169, 0.10925158, 0.99003329, -0.20946],
-        [0., 0., 0., 1.]]))
+    np.testing.assert_almost_equal(
+        imu,
+        np.array(
+            [
+                [0.99003329, -0.09933467, 0.09983342, 0.26719],
+                [0.10925158, 0.98903828, -0.09933467, 0.04783],
+                [-0.08887169, 0.10925158, 0.99003329, -0.20946],
+                [0.0, 0.0, 0.0, 1.0],
+            ]
+        ),
+    )
+
+
+def test_vector3d():
+    biorbd_model = biorbd.Model()
+    vec = np.random.rand(
+        3,
+    )
+    biorbd_model.setGravity(vec)
+
+    if biorbd.currentLinearAlgebraBackend() == 1:
+        from casadi import MX
 
+        vec = MX.ones(3, 1)
+        biorbd_model.setGravity(vec)
diff --git a/test/test_muscles.cpp b/test/test_muscles.cpp
@@ -9,6 +9,7 @@
 #include "RigidBody/GeneralizedVelocity.h"
 #include "RigidBody/GeneralizedAcceleration.h"
 #include "RigidBody/GeneralizedTorque.h"
+#include "RigidBody/NodeSegment.h"
 #ifdef MODULE_MUSCLES
 #include "Muscles/all.h"
 #include "Utils/String.h"
@@ -275,29 +276,56 @@ TEST(IdealizedActuator, copy)
             EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
         }
 
-        // Change the position of the insertion and pennation angle and compare again (length and insertion in Local)
-        biorbd::muscles::Characteristics charac(idealizedActuator.characteristics());
-        charac.setPennationAngle(0.523599);
-        biorbd::utils::Vector3d insertion(idealizedActuator.position().insertionInLocal());
-        insertion.set(0.2, 0.2, 0.2);
-        biorbd::utils::String oldName(insertion.biorbd::utils::Node::name());
-        biorbd::utils::String newName("MyNewName");
-        insertion.setName(newName);
-        idealizedActuator.updateOrientations(model, Q, qDot, 2);
+        {
+            // Change the position of the insertion and pennation angle and compare again (length and insertion in Local)
+            biorbd::muscles::Characteristics charac(idealizedActuator.characteristics());
+            charac.setPennationAngle(0.523599);
+            biorbd::utils::Vector3d insertion(idealizedActuator.position().insertionInLocal());
+            insertion.set(0.2, 0.2, 0.2);
+            biorbd::utils::String oldName(insertion.biorbd::utils::Node::name());
+            biorbd::utils::String newName("MyNewName");
+            insertion.setName(newName);
+            idealizedActuator.updateOrientations(model, Q, qDot, 2);
+
+            {
+                SCALAR_TO_DOUBLE(length, idealizedActuator.position().length());
+                SCALAR_TO_DOUBLE(shallowCopyLength, shallowCopy.position().length());
+                SCALAR_TO_DOUBLE(deepCopyNowLength, deepCopyNow.position().length());
+                SCALAR_TO_DOUBLE(deepCopyLaterLength, deepCopyLater.position().length());
+                EXPECT_NEAR(length, 0.07570761027741163, requiredPrecision);
+                EXPECT_NEAR(shallowCopyLength, 0.07570761027741163, requiredPrecision);
+                EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision);
+                EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
+                EXPECT_EQ(idealizedActuator.position().insertionInLocal().biorbd::utils::Node::name(), newName);
+                EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), newName);
+                EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
+                EXPECT_EQ(deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
+            }
+
+        }
 
         {
-            SCALAR_TO_DOUBLE(length, idealizedActuator.position().length());
-            SCALAR_TO_DOUBLE(shallowCopyLength, shallowCopy.position().length());
-            SCALAR_TO_DOUBLE(deepCopyNowLength, deepCopyNow.position().length());
-            SCALAR_TO_DOUBLE(deepCopyLaterLength, deepCopyLater.position().length());
-            EXPECT_NEAR(length, 0.07570761027741163, requiredPrecision);
-            EXPECT_NEAR(shallowCopyLength, 0.07570761027741163, requiredPrecision);
-            EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision);
-            EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
-            EXPECT_EQ(idealizedActuator.position().insertionInLocal().biorbd::utils::Node::name(), newName);
-            EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), newName);
-            EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
-            EXPECT_EQ(deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
+            // Change the position giving an actual vector
+            biorbd::utils::Vector3d newPosition(1, 2, 3);
+            biorbd::utils::String oldName("MyNewName");
+            biorbd::utils::String newName("MyNewNewName");
+            biorbd::rigidbody::NodeSegment newNode(newPosition, newName, "", true, true, "", 0);
+            {
+                const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setOrigin(newPosition);
+                const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setInsertionInLocal(newPosition);
+                const biorbd::utils::Vector3d& origin = idealizedActuator.position().originInLocal();
+                const biorbd::utils::Vector3d& insertion = idealizedActuator.position().insertionInLocal();
+                EXPECT_STREQ(origin.biorbd::utils::Node::name().c_str(), "TRImed_origin");
+                EXPECT_STREQ(insertion.biorbd::utils::Node::name().c_str(), oldName.c_str());
+            }
+            {
+                const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setOrigin(newNode);
+                const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setInsertionInLocal(newNode);
+                const biorbd::utils::Vector3d& origin = idealizedActuator.position().originInLocal();
+                const biorbd::utils::Vector3d& insertion = idealizedActuator.position().insertionInLocal();
+                EXPECT_STREQ(origin.biorbd::utils::Node::name().c_str(), newName.c_str());
+                EXPECT_STREQ(insertion.biorbd::utils::Node::name().c_str(), newName.c_str());
+            }
         }
     }