From 96e6b3acd6aef1e875fb623d6d087d923f2ae563 Mon Sep 17 00:00:00 2001 From: fbailly Date: Tue, 23 Mar 2021 17:00:55 +0100 Subject: [PATCH 1/7] swig vector3d --- binding/python3/biorbd_python.i.in | 28 ++++++++++++++++++++++------ include/Utils/Vector3d.h | 3 ++- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index c64354d5..44d42543 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -14,6 +14,7 @@ #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedAcceleration.h" #include "RigidBody/GeneralizedTorque.h" +#include "Utils/Vector3d.h" #include "RigidBody/IMU.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(*reinterpret_cast(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/include/Utils/Vector3d.h b/include/Utils/Vector3d.h index b85f38bf..9ef4b6fa 100644 --- a/include/Utils/Vector3d.h +++ b/include/Utils/Vector3d.h @@ -65,6 +65,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio const biorbd::utils::String &name, const biorbd::utils::String &parentName); +#ifndef SWIG /// /// \brief Construct a 3D vector from a Casadi 3D vector (drop the trailling 1) /// \param other The Casadi 3D vector @@ -85,6 +86,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio /// Vector3d( const RigidBodyDynamics::Math::Vector4d& other); +#endif #ifdef BIORBD_USE_EIGEN3_MATH /// @@ -118,7 +120,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio /// Vector3d( const RBDLCasadiMath::MX_Xd_SubMatrix& other); - #endif /// From 97b31e84c4781de0b651dbfb8136436b21e762b0 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Tue, 23 Mar 2021 12:48:27 -0400 Subject: [PATCH 2/7] Made the code compilable --- binding/python3/biorbd_python.i.in | 4 ++-- include/Utils/Vector3d.h | 2 -- src/Utils/RotoTrans.cpp | 2 +- src/Utils/Vector3d.cpp | 6 +++--- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 44d42543..4b652107 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -10,11 +10,11 @@ #include "BiorbdModel.h" #include "Utils/Matrix.h" +#include "Utils/Vector3d.h" #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedAcceleration.h" #include "RigidBody/GeneralizedTorque.h" -#include "Utils/Vector3d.h" #include "RigidBody/IMU.h" %} @@ -842,7 +842,7 @@ biorbd::utils::Vector3d &{ } #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(*reinterpret_cast(argp1)); + $1 = new biorbd::utils::Vector3d(biorbd::utils::Vector(*reinterpret_cast(argp1))); } #endif else if( PyArray_Check($input) ) { diff --git a/include/Utils/Vector3d.h b/include/Utils/Vector3d.h index 9ef4b6fa..c55edbd7 100644 --- a/include/Utils/Vector3d.h +++ b/include/Utils/Vector3d.h @@ -65,7 +65,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio const biorbd::utils::String &name, const biorbd::utils::String &parentName); -#ifndef SWIG /// /// \brief Construct a 3D vector from a Casadi 3D vector (drop the trailling 1) /// \param other The Casadi 3D vector @@ -86,7 +85,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio /// Vector3d( const RigidBodyDynamics::Math::Vector4d& other); -#endif #ifdef BIORBD_USE_EIGEN3_MATH /// diff --git a/src/Utils/RotoTrans.cpp b/src/Utils/RotoTrans.cpp index cead561f..6d3e1faa 100755 --- 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(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 index 6b2bd2d0..94c7817a 100644 --- 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((rt * v).block(0, 0, 3, 1)); + return (rt * v).block(0, 0, 3, 1); } void biorbd::utils::Vector3d::applyRT( From aeab2cad08560818d158e12e594025d01f91ffa2 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 24 Mar 2021 15:16:17 -0400 Subject: [PATCH 3/7] Keep the parent name when changing the position of a muscle --- .../forwardDynamicsFromMusclesExample.cpp | 5 +- src/Muscles/Geometry.cpp | 15 +++- test/test_muscles.cpp | 70 +++++++++++++------ 3 files changed, 65 insertions(+), 25 deletions(-) diff --git a/examples/forwardDynamicsFromMusclesExample.cpp b/examples/forwardDynamicsFromMusclesExample.cpp index e23222cd..9f62dc05 100644 --- 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> states; + std::vector> states = model.stateSet(); for (unsigned int i=0; i(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/src/Muscles/Geometry.cpp b/src/Muscles/Geometry.cpp index 5dc340b5..54966a3f 100755 --- 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(&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(&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/test/test_muscles.cpp b/test/test_muscles.cpp index 6fcabe24..a97e548b 100644 --- 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(idealizedActuator.position()).setOrigin(newPosition); + const_cast(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(idealizedActuator.position()).setOrigin(newNode); + const_cast(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()); + } } } From feea5667723cf5d4943d663af1c332f561772908 Mon Sep 17 00:00:00 2001 From: fbailly Date: Mon, 29 Mar 2021 16:48:55 +0200 Subject: [PATCH 4/7] changed function name tu run tests automatically --- test/binding/Python3/test_conversion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/binding/Python3/test_conversion.py b/test/binding/Python3/test_conversion.py index 6dc55f13..508c2f50 100644 --- a/test/binding/Python3/test_conversion.py +++ b/test/binding/Python3/test_conversion.py @@ -28,7 +28,7 @@ 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(), ) ) From 042d63d3a0559a3792fd41f4b4215c2709bd6322 Mon Sep 17 00:00:00 2001 From: fbailly Date: Mon, 29 Mar 2021 17:17:53 +0200 Subject: [PATCH 5/7] add test for vector3d python binding --- test/binding/Python3/test_conversion.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/test/binding/Python3/test_conversion.py b/test/binding/Python3/test_conversion.py index 508c2f50..cdad4454 100644 --- a/test/binding/Python3/test_conversion.py +++ b/test/binding/Python3/test_conversion.py @@ -47,3 +47,12 @@ def test_imu_to_array(): [-0.08887169, 0.10925158, 0.99003329, -0.20946], [0., 0., 0., 1.]])) +def test_vector3d(): + if biorbd.currentLinearAlgebraBackend() == 1: + from casadi import MX + vec = MX.zeros(3, 1) + biorbd.Vector3d(vec[0], vec[1], vec[2]) + + else: + vec = np.random.rand(3,) + biorbd.Vector3d(vec[0], vec[1], vec[2]) From ed9234c07d92f21ce4012807d64522e53dc1ab7d Mon Sep 17 00:00:00 2001 From: fbailly Date: Mon, 29 Mar 2021 18:47:05 +0200 Subject: [PATCH 6/7] complete test --- test/binding/Python3/test_conversion.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/test/binding/Python3/test_conversion.py b/test/binding/Python3/test_conversion.py index cdad4454..8e727f68 100644 --- a/test/binding/Python3/test_conversion.py +++ b/test/binding/Python3/test_conversion.py @@ -48,11 +48,13 @@ def test_imu_to_array(): [0., 0., 0., 1.]])) 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.zeros(3, 1) - biorbd.Vector3d(vec[0], vec[1], vec[2]) + vec = MX.ones(3, 1) + biorbd_model.setGravity(vec) + - else: - vec = np.random.rand(3,) - biorbd.Vector3d(vec[0], vec[1], vec[2]) From fdbcb12a7cb2ec4f23db540a7057ff27889b5f11 Mon Sep 17 00:00:00 2001 From: fbailly Date: Mon, 29 Mar 2021 18:47:58 +0200 Subject: [PATCH 7/7] black --- test/binding/Python3/test_conversion.py | 27 ++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/test/binding/Python3/test_conversion.py b/test/binding/Python3/test_conversion.py index 8e727f68..5e9ea1ad 100644 --- a/test/binding/Python3/test_conversion.py +++ b/test/binding/Python3/test_conversion.py @@ -30,10 +30,11 @@ def test_np_mx_to_generalized(): # --- Options --- # 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] @@ -41,20 +42,28 @@ def test_imu_to_array(): 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, ) + 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) - -