From 38cadaefc8a036d300d0f7212f892c721dabaed7 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 09:37:02 -0400 Subject: [PATCH 01/14] Joints and GeneralizedAcceleration in a namespace --- binding/biorbd_rigidbody.i.in | 4 +- binding/python3/biorbd_python.i.in | 52 +++--- include/BiorbdModel.h | 2 +- include/Muscles/Compound.h | 10 +- include/Muscles/Geometry.h | 20 +- include/Muscles/HillType.h | 4 +- include/Muscles/IdealizedActuator.h | 4 +- include/Muscles/Muscle.h | 12 +- include/Muscles/WrappingHalfCylinder.h | 4 +- include/Muscles/WrappingObject.h | 10 +- include/Muscles/WrappingSphere.h | 4 +- include/RigidBody/Contacts.h | 1 - include/RigidBody/GeneralizedAcceleration.h | 9 +- include/RigidBody/GeneralizedCoordinates.h | 8 +- include/RigidBody/GeneralizedTorque.h | 8 +- include/RigidBody/GeneralizedVelocity.h | 9 +- include/RigidBody/Joints.h | 192 ++++++++++---------- include/RigidBody/KalmanRecons.h | 12 +- include/RigidBody/KalmanReconsIMU.h | 4 +- include/RigidBody/KalmanReconsMarkers.h | 6 +- include/RigidBody/Markers.h | 12 +- include/RigidBody/Segment.h | 17 +- include/biorbdConfig.h.in | 1 + src/Actuators/Actuators.cpp | 16 +- src/Muscles/Geometry.cpp | 14 +- src/Muscles/HillType.cpp | 4 +- src/Muscles/IdealizedActuator.cpp | 4 +- src/Muscles/Muscle.cpp | 12 +- src/Muscles/Muscles.cpp | 8 +- src/Muscles/WrappingHalfCylinder.cpp | 4 +- src/Muscles/WrappingSphere.cpp | 2 +- src/RigidBody/Contacts.cpp | 6 +- src/RigidBody/GeneralizedAcceleration.cpp | 26 +-- src/RigidBody/GeneralizedCoordinates.cpp | 2 +- src/RigidBody/GeneralizedTorque.cpp | 2 +- src/RigidBody/GeneralizedVelocity.cpp | 2 +- src/RigidBody/IMUs.cpp | 6 +- src/RigidBody/Joints.cpp | 192 ++++++++++---------- src/RigidBody/KalmanRecons.cpp | 4 +- src/RigidBody/KalmanReconsIMU.cpp | 4 +- src/RigidBody/KalmanReconsMarkers.cpp | 6 +- src/RigidBody/Markers.cpp | 28 +-- src/RigidBody/RotoTransNodes.cpp | 6 +- src/RigidBody/Segment.cpp | 10 +- 44 files changed, 422 insertions(+), 341 deletions(-) diff --git a/binding/biorbd_rigidbody.i.in b/binding/biorbd_rigidbody.i.in index c509bbbc..a79b3ed1 100644 --- a/binding/biorbd_rigidbody.i.in +++ b/binding/biorbd_rigidbody.i.in @@ -32,8 +32,8 @@ namespace std { %template(MatBiorbdGeneralizedCoordinates) std::vector>; %template(VecBiorbdGeneralizedVelocity) std::vector; %template(MatBiorbdGeneralizedVelocity) std::vector>; -%template(VecBiorbdGeneralizedAcceleration) std::vector; -%template(MatBiorbdGeneralizedAcceleration) std::vector>; +%template(VecBiorbdGeneralizedAcceleration) std::vector; +%template(MatBiorbdGeneralizedAcceleration) std::vector>; %template(VecBiorbdNodeSegment) std::vector; %template(MatBiorbdNodeSegment) std::vector>; diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 4dd120c6..75ba578a 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -25,6 +25,14 @@ %include "@CMAKE_BINARY_DIR@/include/biorbdConfig.h" +#if defined(BIORBD_USE_CASADI_MATH) +#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedAcceleration +#elif defined(BIORBD_USE_EIGEN3_MATH) +#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedAcceleration +#endif + // -- STRING --// %typemap(typecheck, precedence=2300) biorbd::utils::String &{ void *argp1 = 0; @@ -80,24 +88,24 @@ }; // --- biorbd::rigidbody::Joints --- // -%typemap(typecheck, precedence=2000) biorbd::rigidbody::Joints &{ +%typemap(typecheck, precedence=2000) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &{ void *argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__rigidbody__Joints, 0 | 0)) && argp1) { - // Test if it is a pointer to SWIGTYPE_p_biorbd__rigidbody__Joints already exists + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_JOINTS, 0 | 0)) && argp1) { + // Test if it is a pointer to SWIG_RIGIDBODY_JOINTS already exists $1 = true; } else { $1 = false; } } -%typemap(in) biorbd::rigidbody::Joints &{ +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &{ void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__rigidbody__Joints, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_JOINTS, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::rigidbody::Joints * >(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints * >(argp1); } else { PyErr_SetString(PyExc_ValueError, - "biorbd::rigidbody::Joints must be " - "a biorbd::rigidbody::Joints"); + "biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints must be " + "a biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints"); SWIG_fail; } } @@ -657,9 +665,9 @@ biorbd::rigidbody::GeneralizedVelocity * { // --- GeneralizedAcceleration --- // %typemap(typecheck, precedence=2106) -biorbd::rigidbody::GeneralizedAcceleration & { +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration & { void *argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__rigidbody__GeneralizedAcceleration, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_ACC, 0 | 0)) && argp1) { $1 = true; } #ifdef BIORBD_USE_CASADI_MATH @@ -675,14 +683,14 @@ biorbd::rigidbody::GeneralizedAcceleration & { $1 = false; } } -%typemap(in) biorbd::rigidbody::GeneralizedAcceleration & { +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration & { void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__rigidbody__GeneralizedAcceleration, 0 | 0)) && argp1) { - $1 = reinterpret_cast< biorbd::rigidbody::GeneralizedAcceleration * >(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_ACC, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -702,7 +710,7 @@ biorbd::rigidbody::GeneralizedAcceleration & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQddot(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedAcceleration(nQddot); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); for (unsigned int qddot=0; qddot(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_ACC, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -758,7 +766,7 @@ biorbd::rigidbody::GeneralizedAcceleration * { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQddot(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedAcceleration(nQddot); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); for (unsigned int qddot=0; qddot& musclesPointsInGlobal( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q); /// diff --git a/include/Muscles/WrappingHalfCylinder.h b/include/Muscles/WrappingHalfCylinder.h index ccad5a37..464402ac 100644 --- a/include/Muscles/WrappingHalfCylinder.h +++ b/include/Muscles/WrappingHalfCylinder.h @@ -101,7 +101,7 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param length Length of the muscle (ignored if no value is provided) /// void wrapPoints( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::utils::Vector3d& p1_bone, const biorbd::utils::Vector3d& p2_bone, @@ -128,7 +128,7 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \return The RotoTrans matrix of the half cylinder /// virtual const biorbd::utils::RotoTrans& RT( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates& Q, bool updateKin = true); diff --git a/include/Muscles/WrappingObject.h b/include/Muscles/WrappingObject.h index 7b1a9dec..1dea0ce5 100644 --- a/include/Muscles/WrappingObject.h +++ b/include/Muscles/WrappingObject.h @@ -12,9 +12,15 @@ namespace utils class String; } +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; +} +} + +namespace rigidbody +{ class GeneralizedCoordinates; } @@ -110,7 +116,7 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param muscleLength Length of the muscle (ignored if no value is provided) /// virtual void wrapPoints( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::utils::Vector3d& p1_bone, const biorbd::utils::Vector3d& p2_bone, @@ -139,7 +145,7 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \return The RotoTrans matrix of the wrapping object /// virtual const biorbd::utils::RotoTrans& RT( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates& Q, bool updateKin = true) = 0; diff --git a/include/Muscles/WrappingSphere.h b/include/Muscles/WrappingSphere.h index afd9c4c3..614ddf8b 100644 --- a/include/Muscles/WrappingSphere.h +++ b/include/Muscles/WrappingSphere.h @@ -87,7 +87,7 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \brief Not yet implemented /// virtual void wrapPoints( - biorbd::rigidbody::Joints&, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints&, const biorbd::rigidbody::GeneralizedCoordinates&, const biorbd::utils::Vector3d&, const biorbd::utils::Vector3d&, @@ -111,7 +111,7 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \return The RotoTrans matrix of the sphere /// const biorbd::utils::RotoTrans& RT( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates&Q , bool updateKin = true); diff --git a/include/RigidBody/Contacts.h b/include/RigidBody/Contacts.h index 7cfad91f..9d55942f 100644 --- a/include/RigidBody/Contacts.h +++ b/include/RigidBody/Contacts.h @@ -22,7 +22,6 @@ namespace biorbd { namespace rigidbody { -class Joints; class GeneralizedCoordinates; /// diff --git a/include/RigidBody/GeneralizedAcceleration.h b/include/RigidBody/GeneralizedAcceleration.h index 4f298586..55ed518d 100644 --- a/include/RigidBody/GeneralizedAcceleration.h +++ b/include/RigidBody/GeneralizedAcceleration.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class Joints; @@ -33,14 +35,14 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector /// \brief Create generalized acceleration vector from a joint Model /// \param j The joint model /// - GeneralizedAcceleration(const biorbd::rigidbody::Joints& j); + GeneralizedAcceleration(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); /// /// \brief Construct generalized acceleration vector /// \param Q State vector of the internal joints /// GeneralizedAcceleration( - const biorbd::rigidbody::GeneralizedAcceleration& Q); + const GeneralizedAcceleration& Q); /// /// \brief Construct vector from Casadi vector @@ -89,7 +91,7 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector /// \return The current Generalized Coordinate /// template - biorbd::rigidbody::GeneralizedAcceleration& operator=( + GeneralizedAcceleration& operator=( const Eigen::MatrixBase & other) { this->biorbd::utils::Vector::operator=(other); @@ -124,6 +126,7 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector #endif }; +} } } diff --git a/include/RigidBody/GeneralizedCoordinates.h b/include/RigidBody/GeneralizedCoordinates.h index 102d7a9f..1956a791 100644 --- a/include/RigidBody/GeneralizedCoordinates.h +++ b/include/RigidBody/GeneralizedCoordinates.h @@ -6,9 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; +} +} + +namespace rigidbody +{ /// /// \brief Class GeneralizedCoordinates @@ -34,7 +40,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector /// \param j The joint model /// GeneralizedCoordinates( - const biorbd::rigidbody::Joints& j); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); /// /// \brief Construct generalized coordinates diff --git a/include/RigidBody/GeneralizedTorque.h b/include/RigidBody/GeneralizedTorque.h index 28ecd0b6..bd130014 100644 --- a/include/RigidBody/GeneralizedTorque.h +++ b/include/RigidBody/GeneralizedTorque.h @@ -6,9 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; +} +} + +namespace rigidbody +{ /// /// \brief Class GeneralizedTorque @@ -33,7 +39,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector /// \brief Construct generalized torque from a joint model /// \param j The joint model /// - GeneralizedTorque(const biorbd::rigidbody::Joints& j); + GeneralizedTorque(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); /// /// \brief Construct generalized torque from anoter Generalized torque diff --git a/include/RigidBody/GeneralizedVelocity.h b/include/RigidBody/GeneralizedVelocity.h index 6be5b033..dce0910e 100644 --- a/include/RigidBody/GeneralizedVelocity.h +++ b/include/RigidBody/GeneralizedVelocity.h @@ -6,9 +6,16 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class Joints; +} +} + +namespace rigidbody +{ /// /// \brief Class GeneralizedVelocity @@ -33,7 +40,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector /// \brief Create generalized velocity vector from a joint Model /// \param j The joint model /// - GeneralizedVelocity(const biorbd::rigidbody::Joints& j); + GeneralizedVelocity(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); /// /// \brief Construct generalized velocity vector diff --git a/include/RigidBody/Joints.h b/include/RigidBody/Joints.h index a04c62f6..838b93e9 100644 --- a/include/RigidBody/Joints.h +++ b/include/RigidBody/Joints.h @@ -19,12 +19,14 @@ class Vector3d; class Range; class SpatialVector; } +} +namespace biorbd +{ namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; -class GeneralizedAcceleration; class GeneralizedTorque; class NodeSegment; class MeshFace; @@ -32,6 +34,13 @@ class Segment; class SegmentCharacteristics; class Mesh; class Contacts; +} + +namespace BIORBD_MATH_NAMESPACE +{ +namespace rigidbody +{ +class GeneralizedAcceleration; /// /// \brief This is the core of the musculoskeletal model in biorbd @@ -54,7 +63,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param other The other joint model /// Joints( - const biorbd::rigidbody::Joints& other); + const Joints& other); /// /// \brief Properly destroy class @@ -65,14 +74,14 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \brief Deep copy of the joints /// \return Copy of the joints /// - biorbd::rigidbody::Joints DeepCopy() const; + Joints DeepCopy() const; /// /// \brief Deep copy of the joints /// \param other The joints to copy /// void DeepCopy( - const biorbd::rigidbody::Joints& other); + const Joints& other); /// /// \brief Add a segment to the model @@ -88,15 +97,15 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param forcePlates The number of the force platform attached to the Segment (if -1 no force platform is attached) /// unsigned int AddSegment( - const biorbd::utils::String &segmentName, - const biorbd::utils::String &parentName, - const biorbd::utils::String &translationSequence, - const biorbd::utils::String &rotationSequence, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &segmentName, + const utils::String &parentName, + const utils::String &translationSequence, + const utils::String &rotationSequence, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const biorbd::rigidbody::SegmentCharacteristics& characteristics, - const biorbd::utils::RotoTrans& referenceFrame, + const utils::RotoTrans& referenceFrame, int forcePlates=-1); /// @@ -112,14 +121,14 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param forcePlates The number of the force platform attached to the Segment (if -1 no force platform is attached) /// unsigned int AddSegment( - const biorbd::utils::String &segmentName, - const biorbd::utils::String &parentName, - const biorbd::utils::String &translationSequence, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &segmentName, + const utils::String &parentName, + const utils::String &translationSequence, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const biorbd::rigidbody::SegmentCharacteristics& characteristics, - const biorbd::utils::RotoTrans& referenceFrame, + const utils::RotoTrans& referenceFrame, int forcePlates=-1); @@ -128,14 +137,14 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \brief Get the current gravity /// \return The current gravity /// - biorbd::utils::Vector3d getGravity() const; + utils::Vector3d getGravity() const; /// /// \brief Set the gravity /// \param newGravity The new gravity vector /// void setGravity( - const biorbd::utils::Vector3d& newGravity); + const utils::Vector3d& newGravity); // -- INFORMATION ON THE MODEL -- // /// @@ -144,7 +153,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The biorbd body identification /// int GetBodyBiorbdId( - const biorbd::utils::String &segmentName) const; + const utils::String &segmentName) const; /// /// \brief Return the number of generalized torque @@ -171,14 +180,14 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The index of a DoF in a segment /// unsigned int getDofIndex( - const biorbd::utils::String& SegmentName, - const biorbd::utils::String& dofName); + const utils::String& SegmentName, + const utils::String& dofName); /// /// \brief Return the names of the degree of freedom (DoF) /// \return The names of the DoF /// - std::vector nameDof() const; + std::vector nameDof() const; /// /// \brief Return the number of generalized coordinates (Q) @@ -243,7 +252,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The segment /// const biorbd::rigidbody::Segment& segment( - const biorbd::utils::String& name) const; + const utils::String& name) const; // ------------------------------ // @@ -255,7 +264,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return A spatial vector with the forces /// std::vector dispatchedForce( - std::vector> &spatialVector, + std::vector> &spatialVector, unsigned int frame) const; /// @@ -264,7 +273,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return A spatial vector with the forces /// std::vector dispatchedForce( - std::vector &sv) const; + std::vector &sv) const; // ---------------------------- // @@ -277,7 +286,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model void UpdateKinematicsCustom( const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); // -- POSITION INTERFACE OF THE MODEL -- // @@ -287,7 +296,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param Q The generalized coordinates /// \return The JCS in global reference frame at a given Q /// - std::vector allGlobalJCS( + std::vector allGlobalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q); /// @@ -296,7 +305,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// /// This function assumes kinematics has been already updated /// - std::vector allGlobalJCS() const; + std::vector allGlobalJCS() const; /// /// \brief Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q @@ -304,9 +313,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param name The name of the segment /// \return The JCS of the segment in global reference frame at a given Q /// - biorbd::utils::RotoTrans globalJCS( + utils::RotoTrans globalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::String &name); + const utils::String &name); /// /// \brief Return the joint coordinate system (JCS) for the segment idx in global reference frame at a given Q @@ -314,7 +323,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param idx The index of the segment /// \return The JCS of the segment idx in global reference frame at a given Q /// - biorbd::utils::RotoTrans globalJCS( + utils::RotoTrans globalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int idx); @@ -325,8 +334,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// /// This function assumes kinematics has been already updated /// - biorbd::utils::RotoTrans globalJCS( - const biorbd::utils::String &name) const; + utils::RotoTrans globalJCS( + const utils::String &name) const; /// /// \brief Return the joint coordinate system (JCS) for the segment idx in global reference @@ -335,29 +344,29 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// /// This function assumes kinematics has been already updated /// - biorbd::utils::RotoTrans globalJCS( + utils::RotoTrans globalJCS( unsigned int idx) const; /// /// \brief Return all the joint coordinate system (JCS) in its parent reference frame /// \return All the JCS in parent reference frame /// - std::vector localJCS() const; + std::vector localJCS() const; /// /// \brief Return the joint coordinate system (JCS) of the segment its parent reference frame /// \param name The name of the segment /// \return The JCS of the segment in parent reference frame /// - biorbd::utils::RotoTrans localJCS( - const biorbd::utils::String &name) const; + utils::RotoTrans localJCS( + const utils::String &name) const; /// /// \brief Return the joint coordinate system (JCS) of the segment idx its parent reference frame /// \param idx The index of the segment /// \return The JCS of the segment idx in parent reference frame /// - biorbd::utils::RotoTrans localJCS( + utils::RotoTrans localJCS( const unsigned int idx) const; /// @@ -371,9 +380,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// biorbd::rigidbody::NodeSegment projectPoint( const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::Vector3d &v, + const utils::Vector3d &v, int segmentIdx, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, bool updateKin=true); /// @@ -411,7 +420,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The jacobian matrix of the projected marker /// - biorbd::utils::Matrix projectPointJacobian( + utils::Matrix projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, biorbd::rigidbody::NodeSegment p, bool updateKin); @@ -425,11 +434,11 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The Jacobian matrix of a projected marker on the segment segmentIdx /// - biorbd::utils::Matrix projectPointJacobian( + utils::Matrix projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::Vector3d &v, + const utils::Vector3d &v, int segmentIdx, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, bool updateKin); /// @@ -443,7 +452,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// The vector needs to be equal to the number of markers and in the order given /// by Markers and in global coordinates /// - std::vector projectPointJacobian( + std::vector projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, const std::vector &v, bool updateKin); @@ -455,7 +464,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \brief Return the total mass of the model /// \return The toal mass of the model /// - biorbd::utils::Scalar mass() const; + utils::Scalar mass() const; /// /// \brief Return the position of the center of mass @@ -463,7 +472,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The position of the center of mass /// - biorbd::utils::Vector3d CoM( + utils::Vector3d CoM( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true); @@ -483,7 +492,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The position of the center of mass of each segment /// - biorbd::utils::Matrix CoMbySegmentInMatrix( + utils::Matrix CoMbySegmentInMatrix( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin=true); @@ -494,7 +503,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The position of the center of mass of segment idx /// - biorbd::utils::Vector3d CoMbySegment( + utils::Vector3d CoMbySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const unsigned int idx, bool updateKin=true); @@ -506,7 +515,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The velocity of the center of mass /// - biorbd::utils::Vector3d CoMdot( + utils::Vector3d CoMdot( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true); @@ -519,10 +528,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The acceleration of the center of mass /// - biorbd::utils::Vector3d CoMddot( + utils::Vector3d CoMddot( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); /// @@ -532,7 +541,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The velocity of the center of mass of each segment /// - std::vector CoMdotBySegment( + std::vector CoMdotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin=true); @@ -545,7 +554,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The velocity of the center of mass of segment idx /// - biorbd::utils::Vector3d CoMdotBySegment( + utils::Vector3d CoMdotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const unsigned int idx, @@ -559,10 +568,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The acceleration of the center of mass of each segment /// - std::vector CoMddotBySegment( + std::vector CoMddotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); /// @@ -574,10 +583,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The acceleration of the center of mass of segment idx /// - biorbd::utils::Vector3d CoMddotBySegment( + utils::Vector3d CoMddotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin = true); @@ -587,7 +596,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The jacobian matrix of the center of mass /// - biorbd::utils::Matrix CoMJacobian( + utils::Matrix CoMJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin = true); // ------------------------ // @@ -600,7 +609,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The vertices of the for all segments /// - std::vector> meshPoints( + std::vector> meshPoints( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin = true); @@ -611,7 +620,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The vertices of the of the segment idx /// - std::vector meshPoints( + std::vector meshPoints( const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin = true); @@ -622,7 +631,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return All the vertices /// - std::vector meshPointsInMatrix( + std::vector meshPointsInMatrix( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin = true ); @@ -631,7 +640,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \brief Return the mesh faces for all the segments /// \return The mesh faces for all the segments /// - std::vector > meshFaces() const; + std::vector > meshFaces() const; /// /// \brief Return the mesh faces for segment idx @@ -665,7 +674,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The angular momentum of the center of mass /// - biorbd::utils::Vector3d angularMomentum( + utils::Vector3d angularMomentum( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin = true); // Wrapper pour le moment angulaire @@ -676,7 +685,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics should be updated /// \return The mass matrix /// - biorbd::utils::Matrix massMatrix( + utils::Matrix massMatrix( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin = true); @@ -687,7 +696,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The angular momentum of the center of mass /// - biorbd::utils::Vector3d CalcAngularMomentum ( + utils::Vector3d CalcAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin); @@ -700,10 +709,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The angular momentum of the center of mass /// - biorbd::utils::Vector3d CalcAngularMomentum ( + utils::Vector3d CalcAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); /// @@ -713,7 +722,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The segments center of mass angular momentum /// - std::vector CalcSegmentsAngularMomentum ( + std::vector CalcSegmentsAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin); @@ -726,10 +735,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The segments center of mass angular momentum /// - std::vector CalcSegmentsAngularMomentum ( + std::vector CalcSegmentsAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); // -------------------------------- // @@ -772,8 +781,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model biorbd::rigidbody::GeneralizedTorque InverseDynamics( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedAcceleration &QDDot, - std::vector* f_ext = nullptr); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, + std::vector* f_ext = nullptr); /// /// \brief Interface to NonLinearEffect @@ -785,7 +794,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model biorbd::rigidbody::GeneralizedTorque NonLinearEffect( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, - std::vector* f_ext = nullptr); + std::vector* f_ext = nullptr); /// /// \brief Interface for the forward dynamics of RBDL @@ -795,11 +804,11 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::rigidbody::GeneralizedAcceleration ForwardDynamics( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamics( const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::rigidbody::GeneralizedVelocity& QDot, const biorbd::rigidbody::GeneralizedTorque& Tau, - std::vector* f_ext = nullptr); + std::vector* f_ext = nullptr); /// /// \brief Interface for the forward dynamics with contact of RBDL @@ -810,12 +819,12 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::rigidbody::GeneralizedVelocity& QDot, const biorbd::rigidbody::GeneralizedTorque& Tau, biorbd::rigidbody::Contacts& CS, - std::vector* f_ext = nullptr); + std::vector* f_ext = nullptr); /// /// \brief Interface for contacts of the forward dynamics with contact of RBDL @@ -825,11 +834,11 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Contraint set /// - biorbd::utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect( + utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::rigidbody::GeneralizedVelocity& QDot, const biorbd::rigidbody::GeneralizedTorque& Tau, - std::vector* f_ext = nullptr); + std::vector* f_ext = nullptr); /// /// \brief Interface for the forward dynamics with contact of RBDL @@ -839,11 +848,11 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::rigidbody::GeneralizedVelocity& QDot, const biorbd::rigidbody::GeneralizedTorque& Tau, - std::vector* f_ext = nullptr); + std::vector* f_ext = nullptr); /// /// \brief Compute the QDot post from an impact @@ -870,7 +879,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model m_nRotAQuat; ///< The number of segments per quaternion std::shared_ptr m_isKinematicsComputed; ///< If the kinematics are computed - std::shared_ptr + std::shared_ptr m_totalMass; ///< Mass of all the bodies combined /// @@ -901,8 +910,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param idx The index of the segment /// \return The mesh vertices attached to a segment idx /// - std::vector meshPoints( - const std::vector &RT, + std::vector meshPoints( + const std::vector &RT, unsigned int idx) const; public: @@ -916,11 +925,12 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model void checkGeneralizedDimensions( const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, const biorbd::rigidbody::GeneralizedTorque *torque = nullptr); }; -} -} +} // namespace rigidbody +} // namespace BIORBD_MATH_NAMESPACE +} // namespace biorbd #endif // BIORBD_RIGIDBODY_JOINTS_H diff --git a/include/RigidBody/KalmanRecons.h b/include/RigidBody/KalmanRecons.h index 0568636f..447610aa 100644 --- a/include/RigidBody/KalmanRecons.h +++ b/include/RigidBody/KalmanRecons.h @@ -16,11 +16,17 @@ class Matrix; class Vector; } +namespace BIORBD_MATH_NAMESPACE { +namespace rigidbody +{ +class GeneralizedAcceleration; +} +} + namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; -class GeneralizedAcceleration; /// /// \brief Parameters of the reconstruction @@ -106,7 +112,7 @@ class BIORBD_API KalmanRecons void getState( biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, - biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); /// /// \brief Set the initial guess of the reconstruction @@ -117,7 +123,7 @@ class BIORBD_API KalmanRecons void setInitState( const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); /// /// \brief Proceed to one iteration of the Kalman filter diff --git a/include/RigidBody/KalmanReconsIMU.h b/include/RigidBody/KalmanReconsIMU.h index 2dee8daf..33d3c74b 100644 --- a/include/RigidBody/KalmanReconsIMU.h +++ b/include/RigidBody/KalmanReconsIMU.h @@ -65,7 +65,7 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons const std::vector &IMUobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); /// /// \brief Reconstruct the kinematics @@ -80,7 +80,7 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons const biorbd::utils::Vector &IMUobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); /// /// \brief This function cannot be used to reconstruct frames diff --git a/include/RigidBody/KalmanReconsMarkers.h b/include/RigidBody/KalmanReconsMarkers.h index 70ca2e4b..3a5e7d37 100644 --- a/include/RigidBody/KalmanReconsMarkers.h +++ b/include/RigidBody/KalmanReconsMarkers.h @@ -61,7 +61,7 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons const biorbd::rigidbody::Markers &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes=true); /// @@ -78,7 +78,7 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons const std::vector &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes=true); /// @@ -95,7 +95,7 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons const biorbd::utils::Vector &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, - biorbd::rigidbody::GeneralizedAcceleration *Qddot = nullptr, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, bool removeAxes=true); /// diff --git a/include/RigidBody/Markers.h b/include/RigidBody/Markers.h index 5f96bc94..9546bae5 100644 --- a/include/RigidBody/Markers.h +++ b/include/RigidBody/Markers.h @@ -13,11 +13,17 @@ class String; class Matrix; } +namespace BIORBD_MATH_NAMESPACE { +namespace rigidbody +{ +class GeneralizedAcceleration; +} +} + namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; -class GeneralizedAcceleration; class NodeSegment; /// @@ -210,7 +216,7 @@ class BIORBD_API Markers biorbd::rigidbody::NodeSegment markerAcceleration( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, unsigned int idx, bool removeAxis = true, bool updateKin = true); @@ -228,7 +234,7 @@ class BIORBD_API Markers std::vector markerAcceleration( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &dQdot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &dQdot, bool removeAxis=true, bool updateKin = true); diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index 2af05698..fb20b667 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -15,9 +15,15 @@ class RotoTrans; class Range; } +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; +} +} + +namespace rigidbody +{ class SegmentCharacteristics; /// @@ -46,7 +52,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \param PF Platform index attached to the body (-1 means no force platform acts on the body) /// Segment( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, const biorbd::utils::String &parentName, const biorbd::utils::String &seqT, @@ -72,7 +78,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \param PF Platform index attached to the body (-1 means no force platform acts on the body) /// Segment( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, const biorbd::utils::String &parentName, const biorbd::utils::String &seqR, @@ -224,7 +230,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// inertia for these 3 segments as well. /// void updateCharacteristics( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::SegmentCharacteristics& characteristics); /// @@ -268,7 +274,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \param QDDotRanges Ranges of the translations and rotations dof acceleration. The length of QDDotRanges must be equal to length of translations and rotations /// void setDofs( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &seqT, const biorbd::utils::String &seqR, const std::vector& QRanges, @@ -367,7 +373,8 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \brief Function that adds the segment to the RBDL body set /// \param model The joint model /// - virtual void setJoints(biorbd::rigidbody::Joints& model); + virtual void setJoints( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model); /// /// \brief Determine the rotation axis in relation to the requested sequence diff --git a/include/biorbdConfig.h.in b/include/biorbdConfig.h.in index 79e7a50b..66fb18ce 100644 --- a/include/biorbdConfig.h.in +++ b/include/biorbdConfig.h.in @@ -22,6 +22,7 @@ // Choice of linear algebra backend #cmakedefine BIORBD_USE_EIGEN3_MATH #cmakedefine BIORBD_USE_CASADI_MATH +#define BIORBD_MATH_NAMESPACE Math@MATH_LIBRARY_BACKEND@ namespace biorbd { diff --git a/src/Actuators/Actuators.cpp b/src/Actuators/Actuators.cpp index a612d601..11659b40 100644 --- a/src/Actuators/Actuators.cpp +++ b/src/Actuators/Actuators.cpp @@ -107,8 +107,8 @@ void biorbd::actuator::Actuators::addActuator(const biorbd::actuator::Actuator !*m_isClose, "You can't add actuator after closing the model"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); // Verify that the target dof is associated to a dof that // already exists in the model @@ -192,8 +192,8 @@ void biorbd::actuator::Actuators::addActuator(const biorbd::actuator::Actuator void biorbd::actuator::Actuators::closeActuator() { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); biorbd::utils::Error::check( model.nbDof()==m_all->size(), @@ -261,8 +261,8 @@ biorbd::actuator::Actuators::torqueMax( "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); std::pair maxGeneralizedTorque_all = @@ -343,8 +343,8 @@ biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torqueMax( "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); // Set qdot to be positive if concentric and negative if excentric biorbd::rigidbody::GeneralizedVelocity QdotResigned(Qdot); diff --git a/src/Muscles/Geometry.cpp b/src/Muscles/Geometry.cpp index e95f7faf..65b188b0 100644 --- a/src/Muscles/Geometry.cpp +++ b/src/Muscles/Geometry.cpp @@ -97,7 +97,7 @@ void biorbd::muscles::Geometry::DeepCopy(const biorbd::muscles::Geometry &other) // ------ PUBLIC FUNCTIONS ------ // void biorbd::muscles::Geometry::updateKinematics( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates *Q, const biorbd::rigidbody::GeneralizedVelocity *Qdot, int updateKin) @@ -128,7 +128,7 @@ void biorbd::muscles::Geometry::updateKinematics( _updateKinematics(Qdot); } -void biorbd::muscles::Geometry::updateKinematics(biorbd::rigidbody::Joints +void biorbd::muscles::Geometry::updateKinematics(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::muscles::Characteristics& characteristics, biorbd::muscles::PathModifiers &pathModifiers, @@ -327,7 +327,7 @@ void biorbd::muscles::Geometry::_updateKinematics( } const biorbd::utils::Vector3d &biorbd::muscles::Geometry::originInGlobal( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position @@ -338,7 +338,7 @@ const biorbd::utils::Vector3d &biorbd::muscles::Geometry::originInGlobal( } const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInGlobal( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position @@ -358,7 +358,7 @@ void biorbd::muscles::Geometry::setMusclesPointsInGlobal( } void biorbd::muscles::Geometry::setMusclesPointsInGlobal( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q, biorbd::muscles::PathModifiers *pathModifiers) { @@ -489,7 +489,7 @@ const biorbd::utils::Scalar& biorbd::muscles::Geometry::velocity( return *m_velocity; } -void biorbd::muscles::Geometry::setJacobianDimension(biorbd::rigidbody::Joints +void biorbd::muscles::Geometry::setJacobianDimension(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model) { *m_jacobian = biorbd::utils::Matrix::Zero(static_cast @@ -505,7 +505,7 @@ void biorbd::muscles::Geometry::jacobian(const biorbd::utils::Matrix &jaco) } void biorbd::muscles::Geometry::jacobian( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q) { for (unsigned int i=0; isize(); ++i) { diff --git a/src/Muscles/HillType.cpp b/src/Muscles/HillType.cpp index 63531707..cbf0e4a5 100644 --- a/src/Muscles/HillType.cpp +++ b/src/Muscles/HillType.cpp @@ -205,7 +205,7 @@ const biorbd::utils::Scalar& biorbd::muscles::HillType::force( } const biorbd::utils::Scalar& biorbd::muscles::HillType::force( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const biorbd::muscles::State &emg, @@ -229,7 +229,7 @@ const biorbd::utils::Scalar& biorbd::muscles::HillType::force( } const biorbd::utils::Scalar& biorbd::muscles::HillType::force( - biorbd::rigidbody::Joints &, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, const biorbd::rigidbody::GeneralizedCoordinates &, const biorbd::muscles::State &, int) diff --git a/src/Muscles/IdealizedActuator.cpp b/src/Muscles/IdealizedActuator.cpp index 2358ce6d..e788c9bd 100644 --- a/src/Muscles/IdealizedActuator.cpp +++ b/src/Muscles/IdealizedActuator.cpp @@ -87,7 +87,7 @@ const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( } const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( - biorbd::rigidbody::Joints &, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, const biorbd::rigidbody::GeneralizedCoordinates &, const biorbd::rigidbody::GeneralizedVelocity &, const biorbd::muscles::State &emg, @@ -98,7 +98,7 @@ const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( } const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( - biorbd::rigidbody::Joints &, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, const biorbd::rigidbody::GeneralizedCoordinates &, const biorbd::muscles::State &emg, int) diff --git a/src/Muscles/Muscle.cpp b/src/Muscles/Muscle.cpp index 6b0bef20..663f2c88 100644 --- a/src/Muscles/Muscle.cpp +++ b/src/Muscles/Muscle.cpp @@ -111,7 +111,7 @@ void biorbd::muscles::Muscle::DeepCopy(const biorbd::muscles::Muscle &other) } void biorbd::muscles::Muscle::updateOrientations( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -120,7 +120,7 @@ void biorbd::muscles::Muscle::updateOrientations( updateKin); } void biorbd::muscles::Muscle::updateOrientations( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, int updateKin) @@ -158,7 +158,7 @@ const biorbd::muscles::Geometry &biorbd::muscles::Muscle::position() const } const biorbd::utils::Scalar& biorbd::muscles::Muscle::length( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -174,7 +174,7 @@ const biorbd::utils::Scalar& biorbd::muscles::Muscle::length( } const biorbd::utils::Scalar& biorbd::muscles::Muscle::musculoTendonLength( - biorbd::rigidbody::Joints &m, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &m, const biorbd::rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -190,7 +190,7 @@ const biorbd::utils::Scalar& biorbd::muscles::Muscle::musculoTendonLength( } const biorbd::utils::Scalar& biorbd::muscles::Muscle::velocity( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -226,7 +226,7 @@ void biorbd::muscles::Muscle::computeForce(const biorbd::muscles::State &emg) const std::vector& biorbd::muscles::Muscle::musclesPointsInGlobal( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates &Q) { m_position->updateKinematics(model,*m_characteristics,*m_pathChanger,&Q, diff --git a/src/Muscles/Muscles.cpp b/src/Muscles/Muscles.cpp index f7de4dd5..ad9e9d31 100644 --- a/src/Muscles/Muscles.cpp +++ b/src/Muscles/Muscles.cpp @@ -238,8 +238,8 @@ unsigned int biorbd::muscles::Muscles::nbMuscleGroups() const biorbd::utils::Matrix biorbd::muscles::Muscles::musclesLengthJacobian() { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); biorbd::utils::Matrix tp(nbMuscleTotal(), model.nbDof()); unsigned int cmpMus(0); @@ -282,7 +282,7 @@ void biorbd::muscles::Muscles::updateMuscles( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); // Update all the muscles @@ -304,7 +304,7 @@ void biorbd::muscles::Muscles::updateMuscles( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); // Update all the muscles diff --git a/src/Muscles/WrappingHalfCylinder.cpp b/src/Muscles/WrappingHalfCylinder.cpp index a496f31b..caf3644f 100644 --- a/src/Muscles/WrappingHalfCylinder.cpp +++ b/src/Muscles/WrappingHalfCylinder.cpp @@ -147,7 +147,7 @@ void biorbd::muscles::WrappingHalfCylinder::wrapPoints( } void biorbd::muscles::WrappingHalfCylinder::wrapPoints( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::utils::Vector3d& p1_bone, const biorbd::utils::Vector3d& p2_bone, @@ -173,7 +173,7 @@ void biorbd::muscles::WrappingHalfCylinder::wrapPoints( } const biorbd::utils::RotoTrans& biorbd::muscles::WrappingHalfCylinder::RT( - biorbd::rigidbody::Joints &model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, const biorbd::rigidbody::GeneralizedCoordinates& Q, bool updateKin) { diff --git a/src/Muscles/WrappingSphere.cpp b/src/Muscles/WrappingSphere.cpp index 518151d2..a83c48fd 100644 --- a/src/Muscles/WrappingSphere.cpp +++ b/src/Muscles/WrappingSphere.cpp @@ -61,7 +61,7 @@ void biorbd::muscles::WrappingSphere::DeepCopy(const } const biorbd::utils::RotoTrans& biorbd::muscles::WrappingSphere::RT( - biorbd::rigidbody::Joints &, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, const biorbd::rigidbody::GeneralizedCoordinates &, bool) { diff --git a/src/RigidBody/Contacts.cpp b/src/RigidBody/Contacts.cpp index da12fa18..b5318d66 100644 --- a/src/RigidBody/Contacts.cpp +++ b/src/RigidBody/Contacts.cpp @@ -105,8 +105,8 @@ biorbd::rigidbody::Contacts &biorbd::rigidbody::Contacts::getConstraints() { if (!*m_isBinded) { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); Bind(model); *m_isBinded = true; } @@ -150,7 +150,7 @@ biorbd::rigidbody::Contacts::constraintsInGlobal( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; diff --git a/src/RigidBody/GeneralizedAcceleration.cpp b/src/RigidBody/GeneralizedAcceleration.cpp index 01e583b6..b880c9a7 100644 --- a/src/RigidBody/GeneralizedAcceleration.cpp +++ b/src/RigidBody/GeneralizedAcceleration.cpp @@ -3,33 +3,35 @@ #include "RigidBody/Joints.h" -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration() { } -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( unsigned int nQddot) : biorbd::utils::Vector(nQddot) { } -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( - const biorbd::rigidbody::Joints &j) : +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : biorbd::utils::Vector(j.nbQ()) { } -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( - const biorbd::rigidbody::GeneralizedAcceleration &Q) : +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( + const rigidbody::GeneralizedAcceleration &Q) : biorbd::utils::Vector(Q) { } -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const RigidBodyDynamics::Math::VectorNd &v) : biorbd::utils::Vector (v) { @@ -38,7 +40,7 @@ biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( #ifdef BIORBD_USE_CASADI_MATH -biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( +rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const casadi::MX &v) : biorbd::utils::Vector(v) { @@ -48,12 +50,12 @@ biorbd::rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( #endif -biorbd::rigidbody::GeneralizedAcceleration::~GeneralizedAcceleration() +rigidbody::GeneralizedAcceleration::~GeneralizedAcceleration() { } -void biorbd::rigidbody::GeneralizedAcceleration::operator=( +void rigidbody::GeneralizedAcceleration::operator=( const biorbd::utils::Vector &other) { this->biorbd::utils::Vector::operator=(other); @@ -61,13 +63,13 @@ void biorbd::rigidbody::GeneralizedAcceleration::operator=( #ifdef BIORBD_USE_CASADI_MATH -void biorbd::rigidbody::GeneralizedAcceleration::operator=( +void rigidbody::GeneralizedAcceleration::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { this->biorbd::utils::Vector::operator=(other); } -void biorbd::rigidbody::GeneralizedAcceleration::operator=( +void rigidbody::GeneralizedAcceleration::operator=( const casadi::MX &other) { this->biorbd::utils::Vector::operator=(other); diff --git a/src/RigidBody/GeneralizedCoordinates.cpp b/src/RigidBody/GeneralizedCoordinates.cpp index 9e639a6c..d19970f1 100644 --- a/src/RigidBody/GeneralizedCoordinates.cpp +++ b/src/RigidBody/GeneralizedCoordinates.cpp @@ -16,7 +16,7 @@ biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( } biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( - const biorbd::rigidbody::Joints &j) : + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : biorbd::utils::Vector(j.nbQ()) { diff --git a/src/RigidBody/GeneralizedTorque.cpp b/src/RigidBody/GeneralizedTorque.cpp index 11d3fbdd..3deee4df 100644 --- a/src/RigidBody/GeneralizedTorque.cpp +++ b/src/RigidBody/GeneralizedTorque.cpp @@ -16,7 +16,7 @@ biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( } biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( - const biorbd::rigidbody::Joints &j) : + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : biorbd::utils::Vector(j.nbGeneralizedTorque()) { diff --git a/src/RigidBody/GeneralizedVelocity.cpp b/src/RigidBody/GeneralizedVelocity.cpp index dd566d73..72c30fcc 100644 --- a/src/RigidBody/GeneralizedVelocity.cpp +++ b/src/RigidBody/GeneralizedVelocity.cpp @@ -16,7 +16,7 @@ biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( } biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( - const biorbd::rigidbody::Joints &j) : + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : biorbd::utils::Vector(j.nbQdot()) { diff --git a/src/RigidBody/IMUs.cpp b/src/RigidBody/IMUs.cpp index 21d46302..02f14b20 100644 --- a/src/RigidBody/IMUs.cpp +++ b/src/RigidBody/IMUs.cpp @@ -109,7 +109,7 @@ biorbd::rigidbody::IMU biorbd::rigidbody::IMUs::IMU( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -179,7 +179,7 @@ std::vector biorbd::rigidbody::IMUs::segmentIMU( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); // Segment name to find @@ -221,7 +221,7 @@ std::vector biorbd::rigidbody::IMUs::IMUJacobian( bool lookForTechnical) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); std::vector G; diff --git a/src/RigidBody/Joints.cpp b/src/RigidBody/Joints.cpp index 959f58bf..312f9162 100644 --- a/src/RigidBody/Joints.cpp +++ b/src/RigidBody/Joints.cpp @@ -24,7 +24,9 @@ #include "RigidBody/SegmentCharacteristics.h" #include "RigidBody/Contacts.h" -biorbd::rigidbody::Joints::Joints() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::Joints::Joints() : RigidBodyDynamics::Model(), m_segments(std::make_shared>()), m_nbRoot(std::make_shared(0)), @@ -36,11 +38,11 @@ biorbd::rigidbody::Joints::Joints() : m_isKinematicsComputed(std::make_shared(false)), m_totalMass(std::make_shared(0)) { - this->gravity = biorbd::utils::Vector3d (0, 0, - -9.81); // Redéfinition de la gravité pour qu'elle soit en z + // Redefining gravity so it is on z by default + this->gravity = biorbd::utils::Vector3d (0, 0, -9.81); } -biorbd::rigidbody::Joints::Joints(const biorbd::rigidbody::Joints &other) : +rigidbody::Joints::Joints(const rigidbody::Joints &other) : RigidBodyDynamics::Model(other), m_segments(other.m_segments), m_nbRoot(other.m_nbRoot), @@ -55,19 +57,19 @@ biorbd::rigidbody::Joints::Joints(const biorbd::rigidbody::Joints &other) : } -biorbd::rigidbody::Joints::~Joints() +rigidbody::Joints::~Joints() { } -biorbd::rigidbody::Joints biorbd::rigidbody::Joints::DeepCopy() const +rigidbody::Joints rigidbody::Joints::DeepCopy() const { - biorbd::rigidbody::Joints copy; + rigidbody::Joints copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::Joints::DeepCopy(const biorbd::rigidbody::Joints &other) +void rigidbody::Joints::DeepCopy(const rigidbody::Joints &other) { static_cast(*this) = other; m_segments->resize(other.m_segments->size()); @@ -84,16 +86,16 @@ void biorbd::rigidbody::Joints::DeepCopy(const biorbd::rigidbody::Joints &other) *m_totalMass = *other.m_totalMass; } -unsigned int biorbd::rigidbody::Joints::nbGeneralizedTorque() const +unsigned int rigidbody::Joints::nbGeneralizedTorque() const { return nbQddot(); } -unsigned int biorbd::rigidbody::Joints::nbDof() const +unsigned int rigidbody::Joints::nbDof() const { return *m_nbDof; } -std::vector biorbd::rigidbody::Joints::nameDof() const +std::vector rigidbody::Joints::nameDof() const { std::vector names; for (unsigned int i = 0; i < nbSegment(); ++i) { @@ -110,29 +112,29 @@ std::vector biorbd::rigidbody::Joints::nameDof() const return names; } -unsigned int biorbd::rigidbody::Joints::nbQ() const +unsigned int rigidbody::Joints::nbQ() const { return *m_nbQ; } -unsigned int biorbd::rigidbody::Joints::nbQdot() const +unsigned int rigidbody::Joints::nbQdot() const { return *m_nbQdot; } -unsigned int biorbd::rigidbody::Joints::nbQddot() const +unsigned int rigidbody::Joints::nbQddot() const { return *m_nbQddot; } -unsigned int biorbd::rigidbody::Joints::nbRoot() const +unsigned int rigidbody::Joints::nbRoot() const { return *m_nbRoot; } -biorbd::utils::Scalar biorbd::rigidbody::Joints::mass() const +biorbd::utils::Scalar rigidbody::Joints::mass() const { return *m_totalMass; } -unsigned int biorbd::rigidbody::Joints::AddSegment( +unsigned int rigidbody::Joints::AddSegment( const biorbd::utils::String &segmentName, const biorbd::utils::String &parentName, const biorbd::utils::String &translationSequence, @@ -169,7 +171,7 @@ unsigned int biorbd::rigidbody::Joints::AddSegment( m_segments->push_back(tp); return 0; } -unsigned int biorbd::rigidbody::Joints::AddSegment( +unsigned int rigidbody::Joints::AddSegment( const biorbd::utils::String &segmentName, const biorbd::utils::String &parentName, const biorbd::utils::String &seqR, @@ -198,18 +200,18 @@ unsigned int biorbd::rigidbody::Joints::AddSegment( return 0; } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::getGravity() const +biorbd::utils::Vector3d rigidbody::Joints::getGravity() const { return gravity; } -void biorbd::rigidbody::Joints::setGravity( +void rigidbody::Joints::setGravity( const biorbd::utils::Vector3d &newGravity) { gravity = newGravity; } -void biorbd::rigidbody::Joints::updateSegmentCharacteristics( +void rigidbody::Joints::updateSegmentCharacteristics( unsigned int idx, const biorbd::rigidbody::SegmentCharacteristics& characteristics) { @@ -218,7 +220,7 @@ void biorbd::rigidbody::Joints::updateSegmentCharacteristics( (*m_segments)[idx].updateCharacteristics(*this, characteristics); } -const biorbd::rigidbody::Segment& biorbd::rigidbody::Joints::segment( +const biorbd::rigidbody::Segment& rigidbody::Joints::segment( unsigned int idx) const { biorbd::utils::Error::check(idx < m_segments->size(), @@ -226,19 +228,19 @@ const biorbd::rigidbody::Segment& biorbd::rigidbody::Joints::segment( return (*m_segments)[idx]; } -const biorbd::rigidbody::Segment &biorbd::rigidbody::Joints::segment( +const biorbd::rigidbody::Segment &rigidbody::Joints::segment( const biorbd::utils::String & name) const { return segment(static_cast(GetBodyBiorbdId(name.c_str()))); } -unsigned int biorbd::rigidbody::Joints::nbSegment() const +unsigned int rigidbody::Joints::nbSegment() const { return static_cast(m_segments->size()); } std::vector -biorbd::rigidbody::Joints::dispatchedForce( +rigidbody::Joints::dispatchedForce( std::vector> &spatialVector, unsigned int frame) const { @@ -254,7 +256,7 @@ biorbd::rigidbody::Joints::dispatchedForce( } std::vector -biorbd::rigidbody::Joints::dispatchedForce( +rigidbody::Joints::dispatchedForce( std::vector &sv) const // a spatialVector per platform { @@ -288,7 +290,7 @@ const // a spatialVector per platform return sv_out; } -int biorbd::rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String +int rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String &segmentName) const { for (int i=0; i(m_segments->size()); ++i) @@ -298,14 +300,14 @@ int biorbd::rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String return -1; } -std::vector biorbd::rigidbody::Joints::allGlobalJCS( +std::vector rigidbody::Joints::allGlobalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q) { UpdateKinematicsCustom (&Q, nullptr, nullptr); return allGlobalJCS(); } -std::vector biorbd::rigidbody::Joints::allGlobalJCS() +std::vector rigidbody::Joints::allGlobalJCS() const { std::vector out; @@ -315,7 +317,7 @@ const return out; } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( +biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::String &name) { @@ -323,7 +325,7 @@ biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( return globalJCS(name); } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( +biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int idx) { @@ -332,19 +334,19 @@ biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( return globalJCS(idx); } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( +biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( const biorbd::utils::String &name) const { return globalJCS(static_cast(GetBodyBiorbdId(name))); } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::globalJCS( +biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( unsigned int idx) const { return CalcBodyWorldTransformation((*m_segments)[idx].id()); } -std::vector biorbd::rigidbody::Joints::localJCS() +std::vector rigidbody::Joints::localJCS() const { std::vector out; @@ -355,12 +357,12 @@ const return out; } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::localJCS( +biorbd::utils::RotoTrans rigidbody::Joints::localJCS( const biorbd::utils::String &name) const { return localJCS(static_cast(GetBodyBiorbdId(name.c_str()))); } -biorbd::utils::RotoTrans biorbd::rigidbody::Joints::localJCS( +biorbd::utils::RotoTrans rigidbody::Joints::localJCS( const unsigned int idx) const { return (*m_segments)[idx].localJCS(); @@ -368,7 +370,7 @@ biorbd::utils::RotoTrans biorbd::rigidbody::Joints::localJCS( std::vector -biorbd::rigidbody::Joints::projectPoint( +rigidbody::Joints::projectPoint( const biorbd::rigidbody::GeneralizedCoordinates& Q, const std::vector& v, bool updateKin) @@ -405,7 +407,7 @@ biorbd::rigidbody::Joints::projectPoint( return out; } -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint( +biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::Vector3d &v, int segmentIdx, @@ -431,7 +433,7 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint( return projectPoint(Q, node, false); } -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint( +biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::NodeSegment &n, bool updateKin) @@ -441,7 +443,7 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Joints::projectPoint( updateKin); } -biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian( +biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, biorbd::rigidbody::NodeSegment node, bool updateKin) @@ -479,7 +481,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian( } } -biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian( +biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, @@ -495,7 +497,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Joints::projectPointJacobian( } std::vector -biorbd::rigidbody::Joints::projectPointJacobian( +rigidbody::Joints::projectPointJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, const std::vector &v, bool updateKin) @@ -516,7 +518,7 @@ biorbd::rigidbody::Joints::projectPointJacobian( } RigidBodyDynamics::Math::SpatialTransform -biorbd::rigidbody::Joints::CalcBodyWorldTransformation ( +rigidbody::Joints::CalcBodyWorldTransformation ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const unsigned int segmentIdx, bool updateKin) @@ -532,7 +534,7 @@ biorbd::rigidbody::Joints::CalcBodyWorldTransformation ( } RigidBodyDynamics::Math::SpatialTransform -biorbd::rigidbody::Joints::CalcBodyWorldTransformation( +rigidbody::Joints::CalcBodyWorldTransformation( const unsigned int segmentIdx) const { if (segmentIdx >= this->fixed_body_discriminator) { @@ -553,7 +555,7 @@ biorbd::rigidbody::Joints::CalcBodyWorldTransformation( this->X_base[segmentIdx].E.transpose(), this->X_base[segmentIdx].r); } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoM( +biorbd::utils::Vector3d rigidbody::Joints::CoM( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -572,7 +574,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoM( return com; } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::angularMomentum( +biorbd::utils::Vector3d rigidbody::Joints::angularMomentum( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -580,7 +582,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::angularMomentum( return CalcAngularMomentum(Q, Qdot, updateKin); } -biorbd::utils::Matrix biorbd::rigidbody::Joints::massMatrix ( +biorbd::utils::Matrix rigidbody::Joints::massMatrix ( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -593,7 +595,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Joints::massMatrix ( return massMatrix; } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdot( +biorbd::utils::Vector3d rigidbody::Joints::CoMdot( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -620,10 +622,10 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdot( // Return the velocity of CoM return com_dot; } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddot( +biorbd::utils::Vector3d rigidbody::Joints::CoMddot( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -640,7 +642,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddot( return com_ddot; } -biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMJacobian( +biorbd::utils::Matrix rigidbody::Joints::CoMJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -671,7 +673,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMJacobian( std::vector -biorbd::rigidbody::Joints::CoMbySegment( +rigidbody::Joints::CoMbySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -683,7 +685,7 @@ biorbd::rigidbody::Joints::CoMbySegment( return out; } -biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMbySegmentInMatrix( +biorbd::utils::Matrix rigidbody::Joints::CoMbySegmentInMatrix( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -696,7 +698,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Joints::CoMbySegmentInMatrix( } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMbySegment( +biorbd::utils::Vector3d rigidbody::Joints::CoMbySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const unsigned int idx, bool updateKin) @@ -712,7 +714,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMbySegment( } -std::vector biorbd::rigidbody::Joints::CoMdotBySegment( +std::vector rigidbody::Joints::CoMdotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -726,7 +728,7 @@ std::vector biorbd::rigidbody::Joints::CoMdotBySegment( } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdotBySegment( +biorbd::utils::Vector3d rigidbody::Joints::CoMdotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, const unsigned int idx, @@ -745,10 +747,10 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMdotBySegment( std::vector -biorbd::rigidbody::Joints::CoMddotBySegment( +rigidbody::Joints::CoMddotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { std::vector out; @@ -760,10 +762,10 @@ biorbd::rigidbody::Joints::CoMddotBySegment( } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddotBySegment( +biorbd::utils::Vector3d rigidbody::Joints::CoMddotBySegment( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin) { @@ -778,7 +780,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CoMddotBySegment( } std::vector> - biorbd::rigidbody::Joints::meshPoints( + rigidbody::Joints::meshPoints( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -800,7 +802,7 @@ std::vector> return v; } -std::vector biorbd::rigidbody::Joints::meshPoints( +std::vector rigidbody::Joints::meshPoints( const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int i, bool updateKin) @@ -819,7 +821,7 @@ std::vector biorbd::rigidbody::Joints::meshPoints( } std::vector -biorbd::rigidbody::Joints::meshPointsInMatrix( +rigidbody::Joints::meshPointsInMatrix( const biorbd::rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -843,7 +845,7 @@ biorbd::rigidbody::Joints::meshPointsInMatrix( } return all_points; } -std::vector biorbd::rigidbody::Joints::meshPoints( +std::vector rigidbody::Joints::meshPoints( const std::vector &RT, unsigned int i) const { @@ -860,7 +862,7 @@ std::vector biorbd::rigidbody::Joints::meshPoints( } std::vector> - biorbd::rigidbody::Joints::meshFaces() const + rigidbody::Joints::meshFaces() const { // Gather the position of the meshings for all the segments std::vector> v_all; @@ -870,13 +872,13 @@ std::vector> return v_all; } const std::vector -&biorbd::rigidbody::Joints::meshFaces(unsigned int idx) const +&rigidbody::Joints::meshFaces(unsigned int idx) const { // Find the position of the meshings for a segment i return mesh(idx).faces(); } -std::vector biorbd::rigidbody::Joints::mesh() const +std::vector rigidbody::Joints::mesh() const { std::vector segmentOut; for (unsigned int i=0; i biorbd::rigidbody::Joints::mesh() const return segmentOut; } -const biorbd::rigidbody::Mesh &biorbd::rigidbody::Joints::mesh( +const biorbd::rigidbody::Mesh &rigidbody::Joints::mesh( unsigned int idx) const { return segment(idx).characteristics().mesh(); } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum ( +biorbd::utils::Vector3d rigidbody::Joints::CalcAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -910,10 +912,10 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum ( return angular_momentum; } -biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum ( +biorbd::utils::Vector3d rigidbody::Joints::CalcAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { // Definition of the variables @@ -933,7 +935,7 @@ biorbd::utils::Vector3d biorbd::rigidbody::Joints::CalcAngularMomentum ( } std::vector -biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum ( +rigidbody::Joints::CalcSegmentsAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -973,10 +975,10 @@ biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum ( } std::vector -biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum ( +rigidbody::Joints::CalcSegmentsAngularMomentum ( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -1013,12 +1015,12 @@ biorbd::rigidbody::Joints::CalcSegmentsAngularMomentum ( return h_segment; } -unsigned int biorbd::rigidbody::Joints::nbQuat() const +unsigned int rigidbody::Joints::nbQuat() const { return *m_nRotAQuat; } -biorbd::rigidbody::GeneralizedVelocity biorbd::rigidbody::Joints::computeQdot( +biorbd::rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedCoordinates &QDot, const double k_stab) @@ -1062,10 +1064,10 @@ biorbd::rigidbody::GeneralizedVelocity biorbd::rigidbody::Joints::computeQdot( return QDotOut; } -biorbd::rigidbody::GeneralizedTorque biorbd::rigidbody::Joints::InverseDynamics( +biorbd::rigidbody::GeneralizedTorque rigidbody::Joints::InverseDynamics( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedAcceleration &QDDot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, std::vector* f_ext) { biorbd::rigidbody::GeneralizedTorque Tau(nbGeneralizedTorque()); @@ -1079,7 +1081,7 @@ biorbd::rigidbody::GeneralizedTorque biorbd::rigidbody::Joints::InverseDynamics( return Tau; } -biorbd::rigidbody::GeneralizedTorque biorbd::rigidbody::Joints::NonLinearEffect( +biorbd::rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, std::vector* f_ext) @@ -1095,8 +1097,8 @@ biorbd::rigidbody::GeneralizedTorque biorbd::rigidbody::Joints::NonLinearEffect( return Tau; } -biorbd::rigidbody::GeneralizedAcceleration -biorbd::rigidbody::Joints::ForwardDynamics( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::Joints::ForwardDynamics( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, @@ -1106,7 +1108,7 @@ biorbd::rigidbody::Joints::ForwardDynamics( UpdateKinematicsCustom(&Q, &QDot); #endif - biorbd::rigidbody::GeneralizedAcceleration QDDot(*this); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(*this); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1117,8 +1119,8 @@ biorbd::rigidbody::Joints::ForwardDynamics( return QDDot; } -biorbd::rigidbody::GeneralizedAcceleration -biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::Joints::ForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, @@ -1129,7 +1131,7 @@ biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect( UpdateKinematicsCustom(&Q, &QDot); #endif - biorbd::rigidbody::GeneralizedAcceleration QDDot(*this); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(*this); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1143,7 +1145,7 @@ biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect( } biorbd::utils::Vector -biorbd::rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( +rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, @@ -1155,8 +1157,8 @@ biorbd::rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( return CS.getForce(); } -biorbd::rigidbody::GeneralizedAcceleration -biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::Joints::ForwardDynamicsConstraintsDirect( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &QDot, const biorbd::rigidbody::GeneralizedTorque &Tau, @@ -1169,7 +1171,7 @@ biorbd::rigidbody::Joints::ForwardDynamicsConstraintsDirect( } biorbd::rigidbody::GeneralizedVelocity -biorbd::rigidbody::Joints::ComputeConstraintImpulsesDirect( +rigidbody::Joints::ComputeConstraintImpulsesDirect( const biorbd::rigidbody::GeneralizedCoordinates& Q, const biorbd::rigidbody::GeneralizedVelocity& QDotPre ) @@ -1188,7 +1190,7 @@ biorbd::rigidbody::Joints::ComputeConstraintImpulsesDirect( } } -unsigned int biorbd::rigidbody::Joints::getDofIndex( +unsigned int rigidbody::Joints::getDofIndex( const biorbd::utils::String& segmentName, const biorbd::utils::String& dofName) { @@ -1214,16 +1216,16 @@ unsigned int biorbd::rigidbody::Joints::getDofIndex( return idx; } -void biorbd::rigidbody::Joints::UpdateKinematicsCustom( +void rigidbody::Joints::UpdateKinematicsCustom( const biorbd::rigidbody::GeneralizedCoordinates *Q, const biorbd::rigidbody::GeneralizedVelocity *Qdot, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot) + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { checkGeneralizedDimensions(Q, Qdot, Qddot); RigidBodyDynamics::UpdateKinematicsCustom(*this, Q, Qdot, Qddot); } -void biorbd::rigidbody::Joints::CalcMatRotJacobian( +void rigidbody::Joints::CalcMatRotJacobian( const biorbd::rigidbody::GeneralizedCoordinates &Q, unsigned int segmentIdx, const RigidBodyDynamics::Math::Matrix3d &rotation, @@ -1296,10 +1298,10 @@ void biorbd::rigidbody::Joints::CalcMatRotJacobian( } } -void biorbd::rigidbody::Joints::checkGeneralizedDimensions( +void rigidbody::Joints::checkGeneralizedDimensions( const biorbd::rigidbody::GeneralizedCoordinates *Q, const biorbd::rigidbody::GeneralizedVelocity *Qdot, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, const biorbd::rigidbody::GeneralizedTorque *torque) { #ifndef SKIP_ASSERT diff --git a/src/RigidBody/KalmanRecons.cpp b/src/RigidBody/KalmanRecons.cpp index 69e144b4..73c7b6f6 100644 --- a/src/RigidBody/KalmanRecons.cpp +++ b/src/RigidBody/KalmanRecons.cpp @@ -96,7 +96,7 @@ void biorbd::rigidbody::KalmanRecons::manageOcclusionDuringIteration( void biorbd::rigidbody::KalmanRecons::getState( biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot) + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { if (Q != nullptr) { @@ -212,7 +212,7 @@ biorbd::rigidbody::KalmanRecons::initState( void biorbd::rigidbody::KalmanRecons::setInitState( const biorbd::rigidbody::GeneralizedCoordinates *Q, const biorbd::rigidbody::GeneralizedVelocity *Qdot, - const biorbd::rigidbody::GeneralizedAcceleration *Qddot) + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { if (Q != nullptr) { m_xp->block(0, 0, *m_nbDof, 1) = *Q; diff --git a/src/RigidBody/KalmanReconsIMU.cpp b/src/RigidBody/KalmanReconsIMU.cpp index f5208d94..4a1bb62d 100644 --- a/src/RigidBody/KalmanReconsIMU.cpp +++ b/src/RigidBody/KalmanReconsIMU.cpp @@ -78,7 +78,7 @@ void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame( const std::vector &IMUobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot) + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { // Separate the IMUobs in a big vector biorbd::utils::Vector T(static_cast @@ -98,7 +98,7 @@ void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame( const biorbd::utils::Vector &IMUobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot) + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { // An iteration of the Kalman filter if (*m_firstIteration) { diff --git a/src/RigidBody/KalmanReconsMarkers.cpp b/src/RigidBody/KalmanReconsMarkers.cpp index 94f0a7af..ebb4f5dc 100644 --- a/src/RigidBody/KalmanReconsMarkers.cpp +++ b/src/RigidBody/KalmanReconsMarkers.cpp @@ -81,7 +81,7 @@ void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame( const biorbd::rigidbody::Markers &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector @@ -99,7 +99,7 @@ void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame( const std::vector &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector @@ -118,7 +118,7 @@ void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame( const utils::Vector &Tobs, biorbd::rigidbody::GeneralizedCoordinates *Q, biorbd::rigidbody::GeneralizedVelocity *Qdot, - biorbd::rigidbody::GeneralizedAcceleration *Qddot, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // An iteration of the Kalman filter diff --git a/src/RigidBody/Markers.cpp b/src/RigidBody/Markers.cpp index 9820454e..b72c7573 100644 --- a/src/RigidBody/Markers.cpp +++ b/src/RigidBody/Markers.cpp @@ -88,7 +88,7 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -113,7 +113,7 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -177,7 +177,7 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::markerVelocity( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -214,13 +214,13 @@ biorbd::rigidbody::Markers::markersVelocity( biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::markerAcceleration( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, unsigned int idx, bool removeAxis, bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -242,7 +242,7 @@ std::vector biorbd::rigidbody::Markers::markerAcceleration( const biorbd::rigidbody::GeneralizedCoordinates &Q, const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedAcceleration &Qddot, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool removeAxis, bool updateKin) { @@ -322,7 +322,7 @@ biorbd::rigidbody::Markers::segmentMarkers( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); // Name of the segment to find @@ -349,8 +349,8 @@ unsigned int biorbd::rigidbody::Markers::nbMarkers(unsigned int idxSegment) const { // Assuming that this is also a joint type (via BiorbdModel) - const biorbd::rigidbody::Joints &model = - dynamic_cast(*this); + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = + dynamic_cast(*this); // Name of the segment to find const biorbd::utils::String& name(model.segment(idxSegment).name()); @@ -391,7 +391,7 @@ biorbd::utils::Matrix biorbd::rigidbody::Markers::markersJacobian( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -412,7 +412,7 @@ bool biorbd::rigidbody::Markers::inverseKinematics( biorbd::rigidbody::GeneralizedCoordinates &Q, bool removeAxes) { - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); model.UpdateKinematicsCustom(&Q); // also assert for dimensions @@ -438,7 +438,7 @@ bool biorbd::rigidbody::Markers::inverseKinematics( // Call the base function return RigidBodyDynamics::InverseKinematics( - dynamic_cast(*this), + dynamic_cast(*this), Qinit, body_id, body_pointEigen, markersInRbdl, Q); } #endif @@ -451,7 +451,7 @@ std::vector biorbd::rigidbody::Markers::markersJacobian( bool lookForTechnical) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -498,7 +498,7 @@ unsigned int biorbd::rigidbody::Markers::nbTechnicalMarkers( unsigned int idxSegment) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); unsigned int nTechMarkers = 0; diff --git a/src/RigidBody/RotoTransNodes.cpp b/src/RigidBody/RotoTransNodes.cpp index d95ab624..13bf892b 100644 --- a/src/RigidBody/RotoTransNodes.cpp +++ b/src/RigidBody/RotoTransNodes.cpp @@ -115,7 +115,7 @@ biorbd::utils::RotoTransNode biorbd::rigidbody::RotoTransNodes::RT( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -138,7 +138,7 @@ biorbd::rigidbody::RotoTransNodes::segmentRTs( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); // Segment name to find @@ -162,7 +162,7 @@ biorbd::rigidbody::RotoTransNodes::RTsJacobian( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::rigidbody::Joints &model = dynamic_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; diff --git a/src/RigidBody/Segment.cpp b/src/RigidBody/Segment.cpp index 1d59ef32..5bc72a03 100644 --- a/src/RigidBody/Segment.cpp +++ b/src/RigidBody/Segment.cpp @@ -45,7 +45,7 @@ biorbd::rigidbody::Segment::Segment() : } biorbd::rigidbody::Segment::Segment( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, const biorbd::utils::String &parentName, const biorbd::utils::String &seqT, @@ -93,7 +93,7 @@ biorbd::rigidbody::Segment::Segment( setPF(PF); } biorbd::rigidbody::Segment::Segment( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, // Name of segment const biorbd::utils::String &parentName, // Name of segment const biorbd::utils::String @@ -298,7 +298,7 @@ biorbd::utils::RotoTrans biorbd::rigidbody::Segment::localJCS() const } void biorbd::rigidbody::Segment::updateCharacteristics( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::rigidbody::SegmentCharacteristics& characteristics) { @@ -320,7 +320,7 @@ biorbd::rigidbody::Segment::characteristics() const } void biorbd::rigidbody::Segment::setDofs( - biorbd::rigidbody::Joints& model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &seqT, const biorbd::utils::String &seqR, const std::vector& QRanges, @@ -520,7 +520,7 @@ void biorbd::rigidbody::Segment::setJointAxis() } void biorbd::rigidbody::Segment::setJoints( - biorbd::rigidbody::Joints& model) + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model) { setDofCharacteristicsOnLastBody(); // Apply the segment caracteristics only to the last segment setJointAxis(); // Choose the axis order in relation to the selected sequence From 7a5c37ff23068ad1ab61c0adc6fb6c17561bee64 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 10:31:27 -0400 Subject: [PATCH 02/14] Added namespace to rigidbody --- binding/biorbd_rigidbody.i.in | 29 ++- binding/python3/biorbd_python.i.in | 106 +++++----- include/BiorbdModel.h | 8 +- include/ModelReader.h | 13 +- include/RigidBody/Contacts.h | 9 +- include/RigidBody/GeneralizedCoordinates.h | 15 +- include/RigidBody/GeneralizedTorque.h | 12 +- include/RigidBody/GeneralizedVelocity.h | 12 +- include/RigidBody/IMU.h | 13 +- include/RigidBody/IMUs.h | 47 +++-- include/RigidBody/Joints.h | 190 +++++++++-------- include/RigidBody/Markers.h | 96 ++++----- include/RigidBody/Mesh.h | 17 +- include/RigidBody/MeshFace.h | 9 +- include/RigidBody/NodeSegment.h | 9 +- include/RigidBody/RotoTransNodes.h | 17 +- include/RigidBody/Segment.h | 27 ++- include/RigidBody/SegmentCharacteristics.h | 13 +- include/Utils/Rotation.h | 10 +- include/Utils/RotoTrans.h | 8 +- include/biorbdConfig.h.in | 16 +- src/ModelReader.cpp | 38 ++-- src/RigidBody/Contacts.cpp | 40 ++-- src/RigidBody/GeneralizedCoordinates.cpp | 24 ++- src/RigidBody/GeneralizedTorque.cpp | 24 ++- src/RigidBody/GeneralizedVelocity.cpp | 26 +-- src/RigidBody/IMU.cpp | 26 +-- src/RigidBody/IMUs.cpp | 100 ++++----- src/RigidBody/Joints.cpp | 232 ++++++++++----------- src/RigidBody/Markers.cpp | 189 ++++++++--------- src/RigidBody/Mesh.cpp | 44 ++-- src/RigidBody/MeshFace.cpp | 20 +- src/RigidBody/NodeSegment.cpp | 48 +++-- src/RigidBody/RotoTransNodes.cpp | 50 ++--- src/RigidBody/Segment.cpp | 104 ++++----- src/RigidBody/SegmentCharacteristics.cpp | 40 ++-- src/Utils/Rotation.cpp | 10 +- src/Utils/RotoTrans.cpp | 8 +- 38 files changed, 868 insertions(+), 831 deletions(-) diff --git a/binding/biorbd_rigidbody.i.in b/binding/biorbd_rigidbody.i.in index a79b3ed1..1c4f8a87 100644 --- a/binding/biorbd_rigidbody.i.in +++ b/binding/biorbd_rigidbody.i.in @@ -23,27 +23,26 @@ #include "RigidBody/IMUs.h" %} - namespace std { -%template(VecBiorbdGeneralizedTorque) std::vector; -%template(MatBiorbdGeneralizedTorque) std::vector>; -%template(PairBiorbdGeneralizedTorque) std::pair; -%template(VecBiorbdGeneralizedCoordinates) std::vector; -%template(MatBiorbdGeneralizedCoordinates) std::vector>; -%template(VecBiorbdGeneralizedVelocity) std::vector; -%template(MatBiorbdGeneralizedVelocity) std::vector>; +%template(VecBiorbdGeneralizedTorque) std::vector; +%template(MatBiorbdGeneralizedTorque) std::vector>; +%template(PairBiorbdGeneralizedTorque) std::pair; +%template(VecBiorbdGeneralizedCoordinates) std::vector; +%template(MatBiorbdGeneralizedCoordinates) std::vector>; +%template(VecBiorbdGeneralizedVelocity) std::vector; +%template(MatBiorbdGeneralizedVelocity) std::vector>; %template(VecBiorbdGeneralizedAcceleration) std::vector; %template(MatBiorbdGeneralizedAcceleration) std::vector>; -%template(VecBiorbdNodeSegment) std::vector; -%template(MatBiorbdNodeSegment) std::vector>; -%template(VecBiorbdMeshFace) std::vector; -%template(MatBiorbdMeshFace) std::vector>; +%template(VecBiorbdNodeSegment) std::vector; +%template(MatBiorbdNodeSegment) std::vector>; +%template(VecBiorbdMeshFace) std::vector; +%template(MatBiorbdMeshFace) std::vector>; -%template(PairBiorbdNodeSegment) std::pair; +%template(PairBiorbdNodeSegment) std::pair; -%template(VecBiorbdIMU) std::vector; -%template(MatBiorbdIMU) std::vector>; +%template(VecBiorbdIMU) std::vector; +%template(MatBiorbdIMU) std::vector>; } %include "@CMAKE_SOURCE_DIR@/include/RigidBody/RigidBodyEnums.h" diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 75ba578a..13aa31ff 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -27,10 +27,18 @@ #if defined(BIORBD_USE_CASADI_MATH) #define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedCoordinates +#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedVelocity #define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedAcceleration +#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedTorque +#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Markers #elif defined(BIORBD_USE_EIGEN3_MATH) #define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedCoordinates +#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedVelocity #define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedAcceleration +#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedTorque +#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Markers #endif // -- STRING --// @@ -87,7 +95,7 @@ } }; -// --- biorbd::rigidbody::Joints --- // +// --- Joints --- // %typemap(typecheck, precedence=2000) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_JOINTS, 0 | 0)) && argp1) { @@ -434,15 +442,15 @@ // --- GeneralizedCoordinates --- // %typemap(typecheck, precedence=2100) -biorbd::rigidbody::GeneralizedCoordinates & { +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates & { void *argp1 = 0; - if(SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__rigidbody__GeneralizedCoordinates, 0 | 0)) && argp1){ + if(SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_COORD, 0 | 0)) && argp1){ $1 = true; } #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__rigidbody__GeneralizedCoordinates already exists + // SWIG_RIGIDBODY_GEN_COORD already exists $1 = true; } #endif @@ -454,14 +462,14 @@ biorbd::rigidbody::GeneralizedCoordinates & { $1 = false; } } -%typemap(in) biorbd::rigidbody::GeneralizedCoordinates & { +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates & { void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1,SWIGTYPE_p_biorbd__rigidbody__GeneralizedCoordinates, 0 | 0)) && argp1) { - $1 = reinterpret_cast< biorbd::rigidbody::GeneralizedCoordinates * >(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1,SWIG_RIGIDBODY_GEN_COORD, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -481,15 +489,15 @@ biorbd::rigidbody::GeneralizedCoordinates & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQ(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedCoordinates(nQ); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); for (unsigned int q=0; q(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1,SWIG_RIGIDBODY_GEN_COORD, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -536,7 +544,7 @@ biorbd::rigidbody::GeneralizedCoordinates & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQ(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedCoordinates(nQ); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); for (unsigned int q=0; q(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_VEL, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -597,7 +605,7 @@ biorbd::rigidbody::GeneralizedVelocity & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQdot(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedVelocity(nQdot); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); for (unsigned int qdot=0; qdot(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_VEL, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -653,7 +661,7 @@ biorbd::rigidbody::GeneralizedVelocity * { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQdot(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedVelocity(nQdot); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); for (unsigned int qdot=0; qdot(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_GEN_TORQUE, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::rigidbody::GeneralizedTorque(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -823,7 +831,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * { // Copy the actual data unsigned int nGeneralizedTorque(dims[0]); - $1 = new biorbd::rigidbody::GeneralizedTorque(nGeneralizedTorque); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque(nGeneralizedTorque); for (unsigned int GeneralizedTorque=0; GeneralizedTorque(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers * >(argp1); } else { PyErr_SetString(PyExc_ValueError, - "biorbd::rigidbody::Markers must be " - "a biorbd::rigidbody::Markers"); + "biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers must be " + "a biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers"); SWIG_fail; } } #endif // --- Vector3d --- // -%typemap(typecheck, precedence=2130) +%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) { @@ -1049,7 +1057,7 @@ biorbd::utils::Vector3d &{ } #endif }; -%extend biorbd::rigidbody::IMU{ +%extend biorbd::BIORBD_MATH_NAMESPACE::rigidbody::IMU{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); diff --git a/include/BiorbdModel.h b/include/BiorbdModel.h index 71fd7bb9..d9222364 100644 --- a/include/BiorbdModel.h +++ b/include/BiorbdModel.h @@ -66,10 +66,10 @@ namespace biorbd /// class BIORBD_API Model : public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints - ,public biorbd::rigidbody::Markers - ,public biorbd::rigidbody::IMUs - ,public biorbd::rigidbody::RotoTransNodes - ,public biorbd::rigidbody::Contacts + ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers + ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::IMUs + ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::RotoTransNodes + ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Contacts #ifdef MODULE_ACTUATORS ,public biorbd::actuator::Actuators #endif diff --git a/include/ModelReader.h b/include/ModelReader.h index 0442c469..7fd059ae 100644 --- a/include/ModelReader.h +++ b/include/ModelReader.h @@ -10,11 +10,14 @@ namespace biorbd { class Model; +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; class Mesh; } +} namespace utils { @@ -64,7 +67,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the kinematics /// - static std::vector readQDataFile( + static std::vector readQDataFile( const biorbd::utils::Path &path); /// @@ -155,7 +158,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::rigidbody::Mesh readMeshFileBiorbdSegments( + static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileBiorbdSegments( const biorbd::utils::Path& path); /// @@ -163,7 +166,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::rigidbody::Mesh readMeshFilePly( + static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFilePly( const biorbd::utils::Path& path); /// @@ -171,7 +174,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::rigidbody::Mesh readMeshFileObj( + static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileObj( const biorbd::utils::Path& path); #ifdef MODULE_VTP_FILES_READER @@ -180,7 +183,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::rigidbody::Mesh readMeshFileVtp( + static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileVtp( const biorbd::utils::Path& path); #endif diff --git a/include/RigidBody/Contacts.h b/include/RigidBody/Contacts.h index 9d55942f..9754694f 100644 --- a/include/RigidBody/Contacts.h +++ b/include/RigidBody/Contacts.h @@ -20,6 +20,8 @@ class SpatialVector; namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -43,14 +45,14 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// \brief Deep copy of contacts /// \return Copy of contacts /// - biorbd::rigidbody::Contacts DeepCopy() const; + Contacts DeepCopy() const; /// /// \brief Deep copy of contacts /// \param other The contacts to copy /// void DeepCopy( - const biorbd::rigidbody::Contacts& other); + const Contacts& other); /// /// \brief Add a constraint to the constraint set @@ -147,7 +149,7 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// \return The contraints positions in the global reference /// std::vector constraintsInGlobal( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin); /// @@ -162,6 +164,7 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet }; +} } } diff --git a/include/RigidBody/GeneralizedCoordinates.h b/include/RigidBody/GeneralizedCoordinates.h index 1956a791..8b253233 100644 --- a/include/RigidBody/GeneralizedCoordinates.h +++ b/include/RigidBody/GeneralizedCoordinates.h @@ -6,15 +6,11 @@ namespace biorbd { -namespace BIORBD_MATH_NAMESPACE { -namespace rigidbody +namespace BIORBD_MATH_NAMESPACE { -class Joints; -} -} - namespace rigidbody { +class Joints; /// /// \brief Class GeneralizedCoordinates @@ -40,14 +36,14 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector /// \param j The joint model /// GeneralizedCoordinates( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); + const Joints& j); /// /// \brief Construct generalized coordinates /// \param Q State vector of the internal joints /// GeneralizedCoordinates( - const biorbd::rigidbody::GeneralizedCoordinates& Q); + const GeneralizedCoordinates& Q); /// /// \brief Construct vector from Casadi vector @@ -95,7 +91,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector /// \return The current Generalized Coordinate /// template - biorbd::rigidbody::GeneralizedCoordinates& operator=( + GeneralizedCoordinates& operator=( const Eigen::MatrixBase & other) { this->biorbd::utils::Vector::operator=(other); @@ -131,6 +127,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector #endif }; +} } } diff --git a/include/RigidBody/GeneralizedTorque.h b/include/RigidBody/GeneralizedTorque.h index bd130014..353fc416 100644 --- a/include/RigidBody/GeneralizedTorque.h +++ b/include/RigidBody/GeneralizedTorque.h @@ -10,11 +10,6 @@ namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; -} -} - -namespace rigidbody -{ /// /// \brief Class GeneralizedTorque @@ -39,14 +34,14 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector /// \brief Construct generalized torque from a joint model /// \param j The joint model /// - GeneralizedTorque(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); + GeneralizedTorque(const Joints& j); /// /// \brief Construct generalized torque from anoter Generalized torque /// \param other The other generalized torque /// GeneralizedTorque( - const biorbd::rigidbody::GeneralizedTorque& other); + const GeneralizedTorque& other); /// /// \brief Construct vector from Casadi vector @@ -89,7 +84,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector /// \return The current Generalized Torque /// template - biorbd::rigidbody::GeneralizedTorque& operator=(const Eigen::MatrixBase + GeneralizedTorque& operator=(const Eigen::MatrixBase & other) { this->biorbd::utils::Vector::operator=(other); @@ -124,6 +119,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector #endif }; +} } } diff --git a/include/RigidBody/GeneralizedVelocity.h b/include/RigidBody/GeneralizedVelocity.h index dce0910e..87c3258e 100644 --- a/include/RigidBody/GeneralizedVelocity.h +++ b/include/RigidBody/GeneralizedVelocity.h @@ -11,11 +11,6 @@ namespace BIORBD_MATH_NAMESPACE namespace rigidbody { class Joints; -} -} - -namespace rigidbody -{ /// /// \brief Class GeneralizedVelocity @@ -40,14 +35,14 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector /// \brief Create generalized velocity vector from a joint Model /// \param j The joint model /// - GeneralizedVelocity(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); + GeneralizedVelocity(const Joints& j); /// /// \brief Construct generalized velocity vector /// \param Q State vector of the internal joints /// GeneralizedVelocity( - const biorbd::rigidbody::GeneralizedVelocity& Q); + const GeneralizedVelocity& Q); /// /// \brief Construct vector from Casadi vector @@ -91,7 +86,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector /// \return The current Generalized velocity vector /// template - biorbd::rigidbody::GeneralizedVelocity& operator=( + GeneralizedVelocity& operator=( const Eigen::MatrixBase & other) { this->biorbd::utils::Vector::operator=(other); @@ -127,6 +122,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector #endif }; +} } } diff --git a/include/RigidBody/IMU.h b/include/RigidBody/IMU.h index a0c869c9..3f8929f3 100644 --- a/include/RigidBody/IMU.h +++ b/include/RigidBody/IMU.h @@ -12,6 +12,8 @@ namespace utils class String; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { @@ -57,9 +59,9 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode #endif #ifdef BIORBD_USE_CASADI_MATH IMU( - const biorbd::rigidbody::IMU& imu); + const IMU& imu); - biorbd::rigidbody::IMU operator*(const biorbd::rigidbody::IMU& other) const; + IMU operator*(const IMU& other) const; #endif @@ -67,13 +69,13 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode /// \brief Deep copy of the IMU data /// \return Copy of the IMU data /// - biorbd::rigidbody::IMU DeepCopy() const; + IMU DeepCopy() const; /// /// \brief Deep copy if the IMU data /// \param other The IMU to copy /// - void DeepCopy(const biorbd::rigidbody::IMU& other); + void DeepCopy(const IMU& other); /// /// \brief Return if the IMU is technical @@ -95,7 +97,7 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode /// \return The current IMU /// template - biorbd::rigidbody::IMU& operator=(const Eigen::MatrixBase & other) + IMU& operator=(const Eigen::MatrixBase & other) { this->biorbd::utils::RotoTransNode::operator=(other); return *this; @@ -109,6 +111,7 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode }; +} } } diff --git a/include/RigidBody/IMUs.h b/include/RigidBody/IMUs.h index 25799f91..2d37233d 100644 --- a/include/RigidBody/IMUs.h +++ b/include/RigidBody/IMUs.h @@ -14,6 +14,8 @@ class Matrix; class RotoTransNode; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -34,7 +36,7 @@ class BIORBD_API IMUs /// \param other The other IMU set /// IMUs( - const biorbd::rigidbody::IMUs& other); + const IMUs& other); /// /// \brief Destroy the class properly @@ -45,14 +47,14 @@ class BIORBD_API IMUs /// \brief Deep copy of the inertial measurement units data /// \return A copy of the inertial measurement units data /// - biorbd::rigidbody::IMUs DeepCopy() const; + IMUs DeepCopy() const; /// /// \brief Deep copy the inertial measurement units data /// \param other The IMU data to copy /// void DeepCopy( - const biorbd::rigidbody::IMUs& other); + const IMUs& other); // Set and get @@ -105,14 +107,14 @@ class BIORBD_API IMUs /// \brief Return all the IMU in the local reference of the segment /// \return All the inertial measurement units in local reference frame /// - const std::vector& IMU() const; + const std::vector& IMU() const; /// /// \brief Return all the inertial measurement units (IMU) of a segment /// \param segmentName The name of the segment to return the IMU /// \return All the IMU of attached to the segment /// - std::vector IMU( + std::vector IMU( const biorbd::utils::String& segmentName); /// @@ -120,7 +122,7 @@ class BIORBD_API IMUs /// \param idx The index of the IMU in the set /// \return IMU of idx i /// - const biorbd::rigidbody::IMU& IMU( + const rigidbody::IMU& IMU( unsigned int idx); /// @@ -129,8 +131,8 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return All the IMU at the position given by Q /// - std::vector IMU( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + std::vector IMU( + const GeneralizedCoordinates& Q, bool updateKin = true); /// @@ -140,8 +142,8 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return The IMU of index idx at the position given by Q /// - biorbd::rigidbody::IMU IMU( - const biorbd::rigidbody::GeneralizedCoordinates&Q, + rigidbody::IMU IMU( + const GeneralizedCoordinates&Q, unsigned int idx, bool updateKin = true); @@ -152,8 +154,8 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return All the IMU on the segment of index idx /// - std::vector segmentIMU( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + std::vector segmentIMU( + const GeneralizedCoordinates& Q, unsigned int idx, bool updateKin = true); @@ -175,15 +177,15 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return all the technical IMU /// - std::vector technicalIMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector technicalIMU( + const GeneralizedCoordinates &Q, bool updateKin = true); /// /// \brief Return all the technical inertial measurement units (IMU) in their respective segment local reference frame /// \return All the technical IMU /// - std::vector technicalIMU(); + std::vector technicalIMU(); /// /// \brief Return all the anatomical inertial measurement units (IMU) @@ -191,15 +193,15 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return all the anatomical IMU /// - std::vector anatomicalIMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector anatomicalIMU( + const GeneralizedCoordinates &Q, bool updateKin = true); /// /// \brief Return all the anatomical inertial measurement units (IMU) in their respective segment local reference frame /// \return All the anatomical IMU /// - std::vector anatomicalIMU(); + std::vector anatomicalIMU(); /// /// \brief Return the jacobian of the inertial measurement units (IMU) @@ -208,7 +210,7 @@ class BIORBD_API IMUs /// \return The jacobien of the IMU /// std::vector IMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); /// @@ -218,7 +220,7 @@ class BIORBD_API IMUs /// \return The jacobian of the technical IMU /// std::vector TechnicalIMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); protected: @@ -230,15 +232,16 @@ class BIORBD_API IMUs /// \param lookForTechnical If true, only computes for the technical IMU /// std::vector IMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin, bool lookForTechnical); - std::shared_ptr> + std::shared_ptr> m_IMUs; ///< All the inertial Measurement Units }; +} } } diff --git a/include/RigidBody/Joints.h b/include/RigidBody/Joints.h index 838b93e9..c55f0876 100644 --- a/include/RigidBody/Joints.h +++ b/include/RigidBody/Joints.h @@ -23,24 +23,20 @@ class SpatialVector; namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; class GeneralizedTorque; +class GeneralizedAcceleration; class NodeSegment; class MeshFace; class Segment; class SegmentCharacteristics; class Mesh; class Contacts; -} - -namespace BIORBD_MATH_NAMESPACE -{ -namespace rigidbody -{ -class GeneralizedAcceleration; /// /// \brief This is the core of the musculoskeletal model in biorbd @@ -104,7 +100,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const SegmentCharacteristics& characteristics, const utils::RotoTrans& referenceFrame, int forcePlates=-1); @@ -127,7 +123,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const SegmentCharacteristics& characteristics, const utils::RotoTrans& referenceFrame, int forcePlates=-1); @@ -235,7 +231,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// void updateSegmentCharacteristics( unsigned int idx, - const biorbd::rigidbody::SegmentCharacteristics& characteristics); + const SegmentCharacteristics& characteristics); /// @@ -243,7 +239,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param idx Index of the segment /// \return The segment /// - const biorbd::rigidbody::Segment& segment( + const Segment& segment( unsigned int idx) const; /// @@ -251,7 +247,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param name The name of the segment to return /// \return The segment /// - const biorbd::rigidbody::Segment& segment( + const Segment& segment( const utils::String& name) const; // ------------------------------ // @@ -284,8 +280,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param Qddot The generalized accelerations /// void UpdateKinematicsCustom( - const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, - const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, + const GeneralizedCoordinates *Q = nullptr, + const GeneralizedVelocity *Qdot = nullptr, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); @@ -297,7 +293,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The JCS in global reference frame at a given Q /// std::vector allGlobalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q); + const GeneralizedCoordinates &Q); /// /// \brief Return the joint coordinate system (JCS) in global reference frame at a given Q @@ -314,7 +310,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The JCS of the segment in global reference frame at a given Q /// utils::RotoTrans globalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, const utils::String &name); /// @@ -324,7 +320,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The JCS of the segment idx in global reference frame at a given Q /// utils::RotoTrans globalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, unsigned int idx); /// @@ -378,8 +374,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The position of the projected marker /// - biorbd::rigidbody::NodeSegment projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + NodeSegment projectPoint( + const GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, const utils::String& axesToRemove, @@ -396,9 +392,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// The vector needs to be equal to the number of markers and in the order given /// by Markers and in global coordinates /// - std::vector projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const std::vector &v, + std::vector projectPoint( + const GeneralizedCoordinates &Q, + const std::vector &v, bool updateKin=true); /// @@ -408,9 +404,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The projected markers from a point corresponding to a marker from the model /// - biorbd::rigidbody::NodeSegment projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::NodeSegment& n, + NodeSegment projectPoint( + const GeneralizedCoordinates& Q, + const NodeSegment& n, bool updateKin); /// @@ -421,8 +417,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The jacobian matrix of the projected marker /// utils::Matrix projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - biorbd::rigidbody::NodeSegment p, + const GeneralizedCoordinates &Q, + NodeSegment p, bool updateKin); /// @@ -435,7 +431,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The Jacobian matrix of a projected marker on the segment segmentIdx /// utils::Matrix projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, const utils::String& axesToRemove, @@ -453,8 +449,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// by Markers and in global coordinates /// std::vector projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const std::vector &v, + const GeneralizedCoordinates &Q, + const std::vector &v, bool updateKin); // ------------------------------------- // @@ -473,7 +469,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The position of the center of mass /// utils::Vector3d CoM( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin=true); /// @@ -482,8 +478,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// \return The position of the center of mass of each segment /// - std::vector CoMbySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector CoMbySegment( + const GeneralizedCoordinates &Q, bool updateKin=true); /// @@ -493,7 +489,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The position of the center of mass of each segment /// utils::Matrix CoMbySegmentInMatrix( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin=true); /// @@ -504,7 +500,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The position of the center of mass of segment idx /// utils::Vector3d CoMbySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, const unsigned int idx, bool updateKin=true); @@ -516,8 +512,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The velocity of the center of mass /// utils::Vector3d CoMdot( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool updateKin=true); /// @@ -529,8 +525,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The acceleration of the center of mass /// utils::Vector3d CoMddot( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); @@ -542,8 +538,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The velocity of the center of mass of each segment /// std::vector CoMdotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool updateKin=true); /// @@ -555,8 +551,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The velocity of the center of mass of segment idx /// utils::Vector3d CoMdotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const unsigned int idx, bool updateKin=true); @@ -569,8 +565,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The acceleration of the center of mass of each segment /// std::vector CoMddotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); @@ -584,8 +580,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The acceleration of the center of mass of segment idx /// utils::Vector3d CoMddotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin = true); @@ -597,7 +593,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The jacobian matrix of the center of mass /// utils::Matrix CoMJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); // ------------------------ // @@ -610,7 +606,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The vertices of the for all segments /// std::vector> meshPoints( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); /// @@ -621,7 +617,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The vertices of the of the segment idx /// std::vector meshPoints( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, unsigned int idx, bool updateKin = true); @@ -632,7 +628,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return All the vertices /// std::vector meshPointsInMatrix( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true ); @@ -640,28 +636,28 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \brief Return the mesh faces for all the segments /// \return The mesh faces for all the segments /// - std::vector > meshFaces() const; + std::vector > meshFaces() const; /// /// \brief Return the mesh faces for segment idx /// \param idx The index of the segment /// \return The mesh face for segment idx /// - const std::vector &meshFaces( + const std::vector &meshFaces( unsigned int idx) const; /// /// \brief Return the segment mesh /// \return The segment mesh /// - std::vector mesh() const; + std::vector mesh() const; /// /// \brief Return the segment mesh for segment idx /// \param idx The index of the segment /// \return The Segment mesh for segment idx /// - const biorbd::rigidbody::Mesh& mesh( + const Mesh& mesh( unsigned int idx) const; // ----------------------- // @@ -675,8 +671,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The angular momentum of the center of mass /// utils::Vector3d angularMomentum( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool updateKin = true); // Wrapper pour le moment angulaire /// @@ -686,7 +682,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The mass matrix /// utils::Matrix massMatrix( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); /// @@ -697,8 +693,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The angular momentum of the center of mass /// utils::Vector3d CalcAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool updateKin); /// @@ -710,8 +706,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The angular momentum of the center of mass /// utils::Vector3d CalcAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); @@ -723,8 +719,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The segments center of mass angular momentum /// std::vector CalcSegmentsAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool updateKin); /// @@ -736,8 +732,8 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The segments center of mass angular momentum /// std::vector CalcSegmentsAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); // -------------------------------- // @@ -751,7 +747,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param updateKin If the kinematics of the model should be computed /// void CalcMatRotJacobian ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, unsigned int segmentIdx, const RigidBodyDynamics::Math::Matrix3d &rotation, RigidBodyDynamics::Math::MatrixNd &G, @@ -764,9 +760,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param k_stab /// \return The derivate of Q in function of Qdot /// - biorbd::rigidbody::GeneralizedVelocity computeQdot( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedCoordinates &QDot, + GeneralizedVelocity computeQdot( + const GeneralizedCoordinates &Q, + const GeneralizedCoordinates &QDot, const double k_stab = 1); // ---- DYNAMIC INTERFACE ---- // @@ -778,9 +774,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Torques /// - biorbd::rigidbody::GeneralizedTorque InverseDynamics( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, + GeneralizedTorque InverseDynamics( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &QDot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, std::vector* f_ext = nullptr); @@ -791,9 +787,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Torques of the bias effects /// - biorbd::rigidbody::GeneralizedTorque NonLinearEffect( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, + GeneralizedTorque NonLinearEffect( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &QDot, std::vector* f_ext = nullptr); /// @@ -805,9 +801,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The Generalized Accelerations /// biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamics( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, - const biorbd::rigidbody::GeneralizedTorque& Tau, + const GeneralizedCoordinates& Q, + const GeneralizedVelocity& QDot, + const GeneralizedTorque& Tau, std::vector* f_ext = nullptr); /// @@ -820,10 +816,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The Generalized Accelerations /// biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, - const biorbd::rigidbody::GeneralizedTorque& Tau, - biorbd::rigidbody::Contacts& CS, + const GeneralizedCoordinates& Q, + const GeneralizedVelocity& QDot, + const GeneralizedTorque& Tau, + Contacts& CS, std::vector* f_ext = nullptr); /// @@ -835,9 +831,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The Contraint set /// utils::Vector ContactForcesFromForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, - const biorbd::rigidbody::GeneralizedTorque& Tau, + const GeneralizedCoordinates& Q, + const GeneralizedVelocity& QDot, + const GeneralizedTorque& Tau, std::vector* f_ext = nullptr); /// @@ -849,9 +845,9 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The Generalized Accelerations /// biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, - const biorbd::rigidbody::GeneralizedTorque& Tau, + const GeneralizedCoordinates& Q, + const GeneralizedVelocity& QDot, + const GeneralizedTorque& Tau, std::vector* f_ext = nullptr); /// @@ -860,12 +856,12 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param QDotPre The Generalized Velocities before impact /// \return The Generalized Velocities post acceleration /// - biorbd::rigidbody::GeneralizedVelocity ComputeConstraintImpulsesDirect( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDotPre); + GeneralizedVelocity ComputeConstraintImpulsesDirect( + const GeneralizedCoordinates& Q, + const GeneralizedVelocity& QDotPre); protected: - std::shared_ptr> + std::shared_ptr> m_segments; ///< All the articulations std::shared_ptr @@ -890,7 +886,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \return The JCS of the segment in global reference frame /// RigidBodyDynamics::Math::SpatialTransform CalcBodyWorldTransformation( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, const unsigned int segmentIdx, bool updateKin = true); @@ -923,10 +919,10 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param torque The generalized torques /// void checkGeneralizedDimensions( - const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, - const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, + const GeneralizedCoordinates *Q = nullptr, + const GeneralizedVelocity *Qdot = nullptr, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, - const biorbd::rigidbody::GeneralizedTorque *torque = nullptr); + const GeneralizedTorque *torque = nullptr); }; } // namespace rigidbody diff --git a/include/RigidBody/Markers.h b/include/RigidBody/Markers.h index 9546bae5..befeada4 100644 --- a/include/RigidBody/Markers.h +++ b/include/RigidBody/Markers.h @@ -14,16 +14,11 @@ class Matrix; } namespace BIORBD_MATH_NAMESPACE { -namespace rigidbody -{ -class GeneralizedAcceleration; -} -} - namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; +class GeneralizedAcceleration; class NodeSegment; /// @@ -41,7 +36,7 @@ class BIORBD_API Markers /// \brief Construct markers from another marker set /// \param other The other marker set /// - Markers(const biorbd::rigidbody::Markers& other); + Markers(const Markers& other); /// /// \brief Destroy class properly @@ -52,13 +47,13 @@ class BIORBD_API Markers /// \brief Deep copy of the markers /// \return Deep copy of the markers /// - biorbd::rigidbody::Markers DeepCopy() const; + Markers DeepCopy() const; /// /// \brief Deep copy of the markers /// \param other The markers to copy from /// - void DeepCopy(const biorbd::rigidbody::Markers& other); + void DeepCopy(const Markers& other); /// /// \brief Add a marker to the set @@ -71,7 +66,7 @@ class BIORBD_API Markers /// \param id The index of the parent segment /// void addMarker( - const biorbd::rigidbody::NodeSegment &pos, + const NodeSegment &pos, const biorbd::utils::String &name, const biorbd::utils::String &parentName, bool technical, @@ -85,7 +80,7 @@ class BIORBD_API Markers /// \param idx The marker we want to return /// \return The marker /// - const biorbd::rigidbody::NodeSegment& marker( + const NodeSegment& marker( unsigned int idx) const; /// @@ -93,7 +88,7 @@ class BIORBD_API Markers /// \param name Name of the segment /// \return The markers on the segment /// - std::vector marker( + std::vector marker( const biorbd::utils::String &name) const; /// @@ -122,9 +117,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The marker in the global reference frame /// - biorbd::rigidbody::NodeSegment marker( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::NodeSegment& node, + NodeSegment marker( + const GeneralizedCoordinates& Q, + const NodeSegment& node, bool removeAxis=true, bool updateKin = true); @@ -136,8 +131,8 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The marker idx in the global reference frame /// - biorbd::rigidbody::NodeSegment marker( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + NodeSegment marker( + const GeneralizedCoordinates& Q, unsigned int idx, bool removeAxis=true, bool updateKin = true); @@ -148,7 +143,7 @@ class BIORBD_API Markers /// \param removeAxis If there are axis to remove from the position variables /// \return The marker of index idx /// - biorbd::rigidbody::NodeSegment marker( + NodeSegment marker( unsigned int idx, bool removeAxis); @@ -159,8 +154,8 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return All the markers in the global reference frame /// - std::vector markers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector markers( + const GeneralizedCoordinates &Q, bool removeAxis = true, bool updateKin = true); @@ -169,7 +164,7 @@ class BIORBD_API Markers /// \param removeAxis If there are axis to remove from the position variables /// \return All the markers /// - std::vector markers( + std::vector markers( bool removeAxis = true); /// @@ -181,9 +176,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The velocity of a marker /// - biorbd::rigidbody::NodeSegment markerVelocity( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + NodeSegment markerVelocity( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, unsigned int idx, bool removeAxis = true, bool updateKin = true); @@ -197,9 +192,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The velocity of all the markers /// - std::vector markersVelocity( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + std::vector markersVelocity( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, bool removeAxis=true, bool updateKin = true); @@ -213,9 +208,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The acceleration of a marker /// - biorbd::rigidbody::NodeSegment markerAcceleration( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + NodeSegment markerAcceleration( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, unsigned int idx, bool removeAxis = true, @@ -231,9 +226,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The acceleration of all the markers /// - std::vector markerAcceleration( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + std::vector markerAcceleration( + const GeneralizedCoordinates &Q, + const GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &dQdot, bool removeAxis=true, bool updateKin = true); @@ -245,8 +240,8 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return A vector of all the technical markers /// - std::vector technicalMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector technicalMarkers( + const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -255,7 +250,7 @@ class BIORBD_API Markers /// \param removeAxis If there are axis to remove from the position variables /// \return A vector of all the technical markers in their parent reference frame /// - std::vector technicalMarkers( + std::vector technicalMarkers( bool removeAxis=true); /// @@ -265,8 +260,8 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return A vector of all the anatomical markers /// - std::vector anatomicalMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector anatomicalMarkers( + const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -275,7 +270,7 @@ class BIORBD_API Markers /// \param removeAxis If there are axis to remove from the position variables /// \return A vector of all the anatomical markers in their parent reference frame /// - std::vector anatomicalMarkers( + std::vector anatomicalMarkers( bool removeAxis=true); /// @@ -286,8 +281,8 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return All the markers of the segment idx /// - std::vector segmentMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + std::vector segmentMarkers( + const GeneralizedCoordinates &Q, unsigned int idx, bool removeAxis=true, bool updateKin = true); @@ -333,7 +328,7 @@ class BIORBD_API Markers /// \return The jacobian of the markers /// std::vector markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -345,7 +340,7 @@ class BIORBD_API Markers /// \return The jacobian of the technical markers /// std::vector technicalMarkersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -358,9 +353,9 @@ class BIORBD_API Markers /// \return The jacobian of the chosen marker /// biorbd::utils::Matrix markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, const biorbd::utils::String& parentName, - const biorbd::rigidbody::NodeSegment& p, + const NodeSegment& p, bool updateKin); #ifndef BIORBD_USE_CASADI_MATH @@ -372,9 +367,9 @@ class BIORBD_API Markers /// \param removeAxes If the markers should be projected on the axes /// bool inverseKinematics( - const std::vector& markers, - const biorbd::rigidbody::GeneralizedCoordinates& Qinit, - biorbd::rigidbody::GeneralizedCoordinates &Q, + const std::vector& markers, + const GeneralizedCoordinates& Qinit, + GeneralizedCoordinates &Q, bool removeAxes=true); #endif @@ -388,16 +383,17 @@ class BIORBD_API Markers /// \return The jacobian of the markers /// std::vector markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool removeAxis, bool updateKin, bool lookForTechnical); // Retourne la jacobienne des markers - std::shared_ptr> + std::shared_ptr> m_marks; ///< The markers }; +} } } diff --git a/include/RigidBody/Mesh.h b/include/RigidBody/Mesh.h index 79cee561..ff882b19 100644 --- a/include/RigidBody/Mesh.h +++ b/include/RigidBody/Mesh.h @@ -13,6 +13,8 @@ class Vector3d; class Path; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class MeshFace; @@ -42,20 +44,20 @@ class BIORBD_API Mesh /// Mesh( const std::vector& vertex, - const std::vector& faces); + const std::vector& faces); /// /// \brief Deep copy of the mesh /// \return A copy of mesh /// - biorbd::rigidbody::Mesh DeepCopy() const; + Mesh DeepCopy() const; /// /// \brief Deep copy of the mesh /// \param other The mesh to copy /// void DeepCopy( - const biorbd::rigidbody::Mesh& other); + const Mesh& other); /// /// \brief Add a point to the mesh @@ -83,7 +85,7 @@ class BIORBD_API Mesh /// \param face The face patch to add /// void addFace( - const biorbd::rigidbody::MeshFace& face); + const MeshFace& face); /// /// \brief Add a face patch to the mesh @@ -95,14 +97,14 @@ class BIORBD_API Mesh /// \brief Return the faces of the mesh /// \return The faces of the mesh /// - const std::vector& faces() const; + const std::vector& faces() const; /// /// \brief Return the face of the mesh of a specified idx /// \param idx Position /// \return The face of the mesh of a specified idx /// - const biorbd::rigidbody::MeshFace& face( + const MeshFace& face( unsigned int idx) const; /// @@ -126,11 +128,12 @@ class BIORBD_API Mesh protected: std::shared_ptr> m_vertex; ///< The vertex - std::shared_ptr> + std::shared_ptr> m_faces; ///< The faces std::shared_ptr m_pathFile; ///< The path to the mesh file }; +} } } diff --git a/include/RigidBody/MeshFace.h b/include/RigidBody/MeshFace.h index f03f5019..6a4359f7 100644 --- a/include/RigidBody/MeshFace.h +++ b/include/RigidBody/MeshFace.h @@ -12,6 +12,8 @@ namespace utils class Vector3d; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { @@ -32,14 +34,14 @@ class BIORBD_API MeshFace /// \brief Deep copy of a MeshFace /// \return A deep copy of a MeshFace /// - biorbd::rigidbody::MeshFace DeepCopy() const; + MeshFace DeepCopy() const; /// /// \brief Deep copy of a MeshFace into another one /// \param other The MeshFace to copy /// void DeepCopy( - const biorbd::rigidbody::MeshFace& other); + const MeshFace& other); /// /// \brief Allows to assign/get using () @@ -58,7 +60,7 @@ class BIORBD_API MeshFace /// \param other The other MeshFace /// void setFace( - const biorbd::rigidbody::MeshFace& other); + const MeshFace& other); /// /// \brief convert the integer nature of the face to a double @@ -77,6 +79,7 @@ class BIORBD_API MeshFace }; +} } } diff --git a/include/RigidBody/NodeSegment.h b/include/RigidBody/NodeSegment.h index d6981a3d..ba8027da 100644 --- a/include/RigidBody/NodeSegment.h +++ b/include/RigidBody/NodeSegment.h @@ -12,6 +12,8 @@ namespace utils class String; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { @@ -90,13 +92,13 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \brief Deep copy of the segment node /// \return A deep copy of the segment node /// - biorbd::rigidbody::NodeSegment DeepCopy() const; + NodeSegment DeepCopy() const; /// /// \brief Deep copy of the segment node /// \param other The segment node to copy /// - void DeepCopy(const biorbd::rigidbody::NodeSegment& other); + void DeepCopy(const NodeSegment& other); // Get and Set @@ -179,7 +181,7 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \param other The other vector /// template - biorbd::rigidbody::NodeSegment & operator=(const Eigen::MatrixBase + NodeSegment & operator=(const Eigen::MatrixBase & other) { this->biorbd::utils::Vector3d::operator=(other); @@ -203,6 +205,7 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d }; +} } } diff --git a/include/RigidBody/RotoTransNodes.h b/include/RigidBody/RotoTransNodes.h index e22e1d3a..8b9af1c6 100644 --- a/include/RigidBody/RotoTransNodes.h +++ b/include/RigidBody/RotoTransNodes.h @@ -15,6 +15,8 @@ class Matrix; class RotoTransNode; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -35,7 +37,7 @@ class BIORBD_API RotoTransNodes /// \param other The other RTs set /// RotoTransNodes( - const biorbd::rigidbody::RotoTransNodes& other); + const RotoTransNodes& other); /// /// \brief Destroy the class properly @@ -46,14 +48,14 @@ class BIORBD_API RotoTransNodes /// \brief Deep copy of the RTs data /// \return A copy of the RTs data /// - biorbd::rigidbody::RotoTransNodes DeepCopy() const; + RotoTransNodes DeepCopy() const; /// /// \brief Deep copy the RTs data /// \param other The RT data to copy /// void DeepCopy( - const biorbd::rigidbody::RotoTransNodes& other); + const RotoTransNodes& other); /// /// \brief Add a new RT to the set @@ -114,7 +116,7 @@ class BIORBD_API RotoTransNodes /// \return All the RTs at the position given by Q /// std::vector RTs( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const GeneralizedCoordinates& Q, bool updateKin = true); /// @@ -125,7 +127,7 @@ class BIORBD_API RotoTransNodes /// \return The RT of index idx at the position given by Q /// biorbd::utils::RotoTransNode RT( - const biorbd::rigidbody::GeneralizedCoordinates&Q, + const GeneralizedCoordinates&Q, unsigned int idx, bool updateKin = true); @@ -137,7 +139,7 @@ class BIORBD_API RotoTransNodes /// \return All the RTs on the segment of index idx /// std::vector segmentRTs( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const GeneralizedCoordinates& Q, unsigned int idx, bool updateKin = true); @@ -148,7 +150,7 @@ class BIORBD_API RotoTransNodes /// \return The jacobien of the RTs /// std::vector RTsJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const GeneralizedCoordinates &Q, bool updateKin = true); protected: @@ -157,6 +159,7 @@ class BIORBD_API RotoTransNodes }; +} } } diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index fb20b667..202fe8dc 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -15,15 +15,11 @@ class RotoTrans; class Range; } -namespace BIORBD_MATH_NAMESPACE { -namespace rigidbody +namespace BIORBD_MATH_NAMESPACE { -class Joints; -} -} - namespace rigidbody { +class Joints; class SegmentCharacteristics; /// @@ -60,7 +56,7 @@ class BIORBD_API Segment : public biorbd::utils::Node const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF = -1); @@ -85,7 +81,7 @@ class BIORBD_API Segment : public biorbd::utils::Node const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF = -1); @@ -93,14 +89,14 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \brief Create a deep copy of Segment /// \return Copy of Segment /// - biorbd::rigidbody::Segment DeepCopy() const; + Segment DeepCopy() const; /// /// \brief Deep copy of Segment /// \param other The Segment to copy /// void DeepCopy( - const biorbd::rigidbody::Segment& other); + const Segment& other); /// /// \brief Destroy the class properly @@ -231,13 +227,13 @@ class BIORBD_API Segment : public biorbd::utils::Node /// void updateCharacteristics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::SegmentCharacteristics& characteristics); + const SegmentCharacteristics& characteristics); /// /// \brief Return the segment characteristics /// \return The segment characteristics /// - const biorbd::rigidbody::SegmentCharacteristics& characteristics() const; + const SegmentCharacteristics& characteristics() const; /// /// \brief Return if the rotation DoF of this segment is a quaternion @@ -392,14 +388,15 @@ class BIORBD_API Segment : public biorbd::utils::Node /// void setDofCharacteristicsOnLastBody(); - std::shared_ptr - m_characteristics;///< Non-used virtual segment; it allows to "save" the data and to avoid the use of multiple intermediate variables - std::shared_ptr> + std::shared_ptr + m_characteristics;///< Non-used virtual segment; it allows to "save" the data and to avoid the use of multiple intermediate variables + std::shared_ptr> m_dofCharacteristics; ///< Variable containing the inertial data and other from each segment (on a 6DoF segment, 0 to 4 should be empty and 5 filled) }; +} } } diff --git a/include/RigidBody/SegmentCharacteristics.h b/include/RigidBody/SegmentCharacteristics.h index 967d0d0c..4a13fea1 100644 --- a/include/RigidBody/SegmentCharacteristics.h +++ b/include/RigidBody/SegmentCharacteristics.h @@ -14,6 +14,8 @@ namespace utils class Vector3d; } +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class Mesh; @@ -56,19 +58,19 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body const biorbd::utils::Scalar &mass, const biorbd::utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia, - const biorbd::rigidbody::Mesh &mesh); + const Mesh &mesh); /// /// \brief Deep copy of the segment characteristics /// \return Copy of the segment characteristics /// - biorbd::rigidbody::SegmentCharacteristics DeepCopy() const; + SegmentCharacteristics DeepCopy() const; /// /// \brief Copy the segment characteristics /// \param other The characteristics to copy /// - void DeepCopy(const biorbd::rigidbody::SegmentCharacteristics& other); + void DeepCopy(const SegmentCharacteristics& other); /// /// \brief Set the segment length @@ -111,7 +113,7 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body /// \brief Returns the segment mesh /// \return The segment mesh /// - const biorbd::rigidbody::Mesh& mesh() const; + const Mesh& mesh() const; /// /// \brief Returns the segment inertia matrix @@ -121,9 +123,10 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body protected: std::shared_ptr m_length; ///< Length of the segment - std::shared_ptr m_mesh; ///< Mesh of the segment + std::shared_ptr m_mesh; ///< Mesh of the segment }; +} } } diff --git a/include/Utils/Rotation.h b/include/Utils/Rotation.h index 6b1aa243..fadb84ea 100644 --- a/include/Utils/Rotation.h +++ b/include/Utils/Rotation.h @@ -18,10 +18,12 @@ struct SpatialTransform; namespace biorbd { +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class NodeSegment; } +} namespace utils { @@ -144,9 +146,9 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \return The system of axes /// static biorbd::utils::Matrix fromMarkersNonNormalized( - const std::pair& + const std::pair& axis1markers, - const std::pair& + const std::pair& axis2markers, const std::pair &axesNames, const biorbd::utils::String& axisToRecalculate); @@ -160,9 +162,9 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \return The system of axes /// static biorbd::utils::Rotation fromMarkers( - const std::pair& + const std::pair& axis1markers, - const std::pair& + const std::pair& axis2markers, const std::pair &axesNames, const biorbd::utils::String& axisToRecalculate); diff --git a/include/Utils/RotoTrans.h b/include/Utils/RotoTrans.h index 8485bc66..aaa468c2 100644 --- a/include/Utils/RotoTrans.h +++ b/include/Utils/RotoTrans.h @@ -18,10 +18,12 @@ struct SpatialTransform; namespace biorbd { +namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class NodeSegment; } +} namespace utils { @@ -143,10 +145,10 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \return The system of axes /// static biorbd::utils::RotoTrans fromMarkers( - const biorbd::rigidbody::NodeSegment& origin, - const std::pair& + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment& origin, + const std::pair& axis1markers, - const std::pair& + const std::pair& axis2markers, const std::pair &axesNames, const biorbd::utils::String& axisToRecalculate); diff --git a/include/biorbdConfig.h.in b/include/biorbdConfig.h.in index 66fb18ce..6e5bedd8 100644 --- a/include/biorbdConfig.h.in +++ b/include/biorbdConfig.h.in @@ -77,16 +77,16 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ #ifdef BIORBD_USE_CASADI_MATH #define DECLARE_GENERALIZED_COORDINATES(varname, model) \ casadi::DM varname(model.nbQ(), 1);\ - biorbd::rigidbody::GeneralizedCoordinates varname##_sym(casadi::MX::sym(#varname, model.nbQ(), 1)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates varname##_sym(casadi::MX::sym(#varname, model.nbQ(), 1)); #define DECLARE_GENERALIZED_VELOCITY(varname, model) \ casadi::DM varname(model.nbQdot(), 1);\ - biorbd::rigidbody::GeneralizedVelocity varname##_sym(casadi::MX::sym(#varname, model.nbQdot(), 1)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity varname##_sym(casadi::MX::sym(#varname, model.nbQdot(), 1)); #define DECLARE_GENERALIZED_ACCELERATION(varname, model) \ casadi::DM varname(model.nbQddot(), 1);\ - biorbd::rigidbody::GeneralizedAcceleration varname##_sym(casadi::MX::sym(#varname, model.nbQddot(), 1)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration varname##_sym(casadi::MX::sym(#varname, model.nbQddot(), 1)); #define DECLARE_GENERALIZED_TORQUE(varname, model) \ casadi::DM varname(model.nbGeneralizedTorque(), 1);\ - biorbd::rigidbody::GeneralizedTorque varname##_sym(casadi::MX::sym(#varname, model.nbGeneralizedTorque(), 1)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname##_sym(casadi::MX::sym(#varname, model.nbGeneralizedTorque(), 1)); #define DECLARE_VECTOR(varname, nbElements) \ casadi::DM varname(nbElements, 1);\ biorbd::utils::Vector varname##_sym(casadi::MX::sym(#varname, nbElements, 1)); @@ -115,13 +115,13 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ #else #define DECLARE_GENERALIZED_COORDINATES(varname, model) \ - biorbd::rigidbody::GeneralizedCoordinates varname(model); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates varname(model); #define DECLARE_GENERALIZED_VELOCITY(varname, model) \ - biorbd::rigidbody::GeneralizedVelocity varname(model); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity varname(model); #define DECLARE_GENERALIZED_ACCELERATION(varname, model) \ - biorbd::rigidbody::GeneralizedAcceleration varname(model); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration varname(model); #define DECLARE_GENERALIZED_TORQUE(varname, model) \ - biorbd::rigidbody::GeneralizedTorque varname(model); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname(model); #define DECLARE_VECTOR(varname, nbElements) \ biorbd::utils::Vector varname(nbElements); diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp index 9e425414..cc65044e 100644 --- a/src/ModelReader.cpp +++ b/src/ModelReader.cpp @@ -113,7 +113,7 @@ void biorbd::Reader::readModelFile( RigidBodyDynamics::Math::Matrix3d::Identity()); biorbd::utils::RotoTrans RT(RigidBodyDynamics::Math::Matrix4d::Identity()); biorbd::utils::Vector3d com(0,0,0); - biorbd::rigidbody::Mesh mesh; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; int segmentByFile(-1); // -1 non setté, 0 pas par file, 1 par file int PF = -1; std::vector QRanges; @@ -229,7 +229,7 @@ void biorbd::Reader::readModelFile( } else if (segmentByFile == 1) { biorbd::utils::Error::raise("You must not mix file and mesh in segment"); } - biorbd::rigidbody::MeshFace tp; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace tp; for (unsigned int i=0; i<3; ++i) { file.read(tp(i)); } @@ -305,7 +305,7 @@ void biorbd::Reader::readModelFile( biorbd::utils::Range (-M_PI*100, M_PI*100)); } } - biorbd::rigidbody::SegmentCharacteristics characteristics(mass,com,inertia, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::SegmentCharacteristics characteristics(mass,com,inertia, mesh); model->AddSegment(name, parent_str, trans, rot, QRanges, QDotRanges, QDDotRanges, characteristics, RT, PF); @@ -430,9 +430,9 @@ void biorbd::Reader::readModelFile( } } if (fromMarkers) { - std::vector allMarkerOnSegment(model->marker( + std::vector allMarkerOnSegment(model->marker( parent_str)); - biorbd::rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End; bool isOrigin(false), isAxis1Beg(false), isAxis1End(false), isAxis2Beg(false), isAxis2End(false); for (auto mark : allMarkerOnSegment) { @@ -1122,7 +1122,7 @@ std::vector> return markers; } -std::vector +std::vector biorbd::Reader::readQDataFile( const utils::Path &path) { @@ -1153,7 +1153,7 @@ biorbd::Reader::readQDataFile( file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector kinematics; + std::vector kinematics; // Scroll down until the definition of a marker for (unsigned int j=0; j> return data; } -biorbd::rigidbody::Mesh +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileBiorbdSegments( const biorbd::utils::Path &path) { @@ -1675,7 +1675,7 @@ biorbd::Reader::readMeshFileBiorbdSegments( file.readSpecificTag("nfaces", tp); unsigned int nFaces(static_cast(atoi(tp.c_str()))); - biorbd::rigidbody::Mesh mesh; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; mesh.setPath(path); // Get all the points std::map variable = @@ -1691,7 +1691,7 @@ biorbd::Reader::readMeshFileBiorbdSegments( } for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints) { - biorbd::rigidbody::MeshFace patchTp; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace patchTp; unsigned int nVertices; file.read(nVertices); if (nVertices != 3) { @@ -1706,7 +1706,7 @@ biorbd::Reader::readMeshFileBiorbdSegments( } -biorbd::rigidbody::Mesh biorbd::Reader::readMeshFilePly( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFilePly( const biorbd::utils::Path &path) { // Read a bone file @@ -1741,7 +1741,7 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFilePly( // Trouver le nombre de ?? file.reachSpecificTag("end_header"); - biorbd::rigidbody::Mesh mesh; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; mesh.setPath(path); std::map variable = std::map(); @@ -1758,7 +1758,7 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFilePly( } for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints) { - biorbd::rigidbody::MeshFace patchTp; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace patchTp; unsigned int nVertices; file.read(nVertices); if (nVertices != 3) { @@ -1777,7 +1777,7 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFilePly( return mesh; } -biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileObj( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileObj( const biorbd::utils::Path &path) { // Read a bone file @@ -1796,12 +1796,12 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileObj( // Read file biorbd::utils::String tp; - biorbd::rigidbody::Mesh mesh; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; mesh.setPath(path); // Get all the points biorbd::utils::Vector3d vertex; - biorbd::rigidbody::MeshFace patch; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace patch; biorbd::utils::String text; std::map variable = std::map(); @@ -1841,7 +1841,7 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileObj( #ifdef MODULE_VTP_FILES_READER #include "tinyxml.h" -biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( const biorbd::utils::Path &path) { // Read an opensim formatted mesh file @@ -1857,7 +1857,7 @@ biorbd::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( TiXmlDocument doc(filepath); biorbd::utils::Error::check(doc.LoadFile(), "Failed to load file " + filepath); TiXmlHandle hDoc(&doc); - biorbd::rigidbody::Mesh mesh; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; mesh.setPath(path); // Navigate up to VTKFile/PolyData/Piece diff --git a/src/RigidBody/Contacts.cpp b/src/RigidBody/Contacts.cpp index b5318d66..51c2a8e6 100644 --- a/src/RigidBody/Contacts.cpp +++ b/src/RigidBody/Contacts.cpp @@ -11,7 +11,9 @@ #include "Utils/SpatialVector.h" #include "RigidBody/Joints.h" -biorbd::rigidbody::Contacts::Contacts() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::Contacts::Contacts() : RigidBodyDynamics::ConstraintSet (), m_nbreConstraint(std::make_shared(0)), m_isBinded(std::make_shared(false)) @@ -19,14 +21,14 @@ biorbd::rigidbody::Contacts::Contacts() : } -biorbd::rigidbody::Contacts biorbd::rigidbody::Contacts::DeepCopy() const +rigidbody::Contacts rigidbody::Contacts::DeepCopy() const { - biorbd::rigidbody::Contacts copy; + rigidbody::Contacts copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::Contacts::DeepCopy(const biorbd::rigidbody::Contacts +void rigidbody::Contacts::DeepCopy(const rigidbody::Contacts &other) { static_cast(*this) = other; @@ -35,7 +37,7 @@ void biorbd::rigidbody::Contacts::DeepCopy(const biorbd::rigidbody::Contacts } -unsigned int biorbd::rigidbody::Contacts::AddConstraint( +unsigned int rigidbody::Contacts::AddConstraint( unsigned int body_id, const biorbd::utils::Vector3d& body_point, const biorbd::utils::Vector3d& world_normal, @@ -46,7 +48,7 @@ unsigned int biorbd::rigidbody::Contacts::AddConstraint( return RigidBodyDynamics::ConstraintSet::AddContactConstraint(body_id, body_point, world_normal, name.c_str(), acc); } -unsigned int biorbd::rigidbody::Contacts::AddConstraint( +unsigned int rigidbody::Contacts::AddConstraint( unsigned int body_id, const biorbd::utils::Vector3d& body_point, const biorbd::utils::String& axis, @@ -75,7 +77,7 @@ unsigned int biorbd::rigidbody::Contacts::AddConstraint( return ret; } -unsigned int biorbd::rigidbody::Contacts::AddLoopConstraint( +unsigned int rigidbody::Contacts::AddLoopConstraint( unsigned int body_id_predecessor, unsigned int body_id_successor, const biorbd::utils::RotoTrans &X_predecessor, @@ -96,24 +98,24 @@ unsigned int biorbd::rigidbody::Contacts::AddLoopConstraint( enableStabilization, stabilizationParam, name.c_str()); } -biorbd::rigidbody::Contacts::~Contacts() +rigidbody::Contacts::~Contacts() { } -biorbd::rigidbody::Contacts &biorbd::rigidbody::Contacts::getConstraints() +rigidbody::Contacts &rigidbody::Contacts::getConstraints() { if (!*m_isBinded) { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = + dynamic_cast(*this); Bind(model); *m_isBinded = true; } return *this; } -bool biorbd::rigidbody::Contacts::hasContacts() const +bool rigidbody::Contacts::hasContacts() const { if (*m_nbreConstraint>0) return true; @@ -122,12 +124,12 @@ bool biorbd::rigidbody::Contacts::hasContacts() const } } -unsigned int biorbd::rigidbody::Contacts::nbContacts() const +unsigned int rigidbody::Contacts::nbContacts() const { return *m_nbreConstraint; } -std::vector biorbd::rigidbody::Contacts::contactNames() +std::vector rigidbody::Contacts::contactNames() { std::vector names; for (auto name : RigidBodyDynamics::ConstraintSet::name) { @@ -136,7 +138,7 @@ std::vector biorbd::rigidbody::Contacts::contactNames() return names; } -biorbd::utils::String biorbd::rigidbody::Contacts::contactName(unsigned int i) +biorbd::utils::String rigidbody::Contacts::contactName(unsigned int i) { biorbd::utils::Error::check(i<*m_nbreConstraint, "Idx for contact names is too high.."); @@ -145,12 +147,12 @@ biorbd::utils::String biorbd::rigidbody::Contacts::contactName(unsigned int i) std::vector -biorbd::rigidbody::Contacts::constraintsInGlobal( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::Contacts::constraintsInGlobal( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -175,7 +177,7 @@ biorbd::rigidbody::Contacts::constraintsInGlobal( return tp; } -biorbd::utils::Vector biorbd::rigidbody::Contacts::getForce() const +biorbd::utils::Vector rigidbody::Contacts::getForce() const { return static_cast(this->force); } diff --git a/src/RigidBody/GeneralizedCoordinates.cpp b/src/RigidBody/GeneralizedCoordinates.cpp index d19970f1..550e76f1 100644 --- a/src/RigidBody/GeneralizedCoordinates.cpp +++ b/src/RigidBody/GeneralizedCoordinates.cpp @@ -3,33 +3,35 @@ #include "RigidBody/Joints.h" -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates() { } -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( unsigned int nbQ) : biorbd::utils::Vector(nbQ) { } -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : biorbd::utils::Vector(j.nbQ()) { } -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( - const biorbd::rigidbody::GeneralizedCoordinates &Q) : +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( + const rigidbody::GeneralizedCoordinates &Q) : biorbd::utils::Vector(Q) { } -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const RigidBodyDynamics::Math::VectorNd &v) : biorbd::utils::Vector (v) { @@ -38,7 +40,7 @@ biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( #ifdef BIORBD_USE_CASADI_MATH -biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( +rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const casadi::MX &v) : biorbd::utils::Vector(v) { @@ -47,12 +49,12 @@ biorbd::rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( #endif -biorbd::rigidbody::GeneralizedCoordinates::~GeneralizedCoordinates() +rigidbody::GeneralizedCoordinates::~GeneralizedCoordinates() { } -void biorbd::rigidbody::GeneralizedCoordinates::operator=( +void rigidbody::GeneralizedCoordinates::operator=( const biorbd::utils::Vector &other) { this->biorbd::utils::Vector::operator=(other); @@ -60,13 +62,13 @@ void biorbd::rigidbody::GeneralizedCoordinates::operator=( #ifdef BIORBD_USE_CASADI_MATH -void biorbd::rigidbody::GeneralizedCoordinates::operator=( +void rigidbody::GeneralizedCoordinates::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { this->biorbd::utils::Vector::operator=(other); } -void biorbd::rigidbody::GeneralizedCoordinates::operator=( +void rigidbody::GeneralizedCoordinates::operator=( const casadi::MX &other) { this->biorbd::utils::Vector::operator=(other); diff --git a/src/RigidBody/GeneralizedTorque.cpp b/src/RigidBody/GeneralizedTorque.cpp index 3deee4df..b8dc3983 100644 --- a/src/RigidBody/GeneralizedTorque.cpp +++ b/src/RigidBody/GeneralizedTorque.cpp @@ -3,33 +3,35 @@ #include "RigidBody/Joints.h" -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::GeneralizedTorque::GeneralizedTorque() { } -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( +rigidbody::GeneralizedTorque::GeneralizedTorque( unsigned int nTorque) : biorbd::utils::Vector(nTorque) { } -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : +rigidbody::GeneralizedTorque::GeneralizedTorque( + const rigidbody::Joints &j) : biorbd::utils::Vector(j.nbGeneralizedTorque()) { } -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( - const biorbd::rigidbody::GeneralizedTorque &other) : +rigidbody::GeneralizedTorque::GeneralizedTorque( + const rigidbody::GeneralizedTorque &other) : biorbd::utils::Vector(other) { } -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( +rigidbody::GeneralizedTorque::GeneralizedTorque( const RigidBodyDynamics::Math::VectorNd &v) : biorbd::utils::Vector (v) { @@ -38,7 +40,7 @@ biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( #ifdef BIORBD_USE_CASADI_MATH -biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( +rigidbody::GeneralizedTorque::GeneralizedTorque( const casadi::MX &v) : biorbd::utils::Vector(v) { @@ -47,7 +49,7 @@ biorbd::rigidbody::GeneralizedTorque::GeneralizedTorque( #endif -void biorbd::rigidbody::GeneralizedTorque::operator=( +void rigidbody::GeneralizedTorque::operator=( const biorbd::utils::Vector &other) { this->biorbd::utils::Vector::operator=(other); @@ -55,13 +57,13 @@ void biorbd::rigidbody::GeneralizedTorque::operator=( #ifdef BIORBD_USE_CASADI_MATH -void biorbd::rigidbody::GeneralizedTorque::operator=( +void rigidbody::GeneralizedTorque::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { this->biorbd::utils::Vector::operator=(other); } -void biorbd::rigidbody::GeneralizedTorque::operator=( +void rigidbody::GeneralizedTorque::operator=( const casadi::MX &other) { this->biorbd::utils::Vector::operator=(other); diff --git a/src/RigidBody/GeneralizedVelocity.cpp b/src/RigidBody/GeneralizedVelocity.cpp index 72c30fcc..aa9b1f8f 100644 --- a/src/RigidBody/GeneralizedVelocity.cpp +++ b/src/RigidBody/GeneralizedVelocity.cpp @@ -3,33 +3,35 @@ #include "RigidBody/Joints.h" -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::GeneralizedVelocity::GeneralizedVelocity() { } -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( +rigidbody::GeneralizedVelocity::GeneralizedVelocity( unsigned int nbQdot) : biorbd::utils::Vector(nbQdot) { } -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : +rigidbody::GeneralizedVelocity::GeneralizedVelocity( + const rigidbody::Joints &j) : biorbd::utils::Vector(j.nbQdot()) { } -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( - const biorbd::rigidbody::GeneralizedVelocity &Q) : +rigidbody::GeneralizedVelocity::GeneralizedVelocity( + const rigidbody::GeneralizedVelocity &Q) : biorbd::utils::Vector(Q) { } -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( +rigidbody::GeneralizedVelocity::GeneralizedVelocity( const RigidBodyDynamics::Math::VectorNd &v) : biorbd::utils::Vector (v) { @@ -38,7 +40,7 @@ biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( #ifdef BIORBD_USE_CASADI_MATH -biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( +rigidbody::GeneralizedVelocity::GeneralizedVelocity( const casadi::MX &v) : biorbd::utils::Vector(v) { @@ -48,12 +50,12 @@ biorbd::rigidbody::GeneralizedVelocity::GeneralizedVelocity( #endif -biorbd::rigidbody::GeneralizedVelocity::~GeneralizedVelocity() +rigidbody::GeneralizedVelocity::~GeneralizedVelocity() { } -void biorbd::rigidbody::GeneralizedVelocity::operator=( +void rigidbody::GeneralizedVelocity::operator=( const biorbd::utils::Vector &other) { this->biorbd::utils::Vector::operator=(other); @@ -61,13 +63,13 @@ void biorbd::rigidbody::GeneralizedVelocity::operator=( #ifdef BIORBD_USE_CASADI_MATH -void biorbd::rigidbody::GeneralizedVelocity::operator=( +void rigidbody::GeneralizedVelocity::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { this->biorbd::utils::Vector::operator=(other); } -void biorbd::rigidbody::GeneralizedVelocity::operator=( +void rigidbody::GeneralizedVelocity::operator=( const casadi::MX &other) { this->biorbd::utils::Vector::operator=(other); diff --git a/src/RigidBody/IMU.cpp b/src/RigidBody/IMU.cpp index 1eedc07f..665ac641 100644 --- a/src/RigidBody/IMU.cpp +++ b/src/RigidBody/IMU.cpp @@ -3,7 +3,9 @@ #include "Utils/String.h" -biorbd::rigidbody::IMU::IMU( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::IMU::IMU( bool isTechnical, bool isAnatomical) : biorbd::utils::RotoTransNode(), @@ -13,7 +15,7 @@ biorbd::rigidbody::IMU::IMU( } -biorbd::rigidbody::IMU::IMU( +rigidbody::IMU::IMU( const biorbd::utils::RotoTransNode &RotoTrans, bool isTechnical, bool isAnatomical) : @@ -26,8 +28,8 @@ biorbd::rigidbody::IMU::IMU( #ifdef BIORBD_USE_CASADI_MATH -biorbd::rigidbody::IMU::IMU( - const biorbd::rigidbody::IMU &imu) : +rigidbody::IMU::IMU( + const rigidbody::IMU &imu) : biorbd::utils::RotoTransNode (imu), m_technical(std::make_shared(*imu.m_technical)), m_anatomical(std::make_shared(*imu.m_anatomical)) @@ -35,10 +37,10 @@ biorbd::rigidbody::IMU::IMU( } -biorbd::rigidbody::IMU biorbd::rigidbody::IMU::operator*( - const biorbd::rigidbody::IMU &other) const +rigidbody::IMU rigidbody::IMU::operator*( + const rigidbody::IMU &other) const { - return biorbd::rigidbody::IMU( + return rigidbody::IMU( biorbd::utils::RotoTransNode( this->biorbd::utils::RotoTransNode::operator*(other), this->biorbd::utils::Node::name(), @@ -49,26 +51,26 @@ biorbd::rigidbody::IMU biorbd::rigidbody::IMU::operator*( #endif -biorbd::rigidbody::IMU biorbd::rigidbody::IMU::DeepCopy() const +rigidbody::IMU rigidbody::IMU::DeepCopy() const { - biorbd::rigidbody::IMU copy; + rigidbody::IMU copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::IMU::DeepCopy(const IMU &other) +void rigidbody::IMU::DeepCopy(const IMU &other) { biorbd::utils::RotoTransNode::DeepCopy(other); *m_technical = *other.m_technical; *m_anatomical = *other.m_anatomical; } -bool biorbd::rigidbody::IMU::isAnatomical() const +bool rigidbody::IMU::isAnatomical() const { return *m_anatomical; } -bool biorbd::rigidbody::IMU::isTechnical() const +bool rigidbody::IMU::isTechnical() const { return *m_technical; } diff --git a/src/RigidBody/IMUs.cpp b/src/RigidBody/IMUs.cpp index 02f14b20..d5988ccd 100644 --- a/src/RigidBody/IMUs.cpp +++ b/src/RigidBody/IMUs.cpp @@ -11,30 +11,32 @@ #include "RigidBody/Segment.h" #include "RigidBody/IMU.h" -biorbd::rigidbody::IMUs::IMUs() : - m_IMUs(std::make_shared>()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::IMUs::IMUs() : + m_IMUs(std::make_shared>()) { //ctor } -biorbd::rigidbody::IMUs::IMUs(const biorbd::rigidbody::IMUs &other) +rigidbody::IMUs::IMUs(const rigidbody::IMUs &other) { m_IMUs = other.m_IMUs; } -biorbd::rigidbody::IMUs::~IMUs() +rigidbody::IMUs::~IMUs() { } -biorbd::rigidbody::IMUs biorbd::rigidbody::IMUs::DeepCopy() const +rigidbody::IMUs rigidbody::IMUs::DeepCopy() const { - biorbd::rigidbody::IMUs copy; + rigidbody::IMUs copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::IMUs::DeepCopy(const biorbd::rigidbody::IMUs &other) +void rigidbody::IMUs::DeepCopy(const rigidbody::IMUs &other) { m_IMUs->resize(other.m_IMUs->size()); for (unsigned int i=0; isize(); ++i) { @@ -42,38 +44,38 @@ void biorbd::rigidbody::IMUs::DeepCopy(const biorbd::rigidbody::IMUs &other) } } -void biorbd::rigidbody::IMUs::addIMU( +void rigidbody::IMUs::addIMU( bool technical, bool anatomical) { - m_IMUs->push_back(biorbd::rigidbody::IMU(technical, anatomical)); + m_IMUs->push_back(rigidbody::IMU(technical, anatomical)); } // Add a new marker to the existing pool of markers -void biorbd::rigidbody::IMUs::addIMU( +void rigidbody::IMUs::addIMU( const biorbd::utils::RotoTransNode &RotoTrans, bool technical, bool anatomical) { - m_IMUs->push_back(biorbd::rigidbody::IMU(RotoTrans, technical, anatomical)); + m_IMUs->push_back(rigidbody::IMU(RotoTrans, technical, anatomical)); } -unsigned int biorbd::rigidbody::IMUs::nbIMUs() const +unsigned int rigidbody::IMUs::nbIMUs() const { return static_cast(m_IMUs->size()); } // Get the markers in the global reference -const std::vector& biorbd::rigidbody::IMUs::IMU() const +const std::vector& rigidbody::IMUs::IMU() const { return *m_IMUs; } -std::vector biorbd::rigidbody::IMUs::IMU( +std::vector rigidbody::IMUs::IMU( const biorbd::utils::String& segmentName) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::IMU( return pos; } -const biorbd::rigidbody::IMU& biorbd::rigidbody::IMUs::IMU( +const rigidbody::IMU& rigidbody::IMUs::IMU( unsigned int idx) { return (*m_IMUs)[idx]; } // Get the IMUs at the position given by Q -std::vector biorbd::rigidbody::IMUs::IMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::IMU( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::IMU( } // Get an IMU at the position given by Q -biorbd::rigidbody::IMU biorbd::rigidbody::IMUs::IMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::IMU rigidbody::IMUs::IMU( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin) { @@ -118,7 +120,7 @@ biorbd::rigidbody::IMU biorbd::rigidbody::IMUs::IMU( model.UpdateKinematicsCustom (&Q); } - biorbd::rigidbody::IMU node = IMU(idx); + rigidbody::IMU node = IMU(idx); unsigned int id = static_cast(model.GetBodyBiorbdId( node.parent())); @@ -126,11 +128,11 @@ biorbd::rigidbody::IMU biorbd::rigidbody::IMUs::IMU( } // Get the technical IMUs -std::vector biorbd::rigidbody::IMUs::technicalIMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::technicalIMU( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::technicalIMU( return pos; } // Get the technical IMUs in the local reference -std::vector biorbd::rigidbody::IMUs::technicalIMU() +std::vector rigidbody::IMUs::technicalIMU() { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::technicalIMU() } // Get all the anatomical IMUs -std::vector biorbd::rigidbody::IMUs::anatomicalIMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::anatomicalIMU( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::anatomicalIMU( return pos; } // Get the anatomical IMUs in the local reference -std::vector biorbd::rigidbody::IMUs::anatomicalIMU() +std::vector rigidbody::IMUs::anatomicalIMU() { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::anatomicalIMU() return pos; } -std::vector biorbd::rigidbody::IMUs::segmentIMU( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::segmentIMU( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin) { @@ -185,7 +187,7 @@ std::vector biorbd::rigidbody::IMUs::segmentIMU( // Segment name to find biorbd::utils::String name(model.segment(idx).name()); - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::IMUs::segmentIMU( } // Get the Jacobian of the markers -std::vector biorbd::rigidbody::IMUs::IMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::IMUJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { return IMUJacobian(Q, updateKin, false); @@ -206,8 +208,8 @@ std::vector biorbd::rigidbody::IMUs::IMUJacobian( // Get the Jacobian of the technical markers std::vector -biorbd::rigidbody::IMUs::TechnicalIMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::IMUs::TechnicalIMUJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { return IMUJacobian(Q, updateKin, true); @@ -215,8 +217,8 @@ biorbd::rigidbody::IMUs::TechnicalIMUJacobian( // Protected function -std::vector biorbd::rigidbody::IMUs::IMUJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::IMUs::IMUJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin, bool lookForTechnical) { @@ -228,7 +230,7 @@ std::vector biorbd::rigidbody::IMUs::IMUJacobian( for (unsigned int idx=0; idx biorbd::rigidbody::IMUs::IMUJacobian( return G; } -unsigned int biorbd::rigidbody::IMUs::nbTechIMUs() +unsigned int rigidbody::IMUs::nbTechIMUs() { unsigned int nbTech = 0; if (nbTech == 0) // If the function has never been called before - for (biorbd::rigidbody::IMU imu : *m_IMUs) + for (rigidbody::IMU imu : *m_IMUs) if (imu.isTechnical()) { ++nbTech; } @@ -257,11 +259,11 @@ unsigned int biorbd::rigidbody::IMUs::nbTechIMUs() return nbTech; } -unsigned int biorbd::rigidbody::IMUs::nbAnatIMUs() +unsigned int rigidbody::IMUs::nbAnatIMUs() { unsigned int nbAnat = 0; if (nbAnat == 0) // If the function has never been called before - for (biorbd::rigidbody::IMU imu : *m_IMUs) + for (rigidbody::IMU imu : *m_IMUs) if (imu.isAnatomical()) { ++nbAnat; } @@ -269,7 +271,7 @@ unsigned int biorbd::rigidbody::IMUs::nbAnatIMUs() return nbAnat; } -std::vector biorbd::rigidbody::IMUs::IMUsNames() +std::vector rigidbody::IMUs::IMUsNames() { // Extract the name of all the markers of a model std::vector names; @@ -279,7 +281,7 @@ std::vector biorbd::rigidbody::IMUs::IMUsNames() return names; } -std::vector biorbd::rigidbody::IMUs::technicalIMUsNames() +std::vector rigidbody::IMUs::technicalIMUsNames() { // Extract the names of all the technical markers of a model std::vector names; @@ -292,7 +294,7 @@ std::vector biorbd::rigidbody::IMUs::technicalIMUsNames() } std::vector -biorbd::rigidbody::IMUs::anatomicalIMUsNames() +rigidbody::IMUs::anatomicalIMUsNames() { // Extract the names of all the anatomical markers of a model std::vector names; diff --git a/src/RigidBody/Joints.cpp b/src/RigidBody/Joints.cpp index 312f9162..8f39fc2f 100644 --- a/src/RigidBody/Joints.cpp +++ b/src/RigidBody/Joints.cpp @@ -28,7 +28,7 @@ using namespace biorbd::BIORBD_MATH_NAMESPACE; rigidbody::Joints::Joints() : RigidBodyDynamics::Model(), - m_segments(std::make_shared>()), + m_segments(std::make_shared>()), m_nbRoot(std::make_shared(0)), m_nbDof(std::make_shared(0)), m_nbQ(std::make_shared(0)), @@ -142,11 +142,11 @@ unsigned int rigidbody::Joints::AddSegment( const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const rigidbody::SegmentCharacteristics& characteristics, const biorbd::utils::RotoTrans& referenceFrame, int forcePlates) { - biorbd::rigidbody::Segment tp( + rigidbody::Segment tp( *this, segmentName, parentName, translationSequence, rotationSequence, QRanges, QDotRanges, QDDotRanges, characteristics, RigidBodyDynamics::Math::SpatialTransform(referenceFrame.rot().transpose(), @@ -178,11 +178,11 @@ unsigned int rigidbody::Joints::AddSegment( const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const rigidbody::SegmentCharacteristics& characteristics, const biorbd::utils::RotoTrans& referenceFrame, int forcePlates) { - biorbd::rigidbody::Segment tp( + rigidbody::Segment tp( *this, segmentName, parentName, seqR, QRanges, QDotRanges, QDDotRanges, characteristics, RigidBodyDynamics::Math::SpatialTransform( referenceFrame.rot().transpose(), referenceFrame.trans()), @@ -213,14 +213,14 @@ void rigidbody::Joints::setGravity( void rigidbody::Joints::updateSegmentCharacteristics( unsigned int idx, - const biorbd::rigidbody::SegmentCharacteristics& characteristics) + const rigidbody::SegmentCharacteristics& characteristics) { biorbd::utils::Error::check(idx < m_segments->size(), "Asked for a wrong segment (out of range)"); (*m_segments)[idx].updateCharacteristics(*this, characteristics); } -const biorbd::rigidbody::Segment& rigidbody::Joints::segment( +const rigidbody::Segment& rigidbody::Joints::segment( unsigned int idx) const { biorbd::utils::Error::check(idx < m_segments->size(), @@ -228,7 +228,7 @@ const biorbd::rigidbody::Segment& rigidbody::Joints::segment( return (*m_segments)[idx]; } -const biorbd::rigidbody::Segment &rigidbody::Joints::segment( +const rigidbody::Segment &rigidbody::Joints::segment( const biorbd::utils::String & name) const { return segment(static_cast(GetBodyBiorbdId(name.c_str()))); @@ -301,7 +301,7 @@ int rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String } std::vector rigidbody::Joints::allGlobalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q) + const rigidbody::GeneralizedCoordinates &Q) { UpdateKinematicsCustom (&Q, nullptr, nullptr); return allGlobalJCS(); @@ -318,7 +318,7 @@ const } biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::String &name) { UpdateKinematicsCustom (&Q, nullptr, nullptr); @@ -326,7 +326,7 @@ biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( } biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx) { // update the Kinematics if necessary @@ -369,10 +369,10 @@ biorbd::utils::RotoTrans rigidbody::Joints::localJCS( } -std::vector +std::vector rigidbody::Joints::projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const std::vector& v, + const rigidbody::GeneralizedCoordinates& Q, + const std::vector& v, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -384,16 +384,16 @@ rigidbody::Joints::projectPoint( updateKin = false; // Assuming that this is also a marker type (via BiorbdModel) - const biorbd::rigidbody::Markers& marks = - dynamic_cast(*this); + const rigidbody::Markers& marks = + dynamic_cast(*this); // Sécurité biorbd::utils::Error::check(marks.nbMarkers() == v.size(), "Number of marker must be equal to number of Vector3d"); - std::vector out; + std::vector out; for (unsigned int i = 0; i < marks.nbMarkers(); ++i) { - biorbd::rigidbody::NodeSegment tp(marks.marker(i)); + rigidbody::NodeSegment tp(marks.marker(i)); if (tp.nbAxesToRemove() != 0) { tp = v[i].applyRT(globalJCS(tp.parent()).transpose()); // Prendre la position du nouveau marker avec les infos de celui du modèle @@ -407,8 +407,8 @@ rigidbody::Joints::projectPoint( return out; } -biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::NodeSegment rigidbody::Joints::projectPoint( + const rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::Vector3d &v, int segmentIdx, const biorbd::utils::String& axesToRemove, @@ -424,7 +424,7 @@ biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( // Create a marker const biorbd::utils::String& segmentName(segment(static_cast (segmentIdx)).name()); - biorbd::rigidbody::NodeSegment node( v.applyRT(globalJCS( + rigidbody::NodeSegment node( v.applyRT(globalJCS( static_cast(segmentIdx)).transpose()), "tp", segmentName, true, true, axesToRemove, static_cast(GetBodyId(segmentName.c_str()))); @@ -433,19 +433,19 @@ biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( return projectPoint(Q, node, false); } -biorbd::rigidbody::NodeSegment rigidbody::Joints::projectPoint( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::NodeSegment &n, +rigidbody::NodeSegment rigidbody::Joints::projectPoint( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::NodeSegment &n, bool updateKin) { // Assuming that this is also a Marker type (via BiorbdModel) - return dynamic_cast(*this).marker(Q, n, true, + return dynamic_cast(*this).marker(Q, n, true, updateKin); } biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - biorbd::rigidbody::NodeSegment node, + const rigidbody::GeneralizedCoordinates &Q, + rigidbody::NodeSegment node, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -457,7 +457,7 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( updateKin = false; // Assuming that this is also a Marker type (via BiorbdModel) - biorbd::rigidbody::Markers &marks = dynamic_cast + rigidbody::Markers &marks = dynamic_cast (*this); // If the point has not been projected, there is no effect @@ -482,14 +482,14 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( } biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, const biorbd::utils::String& axesToRemove, bool updateKin) { // Find the point - const biorbd::rigidbody::NodeSegment& p(projectPoint(Q, v, segmentIdx, + const rigidbody::NodeSegment& p(projectPoint(Q, v, segmentIdx, axesToRemove, updateKin)); // Return the value @@ -498,12 +498,12 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( std::vector rigidbody::Joints::projectPointJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const std::vector &v, + const rigidbody::GeneralizedCoordinates &Q, + const std::vector &v, bool updateKin) { // Gather the points - const std::vector& tp(projectPoint(Q, v, + const std::vector& tp(projectPoint(Q, v, updateKin)); // Calculate the Jacobian if the point is not projected @@ -511,7 +511,7 @@ rigidbody::Joints::projectPointJacobian( for (unsigned int i=0; i& com_segment(CoMbySegment(Q, + const std::vector& com_segment(CoMbySegment(Q, updateKin)); biorbd::utils::Vector3d com(0, 0, 0); for (unsigned int i=0; i +std::vector rigidbody::Joints::CoMbySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector out; + std::vector out; for (unsigned int i=0; isize(); ++i) { out.push_back(CoMbySegment(Q,i,updateKin)); updateKin = false; @@ -686,10 +686,10 @@ rigidbody::Joints::CoMbySegment( } biorbd::utils::Matrix rigidbody::Joints::CoMbySegmentInMatrix( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector allCoM(CoMbySegment(Q, updateKin)); + std::vector allCoM(CoMbySegment(Q, updateKin)); biorbd::utils::Matrix CoMs(3, allCoM.size()); for (unsigned int i=0; i rigidbody::Joints::CoMdotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, bool updateKin) { std::vector out; @@ -729,8 +729,8 @@ std::vector rigidbody::Joints::CoMdotBySegment( biorbd::utils::Vector3d rigidbody::Joints::CoMdotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, const unsigned int idx, bool updateKin) { @@ -748,8 +748,8 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMdotBySegment( std::vector rigidbody::Joints::CoMddotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { @@ -763,8 +763,8 @@ rigidbody::Joints::CoMddotBySegment( biorbd::utils::Vector3d rigidbody::Joints::CoMddotBySegment( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin) @@ -781,7 +781,7 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMddotBySegment( std::vector> rigidbody::Joints::meshPoints( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -803,7 +803,7 @@ std::vector> return v; } std::vector rigidbody::Joints::meshPoints( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, unsigned int i, bool updateKin) { @@ -822,7 +822,7 @@ std::vector rigidbody::Joints::meshPoints( std::vector rigidbody::Joints::meshPointsInMatrix( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -861,41 +861,41 @@ std::vector rigidbody::Joints::meshPoints( return v; } -std::vector> +std::vector> rigidbody::Joints::meshFaces() const { // Gather the position of the meshings for all the segments - std::vector> v_all; + std::vector> v_all; for (unsigned int j=0; j +const std::vector &rigidbody::Joints::meshFaces(unsigned int idx) const { // Find the position of the meshings for a segment i return mesh(idx).faces(); } -std::vector rigidbody::Joints::mesh() const +std::vector rigidbody::Joints::mesh() const { - std::vector segmentOut; + std::vector segmentOut; for (unsigned int i=0; i rigidbody::Joints::CalcSegmentsAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -976,8 +976,8 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( std::vector rigidbody::Joints::CalcSegmentsAngularMomentum ( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { @@ -1020,12 +1020,12 @@ unsigned int rigidbody::Joints::nbQuat() const return *m_nRotAQuat; } -biorbd::rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedCoordinates &QDot, +rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &QDot, const double k_stab) { - biorbd::rigidbody::GeneralizedVelocity QDotOut(Q.size()); + rigidbody::GeneralizedVelocity QDotOut(Q.size()); // Verify if there are quaternions, if not the derivate is directly QDot if (!m_nRotAQuat) { QDotOut = QDot; @@ -1034,7 +1034,7 @@ biorbd::rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( unsigned int cmpQuat(0); unsigned int cmpDof(0); for (unsigned int i=0; i* f_ext) { - biorbd::rigidbody::GeneralizedTorque Tau(nbGeneralizedTorque()); + rigidbody::GeneralizedTorque Tau(nbGeneralizedTorque()); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1081,12 +1081,12 @@ biorbd::rigidbody::GeneralizedTorque rigidbody::Joints::InverseDynamics( return Tau; } -biorbd::rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, +rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &QDot, std::vector* f_ext) { - biorbd::rigidbody::GeneralizedTorque Tau(*this); + rigidbody::GeneralizedTorque Tau(*this); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1099,9 +1099,9 @@ biorbd::rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamics( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedTorque &Tau, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &QDot, + const rigidbody::GeneralizedTorque &Tau, std::vector* f_ext) { #ifdef BIORBD_USE_CASADI_MATH @@ -1121,10 +1121,10 @@ rigidbody::Joints::ForwardDynamics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedTorque &Tau, - biorbd::rigidbody::Contacts &CS, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &QDot, + const rigidbody::GeneralizedTorque &Tau, + rigidbody::Contacts &CS, std::vector *f_ext) { #ifdef BIORBD_USE_CASADI_MATH @@ -1146,12 +1146,12 @@ rigidbody::Joints::ForwardDynamicsConstraintsDirect( biorbd::utils::Vector rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedTorque &Tau, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &QDot, + const rigidbody::GeneralizedTorque &Tau, std::vector *f_ext) { - biorbd::rigidbody::Contacts CS = dynamic_cast + rigidbody::Contacts CS = dynamic_cast (this)->getConstraints(); this->ForwardDynamicsConstraintsDirect(Q, QDot, Tau, CS, f_ext); return CS.getForce(); @@ -1159,31 +1159,31 @@ rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamicsConstraintsDirect( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &QDot, - const biorbd::rigidbody::GeneralizedTorque &Tau, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &QDot, + const rigidbody::GeneralizedTorque &Tau, std::vector *f_ext) { - biorbd::rigidbody::Contacts CS = dynamic_cast + rigidbody::Contacts CS = dynamic_cast (this)->getConstraints(); return this->ForwardDynamicsConstraintsDirect(Q, QDot, Tau, CS, f_ext); } -biorbd::rigidbody::GeneralizedVelocity +rigidbody::GeneralizedVelocity rigidbody::Joints::ComputeConstraintImpulsesDirect( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDotPre + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDotPre ) { - biorbd::rigidbody::Contacts CS = dynamic_cast + rigidbody::Contacts CS = dynamic_cast (this)->getConstraints(); if (CS.nbContacts() == 0) { return QDotPre; } else { - CS = dynamic_cast(this)->getConstraints(); + CS = dynamic_cast(this)->getConstraints(); - biorbd::rigidbody::GeneralizedVelocity QDotPost(*this); + rigidbody::GeneralizedVelocity QDotPost(*this); RigidBodyDynamics::ComputeConstraintImpulsesDirect(*this, Q, QDotPre, CS, QDotPost); return QDotPost; @@ -1217,8 +1217,8 @@ unsigned int rigidbody::Joints::getDofIndex( } void rigidbody::Joints::UpdateKinematicsCustom( - const biorbd::rigidbody::GeneralizedCoordinates *Q, - const biorbd::rigidbody::GeneralizedVelocity *Qdot, + const rigidbody::GeneralizedCoordinates *Q, + const rigidbody::GeneralizedVelocity *Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { checkGeneralizedDimensions(Q, Qdot, Qddot); @@ -1226,7 +1226,7 @@ void rigidbody::Joints::UpdateKinematicsCustom( } void rigidbody::Joints::CalcMatRotJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, unsigned int segmentIdx, const RigidBodyDynamics::Math::Matrix3d &rotation, RigidBodyDynamics::Math::MatrixNd &G, @@ -1299,10 +1299,10 @@ void rigidbody::Joints::CalcMatRotJacobian( } void rigidbody::Joints::checkGeneralizedDimensions( - const biorbd::rigidbody::GeneralizedCoordinates *Q, - const biorbd::rigidbody::GeneralizedVelocity *Qdot, + const rigidbody::GeneralizedCoordinates *Q, + const rigidbody::GeneralizedVelocity *Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, - const biorbd::rigidbody::GeneralizedTorque *torque) + const rigidbody::GeneralizedTorque *torque) { #ifndef SKIP_ASSERT if (Q) { diff --git a/src/RigidBody/Markers.cpp b/src/RigidBody/Markers.cpp index b72c7573..d9527d56 100644 --- a/src/RigidBody/Markers.cpp +++ b/src/RigidBody/Markers.cpp @@ -12,33 +12,34 @@ #include "RigidBody/NodeSegment.h" #include "RigidBody/Segment.h" +using namespace biorbd::BIORBD_MATH_NAMESPACE; -biorbd::rigidbody::Markers::Markers() : - m_marks(std::make_shared>()) +rigidbody::Markers::Markers() : + m_marks(std::make_shared>()) { //ctor } -biorbd::rigidbody::Markers::Markers(const biorbd::rigidbody::Markers &other) : +rigidbody::Markers::Markers(const rigidbody::Markers &other) : m_marks(other.m_marks) { } -biorbd::rigidbody::Markers::~Markers() +rigidbody::Markers::~Markers() { //dtor } -biorbd::rigidbody::Markers biorbd::rigidbody::Markers::DeepCopy() const +rigidbody::Markers rigidbody::Markers::DeepCopy() const { - biorbd::rigidbody::Markers copy; + rigidbody::Markers copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::Markers::DeepCopy(const biorbd::rigidbody::Markers - &other) +void rigidbody::Markers::DeepCopy( + const rigidbody::Markers &other) { m_marks->resize(other.m_marks->size()); for (unsigned int i=0; isize(); ++i) { @@ -47,8 +48,8 @@ void biorbd::rigidbody::Markers::DeepCopy(const biorbd::rigidbody::Markers } // Add a new marker to the markers pool -void biorbd::rigidbody::Markers::addMarker( - const biorbd::rigidbody::NodeSegment &pos, +void rigidbody::Markers::addMarker( + const rigidbody::NodeSegment &pos, const biorbd::utils::String &name, const biorbd::utils::String &parentName, bool technical, @@ -56,21 +57,21 @@ void biorbd::rigidbody::Markers::addMarker( const biorbd::utils::String& axesToRemove, int id) { - biorbd::rigidbody::NodeSegment tp(pos, name, parentName, technical, anatomical, + rigidbody::NodeSegment tp(pos, name, parentName, technical, anatomical, axesToRemove, id); m_marks->push_back(tp); } -const biorbd::rigidbody::NodeSegment &biorbd::rigidbody::Markers::marker( +const rigidbody::NodeSegment &rigidbody::Markers::marker( unsigned int idx) const { return (*m_marks)[idx]; } -std::vector biorbd::rigidbody::Markers::marker( +std::vector rigidbody::Markers::marker( const biorbd::utils::String& name) const { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::Markers::marker( } // Return a marker -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::NodeSegment &n, +rigidbody::NodeSegment rigidbody::Markers::marker( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::NodeSegment &n, bool removeAxis, bool updateKin) { @@ -96,18 +97,18 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( unsigned int id = model.GetBodyId(n.parent().c_str()); if (removeAxis) { - return biorbd::rigidbody::NodeSegment( + return rigidbody::NodeSegment( RigidBodyDynamics::CalcBodyToBaseCoordinates(model, Q, id, n.removeAxes(), updateKin)); } else { - return biorbd::rigidbody::NodeSegment( + return rigidbody::NodeSegment( RigidBodyDynamics::CalcBodyToBaseCoordinates(model, Q, id, n, updateKin)); } } // Get a marker -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::NodeSegment rigidbody::Markers::marker( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool removeAxis, bool updateKin) @@ -119,22 +120,22 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( updateKin = true; #endif - const biorbd::rigidbody::NodeSegment& node(marker(idx)); + const rigidbody::NodeSegment& node(marker(idx)); unsigned int id = model.GetBodyId(node.parent().c_str()); // Retrieve the position of the marker in the local reference - const biorbd::rigidbody::NodeSegment& pos = marker(idx, removeAxis); + const rigidbody::NodeSegment& pos = marker(idx, removeAxis); - return biorbd::rigidbody::NodeSegment( + return rigidbody::NodeSegment( RigidBodyDynamics::CalcBodyToBaseCoordinates(model, Q, id, pos, updateKin)); } // Get a marker -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( +rigidbody::NodeSegment rigidbody::Markers::marker( unsigned int idx, bool removeAxis) { - const biorbd::rigidbody::NodeSegment& node(marker(idx)); + const rigidbody::NodeSegment& node(marker(idx)); if (removeAxis) { return node.removeAxes(); } else { @@ -143,12 +144,12 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::marker( } // Get all the markers -std::vector biorbd::rigidbody::Markers::markers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::Markers::markers( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::Markers::markers( return pos; } // Get all the markers in the local reference -std::vector biorbd::rigidbody::Markers::markers( +std::vector rigidbody::Markers::markers( bool removeAxis) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i biorbd::rigidbody::Markers::markers( } // Get a marker's velocity -biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::markerVelocity( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, +rigidbody::NodeSegment rigidbody::Markers::markerVelocity( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, unsigned int idx, bool removeAxis, bool updateKin) @@ -183,26 +184,26 @@ biorbd::rigidbody::NodeSegment biorbd::rigidbody::Markers::markerVelocity( updateKin = true; #endif - const biorbd::rigidbody::NodeSegment& node(marker(idx)); + const rigidbody::NodeSegment& node(marker(idx)); unsigned int id(model.GetBodyId(node.parent().c_str())); // Retrieve the position of the marker in the local reference - const biorbd::rigidbody::NodeSegment& pos(marker(idx, removeAxis)); + const rigidbody::NodeSegment& pos(marker(idx, removeAxis)); // Calculate the velocity of the point - return biorbd::rigidbody::NodeSegment(RigidBodyDynamics::CalcPointVelocity( + return rigidbody::NodeSegment(RigidBodyDynamics::CalcPointVelocity( model, Q, Qdot, id, pos, updateKin)); } // Get the makers' velocities -std::vector -biorbd::rigidbody::Markers::markersVelocity( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, +std::vector +rigidbody::Markers::markersVelocity( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, bool removeAxis, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::markerAcceleration( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, +std::vector +rigidbody::Markers::markerAcceleration( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool removeAxis, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::technicalMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector +rigidbody::Markers::technicalMarkers( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::technicalMarkers( +std::vector +rigidbody::Markers::technicalMarkers( bool removeAxis) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::anatomicalMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector +rigidbody::Markers::anatomicalMarkers( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::anatomicalMarkers( +std::vector +rigidbody::Markers::anatomicalMarkers( bool removeAxis) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i -biorbd::rigidbody::Markers::segmentMarkers( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector +rigidbody::Markers::segmentMarkers( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool removeAxis, bool updateKin) @@ -328,7 +329,7 @@ biorbd::rigidbody::Markers::segmentMarkers( // Name of the segment to find const biorbd::utils::String& name(model.segment(idx).name()); - std::vector pos; + std::vector pos; for (unsigned int i=0; i(m_marks->size()); } -unsigned int biorbd::rigidbody::Markers::nbMarkers(unsigned int idxSegment) +unsigned int rigidbody::Markers::nbMarkers(unsigned int idxSegment) const { // Assuming that this is also a joint type (via BiorbdModel) @@ -366,8 +367,8 @@ const } // Get the Jacobian of the markers -std::vector biorbd::rigidbody::Markers::markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::Markers::markersJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) { @@ -375,8 +376,8 @@ std::vector biorbd::rigidbody::Markers::markersJacobian( } std::vector -biorbd::rigidbody::Markers::technicalMarkersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::Markers::technicalMarkersJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) { @@ -384,10 +385,10 @@ biorbd::rigidbody::Markers::technicalMarkersJacobian( } // Get the Jacobian of the technical markers -biorbd::utils::Matrix biorbd::rigidbody::Markers::markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +biorbd::utils::Matrix rigidbody::Markers::markersJacobian( + const rigidbody::GeneralizedCoordinates &Q, const biorbd::utils::String& parentName, - const biorbd::rigidbody::NodeSegment& p, + const rigidbody::NodeSegment& p, bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) @@ -406,10 +407,10 @@ biorbd::utils::Matrix biorbd::rigidbody::Markers::markersJacobian( } #ifndef BIORBD_USE_CASADI_MATH -bool biorbd::rigidbody::Markers::inverseKinematics( - const std::vector &markers, - const biorbd::rigidbody::GeneralizedCoordinates &Qinit, - biorbd::rigidbody::GeneralizedCoordinates &Q, +bool rigidbody::Markers::inverseKinematics( + const std::vector &markers, + const rigidbody::GeneralizedCoordinates &Qinit, + rigidbody::GeneralizedCoordinates &Q, bool removeAxes) { biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast @@ -417,7 +418,7 @@ bool biorbd::rigidbody::Markers::inverseKinematics( model.UpdateKinematicsCustom(&Q); // also assert for dimensions // Find the technical markers only (body_point) - std::vector body_point( + std::vector body_point( technicalMarkers(removeAxes)); std::vector body_pointEigen; for (unsigned int i=0; i biorbd::rigidbody::Markers::markersJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +std::vector rigidbody::Markers::markersJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin, bool lookForTechnical) @@ -461,7 +462,7 @@ std::vector biorbd::rigidbody::Markers::markersJacobian( for (unsigned int idx=0; idx biorbd::rigidbody::Markers::markersJacobian( return G; } -unsigned int biorbd::rigidbody::Markers::nbTechnicalMarkers() +unsigned int rigidbody::Markers::nbTechnicalMarkers() { unsigned int nTechMarkers = 0; if (nTechMarkers == 0) // If the function has never been called before @@ -494,7 +495,7 @@ unsigned int biorbd::rigidbody::Markers::nbTechnicalMarkers() return nTechMarkers; } -unsigned int biorbd::rigidbody::Markers::nbTechnicalMarkers( +unsigned int rigidbody::Markers::nbTechnicalMarkers( unsigned int idxSegment) { // Assuming that this is also a joint type (via BiorbdModel) @@ -516,7 +517,7 @@ unsigned int biorbd::rigidbody::Markers::nbTechnicalMarkers( } -unsigned int biorbd::rigidbody::Markers::nbAnatomicalMarkers() +unsigned int rigidbody::Markers::nbAnatomicalMarkers() { unsigned int nAnatMarkers = 0; if (nAnatMarkers == 0) // If the function has never been called before @@ -528,7 +529,7 @@ unsigned int biorbd::rigidbody::Markers::nbAnatomicalMarkers() return nAnatMarkers; } -std::vector biorbd::rigidbody::Markers::markerNames() +std::vector rigidbody::Markers::markerNames() const { // Extract the name of all the markers of a model @@ -541,7 +542,7 @@ const } std::vector -biorbd::rigidbody::Markers::technicalMarkerNames() const +rigidbody::Markers::technicalMarkerNames() const { // Extract the name of all the technical markers of a model std::vector names; @@ -554,7 +555,7 @@ biorbd::rigidbody::Markers::technicalMarkerNames() const } std::vector -biorbd::rigidbody::Markers::anatomicalMarkerNames() const +rigidbody::Markers::anatomicalMarkerNames() const { // Extract the names of all the anatomical markers of a model std::vector names; diff --git a/src/RigidBody/Mesh.cpp b/src/RigidBody/Mesh.cpp index f4ae3437..038227f7 100644 --- a/src/RigidBody/Mesh.cpp +++ b/src/RigidBody/Mesh.cpp @@ -5,41 +5,43 @@ #include "Utils/Vector3d.h" #include "RigidBody/MeshFace.h" -biorbd::rigidbody::Mesh::Mesh() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::Mesh::Mesh() : m_vertex(std::make_shared>()), - m_faces(std::make_shared>()), + m_faces(std::make_shared>()), m_pathFile(std::make_shared()) { } -biorbd::rigidbody::Mesh::Mesh(const std::vector &other) +rigidbody::Mesh::Mesh(const std::vector &other) : m_vertex(std::make_shared>(other)), - m_faces(std::make_shared>()), + m_faces(std::make_shared>()), m_pathFile(std::make_shared()) { } -biorbd::rigidbody::Mesh::Mesh(const std::vector +rigidbody::Mesh::Mesh(const std::vector &vertex, - const std::vector & faces) : + const std::vector & faces) : m_vertex(std::make_shared>(vertex)), - m_faces(std::make_shared>(faces)), + m_faces(std::make_shared>(faces)), m_pathFile(std::make_shared()) { } -biorbd::rigidbody::Mesh biorbd::rigidbody::Mesh::DeepCopy() const +rigidbody::Mesh rigidbody::Mesh::DeepCopy() const { - biorbd::rigidbody::Mesh copy; + rigidbody::Mesh copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::Mesh::DeepCopy(const biorbd::rigidbody::Mesh &other) +void rigidbody::Mesh::DeepCopy(const rigidbody::Mesh &other) { m_vertex->resize(other.m_vertex->size()); for (unsigned int i=0; isize(); ++i) { @@ -52,49 +54,49 @@ void biorbd::rigidbody::Mesh::DeepCopy(const biorbd::rigidbody::Mesh &other) *m_pathFile = other.m_pathFile->DeepCopy(); } -void biorbd::rigidbody::Mesh::addPoint(const biorbd::utils::Vector3d &node) +void rigidbody::Mesh::addPoint(const biorbd::utils::Vector3d &node) { m_vertex->push_back(node); } -const biorbd::utils::Vector3d &biorbd::rigidbody::Mesh::point( +const biorbd::utils::Vector3d &rigidbody::Mesh::point( unsigned int idx) const { return (*m_vertex)[idx]; } -unsigned int biorbd::rigidbody::Mesh::nbVertex() const +unsigned int rigidbody::Mesh::nbVertex() const { return static_cast(m_vertex->size()); } -unsigned int biorbd::rigidbody::Mesh::nbFaces() +unsigned int rigidbody::Mesh::nbFaces() { return static_cast(m_faces->size()); } -void biorbd::rigidbody::Mesh::addFace(const biorbd::rigidbody::MeshFace& face) +void rigidbody::Mesh::addFace(const rigidbody::MeshFace& face) { m_faces->push_back(face); } -void biorbd::rigidbody::Mesh::addFace(const std::vector & face) +void rigidbody::Mesh::addFace(const std::vector & face) { - addFace(biorbd::rigidbody::MeshFace(face)); + addFace(rigidbody::MeshFace(face)); } -const std::vector& biorbd::rigidbody::Mesh::faces() +const std::vector& rigidbody::Mesh::faces() const { return *m_faces; } -const biorbd::rigidbody::MeshFace &biorbd::rigidbody::Mesh::face( +const rigidbody::MeshFace &rigidbody::Mesh::face( unsigned int idx) const { return (*m_faces)[idx]; } -void biorbd::rigidbody::Mesh::setPath(const biorbd::utils::Path& path) +void rigidbody::Mesh::setPath(const biorbd::utils::Path& path) { *m_pathFile = path; } -const biorbd::utils::Path &biorbd::rigidbody::Mesh::path() const +const biorbd::utils::Path &rigidbody::Mesh::path() const { return *m_pathFile; } diff --git a/src/RigidBody/MeshFace.cpp b/src/RigidBody/MeshFace.cpp index 0c99bbb9..cab907a1 100644 --- a/src/RigidBody/MeshFace.cpp +++ b/src/RigidBody/MeshFace.cpp @@ -3,48 +3,50 @@ #include "Utils/Vector3d.h" -biorbd::rigidbody::MeshFace::MeshFace(const std::vector& vertex) : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::MeshFace::MeshFace(const std::vector& vertex) : m_face(std::make_shared>(vertex)) { } -biorbd::rigidbody::MeshFace biorbd::rigidbody::MeshFace::DeepCopy() const +rigidbody::MeshFace rigidbody::MeshFace::DeepCopy() const { - biorbd::rigidbody::MeshFace copy; + rigidbody::MeshFace copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::MeshFace::DeepCopy(const biorbd::rigidbody::MeshFace +void rigidbody::MeshFace::DeepCopy(const rigidbody::MeshFace &other) { *m_face = *other.m_face; } -int &biorbd::rigidbody::MeshFace::operator()(unsigned int idx) +int &rigidbody::MeshFace::operator()(unsigned int idx) { return (*m_face)[idx]; } -biorbd::utils::Vector3d biorbd::rigidbody::MeshFace::faceAsDouble() +biorbd::utils::Vector3d rigidbody::MeshFace::faceAsDouble() { return biorbd::utils::Vector3d(static_cast((*m_face)[0]), static_cast((*m_face)[1]), static_cast((*m_face)[2])); } -std::vector biorbd::rigidbody::MeshFace::face() +std::vector rigidbody::MeshFace::face() { return *m_face; } -void biorbd::rigidbody::MeshFace::setFace(const std::vector & pts) +void rigidbody::MeshFace::setFace(const std::vector & pts) { *m_face = pts; } -void biorbd::rigidbody::MeshFace::setFace(const biorbd::rigidbody::MeshFace +void rigidbody::MeshFace::setFace(const rigidbody::MeshFace &other) { m_face = other.m_face; diff --git a/src/RigidBody/NodeSegment.cpp b/src/RigidBody/NodeSegment.cpp index 3ac5c423..a0b03b82 100644 --- a/src/RigidBody/NodeSegment.cpp +++ b/src/RigidBody/NodeSegment.cpp @@ -3,7 +3,9 @@ #include "Utils/Error.h" -biorbd::rigidbody::NodeSegment::NodeSegment() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::NodeSegment::NodeSegment() : biorbd::utils::Vector3d(0, 0, 0), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), @@ -14,7 +16,7 @@ biorbd::rigidbody::NodeSegment::NodeSegment() : setType(); } -biorbd::rigidbody::NodeSegment::NodeSegment( +rigidbody::NodeSegment::NodeSegment( const biorbd::utils::Scalar& x, const biorbd::utils::Scalar& y, const biorbd::utils::Scalar& z) : @@ -28,7 +30,7 @@ biorbd::rigidbody::NodeSegment::NodeSegment( setType(); } -biorbd::rigidbody::NodeSegment::NodeSegment(const biorbd::utils::Vector3d +rigidbody::NodeSegment::NodeSegment(const biorbd::utils::Vector3d &other) : biorbd::utils::Vector3d(other), m_axesRemoved(std::make_shared>(3)), @@ -40,7 +42,7 @@ biorbd::rigidbody::NodeSegment::NodeSegment(const biorbd::utils::Vector3d setType(); } -biorbd::rigidbody::NodeSegment::NodeSegment( +rigidbody::NodeSegment::NodeSegment( const biorbd::utils::Scalar& x, const biorbd::utils::Scalar& y, const biorbd::utils::Scalar& z, @@ -60,7 +62,7 @@ biorbd::rigidbody::NodeSegment::NodeSegment( addAxesToRemove(axesToRemove); } -biorbd::rigidbody::NodeSegment::NodeSegment( +rigidbody::NodeSegment::NodeSegment( const biorbd::utils::Vector3d &node, const biorbd::utils::String &name, const biorbd::utils::String &parentName, @@ -79,15 +81,15 @@ biorbd::rigidbody::NodeSegment::NodeSegment( //ctor } -biorbd::rigidbody::NodeSegment biorbd::rigidbody::NodeSegment::DeepCopy() const +rigidbody::NodeSegment rigidbody::NodeSegment::DeepCopy() const { - biorbd::rigidbody::NodeSegment copy; + rigidbody::NodeSegment copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::NodeSegment::DeepCopy(const - biorbd::rigidbody::NodeSegment&other) +void rigidbody::NodeSegment::DeepCopy(const + rigidbody::NodeSegment&other) { biorbd::utils::Node::DeepCopy(other); *m_axesRemoved = *other.m_axesRemoved; @@ -98,28 +100,28 @@ void biorbd::rigidbody::NodeSegment::DeepCopy(const } -bool biorbd::rigidbody::NodeSegment::isAnatomical() const +bool rigidbody::NodeSegment::isAnatomical() const { return *m_anatomical; } -bool biorbd::rigidbody::NodeSegment::isTechnical() const +bool rigidbody::NodeSegment::isTechnical() const { return *m_technical; } -int biorbd::rigidbody::NodeSegment::parentId() const +int rigidbody::NodeSegment::parentId() const { return *m_id; } -biorbd::rigidbody::NodeSegment biorbd::rigidbody::NodeSegment::removeAxes() +rigidbody::NodeSegment rigidbody::NodeSegment::removeAxes() const { - biorbd::rigidbody::NodeSegment pos(*this); + rigidbody::NodeSegment pos(*this); for (unsigned int i=0; isize(); ++i) if (isAxisRemoved(i)) { pos(i) = 0; @@ -127,27 +129,27 @@ const return pos; } -bool biorbd::rigidbody::NodeSegment::isAxisRemoved(unsigned int i) const +bool rigidbody::NodeSegment::isAxisRemoved(unsigned int i) const { return (*m_axesRemoved)[i]; } -bool biorbd::rigidbody::NodeSegment::isAxisKept(unsigned int i) const +bool rigidbody::NodeSegment::isAxisKept(unsigned int i) const { return !isAxisRemoved(i); } -int biorbd::rigidbody::NodeSegment::nbAxesToRemove() const +int rigidbody::NodeSegment::nbAxesToRemove() const { return *m_nbAxesToRemove; } -void biorbd::rigidbody::NodeSegment::setType() +void rigidbody::NodeSegment::setType() { *m_typeOfNode = biorbd::utils::NODE_TYPE::BONE_POINT; } -void biorbd::rigidbody::NodeSegment::addAxesToRemove(unsigned int axisNumber) +void rigidbody::NodeSegment::addAxesToRemove(unsigned int axisNumber) { if (axisNumber>2) { biorbd::utils::Error::raise("Axis must be 0 (\"x\"), 1 (\"y\") or 2 (\"z\")"); @@ -156,7 +158,7 @@ void biorbd::rigidbody::NodeSegment::addAxesToRemove(unsigned int axisNumber) ++*m_nbAxesToRemove; } -void biorbd::rigidbody::NodeSegment::addAxesToRemove(const +void rigidbody::NodeSegment::addAxesToRemove(const biorbd::utils::String& s) { for (unsigned int i=0; i& axes) { for (unsigned int i=0; i& axes) { for (unsigned int i=0; i>()) { //ctor } -biorbd::rigidbody::RotoTransNodes::RotoTransNodes(const - biorbd::rigidbody::RotoTransNodes &other) +rigidbody::RotoTransNodes::RotoTransNodes(const + rigidbody::RotoTransNodes &other) { m_RTs = other.m_RTs; } -biorbd::rigidbody::RotoTransNodes::~RotoTransNodes() +rigidbody::RotoTransNodes::~RotoTransNodes() { } -biorbd::rigidbody::RotoTransNodes biorbd::rigidbody::RotoTransNodes::DeepCopy() +rigidbody::RotoTransNodes rigidbody::RotoTransNodes::DeepCopy() const { - biorbd::rigidbody::RotoTransNodes copy; + rigidbody::RotoTransNodes copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::RotoTransNodes::DeepCopy(const - biorbd::rigidbody::RotoTransNodes &other) +void rigidbody::RotoTransNodes::DeepCopy(const + rigidbody::RotoTransNodes &other) { m_RTs->resize(other.m_RTs->size()); for (unsigned int i=0; isize(); ++i) { @@ -45,37 +47,37 @@ void biorbd::rigidbody::RotoTransNodes::DeepCopy(const } } -void biorbd::rigidbody::RotoTransNodes::addRT() +void rigidbody::RotoTransNodes::addRT() { m_RTs->push_back(biorbd::utils::RotoTransNode()); } // Add a new marker to the existing pool of markers -void biorbd::rigidbody::RotoTransNodes::addRT( +void rigidbody::RotoTransNodes::addRT( const biorbd::utils::RotoTransNode &RotoTrans) { m_RTs->push_back(biorbd::utils::RotoTransNode(RotoTrans)); } -unsigned int biorbd::rigidbody::RotoTransNodes::nbRTs() const +unsigned int rigidbody::RotoTransNodes::nbRTs() const { return static_cast(m_RTs->size()); } -unsigned int biorbd::rigidbody::RotoTransNodes::size() const +unsigned int rigidbody::RotoTransNodes::size() const { return static_cast(m_RTs->size()); } // Get the markers in the global reference const std::vector& -biorbd::rigidbody::RotoTransNodes::RTs() const +rigidbody::RotoTransNodes::RTs() const { return *m_RTs; } std::vector -biorbd::rigidbody::RotoTransNodes::RTs( +rigidbody::RotoTransNodes::RTs( const biorbd::utils::String& segmentName) { std::vector pos; @@ -87,7 +89,7 @@ biorbd::rigidbody::RotoTransNodes::RTs( return pos; } -const biorbd::utils::RotoTransNode& biorbd::rigidbody::RotoTransNodes::RT( +const biorbd::utils::RotoTransNode& rigidbody::RotoTransNodes::RT( unsigned int idx) { return (*m_RTs)[idx]; @@ -95,8 +97,8 @@ const biorbd::utils::RotoTransNode& biorbd::rigidbody::RotoTransNodes::RT( // Get the RotoTransNodes at the position given by Q std::vector -biorbd::rigidbody::RotoTransNodes::RTs( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::RotoTransNodes::RTs( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { std::vector pos; @@ -109,8 +111,8 @@ biorbd::rigidbody::RotoTransNodes::RTs( } // Get an IMU at the position given by Q -biorbd::utils::RotoTransNode biorbd::rigidbody::RotoTransNodes::RT( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +biorbd::utils::RotoTransNode rigidbody::RotoTransNodes::RT( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin) { @@ -132,8 +134,8 @@ biorbd::utils::RotoTransNode biorbd::rigidbody::RotoTransNodes::RT( } std::vector -biorbd::rigidbody::RotoTransNodes::segmentRTs( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::RotoTransNodes::segmentRTs( + const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, bool updateKin) { @@ -157,8 +159,8 @@ biorbd::rigidbody::RotoTransNodes::segmentRTs( // Get the Jacobian of the markers std::vector -biorbd::rigidbody::RotoTransNodes::RTsJacobian( - const biorbd::rigidbody::GeneralizedCoordinates &Q, +rigidbody::RotoTransNodes::RTsJacobian( + const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) @@ -188,7 +190,7 @@ biorbd::rigidbody::RotoTransNodes::RTsJacobian( return G; } -std::vector biorbd::rigidbody::RotoTransNodes::RTsNames() +std::vector rigidbody::RotoTransNodes::RTsNames() { // Extract the name of all the markers of a model std::vector names; diff --git a/src/RigidBody/Segment.cpp b/src/RigidBody/Segment.cpp index 5bc72a03..69091f1f 100644 --- a/src/RigidBody/Segment.cpp +++ b/src/RigidBody/Segment.cpp @@ -11,7 +11,9 @@ #include "RigidBody/SegmentCharacteristics.h" #include "Utils/Range.h" -biorbd::rigidbody::Segment::Segment() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::Segment::Segment() : biorbd::utils::Node(), m_idxInModel(std::make_shared(-1)), m_idxPF (std::make_shared(-1)), @@ -37,14 +39,14 @@ biorbd::rigidbody::Segment::Segment() : m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), m_characteristics( - std::make_shared()), + std::make_shared()), m_dofCharacteristics( - std::make_shared>()) + std::make_shared>()) { setType(); } -biorbd::rigidbody::Segment::Segment( +rigidbody::Segment::Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, const biorbd::utils::String &parentName, @@ -53,7 +55,7 @@ biorbd::rigidbody::Segment::Segment( const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& characteristics, + const rigidbody::SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF) : @@ -81,10 +83,10 @@ biorbd::rigidbody::Segment::Segment( m_sequenceRot(std::make_shared>()), m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), - m_characteristics(std::make_shared + m_characteristics(std::make_shared (characteristics)), m_dofCharacteristics( - std::make_shared>()) + std::make_shared>()) { setType(); // Call proper functions @@ -92,7 +94,7 @@ biorbd::rigidbody::Segment::Segment( // Add platform setPF(PF); } -biorbd::rigidbody::Segment::Segment( +rigidbody::Segment::Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &name, // Name of segment const biorbd::utils::String &parentName, // Name of segment @@ -101,7 +103,7 @@ biorbd::rigidbody::Segment::Segment( const std::vector& QRanges, const std::vector& QDotRanges, const std::vector& QDDotRanges, - const biorbd::rigidbody::SegmentCharacteristics& + const rigidbody::SegmentCharacteristics& characteristics, // Mass, Center of mass of segment, Inertia of segment, etc. const RigidBodyDynamics::Math::SpatialTransform& cor, // Transformation from parent to child @@ -132,10 +134,10 @@ biorbd::rigidbody::Segment::Segment( m_sequenceRot(std::make_shared>()), m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), - m_characteristics(std::make_shared + m_characteristics(std::make_shared (characteristics)), m_dofCharacteristics( - std::make_shared>()) + std::make_shared>()) { setType(); // Call proper functions @@ -144,15 +146,15 @@ biorbd::rigidbody::Segment::Segment( setPF(PF); } -biorbd::rigidbody::Segment biorbd::rigidbody::Segment::DeepCopy() const +rigidbody::Segment rigidbody::Segment::DeepCopy() const { - biorbd::rigidbody::Segment copy; + rigidbody::Segment copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::Segment::DeepCopy(const - biorbd::rigidbody::Segment&other) +void rigidbody::Segment::DeepCopy(const + rigidbody::Segment&other) { biorbd::utils::Node::DeepCopy(other); *m_idxInModel = *other.m_idxInModel; @@ -191,22 +193,22 @@ void biorbd::rigidbody::Segment::DeepCopy(const } } -biorbd::rigidbody::Segment::~Segment() +rigidbody::Segment::~Segment() { } -bool biorbd::rigidbody::Segment::isRotationAQuaternion() const +bool rigidbody::Segment::isRotationAQuaternion() const { return *m_isQuaternion; } -void biorbd::rigidbody::Segment::setType() +void rigidbody::Segment::setType() { *m_typeOfNode = biorbd::utils::NODE_TYPE::SEGMENT; } -unsigned int biorbd::rigidbody::Segment::id() const +unsigned int rigidbody::Segment::id() const { if (*m_nbDof!=0) { return (*m_idxDof)[*m_nbDof-1]; @@ -215,46 +217,46 @@ unsigned int biorbd::rigidbody::Segment::id() const } } -int biorbd::rigidbody::Segment::platformIdx() const +int rigidbody::Segment::platformIdx() const { return *m_idxPF; } -unsigned int biorbd::rigidbody::Segment::nbGeneralizedTorque() const +unsigned int rigidbody::Segment::nbGeneralizedTorque() const { return nbQddot(); } -unsigned int biorbd::rigidbody::Segment::nbDof() const +unsigned int rigidbody::Segment::nbDof() const { return *m_nbDofTrueOutside; } -unsigned int biorbd::rigidbody::Segment::nbDofTrans() const +unsigned int rigidbody::Segment::nbDofTrans() const { return *m_nbDofTrans; } -unsigned int biorbd::rigidbody::Segment::nbDofRot() const +unsigned int rigidbody::Segment::nbDofRot() const { return *m_nbDofRot; } -unsigned int biorbd::rigidbody::Segment::nbQ() const +unsigned int rigidbody::Segment::nbQ() const { return *m_nbDofTrue; } -unsigned int biorbd::rigidbody::Segment::nbQdot() const +unsigned int rigidbody::Segment::nbQdot() const { return *m_nbQdot; } -unsigned int biorbd::rigidbody::Segment::nbQddot() const +unsigned int rigidbody::Segment::nbQddot() const { return *m_nbQddot; } // Add platform -void biorbd::rigidbody::Segment::setPF(int idx) +void rigidbody::Segment::setPF(int idx) { *m_idxPF = idx; } -const biorbd::utils::String &biorbd::rigidbody::Segment::nameDof( +const biorbd::utils::String &rigidbody::Segment::nameDof( const unsigned int i) const { // Return the number of DoF of the segment @@ -263,43 +265,43 @@ const biorbd::utils::String &biorbd::rigidbody::Segment::nameDof( return (*m_nameDof)[i]; } -const biorbd::utils::String& biorbd::rigidbody::Segment::seqT() const +const biorbd::utils::String& rigidbody::Segment::seqT() const { return *m_seqT; } -const biorbd::utils::String& biorbd::rigidbody::Segment::seqR() const +const biorbd::utils::String& rigidbody::Segment::seqR() const { return *m_seqR; } const std::vector& -biorbd::rigidbody::Segment::QRanges() const +rigidbody::Segment::QRanges() const { return *m_QRanges; } const std::vector& -biorbd::rigidbody::Segment::QDotRanges() const +rigidbody::Segment::QDotRanges() const { return *m_QDotRanges; } const std::vector& -biorbd::rigidbody::Segment::QDDotRanges() const +rigidbody::Segment::QDDotRanges() const { return *m_QDDotRanges; } -biorbd::utils::RotoTrans biorbd::rigidbody::Segment::localJCS() const +biorbd::utils::RotoTrans rigidbody::Segment::localJCS() const { return RigidBodyDynamics::Math::SpatialTransform(m_cor->E.transpose(), m_cor->r); } -void biorbd::rigidbody::Segment::updateCharacteristics( +void rigidbody::Segment::updateCharacteristics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::SegmentCharacteristics& characteristics) + const rigidbody::SegmentCharacteristics& characteristics) { *m_characteristics = characteristics.DeepCopy(); @@ -313,13 +315,13 @@ void biorbd::rigidbody::Segment::updateCharacteristics( model.I[*m_idxInModel] = rbi; } -const biorbd::rigidbody::SegmentCharacteristics& -biorbd::rigidbody::Segment::characteristics() const +const rigidbody::SegmentCharacteristics& +rigidbody::Segment::characteristics() const { return *m_characteristics; } -void biorbd::rigidbody::Segment::setDofs( +void rigidbody::Segment::setDofs( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, const biorbd::utils::String &seqT, const biorbd::utils::String &seqR, @@ -358,7 +360,7 @@ void biorbd::rigidbody::Segment::setDofs( setJoints(model); } -void biorbd::rigidbody::Segment::determineIfRotIsQuaternion( +void rigidbody::Segment::determineIfRotIsQuaternion( const biorbd::utils::String &seqR) { if (!seqR.tolower().compare("q")) { @@ -368,7 +370,7 @@ void biorbd::rigidbody::Segment::determineIfRotIsQuaternion( // Member functions -void biorbd::rigidbody::Segment::str2numSequence( +void rigidbody::Segment::str2numSequence( const biorbd::utils::String &seqT, const biorbd::utils::String &seqR) { @@ -400,7 +402,7 @@ void biorbd::rigidbody::Segment::str2numSequence( } } -void biorbd::rigidbody::Segment::str2numSequence( +void rigidbody::Segment::str2numSequence( std::vector& sequenceInteger, const biorbd::utils::String &sequenceText) { @@ -419,7 +421,7 @@ void biorbd::rigidbody::Segment::str2numSequence( } } } -void biorbd::rigidbody::Segment::setNumberOfDof(unsigned int nTrans, +void rigidbody::Segment::setNumberOfDof(unsigned int nTrans, unsigned int nRot) { *m_nbDofTrans = nTrans; @@ -442,7 +444,7 @@ void biorbd::rigidbody::Segment::setNumberOfDof(unsigned int nTrans, } } -void biorbd::rigidbody::Segment::setSequence(const biorbd::utils::String &seqT, +void rigidbody::Segment::setSequence(const biorbd::utils::String &seqT, const biorbd::utils::String &seqR) // Find the x, y, and z positions in this sequence { @@ -451,7 +453,7 @@ void biorbd::rigidbody::Segment::setSequence(const biorbd::utils::String &seqT, str2numSequence(seqT, seqR); fillSequence(); } -void biorbd::rigidbody::Segment::fillSequence() +void rigidbody::Segment::fillSequence() { m_dofPosition->clear(); m_dofPosition->resize(*m_nbDof); @@ -469,14 +471,14 @@ void biorbd::rigidbody::Segment::fillSequence() } } -void biorbd::rigidbody::Segment::setDofCharacteristicsOnLastBody() +void rigidbody::Segment::setDofCharacteristicsOnLastBody() { m_dofCharacteristics->clear(); if (*m_nbDof!=0) { m_dofCharacteristics->resize(*m_nbDof); for (unsigned int i=0; i<*m_nbDof-1; i++) { - (*m_dofCharacteristics)[i] = biorbd::rigidbody::SegmentCharacteristics(); + (*m_dofCharacteristics)[i] = rigidbody::SegmentCharacteristics(); } (*m_dofCharacteristics)[*m_nbDof-1] = *m_characteristics; @@ -486,7 +488,7 @@ void biorbd::rigidbody::Segment::setDofCharacteristicsOnLastBody() } } -void biorbd::rigidbody::Segment::setJointAxis() +void rigidbody::Segment::setJointAxis() { // Definition of the rotation axis biorbd::utils::Vector3d axis[3]; @@ -519,7 +521,7 @@ void biorbd::rigidbody::Segment::setJointAxis() } } -void biorbd::rigidbody::Segment::setJoints( +void rigidbody::Segment::setJoints( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model) { setDofCharacteristicsOnLastBody(); // Apply the segment caracteristics only to the last segment @@ -568,7 +570,7 @@ void biorbd::rigidbody::Segment::setJoints( *m_idxInModel = model.I.size() - 1; } -unsigned int biorbd::rigidbody::Segment::getDofIdx( +unsigned int rigidbody::Segment::getDofIdx( const biorbd::utils::String &dofName) const { diff --git a/src/RigidBody/SegmentCharacteristics.cpp b/src/RigidBody/SegmentCharacteristics.cpp index b14b694a..90c05888 100644 --- a/src/RigidBody/SegmentCharacteristics.cpp +++ b/src/RigidBody/SegmentCharacteristics.cpp @@ -6,44 +6,46 @@ #include "RigidBody/MeshFace.h" #include "RigidBody/Mesh.h" -biorbd::rigidbody::SegmentCharacteristics::SegmentCharacteristics() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::SegmentCharacteristics::SegmentCharacteristics() : Body(), m_length(std::make_shared(0)), - m_mesh(std::make_shared()) + m_mesh(std::make_shared()) { } -biorbd::rigidbody::SegmentCharacteristics::SegmentCharacteristics( +rigidbody::SegmentCharacteristics::SegmentCharacteristics( const biorbd::utils::Scalar &mass, const biorbd::utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia) : Body(mass, com, inertia), m_length(std::make_shared(0)), - m_mesh(std::make_shared()) + m_mesh(std::make_shared()) { } -biorbd::rigidbody::SegmentCharacteristics::SegmentCharacteristics( +rigidbody::SegmentCharacteristics::SegmentCharacteristics( const biorbd::utils::Scalar &mass, const biorbd::utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia, - const biorbd::rigidbody::Mesh &mesh) : + const rigidbody::Mesh &mesh) : Body(mass, com, inertia), m_length(std::make_shared(0)), - m_mesh(std::make_shared(mesh)) + m_mesh(std::make_shared(mesh)) { } -biorbd::rigidbody::SegmentCharacteristics -biorbd::rigidbody::SegmentCharacteristics::DeepCopy() const +rigidbody::SegmentCharacteristics +rigidbody::SegmentCharacteristics::DeepCopy() const { - biorbd::rigidbody::SegmentCharacteristics copy; + rigidbody::SegmentCharacteristics copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::SegmentCharacteristics::DeepCopy( +void rigidbody::SegmentCharacteristics::DeepCopy( const SegmentCharacteristics &other) { static_cast(*this) = other; @@ -51,47 +53,47 @@ void biorbd::rigidbody::SegmentCharacteristics::DeepCopy( *m_mesh = other.m_mesh->DeepCopy(); } -void biorbd::rigidbody::SegmentCharacteristics::setLength( +void rigidbody::SegmentCharacteristics::setLength( const biorbd::utils::Scalar& val) { *m_length = val; } -biorbd::utils::Scalar biorbd::rigidbody::SegmentCharacteristics::length() const +biorbd::utils::Scalar rigidbody::SegmentCharacteristics::length() const { return *m_length; } -void biorbd::rigidbody::SegmentCharacteristics::setMass( +void rigidbody::SegmentCharacteristics::setMass( const biorbd::utils::Scalar &newMass) { mMass = newMass; } -biorbd::utils::Scalar biorbd::rigidbody::SegmentCharacteristics::mass() const +biorbd::utils::Scalar rigidbody::SegmentCharacteristics::mass() const { return mMass; } -biorbd::utils::Vector3d biorbd::rigidbody::SegmentCharacteristics::CoM() const +biorbd::utils::Vector3d rigidbody::SegmentCharacteristics::CoM() const { return mCenterOfMass; } -void biorbd::rigidbody::SegmentCharacteristics::setCoM( +void rigidbody::SegmentCharacteristics::setCoM( const biorbd::utils::Vector3d &com) { mCenterOfMass = com; } -const biorbd::rigidbody::Mesh &biorbd::rigidbody::SegmentCharacteristics::mesh() +const rigidbody::Mesh &rigidbody::SegmentCharacteristics::mesh() const { return *m_mesh; } const RigidBodyDynamics::Math::Matrix3d -&biorbd::rigidbody::SegmentCharacteristics::inertia() const +&rigidbody::SegmentCharacteristics::inertia() const { return mInertia; } diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 2e7a7cfe..67d9c939 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -106,9 +106,9 @@ biorbd::utils::Rotation biorbd::utils::Rotation::fromEulerAngles( } biorbd::utils::Matrix biorbd::utils::Rotation::fromMarkersNonNormalized( - const std::pair + const std::pair &axis1markers, - const std::pair + const std::pair &axis2markers, const std::pair& axesNames, const biorbd::utils::String &axisToRecalculate) @@ -185,10 +185,8 @@ biorbd::utils::Matrix biorbd::utils::Rotation::fromMarkersNonNormalized( } biorbd::utils::Rotation biorbd::utils::Rotation::fromMarkers( - const std::pair - &axis1markers, - const std::pair - &axis2markers, + const std::pair &axis1markers, + const std::pair &axis2markers, const std::pair& axesNames, const biorbd::utils::String &axisToRecalculate) { diff --git a/src/Utils/RotoTrans.cpp b/src/Utils/RotoTrans.cpp index 1922e7ee..bc970149 100644 --- a/src/Utils/RotoTrans.cpp +++ b/src/Utils/RotoTrans.cpp @@ -71,11 +71,9 @@ biorbd::utils::RotoTrans::RotoTrans( } biorbd::utils::RotoTrans biorbd::utils::RotoTrans::fromMarkers( - const biorbd::rigidbody::NodeSegment& origin, - const std::pair - &axis1markers, - const std::pair - &axis2markers, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment& origin, + const std::pair &axis1markers, + const std::pair &axis2markers, const std::pair& axesNames, const biorbd::utils::String &axisToRecalculate) { From 7e9a87b54d5b2c08d1e3e15247d3d71650bda484 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 11:07:32 -0400 Subject: [PATCH 03/14] Put utils and Reader in namespace --- binding/biorbd_utils.i.in | 44 +- binding/python3/biorbd_python.i.in | 115 ++-- include/BiorbdModel.h | 21 +- include/ModelReader.h | 76 +-- include/RigidBody/Contacts.h | 35 +- include/RigidBody/GeneralizedAcceleration.h | 8 +- include/RigidBody/GeneralizedCoordinates.h | 8 +- include/RigidBody/GeneralizedTorque.h | 11 +- include/RigidBody/GeneralizedVelocity.h | 8 +- include/RigidBody/IMU.h | 12 +- include/RigidBody/IMUs.h | 20 +- include/RigidBody/Joints.h | 7 +- include/RigidBody/Markers.h | 27 +- include/RigidBody/Mesh.h | 20 +- include/RigidBody/MeshFace.h | 6 +- include/RigidBody/NodeSegment.h | 42 +- include/RigidBody/RotoTransNodes.h | 26 +- include/RigidBody/Segment.h | 82 +-- include/RigidBody/SegmentCharacteristics.h | 26 +- include/Utils/Benchmark.h | 21 +- include/Utils/Equation.h | 31 +- include/Utils/Error.h | 9 +- include/Utils/IfStream.h | 27 +- include/Utils/Matrix.h | 12 +- include/Utils/Node.h | 29 +- include/Utils/Path.h | 75 +-- include/Utils/Quaternion.h | 113 ++-- include/Utils/Range.h | 7 +- include/Utils/Rotation.h | 65 +- include/Utils/RotoTrans.h | 77 +-- include/Utils/RotoTransNode.h | 34 +- include/Utils/Scalar.h | 3 + include/Utils/SpatialVector.h | 13 +- include/Utils/String.h | 28 +- include/Utils/Timer.h | 3 + include/Utils/UtilsEnum.h | 5 +- include/Utils/Vector.h | 15 +- include/Utils/Vector3d.h | 51 +- src/BiorbdModel.cpp | 16 +- src/ModelReader.cpp | 682 ++++++++++---------- src/RigidBody/Contacts.cpp | 46 +- src/RigidBody/GeneralizedAcceleration.cpp | 18 +- src/RigidBody/GeneralizedCoordinates.cpp | 18 +- src/RigidBody/GeneralizedTorque.cpp | 18 +- src/RigidBody/GeneralizedVelocity.cpp | 18 +- src/RigidBody/IMU.cpp | 16 +- src/RigidBody/IMUs.cpp | 34 +- src/RigidBody/Joints.cpp | 262 ++++---- src/RigidBody/Markers.cpp | 50 +- src/RigidBody/Mesh.cpp | 24 +- src/RigidBody/MeshFace.cpp | 4 +- src/RigidBody/NodeSegment.cpp | 54 +- src/RigidBody/RotoTransNodes.cpp | 46 +- src/RigidBody/Segment.cpp | 134 ++-- src/RigidBody/SegmentCharacteristics.cpp | 26 +- src/Utils/Benchmark.cpp | 38 +- src/Utils/Equation.cpp | 78 +-- src/Utils/Error.cpp | 14 +- src/Utils/IfStream.cpp | 92 +-- src/Utils/Matrix.cpp | 20 +- src/Utils/Node.cpp | 58 +- src/Utils/Path.cpp | 164 ++--- src/Utils/Quaternion.cpp | 232 +++---- src/Utils/Range.cpp | 22 +- src/Utils/Rotation.cpp | 108 ++-- src/Utils/RotoTrans.cpp | 126 ++-- src/Utils/RotoTransNode.cpp | 52 +- src/Utils/Scalar.cpp | 10 +- src/Utils/SpatialVector.cpp | 22 +- src/Utils/String.cpp | 64 +- src/Utils/Timer.cpp | 20 +- src/Utils/Vector.cpp | 48 +- src/Utils/Vector3d.cpp | 92 +-- 73 files changed, 2014 insertions(+), 1924 deletions(-) diff --git a/binding/biorbd_utils.i.in b/binding/biorbd_utils.i.in index bdf7c5cf..0dbdcc39 100644 --- a/binding/biorbd_utils.i.in +++ b/binding/biorbd_utils.i.in @@ -17,44 +17,44 @@ // Instantiate templates namespace std { -%template(VecBiorbdString) std::vector; -%template(PairBiorbdString) std::pair; +%template(VecBiorbdString) std::vector; +%template(PairBiorbdString) std::pair; -%template(VecBiorbdVector) std::vector; -%template(MatBiorbdVector) std::vector>; -%template(VecBiorbdSpatialVector) std::vector; -%template(MatBiorbdMatrix) std::vector; -%template(VecBiorbdRotoTrans) std::vector; -%template(MatBiorbdRotoTrans) std::vector>; -%template(VecBiorbdRototation) std::vector; -%template(MatBiorbdRototation) std::vector>; -%template(VecBiorbdNode) std::vector; -%template(MatBiorbdNode) std::vector>; -%template(VecBiorbdRange) std::vector; -%template(MatBiorbdRange) std::vector>; +%template(VecBiorbdVector) std::vector; +%template(MatBiorbdVector) std::vector>; +%template(VecBiorbdSpatialVector) std::vector; +%template(MatBiorbdMatrix) std::vector; +%template(VecBiorbdRotoTrans) std::vector; +%template(MatBiorbdRotoTrans) std::vector>; +%template(VecBiorbdRototation) std::vector; +%template(MatBiorbdRototation) std::vector>; +%template(VecBiorbdNode) std::vector; +%template(MatBiorbdNode) std::vector>; +%template(VecBiorbdRange) std::vector; +%template(MatBiorbdRange) std::vector>; } -%extend biorbd::utils::String{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::String{ std::string to_string(){ return *$self; } } -%extend biorbd::utils::Rotation{ - biorbd::utils::Rotation transpose(){ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation{ + biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation transpose(){ return $self->RigidBodyDynamics::Math::Matrix3d::transpose(); } - biorbd::utils::Rotation operator*( - const biorbd::utils::Rotation& other){ + biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation operator*( + const biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation& other){ return $self->RigidBodyDynamics::Math::Matrix3d::operator*(other); } } -%extend biorbd::utils::Vector3d{ - biorbd::utils::NODE_TYPE typeOfNode() const{ - return $self->biorbd::utils::Node::typeOfNode(); +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d{ + biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE typeOfNode() const{ + return $self->biorbd::BIORBD_MATH_NAMESPACE::utils::Node::typeOfNode(); } } diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 13aa31ff..08aa3454 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -26,13 +26,26 @@ %include "@CMAKE_BINARY_DIR@/include/biorbdConfig.h" #if defined(BIORBD_USE_CASADI_MATH) +#define SWIG_UTILS_STRING SWIGTYPE_p_biorbd__MathCasadi__utils__String +#define SWIG_UTILS_PATH SWIGTYPE_p_biorbd__MathCasadi__utils__Path +#define SWIG_UTILS_VECTOR SWIGTYPE_p_biorbd__MathCasadi__utils__Vector +#define SWIG_UTILS_NODE SWIGTYPE_p_biorbd__MathCasadi__utils__Node +#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_biorbd__MathCasadi__utils__SpatialVector + #define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Joints #define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedCoordinates #define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedVelocity #define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedAcceleration #define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedTorque #define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Markers + #elif defined(BIORBD_USE_EIGEN3_MATH) +#define SWIG_UTILS_STRING SWIGTYPE_p_biorbd__MathEigen3__utils__String +#define SWIG_UTILS_PATH SWIGTYPE_p_biorbd__MathEigen3__utils__Path +#define SWIG_UTILS_VECTOR SWIGTYPE_p_biorbd__MathEigen3__utils__Vector +#define SWIG_UTILS_NODE SWIGTYPE_p_biorbd__MathEigen3__utils__Node +#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_biorbd__MathEigen3__utils__SpatialVector + #define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Joints #define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedCoordinates #define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedVelocity @@ -42,9 +55,9 @@ #endif // -- STRING --// -%typemap(typecheck, precedence=2300) biorbd::utils::String &{ +%typemap(typecheck, precedence=2300) biorbd::BIORBD_MATH_NAMESPACE::utils::String &{ void *argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__String, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_STRING, 0 | 0)) && argp1) { // Test if it is a pointer to String already exists $1 = true; } else if( PyUnicode_Check($input) ) { @@ -54,24 +67,24 @@ $1 = false; } }; -%typemap(in) biorbd::utils::String &{ +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::String &{ void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__String, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_STRING, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::utils::String * >(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::String * >(argp1); } else if( PyUnicode_Check($input) ) { // Interpret the string - $1 = new biorbd::utils::String(PyUnicode_AsUTF8($input)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::String(PyUnicode_AsUTF8($input)); } else { - PyErr_SetString(PyExc_ValueError, "String must be a biorbd::utils::String or string"); + PyErr_SetString(PyExc_ValueError, "String must be a biorbd::BIORBD_MATH_NAMESPACE::utils::String or string"); SWIG_fail; } }; // --- Path --- // -%typemap(typecheck, precedence=2310) biorbd::utils::Path &{ +%typemap(typecheck, precedence=2310) biorbd::BIORBD_MATH_NAMESPACE::utils::Path &{ void *argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Path, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_PATH, 0 | 0)) && argp1) { // Test if it is a pointer to Path already exists $1 = true; } else if( PyUnicode_Check($input) ) { @@ -81,14 +94,14 @@ $1 = false; } }; -%typemap(in) biorbd::utils::Path &{ +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Path &{ void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Path, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_PATH, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::utils::Path * >(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Path * >(argp1); } else if( PyUnicode_Check($input) ) { // Interpret the string - $1 = new biorbd::utils::Path(PyUnicode_AsUTF8($input)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Path(PyUnicode_AsUTF8($input)); } else { PyErr_SetString(PyExc_ValueError, "Path must be a Path or string"); SWIG_fail; @@ -119,7 +132,7 @@ } // --- Allows for calling MX as scalar --- // -%typemap(typecheck, precedence=2400) biorbd::utils::Scalar& { +%typemap(typecheck, precedence=2400) biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar& { void *argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { @@ -147,13 +160,13 @@ $1 = false; } }; -%typemap(in) biorbd::utils::Scalar& { +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar& { void * argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { // Recast the pointer try{ - $1 = new biorbd::utils::Scalar(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*reinterpret_cast(argp1)); } catch (...){ PyErr_SetString(PyExc_ValueError, "Scalar must be a 1x1 array or a float"); SWIG_fail; @@ -176,7 +189,7 @@ } // Copy the actual data - $1 = new biorbd::utils::Scalar(*(double*)PyArray_GETPTR1((PyArrayObject*)data, 0)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR1((PyArrayObject*)data, 0)); } else if (ndim == 2) { unsigned int scalar_dim0(dims[0]); @@ -188,7 +201,7 @@ } // Copy the actual data - $1 = new biorbd::utils::Scalar(*(double*)PyArray_GETPTR2((PyArrayObject*)data, 0, 0)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR2((PyArrayObject*)data, 0, 0)); } else { PyErr_SetString(PyExc_ValueError, @@ -197,10 +210,10 @@ } } else if (PyFloat_Check($input)) { - $1 = new biorbd::utils::Scalar(PyFloat_AS_DOUBLE($input)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(PyFloat_AS_DOUBLE($input)); } else if (PyLong_Check($input)) { - $1 = new biorbd::utils::Scalar(static_cast(PyLong_AS_LONG($input))); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(static_cast(PyLong_AS_LONG($input))); } else { PyErr_SetString(PyExc_ValueError, "Scalar must be a 1x1 array or a float"); @@ -209,7 +222,7 @@ }; #ifdef BIORBD_USE_CASADI_MATH -%extend biorbd::utils::Scalar{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar{ casadi::MX to_mx(){ return *dynamic_cast($self); }; @@ -217,7 +230,7 @@ #endif // --- Quaternion --- // -%extend biorbd::utils::Quaternion{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Quaternion{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -247,7 +260,7 @@ } // --- Matrix --- // -%extend biorbd::utils::Matrix{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -360,7 +373,7 @@ #endif // --- Vector --- // -%extend biorbd::utils::Vector{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -382,12 +395,12 @@ }; #endif } -%typemap(typecheck, precedence=2150) biorbd::utils::Vector & { +%typemap(typecheck, precedence=2150) biorbd::BIORBD_MATH_NAMESPACE::utils::Vector & { void *argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { #else - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Vector, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_VECTOR, 0 | 0)) && argp1) { #endif // Test if it is a pointer an MX or Vector $1 = true; @@ -400,18 +413,18 @@ $1 = false; } } -%typemap(in) biorbd::utils::Vector & { +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Vector & { void * argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::utils::Vector(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(*reinterpret_cast(argp1)); #else if (SWIG_IsOK( SWIG_ConvertPtr($input, &argp1, - SWIGTYPE_p_biorbd__utils__Vector, 0 | 0)) + SWIG_UTILS_VECTOR, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::utils::Vector * >(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Vector * >(argp1); #endif } else if( PyArray_Check($input) ) { // Get dimensions of the data:: @@ -428,7 +441,7 @@ PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int n(dims[0]); - $1 = new biorbd::utils::Vector(n); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(n); for (unsigned int i=0; i(argp1); + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d * >(argp1); } #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(argp1))); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d(biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(*reinterpret_cast(argp1))); } #endif else if( PyArray_Check($input) ) { @@ -917,7 +930,7 @@ biorbd::utils::Vector3d &{ PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data - $1 = new biorbd::utils::Vector3d(0, 0, 0); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d(0, 0, 0); for (unsigned int i=0; i<3; ++i) (*$1)[i] = *(double*)PyArray_GETPTR1((PyArrayObject*)data, i); @@ -927,7 +940,7 @@ biorbd::utils::Vector3d &{ } }; -%extend biorbd::utils::Vector3d{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -950,7 +963,7 @@ biorbd::utils::Vector3d &{ }; // --- Spatial Vector --- // -%extend biorbd::utils::SpatialVector{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -971,9 +984,9 @@ biorbd::utils::Vector3d &{ } #endif }; -%typemap(typecheck, precedence=2010) biorbd::utils::SpatialVector &{ +%typemap(typecheck, precedence=2010) biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector &{ void *argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__SpatialVector, 0 | 0)) && argp1) { + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_SPATIAL_VECTOR, 0 | 0)) && argp1) { $1 = true; } #ifdef BIORBD_USE_CASADI_MATH @@ -989,14 +1002,14 @@ biorbd::utils::Vector3d &{ $1 = false; } } -%typemap(in) biorbd::utils::SpatialVector &{ +%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector &{ void * argp1 = 0; - if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__SpatialVector, 0 | 0)) && argp1) { - $1 = reinterpret_cast< biorbd::utils::SpatialVector * >(argp1); + if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_SPATIAL_VECTOR, 0 | 0)) && argp1) { + $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::utils::SpatialVector(*reinterpret_cast(argp1)); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -1021,7 +1034,7 @@ biorbd::utils::Vector3d &{ "SpatialVector must be a (6,) vector when using a numpy array"); SWIG_fail; } - $1 = new biorbd::utils::SpatialVector(); + $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector(); for (unsigned int sv=0; sv($self); @@ -1082,7 +1095,7 @@ biorbd::utils::Vector3d &{ #endif }; -%extend biorbd::utils::Rotation{ +%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); diff --git a/include/BiorbdModel.h b/include/BiorbdModel.h index d9222364..caf1421d 100644 --- a/include/BiorbdModel.h +++ b/include/BiorbdModel.h @@ -57,19 +57,21 @@ /// \brief Returns the current version of biorbd /// \return The current version of biorbd /// -biorbd::utils::String getVersion(); +biorbd::BIORBD_MATH_NAMESPACE::utils::String getVersion(); namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ /// /// \brief The actual musculoskeletal model that holds everything in biorbd /// class BIORBD_API Model : - public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints - ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers - ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::IMUs - ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::RotoTransNodes - ,public biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Contacts + public rigidbody::Joints + ,public rigidbody::Markers + ,public rigidbody::IMUs + ,public rigidbody::RotoTransNodes + ,public rigidbody::Contacts #ifdef MODULE_ACTUATORS ,public biorbd::actuator::Actuators #endif @@ -88,18 +90,19 @@ class BIORBD_API Model : /// \param path The path of the file /// Model( - const biorbd::utils::Path& path); + const utils::Path& path); private: - std::shared_ptr m_path; + std::shared_ptr m_path; public: /// /// \brief Returns the path of .bioMod file used to load the model. If no file was used, it remains empty /// \return The path of .bioMod file used to load the model /// - biorbd::utils::Path path() const; + utils::Path path() const; }; +} } #endif // BIORBD_MODEL_H diff --git a/include/ModelReader.h b/include/ModelReader.h index 7fd059ae..072f550d 100644 --- a/include/ModelReader.h +++ b/include/ModelReader.h @@ -8,16 +8,15 @@ namespace biorbd { -class Model; - namespace BIORBD_MATH_NAMESPACE { +class Model; + namespace rigidbody { class GeneralizedCoordinates; class Mesh; } -} namespace utils { @@ -41,8 +40,8 @@ class BIORBD_API Reader /// \brief Create a biorbd model from a bioMod file /// \param path The path of the file /// - static biorbd::Model readModelFile( - const biorbd::utils::Path &path); + static Model readModelFile( + const utils::Path &path); /// /// \brief Create a biorbd model from a bioMod file @@ -51,15 +50,15 @@ class BIORBD_API Reader /// \return Returns the model to fill /// static void readModelFile( - const biorbd::utils::Path &path, - biorbd::Model *model); + const utils::Path &path, + Model *model); /// /// \brief Read a bioMark file, containing markers data /// \param path The path of the file /// \return Returns the markers /// - static std::vector> readMarkerDataFile( + static std::vector> readMarkerDataFile( const utils::Path &path); /// @@ -68,31 +67,31 @@ class BIORBD_API Reader /// \return Returns the kinematics /// static std::vector readQDataFile( - const biorbd::utils::Path &path); + const utils::Path &path); /// /// \brief Read a bioMus fine, containing muscle activations data /// \param path The path of the file /// \return Returns the activations /// - static std::vector readActivationDataFile( - const biorbd::utils::Path &path); + static std::vector readActivationDataFile( + const utils::Path &path); /// /// \brief Read a bioTorque file containing generalized torques data /// \param path The path of the file /// \return Returns the torque /// - static std::vector readTorqueDataFile( - const biorbd::utils::Path &path); + static std::vector readTorqueDataFile( + const utils::Path &path); /// /// \brief Read a bioGRF file containing ground reaction force (GRF) data /// \param path The path of the file /// \return Returns the ground reaction force /// - static std::vector readGroundReactionForceDataFile( - const biorbd::utils::Path &path); + static std::vector readGroundReactionForceDataFile( + const utils::Path &path); /// /// \brief Read a Vicon ASCII force file @@ -104,21 +103,21 @@ class BIORBD_API Reader /// \param cop The center of pressure (x,y,z) (output) /// static void readViconForceFile( - const biorbd::utils::Path &path, + const utils::Path &path, std::vector> &frame, std::vector &frequency, - std::vector> &force, - std::vector> &moment, - std::vector> &cop); + std::vector> &force, + std::vector> &moment, + std::vector> &cop); /// /// \brief Read a Vicon ASCII force file /// \param path The path of the file /// \return Returns all the data in a spatial transform format /// - static std::vector> + static std::vector> readViconForceFile( - const biorbd::utils::String &path); + const utils::String &path); /// /// \brief Read a Vicon ASCII marker file (CSV formated) @@ -129,9 +128,9 @@ class BIORBD_API Reader /// frames are evenly skip while reading. To get all frames, nFramesToGet /// should be set to -1. /// - static std::vector> + static std::vector> readViconMarkerFile( - const biorbd::utils::Path &path, + const utils::Path &path, int nFramesToGet =-1); /// @@ -148,9 +147,9 @@ class BIORBD_API Reader /// frames are evenly skip while reading. To get all frames, nFramesToGet /// should be set to -1. /// - static std::vector> readViconMarkerFile( - const biorbd::utils::Path &path, - std::vector &markOrder, + static std::vector> readViconMarkerFile( + const utils::Path &path, + std::vector &markOrder, int nFramesToGet = -1); /// @@ -159,7 +158,7 @@ class BIORBD_API Reader /// \return Returns the mesh /// static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileBiorbdSegments( - const biorbd::utils::Path& path); + const utils::Path& path); /// /// \brief Read a PLY file containing the meshing of a segment @@ -167,7 +166,7 @@ class BIORBD_API Reader /// \return Returns the mesh /// static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFilePly( - const biorbd::utils::Path& path); + const utils::Path& path); /// /// \brief Read a OBJ file containing the meshing of a segment @@ -175,7 +174,7 @@ class BIORBD_API Reader /// \return Returns the mesh /// static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileObj( - const biorbd::utils::Path& path); + const utils::Path& path); #ifdef MODULE_VTP_FILES_READER /// @@ -184,7 +183,7 @@ class BIORBD_API Reader /// \return Returns the mesh /// static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileVtp( - const biorbd::utils::Path& path); + const utils::Path& path); #endif protected: @@ -195,9 +194,9 @@ class BIORBD_API Reader /// \param vector The vector to fill /// static void readVector3d( - biorbd::utils::IfStream& file, - const std::map& variable, - biorbd::utils::Vector3d &vector); + utils::IfStream& file, + const std::map& variable, + utils::Vector3d &vector); /// /// \brief Read a Matrix 3x3 @@ -206,8 +205,8 @@ class BIORBD_API Reader /// \param matrix The matrix to fill /// static void readMatrix33( - biorbd::utils::IfStream& file, - const std::map& variable, + utils::IfStream& file, + const std::map& variable, RigidBodyDynamics::Math::Matrix3d &matrix); /// @@ -219,12 +218,13 @@ class BIORBD_API Reader /// \param RT_T The translation part to fill /// static void readRtMatrix( - biorbd::utils::IfStream& file, - const std::map& variable, + utils::IfStream& file, + const std::map& variable, bool RTinMatrix, - biorbd::utils::RotoTrans &RT); + utils::RotoTrans &RT); }; +} } #endif // BIORBD_UTILS_READ_H diff --git a/include/RigidBody/Contacts.h b/include/RigidBody/Contacts.h index 9754694f..f2ea778b 100644 --- a/include/RigidBody/Contacts.h +++ b/include/RigidBody/Contacts.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class RotoTrans; @@ -16,12 +18,7 @@ class Vector; class String; class SpatialVector; } -} -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class GeneralizedCoordinates; @@ -64,9 +61,9 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// unsigned int AddConstraint( unsigned int body_id, - const biorbd::utils::Vector3d &body_point, - const biorbd::utils::Vector3d &world_normal, - const biorbd::utils::String& name, + const utils::Vector3d &body_point, + const utils::Vector3d &world_normal, + const utils::String& name, double acc = 0); /// @@ -80,9 +77,9 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet unsigned int AddConstraint( unsigned int body_id, - const biorbd::utils::Vector3d &body_point, - const biorbd::utils::String& axis, - const biorbd::utils::String& name, + const utils::Vector3d &body_point, + const utils::String& axis, + const utils::String& name, double acc = 0); /// @@ -99,10 +96,10 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet unsigned int AddLoopConstraint( unsigned int body_id_predecessor, unsigned int body_id_successor, - const biorbd::utils::RotoTrans& X_predecessor, - const biorbd::utils::RotoTrans& X_successor, - const biorbd::utils::SpatialVector& axis, - const biorbd::utils::String& name, + const utils::RotoTrans& X_predecessor, + const utils::RotoTrans& X_successor, + const utils::SpatialVector& axis, + const utils::String& name, bool enableStabilization = false, double stabilizationParam = 0.1); @@ -133,14 +130,14 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// \brief Return the name of the all contacts /// \return The name of the contacts /// - std::vector contactNames(); + std::vector contactNames(); /// /// \brief Return the name of the contact of a specified axis /// \param i The axis /// \return The name of the contact of a specified axis /// - biorbd::utils::String contactName(unsigned int i); + utils::String contactName(unsigned int i); /// /// \brief Return the contraints position in the global reference @@ -148,7 +145,7 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// \param updateKin Whether the kinematics of the model should be updated from Q /// \return The contraints positions in the global reference /// - std::vector constraintsInGlobal( + std::vector constraintsInGlobal( const GeneralizedCoordinates &Q, bool updateKin); @@ -156,7 +153,7 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet /// \brief Return the force acting on the contraint /// \return The force acting on the contraint /// - biorbd::utils::Vector getForce() const; + utils::Vector getForce() const; protected: std::shared_ptr m_nbreConstraint; ///< Number of constraints diff --git a/include/RigidBody/GeneralizedAcceleration.h b/include/RigidBody/GeneralizedAcceleration.h index 55ed518d..ea0c57d0 100644 --- a/include/RigidBody/GeneralizedAcceleration.h +++ b/include/RigidBody/GeneralizedAcceleration.h @@ -15,7 +15,7 @@ class Joints; /// /// \brief Class GeneralizedAcceleration /// -class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector +class BIORBD_API GeneralizedAcceleration : public utils::Vector { public: @@ -60,7 +60,7 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector /// template GeneralizedAcceleration( const Eigen::MatrixBase& other) : - biorbd::utils::Vector(other) {} + utils::Vector(other) {} #endif @@ -94,7 +94,7 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector GeneralizedAcceleration& operator=( const Eigen::MatrixBase & other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); return *this; } @@ -105,7 +105,7 @@ class BIORBD_API GeneralizedAcceleration : public biorbd::utils::Vector /// \param other The vector to copy /// void operator=( - const biorbd::utils::Vector& other); + const utils::Vector& other); #ifdef BIORBD_USE_CASADI_MATH /// diff --git a/include/RigidBody/GeneralizedCoordinates.h b/include/RigidBody/GeneralizedCoordinates.h index 8b253233..6f2d9dce 100644 --- a/include/RigidBody/GeneralizedCoordinates.h +++ b/include/RigidBody/GeneralizedCoordinates.h @@ -15,7 +15,7 @@ class Joints; /// /// \brief Class GeneralizedCoordinates /// -class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector +class BIORBD_API GeneralizedCoordinates : public utils::Vector { public: @@ -60,7 +60,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector /// template GeneralizedCoordinates( const Eigen::MatrixBase& other) : - biorbd::utils::Vector(other) {} + utils::Vector(other) {} #endif @@ -94,7 +94,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector GeneralizedCoordinates& operator=( const Eigen::MatrixBase & other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); return *this; } @@ -105,7 +105,7 @@ class BIORBD_API GeneralizedCoordinates : public biorbd::utils::Vector /// \param other The vector to copy /// void operator=( - const biorbd::utils::Vector& other); + const utils::Vector& other); #ifdef BIORBD_USE_CASADI_MATH diff --git a/include/RigidBody/GeneralizedTorque.h b/include/RigidBody/GeneralizedTorque.h index 353fc416..66101afa 100644 --- a/include/RigidBody/GeneralizedTorque.h +++ b/include/RigidBody/GeneralizedTorque.h @@ -6,7 +6,8 @@ namespace biorbd { -namespace BIORBD_MATH_NAMESPACE { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class Joints; @@ -14,7 +15,7 @@ class Joints; /// /// \brief Class GeneralizedTorque /// -class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector +class BIORBD_API GeneralizedTorque : public utils::Vector { public: @@ -58,7 +59,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector /// template GeneralizedTorque( const Eigen::MatrixBase& other) : - biorbd::utils::Vector(other) {} + utils::Vector(other) {} #endif @@ -87,7 +88,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector GeneralizedTorque& operator=(const Eigen::MatrixBase & other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); return *this; } @@ -98,7 +99,7 @@ class BIORBD_API GeneralizedTorque : public biorbd::utils::Vector /// \param other The vector to copy /// void operator=( - const biorbd::utils::Vector& other); + const utils::Vector& other); #ifdef BIORBD_USE_CASADI_MATH /// diff --git a/include/RigidBody/GeneralizedVelocity.h b/include/RigidBody/GeneralizedVelocity.h index 87c3258e..7b65ff1d 100644 --- a/include/RigidBody/GeneralizedVelocity.h +++ b/include/RigidBody/GeneralizedVelocity.h @@ -15,7 +15,7 @@ class Joints; /// /// \brief Class GeneralizedVelocity /// -class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector +class BIORBD_API GeneralizedVelocity : public utils::Vector { public: @@ -58,7 +58,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector /// template GeneralizedVelocity( const Eigen::MatrixBase& other) : - biorbd::utils::Vector(other) {} + utils::Vector(other) {} #endif #ifdef BIORBD_USE_CASADI_MATH @@ -89,7 +89,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector GeneralizedVelocity& operator=( const Eigen::MatrixBase & other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); return *this; } @@ -100,7 +100,7 @@ class BIORBD_API GeneralizedVelocity : public biorbd::utils::Vector /// \param other The vector to copy /// void operator=( - const biorbd::utils::Vector& other); + const utils::Vector& other); #ifdef BIORBD_USE_CASADI_MATH diff --git a/include/RigidBody/IMU.h b/include/RigidBody/IMU.h index 3f8929f3..d9666440 100644 --- a/include/RigidBody/IMU.h +++ b/include/RigidBody/IMU.h @@ -7,13 +7,13 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { @@ -23,7 +23,7 @@ namespace rigidbody #ifdef SWIG class BIORBD_API IMU #else -class BIORBD_API IMU : public biorbd::utils::RotoTransNode +class BIORBD_API IMU : public utils::RotoTransNode #endif { public: @@ -44,7 +44,7 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode /// \param isAnatomical True if the IMU is an anatomical IMU /// IMU( - const biorbd::utils::RotoTransNode& RotoTrans, + const utils::RotoTransNode& RotoTrans, bool isTechnical = true, bool isAnatomical = true); @@ -55,7 +55,7 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode /// template IMU( const Eigen::MatrixBase& other) : - biorbd::utils::RotoTransNode(other) {} + utils::RotoTransNode(other) {} #endif #ifdef BIORBD_USE_CASADI_MATH IMU( @@ -99,7 +99,7 @@ class BIORBD_API IMU : public biorbd::utils::RotoTransNode template IMU& operator=(const Eigen::MatrixBase & other) { - this->biorbd::utils::RotoTransNode::operator=(other); + this->utils::RotoTransNode::operator=(other); return *this; } #endif diff --git a/include/RigidBody/IMUs.h b/include/RigidBody/IMUs.h index 2d37233d..c01e8df1 100644 --- a/include/RigidBody/IMUs.h +++ b/include/RigidBody/IMUs.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -14,8 +16,6 @@ class Matrix; class RotoTransNode; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class GeneralizedCoordinates; @@ -74,7 +74,7 @@ class BIORBD_API IMUs /// \param anatomical True if the IMU is anatomical /// void addIMU( - const biorbd::utils::RotoTransNode &RotoTrans, + const utils::RotoTransNode &RotoTrans, bool technical = true, bool anatomical = true); @@ -88,19 +88,19 @@ class BIORBD_API IMUs /// \brief Return the names of the inertial measurement units (IMU) /// \return The names of the IMU /// - std::vector IMUsNames(); + std::vector IMUsNames(); /// /// \brief Return the names of the technical inertial measurement units (IMU) /// \return The names of the technical IMU /// - std::vector technicalIMUsNames(); + std::vector technicalIMUsNames(); /// /// \brief Return the names of the anatomical inertial measurement units (IMU) /// \return The names of the anatomical IMU /// - std::vector anatomicalIMUsNames(); + std::vector anatomicalIMUsNames(); /// @@ -115,7 +115,7 @@ class BIORBD_API IMUs /// \return All the IMU of attached to the segment /// std::vector IMU( - const biorbd::utils::String& segmentName); + const utils::String& segmentName); /// /// \brief Return the inertial measurement unit (IMU) of a specified index @@ -209,7 +209,7 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return The jacobien of the IMU /// - std::vector IMUJacobian( + std::vector IMUJacobian( const GeneralizedCoordinates &Q, bool updateKin = true); @@ -219,7 +219,7 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \return The jacobian of the technical IMU /// - std::vector TechnicalIMUJacobian( + std::vector TechnicalIMUJacobian( const GeneralizedCoordinates &Q, bool updateKin = true); @@ -231,7 +231,7 @@ class BIORBD_API IMUs /// \param updateKin If the model should be updated /// \param lookForTechnical If true, only computes for the technical IMU /// - std::vector IMUJacobian( + std::vector IMUJacobian( const GeneralizedCoordinates &Q, bool updateKin, bool lookForTechnical); diff --git a/include/RigidBody/Joints.h b/include/RigidBody/Joints.h index c55f0876..07ee57ed 100644 --- a/include/RigidBody/Joints.h +++ b/include/RigidBody/Joints.h @@ -9,6 +9,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -19,12 +21,7 @@ class Vector3d; class Range; class SpatialVector; } -} -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class GeneralizedCoordinates; diff --git a/include/RigidBody/Markers.h b/include/RigidBody/Markers.h index befeada4..7decb56c 100644 --- a/include/RigidBody/Markers.h +++ b/include/RigidBody/Markers.h @@ -7,13 +7,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; class Matrix; } -namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class GeneralizedCoordinates; @@ -67,11 +68,11 @@ class BIORBD_API Markers /// void addMarker( const NodeSegment &pos, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, + const utils::String &name, + const utils::String &parentName, bool technical, bool anatomical, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, int id = -1 ); @@ -89,25 +90,25 @@ class BIORBD_API Markers /// \return The markers on the segment /// std::vector marker( - const biorbd::utils::String &name) const; + const utils::String &name) const; /// /// \brief Return the names of all the markers /// \return The names of the markers /// - std::vector markerNames() const; + std::vector markerNames() const; /// /// \brief Return the names of all the technical markers /// \return The names of the technical markers /// - std::vector technicalMarkerNames() const; + std::vector technicalMarkerNames() const; /// /// \brief Return the names of all the anatomical markers /// \return The names of the anatomical markers /// - std::vector anatomicalMarkerNames() const; + std::vector anatomicalMarkerNames() const; /// /// \brief Compute and return the position of a marker at given Q in the global reference frame @@ -327,7 +328,7 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The jacobian of the markers /// - std::vector markersJacobian( + std::vector markersJacobian( const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -339,7 +340,7 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The jacobian of the technical markers /// - std::vector technicalMarkersJacobian( + std::vector technicalMarkersJacobian( const GeneralizedCoordinates &Q, bool removeAxis=true, bool updateKin = true); @@ -352,9 +353,9 @@ class BIORBD_API Markers /// \param updateKin If the model should be updated /// \return The jacobian of the chosen marker /// - biorbd::utils::Matrix markersJacobian( + utils::Matrix markersJacobian( const GeneralizedCoordinates &Q, - const biorbd::utils::String& parentName, + const utils::String& parentName, const NodeSegment& p, bool updateKin); @@ -382,7 +383,7 @@ class BIORBD_API Markers /// \param lookForTechnical Check if only technical markers are to be computed /// \return The jacobian of the markers /// - std::vector markersJacobian( + std::vector markersJacobian( const GeneralizedCoordinates &Q, bool removeAxis, bool updateKin, diff --git a/include/RigidBody/Mesh.h b/include/RigidBody/Mesh.h index ff882b19..d88fb16a 100644 --- a/include/RigidBody/Mesh.h +++ b/include/RigidBody/Mesh.h @@ -7,14 +7,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; class Path; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class MeshFace; @@ -35,7 +35,7 @@ class BIORBD_API Mesh /// \brief other The other mesh /// Mesh( - const std::vector& other); + const std::vector& other); /// /// \brief Construct mesh @@ -43,7 +43,7 @@ class BIORBD_API Mesh /// \param faces The faces of the geometry /// Mesh( - const std::vector& vertex, + const std::vector& vertex, const std::vector& faces); /// @@ -64,14 +64,14 @@ class BIORBD_API Mesh /// \param node The point to add /// void addPoint( - const biorbd::utils::Vector3d& node); + const utils::Vector3d& node); /// /// \brief Return the point of a specific index /// \param idx The index of the point /// \return The point of a specific index /// - const biorbd::utils::Vector3d& point( + const utils::Vector3d& point( unsigned int idx) const; /// @@ -118,19 +118,19 @@ class BIORBD_API Mesh /// \param path Path for the mesh file /// void setPath( - const biorbd::utils::Path& path); + const utils::Path& path); /// /// \brief Return the path of the mesh file /// \return The path of the mesh file /// - const biorbd::utils::Path& path() const; + const utils::Path& path() const; protected: - std::shared_ptr> m_vertex; ///< The vertex + std::shared_ptr> m_vertex; ///< The vertex std::shared_ptr> m_faces; ///< The faces - std::shared_ptr m_pathFile; ///< The path to the mesh file + std::shared_ptr m_pathFile; ///< The path to the mesh file }; } diff --git a/include/RigidBody/MeshFace.h b/include/RigidBody/MeshFace.h index 6a4359f7..8f2fea87 100644 --- a/include/RigidBody/MeshFace.h +++ b/include/RigidBody/MeshFace.h @@ -7,13 +7,13 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { @@ -66,7 +66,7 @@ class BIORBD_API MeshFace /// \brief convert the integer nature of the face to a double /// \return The vertex index in double format /// - biorbd::utils::Vector3d faceAsDouble(); + utils::Vector3d faceAsDouble(); /// /// \brief Returns the face diff --git a/include/RigidBody/NodeSegment.h b/include/RigidBody/NodeSegment.h index ba8027da..96b292b5 100644 --- a/include/RigidBody/NodeSegment.h +++ b/include/RigidBody/NodeSegment.h @@ -7,20 +7,20 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { /// /// \brief A point attached to a segment, generally speaking a skin marker /// -class BIORBD_API NodeSegment : public biorbd::utils::Vector3d +class BIORBD_API NodeSegment : public utils::Vector3d { public: /// @@ -35,16 +35,16 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \param z Z-Component of the node /// NodeSegment( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z); /// /// \brief Construct a segment node from another node /// \param other The other node /// NodeSegment( - const biorbd::utils::Vector3d& other); + const utils::Vector3d& other); /// /// \brief Construct a segment node @@ -59,14 +59,14 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \param parentID The index of the parent segment /// NodeSegment( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String& name, - const biorbd::utils::String& parentName, + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String& name, + const utils::String& parentName, bool isTechnical, bool isAnatomical, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, int parentID); /// @@ -80,12 +80,12 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \param parentID The index of the parent segment /// NodeSegment( - const biorbd::utils::Vector3d& node, - const biorbd::utils::String& name, - const biorbd::utils::String& parentName, + const utils::Vector3d& node, + const utils::String& name, + const utils::String& parentName, bool isTechnical, bool isAnatomical, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, int parentID); /// @@ -148,7 +148,7 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \brief Add axis to remove /// \param axis The name of the axis to remove ("x", "y" or "z") /// - void addAxesToRemove(const biorbd::utils::String& axis); + void addAxesToRemove(const utils::String& axis); /// /// \brief Add multiple axes to remove @@ -160,13 +160,13 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d /// \brief Add multiple axes to remove /// \param axes The multiples axes names to remove ("x", "y" or "z") /// - void addAxesToRemove(const std::vector& axes); + void addAxesToRemove(const std::vector& axes); /// /// \brief Return the axes to removed /// \return The axes to removed /// - biorbd::utils::String axesToRemove(); + utils::String axesToRemove(); /// /// \brief Return the number of axes to remove @@ -184,7 +184,7 @@ class BIORBD_API NodeSegment : public biorbd::utils::Vector3d NodeSegment & operator=(const Eigen::MatrixBase & other) { - this->biorbd::utils::Vector3d::operator=(other); + this->utils::Vector3d::operator=(other); return *this; } #endif diff --git a/include/RigidBody/RotoTransNodes.h b/include/RigidBody/RotoTransNodes.h index 8b9af1c6..928816a1 100644 --- a/include/RigidBody/RotoTransNodes.h +++ b/include/RigidBody/RotoTransNodes.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -15,8 +17,6 @@ class Matrix; class RotoTransNode; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class GeneralizedCoordinates; @@ -67,7 +67,7 @@ class BIORBD_API RotoTransNodes /// \param RotoTrans The RotaTrans of the RT /// void addRT( - const biorbd::utils::RotoTransNode &RotoTrans); + const utils::RotoTransNode &RotoTrans); /// /// \brief Return the number of RTs in the set @@ -85,28 +85,28 @@ class BIORBD_API RotoTransNodes /// \brief Return the names of the RTs /// \return The names of the RTs /// - std::vector RTsNames(); + std::vector RTsNames(); /// /// \brief Return all the RTs in the local reference of the segment /// \return All the RTs in local reference frame /// - const std::vector& RTs() const; + const std::vector& RTs() const; /// /// \brief Return all the RTs of a segment /// \param segmentName The name of the segment to return the RTs /// \return All the RTs of attached to the segment /// - std::vector RTs( - const biorbd::utils::String& segmentName); + std::vector RTs( + const utils::String& segmentName); /// /// \brief Return the RTs of a specified index /// \param idx The index of the RT in the set /// \return RT of idx i /// - const biorbd::utils::RotoTransNode& RT( + const utils::RotoTransNode& RT( unsigned int idx); /// @@ -115,7 +115,7 @@ class BIORBD_API RotoTransNodes /// \param updateKin If the model should be updated /// \return All the RTs at the position given by Q /// - std::vector RTs( + std::vector RTs( const GeneralizedCoordinates& Q, bool updateKin = true); @@ -126,7 +126,7 @@ class BIORBD_API RotoTransNodes /// \param updateKin If the model should be updated /// \return The RT of index idx at the position given by Q /// - biorbd::utils::RotoTransNode RT( + utils::RotoTransNode RT( const GeneralizedCoordinates&Q, unsigned int idx, bool updateKin = true); @@ -138,7 +138,7 @@ class BIORBD_API RotoTransNodes /// \param updateKin If the model should be updated /// \return All the RTs on the segment of index idx /// - std::vector segmentRTs( + std::vector segmentRTs( const GeneralizedCoordinates& Q, unsigned int idx, bool updateKin = true); @@ -149,12 +149,12 @@ class BIORBD_API RotoTransNodes /// \param updateKin If the model should be updated /// \return The jacobien of the RTs /// - std::vector RTsJacobian( + std::vector RTsJacobian( const GeneralizedCoordinates &Q, bool updateKin = true); protected: - std::shared_ptr> + std::shared_ptr> m_RTs; ///< All the RTs }; diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index 202fe8dc..b9dff078 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -9,14 +9,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class RotoTrans; class Range; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class Joints; @@ -25,7 +25,7 @@ class SegmentCharacteristics; /// /// \brief Description of a segment /// -class BIORBD_API Segment : public biorbd::utils::Node +class BIORBD_API Segment : public utils::Node { public: /// @@ -49,13 +49,13 @@ class BIORBD_API Segment : public biorbd::utils::Node /// Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &name, + const utils::String &parentName, + const utils::String &seqT, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF = -1); @@ -75,12 +75,12 @@ class BIORBD_API Segment : public biorbd::utils::Node /// Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &name, + const utils::String &parentName, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF = -1); @@ -119,33 +119,33 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \brief Return the translation sequence in text /// \return The translation sequence in text /// - const biorbd::utils::String& seqT() const; + const utils::String& seqT() const; /// /// \brief Return the angle sequence in text /// \return The angle sequence in text /// - const biorbd::utils::String& seqR() const; + const utils::String& seqR() const; /// /// \brief Return the ranges for all the dof, translations and rotations respectively /// \return The ranges for all the dof, translations and rotations respectively /// - const std::vector& + const std::vector& QRanges() const; /// /// \brief Return the ranges for all the dof velocity, translations and rotations respectively /// \return The ranges for all the dof velocity, translations and rotations respectively /// - const std::vector& + const std::vector& QDotRanges() const; /// /// \brief Return the ranges for all the dof acceleration, translations and rotations respectively /// \return The ranges for all the dofa acceleration, translations and rotations respectively /// - const std::vector& + const std::vector& QDDotRanges() const; /// @@ -197,20 +197,20 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \return The index of a specified DoF /// unsigned int getDofIdx( - const biorbd::utils::String &dofName) const; + const utils::String &dofName) const; /// /// \brief Return the name of the specified DoF /// \return The name of the specified DoF /// - const biorbd::utils::String& nameDof( + const utils::String& nameDof( const unsigned int i) const; /// /// \brief Return the joint coordinate system (JCS) in the parent reference frame /// \return The joint coordinate system in the parent reference frame /// - biorbd::utils::RotoTrans localJCS() const; + utils::RotoTrans localJCS() const; /// /// \brief updateCharacteristics Change the inertia characteristics of the segment @@ -271,11 +271,11 @@ class BIORBD_API Segment : public biorbd::utils::Node /// void setDofs( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges); + const utils::String &seqT, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges); /// /// \brief Set the total number of DoF @@ -286,13 +286,13 @@ class BIORBD_API Segment : public biorbd::utils::Node unsigned int nbTrans, unsigned int nbRot); - std::shared_ptr m_seqT; ///< Translation sequence - std::shared_ptr m_seqR; ///< Euler rotation sequence - std::shared_ptr> + std::shared_ptr m_seqT; ///< Translation sequence + std::shared_ptr m_seqR; ///< Euler rotation sequence + std::shared_ptr> m_QRanges; ///< Minimum and maximum coordinate values that each dof should hold. This is only prescriptive and can be ignored when setting the GeneralizedCoordinates - std::shared_ptr> + std::shared_ptr> m_QDotRanges; ///< Minimum and maximum velocity values that each dof should hold. This is only prescriptive and can be ignored when setting the GeneralizedVelocities - std::shared_ptr> + std::shared_ptr> m_QDDotRanges; ///< Minimum and maximum acceleration values that each dof should hold. This is only prescriptive and can be ignored when setting the GeneralizedAccelerations std::shared_ptr m_nbDof; ///< Number of degrees of freedom std::shared_ptr m_nbQdot; ///< Number of generalized velocities @@ -317,7 +317,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// /// If seqR is equal to "q" then it is a quaternion /// - void determineIfRotIsQuaternion(const biorbd::utils::String &seqR); + void determineIfRotIsQuaternion(const utils::String &seqR); std::shared_ptr> m_dof; ///< Actual DoF: t1, t2, t3, r1, r2, r3; where the order depends on seqT and seqR @@ -330,8 +330,8 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \param seqR Angle sequence of the Euler rotations /// void setSequence( - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR); + const utils::String &seqT, + const utils::String &seqR); /// /// \brief Fill the transation and rotation sequences @@ -347,7 +347,7 @@ class BIORBD_API Segment : public biorbd::utils::Node /// void str2numSequence( std::vector &sequenceInteger, - const biorbd::utils::String &sequenceText); + const utils::String &sequenceText); /// /// \brief Store the sequences @@ -355,14 +355,14 @@ class BIORBD_API Segment : public biorbd::utils::Node /// \param seqR Angle sequence of the Euler rotations /// void str2numSequence( - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR); + const utils::String &seqT, + const utils::String &seqR); std::shared_ptr> m_sequenceTrans; ///< Translation sequence std::shared_ptr> m_sequenceRot; ///< Euler rotation sequence - std::shared_ptr> + std::shared_ptr> m_nameDof; ///< To store the DoF names /// diff --git a/include/RigidBody/SegmentCharacteristics.h b/include/RigidBody/SegmentCharacteristics.h index 4a13fea1..0f3a2c1c 100644 --- a/include/RigidBody/SegmentCharacteristics.h +++ b/include/RigidBody/SegmentCharacteristics.h @@ -9,13 +9,13 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; } -namespace BIORBD_MATH_NAMESPACE -{ namespace rigidbody { class Mesh; @@ -43,8 +43,8 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body /// \param inertia The inertia matrix /// SegmentCharacteristics( - const biorbd::utils::Scalar& mass, - const biorbd::utils::Vector3d &com, + const utils::Scalar& mass, + const utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia); /// @@ -55,8 +55,8 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body /// \param mesh The mesh geometry of the segment /// SegmentCharacteristics( - const biorbd::utils::Scalar &mass, - const biorbd::utils::Vector3d &com, + const utils::Scalar &mass, + const utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia, const Mesh &mesh); @@ -77,37 +77,37 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body /// \param val Value of the new length /// void setLength( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Returns the segment length /// \return The segment length /// - biorbd::utils::Scalar length() const; + utils::Scalar length() const; /// /// \brief Set the new mass /// \param newMass The new mass /// - void setMass(const biorbd::utils::Scalar& newMass); + void setMass(const utils::Scalar& newMass); /// /// \brief Returns the segment mass /// \return The segment mass /// - biorbd::utils::Scalar mass() const; + utils::Scalar mass() const; /// /// \brief CoM Returns the position of the center of mass in the local reference frame /// \return The position of the center of mass in the local reference frame /// - biorbd::utils::Vector3d CoM() const; + utils::Vector3d CoM() const; /// /// \brief setCoM Change the position of the center of mass /// \param com The new position for the CoM /// - void setCoM(const biorbd::utils::Vector3d& com); + void setCoM(const utils::Vector3d& com); /// /// \brief Returns the segment mesh @@ -122,7 +122,7 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body const RigidBodyDynamics::Math::Matrix3d& inertia() const; protected: - std::shared_ptr m_length; ///< Length of the segment + std::shared_ptr m_length; ///< Length of the segment std::shared_ptr m_mesh; ///< Mesh of the segment }; diff --git a/include/Utils/Benchmark.h b/include/Utils/Benchmark.h index 12e32623..d58f75bf 100644 --- a/include/Utils/Benchmark.h +++ b/include/Utils/Benchmark.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -32,7 +34,7 @@ class BIORBD_API Benchmark /// \param force If force is true, the timer starts even though it is already started /// void startTimer( - const biorbd::utils::String& name, + const String& name, bool force = false); /// @@ -40,14 +42,14 @@ class BIORBD_API Benchmark /// \param name Name of the timer to pause /// void pauseTimer( - const biorbd::utils::String& name); + const String& name); /// /// \brief Resume a timer of a specified name /// \param name Name of the timer to resume /// void resumeTimer( - const biorbd::utils::String& name); + const String& name); /// /// \brief Get lap time of a specified timer @@ -55,7 +57,7 @@ class BIORBD_API Benchmark /// \return The lap time of a specified timer /// double getLap( - const biorbd::utils::String& name); + const String& name); /// /// \brief Stop the timer of a specified name and get lap time @@ -63,7 +65,7 @@ class BIORBD_API Benchmark /// \return The lap time of a specified timer /// double stopTimer( - const biorbd::utils::String& name); + const String& name); /// /// \brief To waste time (similar to a sleep function) @@ -76,7 +78,7 @@ class BIORBD_API Benchmark /// \param name The name of the timer to add /// void addTimer( - const biorbd::utils::String& name); + const String& name); /// /// \brief Get the index of the specified timer @@ -84,14 +86,15 @@ class BIORBD_API Benchmark /// \return The index of the specified timer /// int getTimerIdx( - const biorbd::utils::String& name); + const String& name); protected: - std::map m_timers;///< Timers - std::map m_counts;///< Counts + std::map m_timers;///< Timers + std::map m_counts;///< Counts }; +} } } diff --git a/include/Utils/Equation.h b/include/Utils/Equation.h index 5028f888..c0b3c447 100644 --- a/include/Utils/Equation.h +++ b/include/Utils/Equation.h @@ -8,13 +8,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { /// /// \brief Strings that are to be interpreted as equation that can be evaluated /// -class BIORBD_API Equation : public biorbd::utils::String +class BIORBD_API Equation : public String { public: /// @@ -32,7 +34,7 @@ class BIORBD_API Equation : public biorbd::utils::String /// \brief Construct Equation /// \param string The equation in a string format /// - Equation(const biorbd::utils::String &string); + Equation(const String &string); /// /// \brief Construct Equation @@ -46,9 +48,9 @@ class BIORBD_API Equation : public biorbd::utils::String /// \param variables The set of variables in the equation /// \return The split equation /// - static std::vector splitIntoEquation( - biorbd::utils::Equation wholeEq, - const std::map& variables); + static std::vector splitIntoEquation( + Equation wholeEq, + const std::map& variables); /// /// \brief Evaluate and return an equation @@ -56,7 +58,7 @@ class BIORBD_API Equation : public biorbd::utils::String /// \return The evaluated equation /// static double evaluateEquation( - std::vector wholeEq); + std::vector wholeEq); /// /// \brief Evaluate and return an equation @@ -64,7 +66,7 @@ class BIORBD_API Equation : public biorbd::utils::String /// \return The evaluated equation /// static double evaluateEquation( - biorbd::utils::Equation wholeEq); + Equation wholeEq); /// /// \brief Evaluate and return an equation @@ -73,8 +75,8 @@ class BIORBD_API Equation : public biorbd::utils::String /// \return The evaluated equation /// static double evaluateEquation( - biorbd::utils::Equation wholeEq, - const std::map& variables); + Equation wholeEq, + const std::map& variables); /// /// \brief Replace constants in the split equation by a number @@ -86,7 +88,7 @@ class BIORBD_API Equation : public biorbd::utils::String /// /// static void replaceCste( - std::vector &eq); + std::vector &eq); /// /// \brief Replace the varirables in the equation by their values @@ -94,8 +96,8 @@ class BIORBD_API Equation : public biorbd::utils::String /// \param variables The variable set /// static void replaceVar( - biorbd::utils::Equation &eq, - const std::map& variables); + Equation &eq, + const std::map& variables); protected: /// @@ -104,7 +106,7 @@ class BIORBD_API Equation : public biorbd::utils::String /// \param math The mathematical symbol that is being evaluated now /// static double evaluateEquation( - std::vector eq, + std::vector eq, unsigned int math); /// @@ -121,9 +123,10 @@ class BIORBD_API Equation : public biorbd::utils::String /// - "+" -- Addition /// - "-" -- Subtraction, or negative number if it starts the equation /// - static std::vector prepareMathSymbols(); + static std::vector prepareMathSymbols(); }; +} } } diff --git a/include/Utils/Error.h b/include/Utils/Error.h index 6776542c..e65ccfed 100644 --- a/include/Utils/Error.h +++ b/include/Utils/Error.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -21,7 +23,7 @@ class BIORBD_API Error /// \param message The error message to display /// [[noreturn]] static void raise( - const biorbd::utils::String &message); + const String &message); /// /// \brief Assert that raises the error message if false @@ -30,7 +32,7 @@ class BIORBD_API Error /// static void check( bool cond, - const biorbd::utils::String &message); + const String &message); /// /// \brief Non-blocking assert that displays the error message if false @@ -39,9 +41,10 @@ class BIORBD_API Error /// static void warning( bool cond, - const biorbd::utils::String &message); + const String &message); }; +} } } diff --git a/include/Utils/IfStream.h b/include/Utils/IfStream.h index 0981efc3..2e2fa121 100644 --- a/include/Utils/IfStream.h +++ b/include/Utils/IfStream.h @@ -9,6 +9,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Equation; @@ -29,7 +31,7 @@ class BIORBD_API IfStream /// \param mode The open mode of "std::ios_base" base /// IfStream( - const biorbd::utils::Path& path, + const Path& path, std::ios_base::openmode mode ); /// @@ -48,7 +50,7 @@ class BIORBD_API IfStream /// \return True on success /// bool open( - const biorbd::utils::Path& path, + const Path& path, std::ios_base::openmode mode ); /// @@ -57,7 +59,7 @@ class BIORBD_API IfStream /// \return True on success /// bool read( - biorbd::utils::String& text); + String& text); /// /// \brief Read a word in the file @@ -65,7 +67,7 @@ class BIORBD_API IfStream /// \return True on success /// bool readAWord( - biorbd::utils::String& text); + String& text); /// /// \brief Read an integer in the file @@ -116,7 +118,7 @@ class BIORBD_API IfStream /// bool read( double& result, - const std::map &variables); + const std::map &variables); #ifdef BIORBD_USE_CASADI_MATH /// /// \brief Read and evaluate an equation @@ -126,7 +128,7 @@ class BIORBD_API IfStream /// bool read( RBDLCasadiMath::MX_Xd_SubMatrix result, - const std::map &variables); + const std::map &variables); #endif /// @@ -136,8 +138,8 @@ class BIORBD_API IfStream /// \return True on success /// bool readSpecificTag( - const biorbd::utils::String& tag, - biorbd::utils::String& text); + const String& tag, + String& text); /// /// \brief Advance in the file to a specific tag @@ -145,7 +147,7 @@ class BIORBD_API IfStream /// \return True on success /// bool reachSpecificTag( - const biorbd::utils::String& tag); + const String& tag); /// /// \brief Counts the number of consecutive lines starting with the same tag and then brings it back to the initial position @@ -153,14 +155,14 @@ class BIORBD_API IfStream /// \return The number of consecutive lines starting with the same tag /// int countTagsInAConsecutiveLines( - const biorbd::utils::String& tag); + const String& tag); /// /// \brief Read a whole line /// \param text The text read (output) /// void getline( - biorbd::utils::String& text); + String& text); /// /// \brief Close the file @@ -178,9 +180,10 @@ class BIORBD_API IfStream private: std::shared_ptr m_ifs;///< the ifstream - std::shared_ptr m_path;///< The path of the file + std::shared_ptr m_path;///< The path of the file }; +} } } diff --git a/include/Utils/Matrix.h b/include/Utils/Matrix.h index 001c86fe..17aa64b4 100644 --- a/include/Utils/Matrix.h +++ b/include/Utils/Matrix.h @@ -6,11 +6,8 @@ namespace biorbd { -namespace rigidbody +namespace BIORBD_MATH_NAMESPACE { -class GeneralizedCoordinates; -} - namespace utils { /// @@ -44,7 +41,7 @@ class BIORBD_API Matrix : public RigidBodyDynamics::Math::MatrixNd /// \param other The matrix to copy /// Matrix( - const biorbd::utils::Matrix& other); + const Matrix& other); /// /// \brief Construct matrix from Casadi matrix @@ -79,7 +76,7 @@ class BIORBD_API Matrix : public RigidBodyDynamics::Math::MatrixNd /// \param other The other Eigen matrix /// template - biorbd::utils::Matrix& operator=(const Eigen::MatrixBase & other) + Matrix& operator=(const Eigen::MatrixBase & other) { this->Eigen::MatrixXd::operator=(other); return *this; @@ -92,7 +89,7 @@ class BIORBD_API Matrix : public RigidBodyDynamics::Math::MatrixNd /// \param other The matrix to copy /// void operator=( - const biorbd::utils::Matrix& other); + const Matrix& other); /// /// \brief operator= For submatrices @@ -105,6 +102,7 @@ class BIORBD_API Matrix : public RigidBodyDynamics::Math::MatrixNd #endif }; +} } } diff --git a/include/Utils/Node.h b/include/Utils/Node.h index b644306b..8a84e141 100644 --- a/include/Utils/Node.h +++ b/include/Utils/Node.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -27,14 +29,14 @@ class BIORBD_API Node /// \param other The other node /// Node( - const biorbd::utils::Node& other); + const Node& other); /// /// \brief Construct Node /// \param name Name of the node /// Node( - const biorbd::utils::String &name); + const String &name); /// /// \brief Construct Node @@ -42,8 +44,8 @@ class BIORBD_API Node /// \param parentName Name of the parent of the node /// Node( - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const String &name, + const String &parentName); /// /// \brief Destroy class properly @@ -54,35 +56,35 @@ class BIORBD_API Node /// \brief Deep copy of the node in another node /// \param other The node to copy /// - void DeepCopy(const biorbd::utils::Node& other); + void DeepCopy(const Node& other); /// /// \brief Set the name of the node /// \param name Name to set /// - void setName(const biorbd::utils::String &name); + void setName(const String &name); /// /// \brief Return the name of the node /// - const biorbd::utils::String& name() const; + const String& name() const; /// /// \brief Return the parent name of the node /// - const biorbd::utils::String& parent() const; + const String& parent() const; /// /// \brief Set the parent name of the node /// \param name The name of the parent /// void setParent( - const biorbd::utils::String &name); + const String &name); /// /// \brief Return the type of node /// - biorbd::utils::NODE_TYPE typeOfNode() const; + NODE_TYPE typeOfNode() const; protected: /// @@ -90,13 +92,14 @@ class BIORBD_API Node /// virtual void setType() = 0; - std::shared_ptr m_name; ///< The name of the node - std::shared_ptr + std::shared_ptr m_name; ///< The name of the node + std::shared_ptr m_parentName; ///< The parent name of the node - std::shared_ptr m_typeOfNode;///< The type of the node + std::shared_ptr m_typeOfNode;///< The type of the node }; +} } } diff --git a/include/Utils/Path.h b/include/Utils/Path.h index 2b41baea..4985a834 100644 --- a/include/Utils/Path.h +++ b/include/Utils/Path.h @@ -9,6 +9,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -35,7 +37,7 @@ class BIORBD_API Path /// \param path The path in string format /// Path( - const biorbd::utils::String& path); + const String& path); /// /// \brief Construct path @@ -48,14 +50,14 @@ class BIORBD_API Path /// \brief Deep copy of path /// \return A deep copy of path /// - biorbd::utils::Path DeepCopy() const; + Path DeepCopy() const; /// /// \brief Deep copy of path from another path /// \param other The path to copy /// void DeepCopy( - const biorbd::utils::Path& other); + const Path& other); /// /// \brief Parse a path in folder, filename and extension @@ -65,10 +67,10 @@ class BIORBD_API Path /// \param ext The extension of the file /// static void parseFileName( - const biorbd::utils::String& path, - biorbd::utils::String& folder, - biorbd::utils::String& filename, - biorbd::utils::String& ext); + const String& path, + String& folder, + String& filename, + String& ext); #ifndef SWIG /// @@ -83,7 +85,7 @@ class BIORBD_API Path /// \return If file exist on the computer /// static bool isFileExist( - const biorbd::utils::Path& path); + const Path& path); #endif /// @@ -92,7 +94,7 @@ class BIORBD_API Path /// \return If file exist on the computer /// static bool isFileExist( - const biorbd::utils::String& path); + const String& path); /// /// \brief Test if file exist on the computer and is accessible @@ -113,7 +115,7 @@ class BIORBD_API Path /// \return If folder exists /// static bool isFolderExist( - const biorbd::utils::Path& path); + const Path& path); #endif /// @@ -122,7 +124,7 @@ class BIORBD_API Path /// \return If folder exists /// static bool isFolderExist( - const biorbd::utils::String& path); + const String& path); /// /// \brief To create folder on the computer @@ -133,13 +135,13 @@ class BIORBD_API Path /// /brief Return current directory /// \return Teh current directory /// - static biorbd::utils::String currentDir(); + static String currentDir(); /// /// \brief Return relative path to current working directory /// \return The relative path to curent working directory /// - biorbd::utils::String relativePath() const; + String relativePath() const; #ifndef SWIG /// @@ -147,8 +149,8 @@ class BIORBD_API Path /// \param relativeTo Relative to that path /// \return The relative path /// - biorbd::utils::String relativePath( - const biorbd::utils::String &relativeTo) const; + String relativePath( + const String &relativeTo) const; /// /// \brief Return relative path of a specified path to the specified folder @@ -156,9 +158,9 @@ class BIORBD_API Path /// \param relativeTo Relative to that path /// \return The relative path /// - static biorbd::utils::String relativePath( - const biorbd::utils::Path &path, - const biorbd::utils::String &relativeTo); + static String relativePath( + const Path &path, + const String &relativeTo); /// /// \brief Return the absolute path relative to root @@ -168,14 +170,14 @@ class BIORBD_API Path /// On Windows, the current implementation only works if the file is on the /// C drive. /// - static biorbd::utils::String absoluteFolder( - const biorbd::utils::Path &path); + static String absoluteFolder( + const Path &path); /// /// \brief Return the absolute folder relative to root /// \return The absolute folder relative to root /// - biorbd::utils::String absoluteFolder() const; + String absoluteFolder() const; #endif @@ -183,61 +185,61 @@ class BIORBD_API Path /// \brief Return the absolute path relative to root /// \return The absolute path relative to root /// - biorbd::utils::String absolutePath() const; + String absolutePath() const; /// /// \brief Return the path to Unix format /// \param path The path to convert /// \return The path in Unix format /// - static biorbd::utils::String toUnixFormat( - const biorbd::utils::String& path); + static String toUnixFormat( + const String& path); /// /// \brief Return the path to Windows format /// \param path The path to convert /// \return The path in Windows format /// - static biorbd::utils::String toWindowsFormat( - const biorbd::utils::String& path); + static String toWindowsFormat( + const String& path); /// /// \brief Return original path as it was at constructor time /// \return The original path /// - const biorbd::utils::String& originalPath() const; + const String& originalPath() const; /// /// \brief Return the folder of the file /// \return The folder of the file /// - const biorbd::utils::String& folder() const; + const String& folder() const; /// /// \brief Set the filename /// \param name The filename /// void setFilename( - const biorbd::utils::String& name); + const String& name); /// /// \brief Return the filename /// \return The filename /// - const biorbd::utils::String& filename() const; + const String& filename() const; /// /// \brief Set the extension /// \param ext The extension /// void setExtension( - const biorbd::utils::String& ext); + const String& ext); /// /// \brief Return the extension of the file /// \return The extension of the file /// - const biorbd::utils::String& extension() const; + const String& extension() const; protected: @@ -246,14 +248,15 @@ class BIORBD_API Path /// void setIsFolderAbsolute(); - std::shared_ptr + std::shared_ptr m_originalPath; ///< The original path at construction time - std::shared_ptr m_folder; ///< The folder + std::shared_ptr m_folder; ///< The folder std::shared_ptr m_isFolderAbsolute; ///< If folder is absolute - std::shared_ptr m_filename; ///< The filename - std::shared_ptr m_extension; ///< The extension + std::shared_ptr m_filename; ///< The filename + std::shared_ptr m_extension; ///< The extension }; +} } } diff --git a/include/Utils/Quaternion.h b/include/Utils/Quaternion.h index 91600e11..db291f70 100644 --- a/include/Utils/Quaternion.h +++ b/include/Utils/Quaternion.h @@ -8,6 +8,8 @@ #include "biorbdConfig.h" namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; @@ -41,7 +43,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param other Other quaternion /// Quaternion( - const biorbd::utils::Quaternion &other); + const Quaternion &other); /// /// \brief Construct Quaternion @@ -61,10 +63,10 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param kStabilizer The value of the kstabilizer /// Quaternion ( - const biorbd::utils::Scalar& w, - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, + const Scalar& w, + const Scalar& x, + const Scalar& y, + const Scalar& z, double kStabilizer = 1); /// @@ -74,33 +76,33 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param kStabilizer The value of the kstabilizer /// Quaternion ( - const biorbd::utils::Scalar& w, - const biorbd::utils::Vector3d &vec3, + const Scalar& w, + const Vector3d &vec3, double kStabilizer = 1); /// /// \brief Return the real part (w) the Quaternion /// \return The real part of the Quaternion /// - biorbd::utils::Scalar w() const; + Scalar w() const; /// /// \brief Return the X-Component of the imaginary part of the Quaternion /// \return The X-Component of the imaginary part of the Quaternion /// - biorbd::utils::Scalar x() const; + Scalar x() const; /// /// \brief Return the Y-Component of the imaginary part of the Quaternion /// \return The Y-Component of the imaginary part of the Quaternion /// - biorbd::utils::Scalar y() const; + Scalar y() const; /// /// \brief Return the Z-Component of the imaginary part of the Quaternion /// \return The Z-Component of the imaginary part of the Quaternion /// - biorbd::utils::Scalar z() const; + Scalar z() const; /// /// \brief Set the k stabilizer @@ -126,12 +128,12 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param other The other quaternion /// template - biorbd::utils::Quaternion& operator=( + Quaternion& operator=( const Eigen::MatrixBase & other) { this->Eigen::Vector4d::operator=(other); // I don't understand why the next line doesn't SegFault... - this->m_Kstab = static_cast(other).m_Kstab; + this->m_Kstab = static_cast(other).m_Kstab; return *this; } @@ -143,21 +145,21 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \brief Quaternion multiplication /// \param other The other quaternion /// - biorbd::utils::Quaternion operator*( - const biorbd::utils::Quaternion& other) const; + Quaternion operator*( + const Quaternion& other) const; /// /// \brief Multiply the quaternion with a scalar /// \param scalar The scalar to multiply with /// - biorbd::utils::Quaternion operator*( - const biorbd::utils::Scalar& scalar) const; + Quaternion operator*( + const Scalar& scalar) const; /// /// \brief Multiply the quaternion with a scalar /// \param scalar The scalar to multiply with /// - biorbd::utils::Quaternion operator*( + Quaternion operator*( float scalar) const; #ifdef BIORBD_USE_CASADI_MATH @@ -165,7 +167,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \brief Multiply the quaternion with a scalar /// \param scalar The scalar to multiply with /// - biorbd::utils::Quaternion operator*( + Quaternion operator*( double scalar) const; #endif @@ -173,15 +175,15 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \brief Add the quaternion to another /// \param other The other quaternion to add /// - biorbd::utils::Quaternion operator+( - const biorbd::utils::Quaternion& other) const; + Quaternion operator+( + const Quaternion& other) const; /// /// \brief Subtract the quaternion to another /// \param other Other quaternion to substract /// - biorbd::utils::Quaternion operator-( - const biorbd::utils::Quaternion& other) const; + Quaternion operator-( + const Quaternion& other) const; /// /// \brief Construct Quaternion from a GL @@ -191,11 +193,11 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param z The Z-Component of quaternion /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromGLRotate ( - const biorbd::utils::Scalar& angle, - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, + static Quaternion fromGLRotate ( + const Scalar& angle, + const Scalar& x, + const Scalar& y, + const Scalar& z, double kStab = 1); /// @@ -204,9 +206,9 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param axis The 3d vector of the axis /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromAxisAngle ( - const biorbd::utils::Scalar& angle, - const biorbd::utils::Vector3d &axis, + static Quaternion fromAxisAngle ( + const Scalar& angle, + const Vector3d &axis, double kStab = 1); /// @@ -214,8 +216,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param rt RotoTrans matrix /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromMatrix ( - const biorbd::utils::RotoTrans& rt, + static Quaternion fromMatrix ( + const RotoTrans& rt, double kStab = 1); /// @@ -223,8 +225,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param mat The rotation matrix /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromMatrix ( - const biorbd::utils::Rotation &mat, + static Quaternion fromMatrix ( + const Rotation &mat, double kStab = 1); /// @@ -232,8 +234,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param zyx_angles The Euler angles in a sequence where the first element is the Z-component /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromZYXAngles ( - const biorbd::utils::Vector3d &zyx_angles, + static Quaternion fromZYXAngles ( + const Vector3d &zyx_angles, double kStab = 1); /// @@ -241,8 +243,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param yxz_angles The Euler angles in a sequence where the first element is the Y-component /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromYXZAngles ( - const biorbd::utils::Vector3d &yxz_angles, + static Quaternion fromYXZAngles ( + const Vector3d &yxz_angles, double kStab = 1); /// @@ -250,8 +252,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param xyz_angles The Euler angles in a sequence where the first element is the X-component /// \param kStab The value of the kstabilizer /// - static biorbd::utils::Quaternion fromXYZAngles ( - const biorbd::utils::Vector3d &xyz_angles, + static Quaternion fromXYZAngles ( + const Vector3d &xyz_angles, double kStab = 1); /// @@ -264,7 +266,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// computation of the norm, the norm-squared is evaluated. The threshold is /// 1e-10 for the norm-squared /// - biorbd::utils::Rotation toMatrix( + Rotation toMatrix( bool skipAsserts = false) const; #ifndef BIORBD_USE_CASADI_MATH @@ -273,7 +275,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param alpha The proportion of the rotation /// \param quat The quaternion to targe /// - biorbd::utils::Quaternion slerp ( + Quaternion slerp ( double alpha, const Quaternion &quat) const; #endif @@ -282,7 +284,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \brief Return the conjugate of the quaternion /// \return The conjugate of the quaternion /// - biorbd::utils::Quaternion conjugate() const; + Quaternion conjugate() const; /// /// \brief Integrate the quaternion from its velocity @@ -290,8 +292,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param dt The time step to intergrate on /// \return The rotated quaternion /// - biorbd::utils::Quaternion timeStep ( - const biorbd::utils::Vector3d &omega, + Quaternion timeStep ( + const Vector3d &omega, double dt); /// @@ -299,8 +301,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// \param vec The vector to rotate /// \return The rotated vector /// - biorbd::utils::Vector3d rotate ( - const biorbd::utils::Vector3d &vec) const; + Vector3d rotate ( + const Vector3d &vec) const; /// /// \brief Converts a 3d angular velocity vector @@ -310,8 +312,8 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// Converts a 3d angular velocity vector into a 4d derivative of /// the components of the quaternion /// - biorbd::utils::Quaternion omegaToQDot( - const biorbd::utils::Vector3d& omega) const; + Quaternion omegaToQDot( + const Vector3d& omega) const; /// /// \brief Converts a 3d angular velocity vector @@ -325,17 +327,17 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d /// https://davidbrown3.github.io/2017-07-25/EulerAngles/ /// for correct equations. /// - biorbd::utils::Vector3d eulerDotToOmega( - const biorbd::utils::Vector3d &eulerDot, - const biorbd::utils::Vector3d &euler, - const biorbd::utils::String& seq); + Vector3d eulerDotToOmega( + const Vector3d &eulerDot, + const Vector3d &euler, + const String& seq); /// /// \brief Return the time derivative of the quaterion /// \param w The vector of time derivative (output) /// void derivate( - const biorbd::utils::Vector &w); + const Vector &w); /// /// \brief Force the normalization of the quaternion @@ -347,6 +349,7 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d }; +} } } diff --git a/include/Utils/Range.h b/include/Utils/Range.h index e5d4e934..8a55421b 100644 --- a/include/Utils/Range.h +++ b/include/Utils/Range.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { @@ -28,13 +30,13 @@ class BIORBD_API Range /// \brief Deep copy of the Range /// \return Deep copy of the Range /// - biorbd::utils::Range DeepCopy() const; + Range DeepCopy() const; /// /// \brief Deep copy of Range /// \param other The Range to copy /// - void DeepCopy(const biorbd::utils::Range& other); + void DeepCopy(const Range& other); /// /// \brief Set a new value for the minimum @@ -65,6 +67,7 @@ class BIORBD_API Range std::shared_ptr m_max; ///< The maximal value allowed by the range }; +} } } diff --git a/include/Utils/Rotation.h b/include/Utils/Rotation.h index fadb84ea..2a0cee10 100644 --- a/include/Utils/Rotation.h +++ b/include/Utils/Rotation.h @@ -18,12 +18,12 @@ struct SpatialTransform; namespace biorbd { -namespace BIORBD_MATH_NAMESPACE { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class NodeSegment; } -} namespace utils { @@ -83,12 +83,12 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param v21 Row 2, Col 1 /// \param v22 Row 2, Col 2 /// - Rotation(const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, - const biorbd::utils::Scalar& v02, - const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, - const biorbd::utils::Scalar& v12, - const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, - const biorbd::utils::Scalar& v22); + Rotation(const Scalar& v00, const Scalar& v01, + const Scalar& v02, + const Scalar& v10, const Scalar& v11, + const Scalar& v12, + const Scalar& v20, const Scalar& v21, + const Scalar& v22); /// /// \brief Contruct a Rotation matrix @@ -99,8 +99,8 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// sequence /// Rotation( - const biorbd::utils::Vector& rotation, - const biorbd::utils::String& rotationSequence); + const Vector& rotation, + const String& rotationSequence); /// /// \brief Contruct Rototrans @@ -114,7 +114,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param idx The index of axis (x = 0, y = 1 and z = 2) /// \return The axis /// - biorbd::utils::Vector3d axe( + Vector3d axe( unsigned int idx) const; /// @@ -122,7 +122,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param st The spatial transform /// \return The matrix of Rotation /// - static biorbd::utils::Rotation fromSpatialTransform( + static Rotation fromSpatialTransform( const RigidBodyDynamics::Math::SpatialTransform& st); /// @@ -133,9 +133,9 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// The number of rotation must match the number of axes in the rotation /// sequence /// - static biorbd::utils::Rotation fromEulerAngles( - const biorbd::utils::Vector& rot, - const biorbd::utils::String& seq); + static Rotation fromEulerAngles( + const Vector& rot, + const String& seq); /// /// \brief fromMarkers Creates a system of axes from two axes defined by markers @@ -145,13 +145,13 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param axisToRecalculate The axis to recalculate to ensure orthonormal system of axes /// \return The system of axes /// - static biorbd::utils::Matrix fromMarkersNonNormalized( - const std::pair& + static Matrix fromMarkersNonNormalized( + const std::pair& axis1markers, - const std::pair& + const std::pair& axis2markers, - const std::pair &axesNames, - const biorbd::utils::String& axisToRecalculate); + const std::pair &axesNames, + const String& axisToRecalculate); /// /// \brief fromMarkers Creates a system of axes from two axes defined by markers @@ -161,13 +161,13 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param axisToRecalculate The axis to recalculate to ensure orthonormal system of axes /// \return The system of axes /// - static biorbd::utils::Rotation fromMarkers( - const std::pair& + static Rotation fromMarkers( + const std::pair& axis1markers, - const std::pair& + const std::pair& axis2markers, - const std::pair &axesNames, - const biorbd::utils::String& axisToRecalculate); + const std::pair &axesNames, + const String& axisToRecalculate); /// /// \brief Return extracted angles from the rotation matrix into Euler angles using the provided sequence @@ -177,9 +177,9 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// /// The rotation sequence can be any combination of x, y and z /// - static biorbd::utils::Vector toEulerAngles( - const biorbd::utils::Rotation& r, - const biorbd::utils::String& seq); + static Vector toEulerAngles( + const Rotation& r, + const String& seq); #ifndef BIORBD_USE_CASADI_MATH /// @@ -187,8 +187,8 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param mToMean The Rotation matrices to mean /// \return The mean Rotation matrix /// - static biorbd::utils::Rotation mean( - const std::vector& mToMean); + static Rotation mean( + const std::vector& mToMean); #endif #ifndef SWIG @@ -198,7 +198,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param other The other Rotation matrix /// template - biorbd::utils::Rotation& operator=( + Rotation& operator=( const Eigen::MatrixBase & other) { Eigen::Matrix3d::operator=(other); @@ -216,6 +216,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d void checkUnitary(); }; +} } } @@ -225,7 +226,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param os osstream /// \param rt The Rotation matrix /// - std::ostream& operator<<(std::ostream& os, const biorbd::utils::Rotation &rt); + std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation &rt); #endif #endif // BIORBD_UTILS_ROTO_TRANS_H diff --git a/include/Utils/RotoTrans.h b/include/Utils/RotoTrans.h index aaa468c2..0da7a0e5 100644 --- a/include/Utils/RotoTrans.h +++ b/include/Utils/RotoTrans.h @@ -23,7 +23,6 @@ namespace rigidbody { class NodeSegment; } -} namespace utils { @@ -70,25 +69,18 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param v33 Row 3, Col 3 /// RotoTrans( - const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, - const biorbd::utils::Scalar& v02, - const biorbd::utils::Scalar& v03, - const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, - const biorbd::utils::Scalar& v12, - const biorbd::utils::Scalar& v13, - const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, - const biorbd::utils::Scalar& v22, - const biorbd::utils::Scalar& v23, - const biorbd::utils::Scalar& v30, const biorbd::utils::Scalar& v31, - const biorbd::utils::Scalar& v32, - const biorbd::utils::Scalar& v33); + const Scalar& v00, const Scalar& v01, const Scalar& v02, const Scalar& v03, + const Scalar& v10, const Scalar& v11, const Scalar& v12, const Scalar& v13, + const Scalar& v20, const Scalar& v21, const Scalar& v22, const Scalar& v23, + const Scalar& v30, const Scalar& v31, const Scalar& v32, const Scalar& v33 + ); /// /// \brief Contruct Rototrans /// \param rot The rotation matrix /// RotoTrans( - const biorbd::utils::Rotation& rot); + const Rotation& rot); /// /// \brief Contruct Rototrans @@ -96,8 +88,8 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param trans Translation vector /// RotoTrans( - const biorbd::utils::Rotation& rot, - const biorbd::utils::Vector3d& trans); + const Rotation& rot, + const Vector3d& trans); /// /// \brief Contruct Rototrans @@ -109,9 +101,9 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// sequence /// RotoTrans( - const biorbd::utils::Vector& rotation, - const biorbd::utils::Vector3d& translation, - const biorbd::utils::String &rotationSequence); + const Vector& rotation, + const Vector3d& translation, + const String &rotationSequence); /// /// \brief Contruct Rototrans @@ -144,40 +136,40 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param axisToRecalculate The axis to recalculate to ensure orthonormal system of axes /// \return The system of axes /// - static biorbd::utils::RotoTrans fromMarkers( + static RotoTrans fromMarkers( const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment& origin, const std::pair& axis1markers, const std::pair& axis2markers, - const std::pair &axesNames, - const biorbd::utils::String& axisToRecalculate); + const std::pair &axesNames, + const String& axisToRecalculate); /// /// \brief Get a particular axis of the rotation matrix /// \param idx The index of axis (x = 0, y = 1 and z = 2) /// \return The axis /// - biorbd::utils::Vector3d axe( + Vector3d axe( unsigned int idx) const ; /// /// \brief Return the tranposed matrix /// \return The transposed matrix /// - biorbd::utils::RotoTrans transpose() const; + RotoTrans transpose() const; /// /// \brief Return the translation vector /// \return The translation vector /// - biorbd::utils::Vector3d trans() const; + Vector3d trans() const; /// /// \brief Return the rotation matrix /// \return The rotation matrix /// - biorbd::utils::Rotation rot() const; + Rotation rot() const; /// /// \brief Set the RotoTrans from a rotation and a translation @@ -185,16 +177,16 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param trans The vector of translation /// \return The matrix of RotoTrans /// - static biorbd::utils::RotoTrans combineRotAndTrans( - const biorbd::utils::Rotation& rot, - const biorbd::utils::Vector3d& trans); + static RotoTrans combineRotAndTrans( + const Rotation& rot, + const Vector3d& trans); /// /// \brief set the RotoTrans from a spatial transform /// \param st The spatial transform /// \return The matrix of RotoTrans /// - static biorbd::utils::RotoTrans fromSpatialTransform( + static RotoTrans fromSpatialTransform( const RigidBodyDynamics::Math::SpatialTransform& st); /// @@ -206,10 +198,10 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// The number of rotation umust match the number of axes in the rotation /// sequence /// - static biorbd::utils::RotoTrans fromEulerAngles( - const biorbd::utils::Vector &rot, - const biorbd::utils::Vector3d& trans, - const biorbd::utils::String& seq); + static RotoTrans fromEulerAngles( + const Vector &rot, + const Vector3d& trans, + const String& seq); /// /// \brief Return extracted angles from the rotation matrix into Euler angles using the provided sequence @@ -219,9 +211,9 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// /// The rotation sequence can be any combination of x, y and z /// - static biorbd::utils::Vector toEulerAngles( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::String &seq); + static Vector toEulerAngles( + const RotoTrans& rt, + const String &seq); #ifndef BIORBD_USE_CASADI_MATH /// @@ -229,8 +221,8 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param rt The RotoTrans matrices to mean /// \return The mean RotoTrans /// - static biorbd::utils::RotoTrans mean( - const std::vector&rt); + static RotoTrans mean( + const std::vector&rt); #endif #ifndef SWIG @@ -240,7 +232,7 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param other The other rotoTrans matrix /// template - biorbd::utils::RotoTrans& operator=(const Eigen::MatrixBase & + RotoTrans& operator=(const Eigen::MatrixBase & other) { Eigen::Matrix4d::operator=(other); @@ -254,7 +246,7 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \brief Expand 3D vector to 4D (padding with an extra 1) /// \param v1 Vector to expand /// - RigidBodyDynamics::Math::Vector4d expand3dTo4d(const biorbd::utils::Vector3d& + RigidBodyDynamics::Math::Vector4d expand3dTo4d(const Vector3d& v1); /// @@ -265,6 +257,7 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d void checkUnitary(); }; +} } } @@ -274,7 +267,7 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param os osstream /// \param rt The RotoTrans matrix /// - std::ostream& operator<<(std::ostream& os, const biorbd::utils::RotoTrans &rt); + std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans &rt); #endif #endif // BIORBD_UTILS_ROTO_TRANS_H diff --git a/include/Utils/RotoTransNode.h b/include/Utils/RotoTransNode.h index b844d41b..03a82b92 100644 --- a/include/Utils/RotoTransNode.h +++ b/include/Utils/RotoTransNode.h @@ -8,14 +8,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; /// /// \brief A RotoTrans which is attached to a segment /// -class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, - public biorbd::utils::Node +class BIORBD_API RotoTransNode : public RotoTrans, public Node { public: /// @@ -30,9 +31,9 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// \param parentName The name of the parent segment /// RotoTransNode( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const RotoTrans& rt, + const String &name, + const String &parentName); #ifdef BIORBD_USE_EIGEN3_MATH /// @@ -41,20 +42,20 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// template RotoTransNode( const Eigen::MatrixBase& other) : - biorbd::utils::RotoTrans(other) {} + RotoTrans(other) {} #endif /// /// \brief Deep copy of a RotoTransNode /// \return A deep copy of a RotoTrans node /// - biorbd::utils::RotoTransNode DeepCopy() const; + RotoTransNode DeepCopy() const; /// /// \brief Deep copy of a RotoTransNode into another RotoTransNode /// \param other The RotoTransNode to copy /// - void DeepCopy(const biorbd::utils::RotoTransNode& other); + void DeepCopy(const RotoTransNode& other); #ifndef SWIG @@ -63,7 +64,7 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// \return Rotated matrix /// void operator=( - const biorbd::utils::RotoTrans& other); + const RotoTrans& other); #endif @@ -71,8 +72,8 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// \brief operator* Matrix multiplication /// \return Rotated matrix /// - biorbd::utils::RotoTrans operator*( - const biorbd::utils::RotoTransNode& other) const; + RotoTrans operator*( + const RotoTransNode& other) const; #ifndef SWIG @@ -82,10 +83,10 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// \param other The Eigen matrix /// template - biorbd::utils::RotoTransNode& operator=(const Eigen::MatrixBase & + RotoTransNode& operator=(const Eigen::MatrixBase & other) { - this->biorbd::utils::RotoTrans::operator=(other); + this->RotoTrans::operator=(other); return *this; } #endif @@ -104,11 +105,12 @@ class BIORBD_API RotoTransNode : public biorbd::utils::RotoTrans, /// \brief operator* Matrix multiplication /// \return Rotated matrix /// -biorbd::utils::RotoTransNode operator*( - const biorbd::utils::RotoTrans& other, - const biorbd::utils::RotoTransNode& me); +RotoTransNode operator*( + const RotoTrans& other, + const RotoTransNode& me); +} } } diff --git a/include/Utils/Scalar.h b/include/Utils/Scalar.h index 8051bd4f..e9f43643 100644 --- a/include/Utils/Scalar.h +++ b/include/Utils/Scalar.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { @@ -55,6 +57,7 @@ class Scalar : public RigidBodyDynamics::Math::Scalar #endif +} } } diff --git a/include/Utils/SpatialVector.h b/include/Utils/SpatialVector.h index a82f04ca..4c7cdcb2 100644 --- a/include/Utils/SpatialVector.h +++ b/include/Utils/SpatialVector.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { @@ -30,7 +32,7 @@ class BIORBD_API SpatialVector : public RigidBodyDynamics::Math::SpatialVector /// \param other The SpatialVector to copy /// SpatialVector( - const biorbd::utils::SpatialVector& other); + const SpatialVector& other); /// /// \brief Construct SpatialVector by its values @@ -42,8 +44,8 @@ class BIORBD_API SpatialVector : public RigidBodyDynamics::Math::SpatialVector /// \param v6 Sixth element /// SpatialVector( - biorbd::utils::Scalar v1, biorbd::utils::Scalar v2, biorbd::utils::Scalar v3, - biorbd::utils::Scalar v4, biorbd::utils::Scalar v5, biorbd::utils::Scalar v6); + Scalar v1, Scalar v2, Scalar v3, + Scalar v4, Scalar v5, Scalar v6); #ifdef BIORBD_USE_CASADI_MATH /// @@ -67,14 +69,14 @@ class BIORBD_API SpatialVector : public RigidBodyDynamics::Math::SpatialVector /// \param other The SpatialVector to copy /// void operator=( - const biorbd::utils::SpatialVector& other); + const SpatialVector& other); #ifdef BIORBD_USE_EIGEN3_MATH /// /// \brief Allow the use operator= on SpatialVector /// \param other The other matrix /// template - biorbd::utils::SpatialVector& operator=(const Eigen::MatrixBase & + SpatialVector& operator=(const Eigen::MatrixBase & other) { this->Eigen::Matrix::operator=(other); @@ -100,6 +102,7 @@ class BIORBD_API SpatialVector : public RigidBodyDynamics::Math::SpatialVector #endif }; +} } } diff --git a/include/Utils/String.h b/include/Utils/String.h index ec8169d8..eda228a0 100644 --- a/include/Utils/String.h +++ b/include/Utils/String.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { /// @@ -35,7 +37,7 @@ class BIORBD_API String : public std::string /// \param text The string in string format /// String( - const biorbd::utils::String& text); + const String& text); /// /// \brief Construct string @@ -50,7 +52,7 @@ class BIORBD_API String : public std::string /// \param other The other string /// String& operator=( - const biorbd::utils::String& other); + const String& other); #endif /// @@ -108,26 +110,26 @@ class BIORBD_API String : public std::string /// \param str The string to convert /// \return The new string /// - static biorbd::utils::String tolower(const biorbd::utils::String &str); + static String tolower(const String &str); /// /// \brief Return a lower case string /// \return The lower case string /// - biorbd::utils::String tolower() const; + String tolower() const; /// /// \brief Convert a string to a upper case string /// \param str The string to convert /// \return The new string /// - static biorbd::utils::String toupper(const biorbd::utils::String &str); + static String toupper(const String &str); /// /// \brief Return an upper case string /// \return The upper case string /// - biorbd::utils::String toupper() const; + String toupper() const; #endif /// @@ -135,7 +137,7 @@ class BIORBD_API String : public std::string /// \param val The double to convert /// \return The double converter to string /// - static biorbd::utils::String to_string( + static String to_string( double val); /// @@ -143,7 +145,7 @@ class BIORBD_API String : public std::string /// \param val The float to convert /// \return The float converter to string /// - static biorbd::utils::String to_string( + static String to_string( float val); /// @@ -154,20 +156,22 @@ class BIORBD_API String : public std::string /// /// Example : The string "2.82823000" will return "2.82823" for the tag "0" /// - static biorbd::utils::String removeTrailing( - const biorbd::utils::String& origin, - const biorbd::utils::String& trailTag); + static String removeTrailing( + const String& origin, + const String& trailTag); }; } } +} + #ifndef SWIG /// /// \brief To use operator<< /// \param os The ostream /// \param a The string to operate on /// - std::ostream& operator<<(std::ostream& os, const biorbd::utils::String &a); + std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::String &a); #endif #endif // BIORBD_UTILS_STRING_H diff --git a/include/Utils/Timer.h b/include/Utils/Timer.h index 2d8e7e75..f4657359 100644 --- a/include/Utils/Timer.h +++ b/include/Utils/Timer.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { @@ -77,6 +79,7 @@ class BIORBD_API Timer }; +} } } diff --git a/include/Utils/UtilsEnum.h b/include/Utils/UtilsEnum.h index 7e147155..bca0f93c 100644 --- a/include/Utils/UtilsEnum.h +++ b/include/Utils/UtilsEnum.h @@ -3,6 +3,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { @@ -27,7 +29,7 @@ enum NODE_TYPE { /// \return The name of the type /// inline const char* NODE_TYPE_toStr( - biorbd::utils::NODE_TYPE type) + NODE_TYPE type) { switch (type) { case VECTOR3D: @@ -51,6 +53,7 @@ inline const char* NODE_TYPE_toStr( } } +} } } diff --git a/include/Utils/Vector.h b/include/Utils/Vector.h index 79d93552..a0b055e5 100644 --- a/include/Utils/Vector.h +++ b/include/Utils/Vector.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; @@ -38,7 +40,7 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd /// \param other The vector to copy /// Vector( - const biorbd::utils::Vector& other); + const Vector& other); /// /// \brief Construct vector from Casadi vector @@ -53,7 +55,7 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd /// \param other The vector to copy /// Vector( - const biorbd::utils::Vector3d& other); + const Vector3d& other); #ifdef BIORBD_USE_EIGEN3_MATH /// @@ -87,7 +89,7 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd /// \param skipRoot To perform or not the sqrt_p() /// \return The norm of the vector /// - biorbd::utils::Scalar norm( + Scalar norm( unsigned int p = 2, bool skipRoot = false) const; @@ -97,7 +99,7 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd /// \param skipRoot To perform or not the sqrt_p() /// \return The gradient of the norm /// - biorbd::utils::Vector normGradient( + Vector normGradient( unsigned int p = 2, bool skipRoot = false); @@ -107,14 +109,14 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd /// \param other The vector to copy /// void operator=( - const biorbd::utils::Vector& other); + const Vector& other); #ifdef BIORBD_USE_EIGEN3_MATH /// /// \brief Allow the use operator= on vector /// \param other The other matrix /// template - biorbd::utils::Vector& operator=(const Eigen::MatrixBase & other) + Vector& operator=(const Eigen::MatrixBase & other) { this->Eigen::VectorXd::operator=(other); return *this; @@ -139,6 +141,7 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd #endif }; +} } } diff --git a/include/Utils/Vector3d.h b/include/Utils/Vector3d.h index bc1cbf43..641732b6 100644 --- a/include/Utils/Vector3d.h +++ b/include/Utils/Vector3d.h @@ -9,6 +9,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class RotoTrans; @@ -22,7 +24,7 @@ class Vector; class BIORBD_API Vector3d #else class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, - public biorbd::utils::Node + public Node #endif { public: @@ -38,9 +40,9 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \param z Z-Component of the vector /// Vector3d( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z); + const Scalar& x, + const Scalar& y, + const Scalar& z); /// /// \brief Construct a 3D vector @@ -51,11 +53,11 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \param parentName Name of the parent segment /// Vector3d( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const Scalar& x, + const Scalar& y, + const Scalar& z, + const String &name, + const String &parentName); /// /// \brief Construct a 3D vector @@ -64,9 +66,9 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \param parentName Name of the parent segment /// Vector3d( - const biorbd::utils::Vector3d vec, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const Vector3d vec, + const String &name, + const String &parentName); /// /// \brief Construct a 3D vector from a Casadi 3D vector (drop the trailling 1) @@ -96,7 +98,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// template Vector3d( const Eigen::MatrixBase& other) : - RigidBodyDynamics::Math::Vector3d(other), biorbd::utils::Node () + RigidBodyDynamics::Math::Vector3d(other), Node () { } @@ -108,9 +110,9 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// template Vector3d( const Eigen::MatrixBase& other, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - RigidBodyDynamics::Math::Vector3d(other), biorbd::utils::Node (name, parentName) + const String &name, + const String &parentName) : + RigidBodyDynamics::Math::Vector3d(other), Node (name, parentName) { } @@ -129,20 +131,20 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \brief Deep copy of a 3D vector /// \return A deep copy of a 3D vector /// - biorbd::utils::Vector3d DeepCopy() const; + Vector3d DeepCopy() const; /// /// \brief Deep copy of a 3D vector into another 3D vector /// \param other The 3D vector to copy /// - void DeepCopy(const biorbd::utils::Vector3d& other); + void DeepCopy(const Vector3d& other); /// /// \brief Apply a RotoTrans to the 3D vector /// \param rt RotoTrans to apply /// \return The transformed vector // - biorbd::utils::Vector3d applyRT( + Vector3d applyRT( const RotoTrans& rt) const; /// @@ -157,7 +159,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \param v The new position /// void setPosition( - const biorbd::utils::Vector3d& v); + const Vector3d& v); #ifndef SWIG @@ -167,7 +169,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \param other The eigen matrix /// template - biorbd::utils::Vector3d& operator=(const Eigen::MatrixBase & + Vector3d& operator=(const Eigen::MatrixBase & other) { this->Eigen::Vector3d::operator=(other); @@ -181,19 +183,19 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, /// \brief Accessor for the first element /// \return The first element /// - biorbd::utils::Scalar x() const; + Scalar x() const; /// /// \brief Accessor for the second element /// \return The second element /// - biorbd::utils::Scalar y() const; + Scalar y() const; /// /// \brief Accessor for the third element /// \return The third element /// - biorbd::utils::Scalar z() const; + Scalar z() const; /// /// \brief Construct a 3D vector from a Casadi 4D vector (drop the trailling 1) @@ -231,6 +233,7 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, void setType(); }; +} } } diff --git a/src/BiorbdModel.cpp b/src/BiorbdModel.cpp index 7dc30946..e4f76af0 100644 --- a/src/BiorbdModel.cpp +++ b/src/BiorbdModel.cpp @@ -8,25 +8,27 @@ #include "RigidBody/NodeSegment.h" #include "Utils/String.h" -biorbd::utils::String getVersion() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::String getVersion() { return BIORBD_VERSION; } -biorbd::Model::Model() : - m_path(std::make_shared()) +Model::Model() : + m_path(std::make_shared()) { } -biorbd::Model::Model(const biorbd::utils::Path &path) : - m_path(std::make_shared(path)) +Model::Model(const utils::Path &path) : + m_path(std::make_shared(path)) { - biorbd::Reader::readModelFile(*m_path, this); + Reader::readModelFile(*m_path, this); } -biorbd::utils::Path biorbd::Model::path() const +utils::Path Model::path() const { return *m_path; } diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp index cc65044e..fac39d7a 100644 --- a/src/ModelReader.cpp +++ b/src/ModelReader.cpp @@ -42,45 +42,47 @@ #include "Muscles/StateDynamicsBuchanan.h" #endif // MODULE_MUSCLES +using namespace biorbd::BIORBD_MATH_NAMESPACE; + // ------ Public methods ------ // -biorbd::Model biorbd::Reader::readModelFile(const biorbd::utils::Path &path) +Model Reader::readModelFile(const utils::Path &path) { // Add the elements that have been entered - biorbd::Model model; - readModelFile(path, &model); + Model model; + Reader::readModelFile(path, &model); return model; } -void biorbd::Reader::readModelFile( - const biorbd::utils::Path &path, - biorbd::Model *model) +void Reader::readModelFile( + const utils::Path &path, + Model *model) { // Open file if (!path.isFileReadable()) - biorbd::utils::Error::raise("File " + path.absolutePath() + utils::Error::raise("File " + path.absolutePath() + " could not be open"); #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String main_tag; - biorbd::utils::String property_tag; - biorbd::utils::String subproperty_tag; + utils::String main_tag; + utils::String property_tag; + utils::String subproperty_tag; // Variable used to replace doubles - std::map variable; + std::map variable; // Determine the file version file.readSpecificTag("version", main_tag); unsigned int version(static_cast(atoi(main_tag.c_str()))); - biorbd::utils::Error::check((version == 1 || version == 2 || version == 3 + utils::Error::check((version == 1 || version == 2 || version == 3 || version == 4), "Version " + main_tag + " is not implemented yet"); @@ -88,7 +90,7 @@ void biorbd::Reader::readModelFile( bool hasActuators = false; #endif // MODULE_ACTUATORS - biorbd::utils::String name; + utils::String name; try { while(file.read( main_tag)) { // Attempt read into main_tag, return false if it fails @@ -100,9 +102,9 @@ void biorbd::Reader::readModelFile( // If it is a segment if (!main_tag.tolower().compare("segment")) { file.read(name); - biorbd::utils::String parent_str("root"); - biorbd::utils::String trans = ""; - biorbd::utils::String rot = ""; + utils::String parent_str("root"); + utils::String trans = ""; + utils::String rot = ""; bool RTinMatrix(true); if (version >= 3) { // By default for version 3 (not in matrix) RTinMatrix = false; @@ -111,14 +113,14 @@ void biorbd::Reader::readModelFile( double mass = 0.00000001; RigidBodyDynamics::Math::Matrix3d inertia( RigidBodyDynamics::Math::Matrix3d::Identity()); - biorbd::utils::RotoTrans RT(RigidBodyDynamics::Math::Matrix4d::Identity()); - biorbd::utils::Vector3d com(0,0,0); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; + utils::RotoTrans RT(RigidBodyDynamics::Math::Matrix4d::Identity()); + utils::Vector3d com(0,0,0); + rigidbody::Mesh mesh; int segmentByFile(-1); // -1 non setté, 0 pas par file, 1 par file int PF = -1; - std::vector QRanges; - std::vector QDotRanges; - std::vector QDDotRanges; + std::vector QRanges; + std::vector QDotRanges; + std::vector QDDotRanges; bool isRangeQSet( false); // Ranges must be done only after translation AND rotations tags bool isRangeQDotSet( @@ -130,23 +132,23 @@ void biorbd::Reader::readModelFile( // Dynamically find the parent number file.read(parent_str); if (parent_str.tolower().compare("root")) { - biorbd::utils::Error::check(model->GetBodyId(parent_str.c_str()), + utils::Error::check(model->GetBodyId(parent_str.c_str()), "Wrong name in a segment"); } } else if (!property_tag.tolower().compare("translations")) { - biorbd::utils::Error::check(!isRangeQSet, + utils::Error::check(!isRangeQSet, "Translations must appear before the rangesq tag"); - biorbd::utils::Error::check(!isRangeQDotSet, + utils::Error::check(!isRangeQDotSet, "Translations must appear before the rangesqdot tag"); - biorbd::utils::Error::check(!isRangeQDDotSet, + utils::Error::check(!isRangeQDDotSet, "Translations must appear before the rangesqddot tag"); file.read(trans); } else if (!property_tag.tolower().compare("rotations")) { - biorbd::utils::Error::check(!isRangeQSet, + utils::Error::check(!isRangeQSet, "Rotations must appear before the rangesq tag"); - biorbd::utils::Error::check(!isRangeQDotSet, + utils::Error::check(!isRangeQDotSet, "Rotations must appear before the rangesqdot tag"); - biorbd::utils::Error::check(!isRangeQDDotSet, + utils::Error::check(!isRangeQDDotSet, "Rotations must appear before the rangesqddot tag"); file.read(rot); } else if (!property_tag.tolower().compare("ranges") || @@ -163,7 +165,7 @@ void biorbd::Reader::readModelFile( file.read(min); file.read(max); QRanges.push_back( - biorbd::utils::Range (min, max)); + utils::Range (min, max)); } isRangeQSet = true; } else if (!property_tag.tolower().compare("rangesqdot")) { @@ -179,7 +181,7 @@ void biorbd::Reader::readModelFile( file.read(min); file.read(max); QDotRanges.push_back( - biorbd::utils::Range (min, max)); + utils::Range (min, max)); } isRangeQDotSet = true; } else if (!property_tag.tolower().compare("rangesqddot")) { @@ -195,7 +197,7 @@ void biorbd::Reader::readModelFile( file.read(min); file.read(max); QDDotRanges.push_back( - biorbd::utils::Range (min, max)); + utils::Range (min, max)); } isRangeQDDotSet = true; } else if (!property_tag.tolower().compare("mass")) { @@ -203,7 +205,7 @@ void biorbd::Reader::readModelFile( } else if (!property_tag.tolower().compare("inertia")) { readMatrix33(file, variable, inertia); } else if (!property_tag.tolower().compare("rtinmatrix")) { - biorbd::utils::Error::check(isRTset==false, + utils::Error::check(isRTset==false, "RT should not appear before RTinMatrix"); file.read(RTinMatrix); } else if (!property_tag.tolower().compare("rt")) { @@ -218,18 +220,18 @@ void biorbd::Reader::readModelFile( if (segmentByFile==-1) { segmentByFile = 0; } else if (segmentByFile == 1) { - biorbd::utils::Error::raise("You must not mix file and mesh in segment"); + utils::Error::raise("You must not mix file and mesh in segment"); } - biorbd::utils::Vector3d tp(0, 0, 0); + utils::Vector3d tp(0, 0, 0); readVector3d(file, variable, tp); mesh.addPoint(tp); } else if (!property_tag.tolower().compare("patch")) { if (segmentByFile==-1) { segmentByFile = 0; } else if (segmentByFile == 1) { - biorbd::utils::Error::raise("You must not mix file and mesh in segment"); + utils::Error::raise("You must not mix file and mesh in segment"); } - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace tp; + rigidbody::MeshFace tp; for (unsigned int i=0; i<3; ++i) { file.read(tp(i)); } @@ -238,11 +240,11 @@ void biorbd::Reader::readModelFile( if (segmentByFile==-1) { segmentByFile = 1; } else if (segmentByFile == 0) { - biorbd::utils::Error::raise("You must not mix file and mesh in segment"); + utils::Error::raise("You must not mix file and mesh in segment"); } - biorbd::utils::String filePathInString; + utils::String filePathInString; file.read(filePathInString); - biorbd::utils::Path filePath(filePathInString); + utils::Path filePath(filePathInString); if (!filePath.extension().compare("bioMesh")) { mesh = readMeshFileBiorbdSegments(path.folder() + filePath.relativePath()); } else if (!filePath.extension().compare("ply")) { @@ -256,7 +258,7 @@ void biorbd::Reader::readModelFile( } #endif else { - biorbd::utils::Error::raise(filePath.extension() + + utils::Error::raise(filePath.extension() + " is an unrecognized mesh file"); } } @@ -272,10 +274,10 @@ void biorbd::Reader::readModelFile( for (size_t i=0; i=trans.length()) { QRanges.push_back( - biorbd::utils::Range (-1, 1)); + utils::Range (-1, 1)); } else { QRanges.push_back( - biorbd::utils::Range ()); + utils::Range ()); } } } @@ -289,7 +291,7 @@ void biorbd::Reader::readModelFile( } for (size_t i=0; iAddSegment(name, parent_str, trans, rot, QRanges, QDotRanges, QDDotRanges, characteristics, RT, PF); } else if (!main_tag.tolower().compare("gravity")) { - biorbd::utils::Vector3d gravity(0,0,0); + utils::Vector3d gravity(0,0,0); readVector3d(file, variable, gravity); model->gravity = gravity; } else if (!main_tag.tolower().compare("variables")) { - biorbd::utils::String var; + utils::String var; while(file.read(var) && var.tolower().compare("endvariables")) { if (!var(0).compare("$")) { double value; file.read(value); - biorbd::utils::Error::check(variable.find(var) == variable.end(), + utils::Error::check(variable.find(var) == variable.end(), "Variable already defined"); variable[var] = value; } } } else if (!main_tag.tolower().compare("marker")) { - biorbd::utils::String name; + utils::String name; file.read(name); unsigned int parent_int = 0; - biorbd::utils::String parent_str("root"); - biorbd::utils::Vector3d pos(0,0,0); + utils::String parent_str("root"); + utils::Vector3d pos(0,0,0); bool technical = true; bool anatomical = false; - biorbd::utils::String axesToRemove; + utils::String axesToRemove; while(file.read(property_tag) && property_tag.tolower().compare("endmarker")) if (!property_tag.tolower().compare("parent")) { // Dynamically find the parent number file.read(parent_str); parent_int = model->GetBodyId(parent_str.c_str()); // if parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(parent_int), + utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment"); } else if (!property_tag.tolower().compare("position")) { readVector3d(file, variable, pos); @@ -354,15 +356,15 @@ void biorbd::Reader::readModelFile( model->addMarker(pos, name, parent_str, technical, anatomical, axesToRemove, static_cast(parent_int)); } else if (!main_tag.tolower().compare("mimu") && version >= 4) { - biorbd::utils::Error::raise("MIMU is no more the right tag, change it to IMU!"); + utils::Error::raise("MIMU is no more the right tag, change it to IMU!"); } else if (!main_tag.tolower().compare("imu") || !main_tag.tolower().compare("mimu") || !main_tag.tolower().compare("customrt")) { - biorbd::utils::String rtType(main_tag.tolower()); - biorbd::utils::String name; + utils::String rtType(main_tag.tolower()); + utils::String name; file.read(name); - biorbd::utils::String parent_str("root"); - biorbd::utils::RotoTransNode RT; + utils::String parent_str("root"); + utils::RotoTransNode RT; bool RTinMatrix(true); if (version >= 3) { // By default for version 3 (not in matrix) RTinMatrix = false; @@ -372,12 +374,12 @@ void biorbd::Reader::readModelFile( bool anatomical = false; bool firstTag = true; bool fromMarkers(false); - biorbd::utils::String originMarkerName(""); - biorbd::utils::String firstAxis(""); - std::vector firstAxisMarkerNames(2); - biorbd::utils::String secondAxis(""); - std::vector secondAxisMarkerNames(2); - biorbd::utils::String axisToRecalculate(""); + utils::String originMarkerName(""); + utils::String firstAxis(""); + std::vector firstAxisMarkerNames(2); + utils::String secondAxis(""); + std::vector secondAxisMarkerNames(2); + utils::String axisToRecalculate(""); while(file.read(property_tag) && !(!property_tag.tolower().compare("endimu") || !property_tag.tolower().compare("endmimu") || !property_tag.tolower().compare("endcustomrt"))) { @@ -385,11 +387,11 @@ void biorbd::Reader::readModelFile( // Dynamically find the parent number file.read(parent_str); // If parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(model->GetBodyId( + utils::Error::check(model->IsBodyId(model->GetBodyId( parent_str.c_str())), "Wrong name in a segment"); } else if (!property_tag.tolower().compare("frommarkers")) { if (!firstTag) { - biorbd::utils::Error::raise("The tag 'fromMarkers' should appear first in the IMU " + utils::Error::raise("The tag 'fromMarkers' should appear first in the IMU " + name); } else { fromMarkers = true; @@ -420,7 +422,7 @@ void biorbd::Reader::readModelFile( } } else { if (!property_tag.tolower().compare("rtinmatrix")) { - biorbd::utils::Error::check(isRTset==false, + utils::Error::check(isRTset==false, "RT should not appear before RTinMatrix"); file.read(RTinMatrix); } else if (!property_tag.tolower().compare("rt")) { @@ -430,48 +432,48 @@ void biorbd::Reader::readModelFile( } } if (fromMarkers) { - std::vector allMarkerOnSegment(model->marker( + std::vector allMarkerOnSegment(model->marker( parent_str)); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End; + rigidbody::NodeSegment origin, axis1Beg, axis1End, axis2Beg, axis2End; bool isOrigin(false), isAxis1Beg(false), isAxis1End(false), isAxis2Beg(false), isAxis2End(false); for (auto mark : allMarkerOnSegment) { - if (!mark.biorbd::utils::Node::name().compare(originMarkerName)) { + if (!mark.utils::Node::name().compare(originMarkerName)) { origin = mark; isOrigin = true; } - if (!mark.biorbd::utils::Node::name().compare(firstAxisMarkerNames[0])) { + if (!mark.utils::Node::name().compare(firstAxisMarkerNames[0])) { axis1Beg = mark; isAxis1Beg = true; } - if (!mark.biorbd::utils::Node::name().compare(firstAxisMarkerNames[1])) { + if (!mark.utils::Node::name().compare(firstAxisMarkerNames[1])) { axis1End = mark; isAxis1End = true; } - if (!mark.biorbd::utils::Node::name().compare(secondAxisMarkerNames[0])) { + if (!mark.utils::Node::name().compare(secondAxisMarkerNames[0])) { axis2Beg = mark; isAxis2Beg = true; } - if (!mark.biorbd::utils::Node::name().compare(secondAxisMarkerNames[1])) { + if (!mark.utils::Node::name().compare(secondAxisMarkerNames[1])) { axis2End = mark; isAxis2End = true; } } if (! (isAxis1Beg && isAxis1End && isAxis2Beg && isAxis2End && isOrigin)) { - biorbd::utils::Error::raise("All the axes name and origin for the " + rtType + + utils::Error::raise("All the axes name and origin for the " + rtType + "(" + name + ") must be set and correspond to marker names previously defined on the same parent"); } if (!(!axisToRecalculate.tolower().compare("firstaxis") || !axisToRecalculate.tolower().compare("secondaxis"))) { - biorbd::utils::Error::raise("The 'recalculate' option for " + rtType + "(" + + utils::Error::raise("The 'recalculate' option for " + rtType + "(" + name + ") must be 'firstaxis' or 'secondaxis'"); } axisToRecalculate = !axisToRecalculate.tolower().compare("firstaxis") ? firstAxis : secondAxis; - RT = biorbd::utils::RotoTrans::fromMarkers( + RT = utils::RotoTrans::fromMarkers( origin, {axis1Beg, axis1End}, {axis2Beg, axis2End}, @@ -485,13 +487,13 @@ void biorbd::Reader::readModelFile( model->addIMU(RT, technical, anatomical); } } else if (!main_tag.tolower().compare("contact")) { - biorbd::utils::String name; + utils::String name; file.read(name); unsigned int parent_int = 0; - biorbd::utils::String parent_str("root"); - biorbd::utils::Vector3d pos(0,0,0); - biorbd::utils::Vector3d norm(0,0,0); - biorbd::utils::String axis(""); + utils::String parent_str("root"); + utils::Vector3d pos(0,0,0); + utils::Vector3d norm(0,0,0); + utils::String axis(""); double acc = 0; while(file.read(property_tag) && property_tag.tolower().compare("endcontact")) { if (!property_tag.tolower().compare("parent")) { @@ -499,7 +501,7 @@ void biorbd::Reader::readModelFile( file.read(parent_str); parent_int = model->GetBodyId(parent_str.c_str()); // If parent_int equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(parent_int), + utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment"); } else if (!property_tag.tolower().compare("position")) { readVector3d(file, variable, pos); @@ -513,23 +515,23 @@ void biorbd::Reader::readModelFile( } if (version == 1) { #ifndef BIORBD_USE_CASADI_MATH - biorbd::utils::Error::check(norm.norm() == 1.0, + utils::Error::check(norm.norm() == 1.0, "Normal of the contact must be provided" ); #endif model->AddConstraint(parent_int, pos, norm, name, acc); } else if (version >= 2) { - biorbd::utils::Error::check(axis.compare(""), "Axis must be provided"); + utils::Error::check(axis.compare(""), "Axis must be provided"); model->AddConstraint(parent_int, pos, axis, name, acc); } } else if (!main_tag.tolower().compare("loopconstraint")) { - biorbd::utils::String name; + utils::String name; unsigned int id_predecessor = 0; unsigned int id_successor = 0; - biorbd::utils::String predecessor_str("root"); - biorbd::utils::String successor_str("root"); - biorbd::utils::RotoTrans X_predecessor; - biorbd::utils::RotoTrans X_successor; - biorbd::utils::SpatialVector axis; + utils::String predecessor_str("root"); + utils::String successor_str("root"); + utils::RotoTrans X_predecessor; + utils::RotoTrans X_successor; + utils::SpatialVector axis; bool enableStabilization(false); double stabilizationParam(-1); while(file.read(property_tag) @@ -539,7 +541,7 @@ void biorbd::Reader::readModelFile( file.read(predecessor_str); id_predecessor = model->GetBodyId(predecessor_str.c_str()); // If parent_int equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(id_predecessor), + utils::Error::check(model->IsBodyId(id_predecessor), "Wrong name in a segment"); } if (!property_tag.tolower().compare("successor")) { @@ -547,30 +549,30 @@ void biorbd::Reader::readModelFile( file.read(successor_str); id_successor = model->GetBodyId(successor_str.c_str()); // If parent_int equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(id_successor), + utils::Error::check(model->IsBodyId(id_successor), "Wrong name in a segment"); } else if (!property_tag.tolower().compare("rtpredecessor")) { - biorbd::utils::String seq("xyz"); - biorbd::utils::Vector3d rot(0, 0, 0); - biorbd::utils::Vector3d trans(0, 0, 0); + utils::String seq("xyz"); + utils::Vector3d rot(0, 0, 0); + utils::Vector3d trans(0, 0, 0); // Transcribe the rotations readVector3d(file, variable, rot); // Transcribe the angle sequence for the rotations file.read(seq); // Transcribe the translations readVector3d(file, variable, trans); - X_predecessor = biorbd::utils::RotoTrans(rot, trans, seq); + X_predecessor = utils::RotoTrans(rot, trans, seq); } else if (!property_tag.tolower().compare("rtsuccessor")) { - biorbd::utils::String seq("xyz"); - biorbd::utils::Vector3d rot(0, 0, 0); - biorbd::utils::Vector3d trans(0, 0, 0); + utils::String seq("xyz"); + utils::Vector3d rot(0, 0, 0); + utils::Vector3d trans(0, 0, 0); // Transcribe the rotations readVector3d(file, variable, rot); // Transcribe the angle sequence for the roations file.read(seq); // Transcribe the translations readVector3d(file, variable, trans); - X_successor = biorbd::utils::RotoTrans(rot, trans, seq); + X_successor = utils::RotoTrans(rot, trans, seq); } else if (!property_tag.tolower().compare("axis")) for (unsigned int i=0; iGetBodyId(name.c_str()); // If parent_int equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(parent_int), + utils::Error::check(model->IsBodyId(parent_int), "Wrong name in a segment"); // Declaration of all the parameters for all types - biorbd::utils::String type; + utils::String type; bool isTypeSet = false; unsigned int dofIdx(INT_MAX); bool isDofSet = false; - biorbd::utils::String str_direction; + utils::String str_direction; bool isDirectionSet = false; int int_direction = 0; double Tmax(-1); @@ -645,13 +647,13 @@ void biorbd::Reader::readModelFile( file.read(type); isTypeSet = true; } else if (!property_tag.tolower().compare("dof")) { - biorbd::utils::String dofName; + utils::String dofName; file.read(dofName); dofIdx = model->getDofIndex(name, dofName); isDofSet = true; } else if (!property_tag.tolower().compare("direction")) { file.read(str_direction); - biorbd::utils::Error::check(!str_direction.tolower().compare("positive") || + utils::Error::check(!str_direction.tolower().compare("positive") || !str_direction.tolower().compare("negative"), "Direction should be \"positive\" or \"negative\""); if (!str_direction.tolower().compare("positive")) { @@ -712,28 +714,28 @@ void biorbd::Reader::readModelFile( } } // Verify that everything is there - biorbd::utils::Error::check(isTypeSet!=0, "Actuator type must be defined"); + utils::Error::check(isTypeSet!=0, "Actuator type must be defined"); biorbd::actuator::Actuator* actuator; if (!type.tolower().compare("gauss3p")) { - biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set + utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet && iswcSet && isaminSet && iswrSet && isw1Set && isrSet && isqoptSet, "Make sure all parameters are defined"); actuator = new biorbd::actuator::ActuatorGauss3p(int_direction,Tmax,T0,wmax,wc, amin,wr,w1,r,qopt,dofIdx,name); } else if (!type.tolower().compare("constant")) { - biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet, + utils::Error::check(isDofSet && isDirectionSet && isTmaxSet, "Make sure all parameters are defined"); actuator = new biorbd::actuator::ActuatorConstant(int_direction,Tmax,dofIdx, name); } else if (!type.tolower().compare("linear")) { - biorbd::utils::Error::check(isDofSet && isDirectionSet && isSlopeSet && isT0Set, + utils::Error::check(isDofSet && isDirectionSet && isSlopeSet && isT0Set, "Make sure all parameters are defined"); actuator = new biorbd::actuator::ActuatorLinear(int_direction,T0,slope,dofIdx, name); } else if (!type.tolower().compare("gauss6p")) { - biorbd::utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set + utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet && iswcSet && isaminSet && iswrSet && isw1Set && isrSet && isqoptSet && isFacteur6pSet && isr2Set && isqopt2Set, @@ -742,13 +744,13 @@ void biorbd::Reader::readModelFile( amin,wr,w1,r,qopt,facteur6p, r2, qopt2, dofIdx,name); } else if (!type.tolower().compare("sigmoidgauss3p")) { - biorbd::utils::Error::check(isDofSet && isThetaSet && isLambdaSet + utils::Error::check(isDofSet && isThetaSet && isLambdaSet && isOffsetSet && isrSet && isqoptSet, "Make sure all parameters are defined"); actuator = new biorbd::actuator::ActuatorSigmoidGauss3p(int_direction, theta, lambda, offset, r, qopt, dofIdx, name); } else { - biorbd::utils::Error::raise("Actuator do not correspond to an implemented one"); + utils::Error::raise("Actuator do not correspond to an implemented one"); #ifdef _WIN32 actuator = new biorbd::actuator::ActuatorConstant(int_direction, Tmax, dofIdx, name); // Échec de compilation sinon @@ -758,15 +760,15 @@ void biorbd::Reader::readModelFile( model->addActuator(*actuator); delete actuator; #else // MODULE_ACTUATORS - biorbd::utils::Error::raise("Biorbd was build without the module Actuators but the model defines ones"); + utils::Error::raise("Biorbd was build without the module Actuators but the model defines ones"); #endif // MODULE_ACTUATORS } else if (!main_tag.tolower().compare("musclegroup")) { #ifdef MODULE_MUSCLES - biorbd::utils::String name; + utils::String name; file.read(name); // Name of the muscular group // Declaration of the variables - biorbd::utils::String origin_parent_str("root"); - biorbd::utils::String insert_parent_str("root"); + utils::String origin_parent_str("root"); + utils::String insert_parent_str("root"); // Read the file while(file.read(property_tag) && property_tag.tolower().compare("endmusclegroup")) { @@ -775,24 +777,24 @@ void biorbd::Reader::readModelFile( file.read(origin_parent_str); unsigned int idx = model->GetBodyId(origin_parent_str.c_str()); // If parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(idx), + utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle"); } else if (!property_tag.tolower().compare("insertionparent")) { // Dynamically find the parent number file.read(insert_parent_str); unsigned int idx = model->GetBodyId(insert_parent_str.c_str()); // If parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(idx), + utils::Error::check(model->IsBodyId(idx), "Wrong insertion parent name for a muscle"); } } model->addMuscleGroup(name, origin_parent_str, insert_parent_str); #else // MODULE_MUSCLES - biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle group"); + utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle group"); #endif // MODULE_MUSCLES } else if (!main_tag.tolower().compare("muscle")) { #ifdef MODULE_MUSCLES - biorbd::utils::String name; + utils::String name; file.read(name); // Name of the muscle // Declaration of the variables biorbd::muscles::MUSCLE_TYPE type(biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE); @@ -800,10 +802,10 @@ void biorbd::Reader::readModelFile( biorbd::muscles::STATE_TYPE::NO_STATE_TYPE); biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType( biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); - biorbd::utils::String muscleGroup(""); + utils::String muscleGroup(""); int idxGroup(-1); - biorbd::utils::Vector3d origin_pos(0,0,0); - biorbd::utils::Vector3d insert_pos(0,0,0); + utils::Vector3d origin_pos(0,0,0); + utils::Vector3d insert_pos(0,0,0); double optimalLength(0); double maxForce(0); double tendonSlackLength(0); @@ -821,9 +823,9 @@ void biorbd::Reader::readModelFile( file.read(muscleGroup); idxGroup = model->getMuscleGroupId(muscleGroup); // If parent_int is still equal to zero, no name has concurred - biorbd::utils::Error::check(idxGroup!=-1, "Could not find muscle group"); + utils::Error::check(idxGroup!=-1, "Could not find muscle group"); } else if (!property_tag.tolower().compare("type")) { - biorbd::utils::String tp_type; + utils::String tp_type; file.read(tp_type); if (!tp_type.tolower().compare("idealizedactuator")) { type = biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR; @@ -839,17 +841,17 @@ void biorbd::Reader::readModelFile( || !tp_type.tolower().compare("thelenfatigable")) { type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE; } else { - biorbd::utils::Error::raise(property_tag + " is not a valid muscle type"); + utils::Error::raise(property_tag + " is not a valid muscle type"); } } else if (!property_tag.tolower().compare("statetype")) { - biorbd::utils::String tp_state; + utils::String tp_state; file.read(tp_state); if (!tp_state.tolower().compare("buchanan")) { stateType = biorbd::muscles::STATE_TYPE::BUCHANAN; } else if (!tp_state.tolower().compare("degroote")) { stateType = biorbd::muscles::STATE_TYPE::DE_GROOTE; } else { - biorbd::utils::Error::raise(property_tag + " is not a valid muscle state type"); + utils::Error::raise(property_tag + " is not a valid muscle state type"); } } else if (!property_tag.tolower().compare("originposition")) { readVector3d(file, variable, origin_pos); @@ -871,14 +873,14 @@ void biorbd::Reader::readModelFile( while(file.read(subproperty_tag) && subproperty_tag.tolower().compare("endfatigueparameters")) { if (!subproperty_tag.tolower().compare("type")) { - biorbd::utils::String tp_fatigue_type; + utils::String tp_fatigue_type; file.read(tp_fatigue_type); if (!tp_fatigue_type.tolower().compare("simple")) { dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE; } else if (!tp_fatigue_type.tolower().compare("xia")) { dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA; } else { - biorbd::utils::Error::raise(tp_fatigue_type + + utils::Error::raise(tp_fatigue_type + " is not a value fatigue parameter type"); } } else { @@ -899,11 +901,11 @@ void biorbd::Reader::readModelFile( file.read(shapeFactor); } } - biorbd::utils::Error::check(idxGroup!=-1, "No muscle group was provided!"); + utils::Error::check(idxGroup!=-1, "No muscle group was provided!"); biorbd::muscles::Geometry geo( - biorbd::utils::Vector3d(origin_pos, name + "_origin", + utils::Vector3d(origin_pos, name + "_origin", model->muscleGroup(static_cast(idxGroup)).origin()), - biorbd::utils::Vector3d(insert_pos, name + "_insertion", + utils::Vector3d(insert_pos, name + "_insertion", model->muscleGroup(static_cast(idxGroup)).insertion())); biorbd::muscles::State stateMax(maxExcitation, maxActivation); biorbd::muscles::Characteristics characteristics(optimalLength, maxForce, PCSA, @@ -920,17 +922,17 @@ void biorbd::Reader::readModelFile( shapeFactor); } #else // MODULE_MUSCLES - biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle"); + utils::Error::raise("Biorbd was build without the module Muscles but the model defines a muscle"); #endif // MODULE_MUSCLES } else if (!main_tag.tolower().compare("viapoint")) { #ifdef MODULE_MUSCLES - biorbd::utils::String name; + utils::String name; file.read(name); // Name of muscle... Eventually add the muscle group // Declaration of the variables - biorbd::utils::String parent(""); - biorbd::utils::String muscle(""); - biorbd::utils::String musclegroup(""); + utils::String parent(""); + utils::String muscle(""); + utils::String musclegroup(""); int iMuscleGroup(-1); int iMuscle(-1); biorbd::muscles::ViaPoint position(0,0,0); @@ -943,7 +945,7 @@ void biorbd::Reader::readModelFile( file.read(parent); unsigned int idx = model->GetBodyId(parent.c_str()); // If parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(idx), + utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle"); } else if (!property_tag.tolower().compare("muscle")) { file.read(muscle); @@ -954,30 +956,30 @@ void biorbd::Reader::readModelFile( } } iMuscleGroup = model->getMuscleGroupId(musclegroup); - biorbd::utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!"); + utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!"); iMuscle = model->muscleGroup(static_cast(iMuscleGroup)).muscleID( muscle); - biorbd::utils::Error::check(iMuscle!=-1, "No muscle was provided!"); + utils::Error::check(iMuscle!=-1, "No muscle was provided!"); position.setName(name); position.setParent(parent); model->muscleGroup(static_cast(iMuscleGroup)) .muscle(static_cast(iMuscle)).addPathObject(position); #else // MODULE_MUSCLES - biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a viapoint"); + utils::Error::raise("Biorbd was build without the module Muscles but the model defines a viapoint"); #endif // MODULE_MUSCLES } else if (!main_tag.tolower().compare("wrapping")) { #ifdef MODULE_MUSCLES - biorbd::utils::String name; + utils::String name; file.read(name); // Name of the wrapping // Declaration of the variables - biorbd::utils::String muscle(""); - biorbd::utils::String musclegroup(""); - biorbd::utils::String wrapType(""); + utils::String muscle(""); + utils::String musclegroup(""); + utils::String wrapType(""); int iMuscleGroup(-1); int iMuscle(-1); - biorbd::utils::String parent(""); - biorbd::utils::RotoTrans RT; + utils::String parent(""); + utils::RotoTrans RT; bool isRTset(false); bool RTinMatrix(false); double radius(0); @@ -991,10 +993,10 @@ void biorbd::Reader::readModelFile( file.read(parent); unsigned int idx = model->GetBodyId(parent.c_str()); // If parent_int still equals zero, no name has concurred - biorbd::utils::Error::check(model->IsBodyId(idx), + utils::Error::check(model->IsBodyId(idx), "Wrong origin parent name for a muscle"); } else if (!property_tag.tolower().compare("rtinmatrix")) { - biorbd::utils::Error::check(isRTset==false, + utils::Error::check(isRTset==false, "RT should not appear before RTinMatrix"); file.read(RTinMatrix); } else if (!property_tag.tolower().compare("rt")) { @@ -1012,34 +1014,34 @@ void biorbd::Reader::readModelFile( file.read(length, variable); } } - biorbd::utils::Error::check(parent != "", "Parent was not defined"); + utils::Error::check(parent != "", "Parent was not defined"); iMuscleGroup = model->getMuscleGroupId(musclegroup); - biorbd::utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!"); + utils::Error::check(iMuscleGroup!=-1, "No muscle group was provided!"); iMuscle = model->muscleGroup(static_cast(iMuscleGroup)).muscleID( muscle); - biorbd::utils::Error::check(iMuscle!=-1, "No muscle was provided!"); + utils::Error::check(iMuscle!=-1, "No muscle was provided!"); if (!wrapType.tolower().compare("halfcylinder")) { - biorbd::utils::Error::check(radius > 0.0, + utils::Error::check(radius > 0.0, "Radius must be defined and positive"); - biorbd::utils::Error::check(length >= 0.0, "Length was must be positive"); + utils::Error::check(length >= 0.0, "Length was must be positive"); biorbd::muscles::WrappingHalfCylinder cylinder(RT,radius,length,name,parent); model->muscleGroup(static_cast(iMuscleGroup)).muscle( static_cast(iMuscle)).addPathObject( cylinder); } else { - biorbd::utils::Error::raise("Wrapping type must be defined (choices: 'cylinder')"); + utils::Error::raise("Wrapping type must be defined (choices: 'cylinder')"); } #else // MODULE_MUSCLES - biorbd::utils::Error::raise("Biorbd was build without the module Muscles but the model defines a wrapping object"); + utils::Error::raise("Biorbd was build without the module Muscles but the model defines a wrapping object"); #endif // MODULE_MUSCLES } } } catch (std::runtime_error message) { - biorbd::utils::String error_message("Reading of file \"" + path.filename() + "." + utils::String error_message("Reading of file \"" + path.filename() + "." + path.extension() + "\" failed with the following error:"); - error_message += "\n" + biorbd::utils::String(message.what()) + "\n"; + error_message += "\n" + utils::String(message.what()) + "\n"; if (name.compare("")) { error_message += "Element: " + main_tag + ", named: " + name + "\n"; } @@ -1050,7 +1052,7 @@ void biorbd::Reader::readModelFile( error_message += "Subproperty: " + subproperty_tag + "\n"; } - biorbd::utils::Error::raise(error_message); + utils::Error::raise(error_message); } #ifdef MODULE_ACTUATORS if (hasActuators) { @@ -1061,28 +1063,28 @@ void biorbd::Reader::readModelFile( // std::cout << "Model file successfully loaded" << std::endl; file.close(); } -std::vector> - biorbd::Reader::readMarkerDataFile( - const biorbd::utils::Path &path) +std::vector> + Reader::readMarkerDataFile( + const utils::Path &path) { // Open file // std::cout << "Loading marker file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String tp; + utils::String tp; // Determine the version of the file file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1, "Version not implemented yet"); + utils::Error::check(version == 1, "Version not implemented yet"); // Determine the number of markers file.readSpecificTag("nbmark", tp); @@ -1092,13 +1094,13 @@ std::vector> file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector> markers; - std::vector position; + std::vector> markers; + std::vector position; // Scroll down until the definition of a marker for (unsigned int j=0; j> file.read(noMarker); for (unsigned int i=0; i<=nbIntervals; i++) { // <= because there is nbIntervals+1 values - biorbd::utils::Vector3d mark(0, 0, 0); - readVector3d(file, std::map(), mark); + utils::Vector3d mark(0, 0, 0); + readVector3d(file, std::map(), mark); position.push_back(mark); } markers.push_back(position); @@ -1122,28 +1124,28 @@ std::vector> return markers; } -std::vector -biorbd::Reader::readQDataFile( +std::vector +Reader::readQDataFile( const utils::Path &path) { // Open file // std::cout << "Loading kin file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String tp; + utils::String tp; // Determine the file version file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1, "Version not implemented yet"); + utils::Error::check(version == 1, "Version not implemented yet"); // Determine the number of markers file.readSpecificTag("nddl", tp); @@ -1153,18 +1155,18 @@ biorbd::Reader::readQDataFile( file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector kinematics; + std::vector kinematics; // Scroll down until the definition of a marker for (unsigned int j=0; j -biorbd::Reader::readActivationDataFile( +std::vector +Reader::readActivationDataFile( const utils::Path &path) { // Open file // std::cout << "Loading kin file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String tp; + utils::String tp; // Determine the file version file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1, "Version not implemented yet"); + utils::Error::check(version == 1, "Version not implemented yet"); // Determine the number of markers file.readSpecificTag("nbmuscles", tp); @@ -1211,18 +1213,18 @@ biorbd::Reader::readActivationDataFile( file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector activations; + std::vector activations; // Scroll down until the definition of a marker for (unsigned int j=0; j -biorbd::Reader::readTorqueDataFile( +std::vector +Reader::readTorqueDataFile( const utils::Path &path) { // Open file // std::cout << "Loading kin file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif //Read file - biorbd::utils::String tp; + utils::String tp; // Determine the file version file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1, "Version not implemented yet"); + utils::Error::check(version == 1, "Version not implemented yet"); // Determine the number of GeneralizedTorque file.readSpecificTag("nGeneralizedTorque", tp); // @@ -1269,12 +1271,12 @@ biorbd::Reader::readTorqueDataFile( file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector torque; + std::vector torque; // Scroll down until the definition of a torque for (unsigned int j=0; j -biorbd::Reader::readGroundReactionForceDataFile( +std::vector +Reader::readGroundReactionForceDataFile( const utils::Path &path) { // Open file // std::cout << "Loading ground reaction force file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String tp; + utils::String tp; // Determine the file version file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1, "Version not implemented yet"); + utils::Error::check(version == 1, "Version not implemented yet"); // Determine the number of grf (ground reaction force) file.readSpecificTag("ngrf", tp); // @@ -1330,12 +1332,12 @@ biorbd::Reader::readGroundReactionForceDataFile( file.readSpecificTag("nbintervals", tp); unsigned int nbIntervals(static_cast(atoi(tp.c_str()))); - std::vector grf; + std::vector grf; // Scroll down until the definition of a torque for (unsigned int j=0; j>& frame, // Frame vector (time is frame/frequency) std::vector& frequency,// Acquisition frequency - std::vector>& + std::vector>& force, // Linear forces (x,y,z) - std::vector>& moment, // Moments (x,y,z) - std::vector>& + std::vector>& moment, // Moments (x,y,z) + std::vector>& cop) // Center of pressure (x,y,z) * number of pf { // Open file // std::cout << "Loading force file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif @@ -1388,15 +1390,15 @@ void biorbd::Reader::readViconForceFile( cop.clear(); // Read file - biorbd::utils::String tp; + utils::String tp; while(!file.eof()) { // Put everything in temporary per platform (Mettre tout en temporaire par plate-forme) std::vector frame1pf; - std::vector force1fp; - std::vector moment1fp; - std::vector cop1fp; + std::vector force1fp; + std::vector moment1fp; + std::vector cop1fp; // Get the acquisition frequency file.readSpecificTag("devices", tp); @@ -1418,7 +1420,7 @@ void biorbd::Reader::readViconForceFile( // Otherwise, we save the line // now we'll use a stringstream to separate the fields out of the line (comma separated) std::stringstream ss( tp ); - biorbd::utils::String field; + utils::String field; std::vector data; data.clear(); while (getline( ss, field, ',' )) { @@ -1432,15 +1434,15 @@ void biorbd::Reader::readViconForceFile( data.push_back( f ); } // Make sure that the line has the right number of elements(2 times, 3 cop, 3 forces, 3 moments) - biorbd::utils::Error::check(data.size()==11, + utils::Error::check(data.size()==11, "Wrong number of element in a line in the force file"); // Fill in the fields frame1pf.push_back(static_cast(data[0])); // Frame stamp // unsigned int subFrame = static_cast(data[1]); // Subframes (not interesting) - biorbd::utils::Vector3d cop_tp(0, 0, 0); // Center of pressure - biorbd::utils::Vector3d for_tp(0, 0, 0); // Force - biorbd::utils::Vector3d M_tp(0, 0, 0); // Moment + utils::Vector3d cop_tp(0, 0, 0); // Center of pressure + utils::Vector3d for_tp(0, 0, 0); // Force + utils::Vector3d M_tp(0, 0, 0); // Moment for (unsigned int i=0; i<3; ++i) { cop_tp[i] = data[i+2]/1000; // from mm to m for_tp[i] = data[i+5]; @@ -1459,49 +1461,49 @@ void biorbd::Reader::readViconForceFile( } } -std::vector> - biorbd::Reader::readViconForceFile(const biorbd::utils::String &path) +std::vector> + Reader::readViconForceFile(const utils::String &path) { // Read file std::vector> frame; std::vector frequency;// Acquisition frequency - std::vector> + std::vector> force; // Linear forces (x,y,z) - std::vector> moment; // Moments (x,y,z) - std::vector> + std::vector> moment; // Moments (x,y,z) + std::vector> cop; // Center of pressure (x,y,z) readViconForceFile(path, frame, frequency, force, moment, cop); // Redispatch the values in a vector of SpatialTransform - std::vector> + std::vector> st; // nb of platform per time for (unsigned int j=0; j tp; + std::vector tp; for (unsigned int i=0; i> - biorbd::Reader::readViconMarkerFile(const biorbd::utils::Path& path, - std::vector& markOrder, +std::vector> + Reader::readViconMarkerFile(const utils::Path& path, + std::vector& markOrder, int nFramesToGet) { // Read file #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif - biorbd::utils::String t; + utils::String t; // Get the acquisition frequency @@ -1521,9 +1523,9 @@ std::vector> idx_end.push_back(static_cast(t.find(",", idx_tp+1))); } // Keep the names between the separators - std::vector MarkersInFile; + std::vector MarkersInFile; for (unsigned int i=0; i> } for (int i=0; i(markOrder.size()); ++i) { unsigned int cmp=0; - biorbd::utils::String m1 = (*(markOrder.begin()+i)); + utils::String m1 = (*(markOrder.begin()+i)); while (1) { - biorbd::utils::String m2 = (*(MarkersInFile.begin()+cmp)); + utils::String m2 = (*(MarkersInFile.begin()+cmp)); if (!m1.compare(m2)) { ordre[3*cmp] = 3*i; ordre[3*cmp+1] = 3*i+1; @@ -1550,7 +1552,7 @@ std::vector> } else { ++cmp; } -// biorbd::utils::Error::check(cmp=MarkersInFile.size()) { break; @@ -1573,18 +1575,18 @@ std::vector> } file.close(); #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Skip the header for (unsigned int i=0; i<7; ++i) { file.read(t); } - biorbd::utils::Error::check(nFramesToGet!=0 && nFramesToGet!=1 + utils::Error::check(nFramesToGet!=0 && nFramesToGet!=1 && static_cast(nFramesToGet)<=nbFrames, "nNode should not be 0, 1 or greater " "than number of frame"); @@ -1592,16 +1594,16 @@ std::vector> } - std::vector> data; + std::vector> data; // now we'll use a stringstream to separate the fields out of the line (comma separated) unsigned int cmpFrames(1); while(!file.eof()) { - biorbd::utils::Vector data_tp = biorbd::utils::Vector( + utils::Vector data_tp = utils::Vector( static_cast(3*markOrder.size())); data_tp.setZero(); std::stringstream ss( t ); - biorbd::utils::String field; + utils::String field; unsigned int cmp = 0; while (getline( ss, field, ',' )) { // for each field we wish to convert it to a double @@ -1619,9 +1621,9 @@ std::vector> ++cmp; } // Once the markers are in order, separate them - std::vector data_tp2; + std::vector data_tp2; for (unsigned int i=0; i(data_tp.size())/3; ++i) { - biorbd::utils::Vector3d node(data_tp.block(3*i, 0, 3, 1)/1000); + utils::Vector3d node(data_tp.block(3*i, 0, 3, 1)/1000); data_tp2.push_back(node); } // Stock the marker vector a that time t @@ -1643,30 +1645,30 @@ std::vector> return data; } -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh -biorbd::Reader::readMeshFileBiorbdSegments( - const biorbd::utils::Path &path) +rigidbody::Mesh +Reader::readMeshFileBiorbdSegments( + const utils::Path &path) { // Read a segment file // Open file // std::cout << "Loading marker file: " << path << std::endl; #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif // Read file - biorbd::utils::String tp; + utils::String tp; // Determine the file version file.readSpecificTag("version", tp); unsigned int version(static_cast(atoi(tp.c_str()))); - biorbd::utils::Error::check(version == 1 + utils::Error::check(version == 1 || version == 2, "Version not implemented yet"); // Know the number of points @@ -1675,14 +1677,14 @@ biorbd::Reader::readMeshFileBiorbdSegments( file.readSpecificTag("nfaces", tp); unsigned int nFaces(static_cast(atoi(tp.c_str()))); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; + rigidbody::Mesh mesh; mesh.setPath(path); // Get all the points - std::map variable = - std::map(); - biorbd::utils::Vector3d dump; // Ignore columns 4 5 6 + std::map variable = + std::map(); + utils::Vector3d dump; // Ignore columns 4 5 6 for (unsigned int iPoints=0; iPoints < nPoints; ++iPoints) { - biorbd::utils::Vector3d nodeTp(0, 0, 0); + utils::Vector3d nodeTp(0, 0, 0); readVector3d(file, variable, nodeTp); mesh.addPoint(nodeTp); if (version == 2) { @@ -1691,11 +1693,11 @@ biorbd::Reader::readMeshFileBiorbdSegments( } for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace patchTp; + rigidbody::MeshFace patchTp; unsigned int nVertices; file.read(nVertices); if (nVertices != 3) { - biorbd::utils::Error::raise("Patches must be 3 vertices!"); + utils::Error::raise("Patches must be 3 vertices!"); } for (unsigned int i=0; i variable = - std::map(); + std::map variable = + std::map(); // Get all the points for (unsigned int iPoints=0; iPoints < nVertex; ++iPoints) { - biorbd::utils::Vector3d nodeTp(0, 0, 0); + utils::Vector3d nodeTp(0, 0, 0); readVector3d(file, variable, nodeTp); mesh.addPoint(nodeTp); // Ignore the columns post XYZ @@ -1758,11 +1760,11 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFilePly( } for (unsigned int iPoints=0; iPoints < nFaces; ++iPoints) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::MeshFace patchTp; + rigidbody::MeshFace patchTp; unsigned int nVertices; file.read(nVertices); if (nVertices != 3) { - biorbd::utils::Error::raise("Patches must be 3 vertices!"); + utils::Error::raise("Patches must be 3 vertices!"); } for (unsigned int i=0; i variable = - std::map(); + utils::Vector3d vertex; + rigidbody::MeshFace patch; + utils::String text; + std::map variable = + std::map(); while (true) { // If we get to the end of file, exit loop if (file.eof()) { @@ -1824,7 +1826,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileObj( for (unsigned int i=0; i<3; ++i) { file.read(text); size_t idxSlash = text.find("/"); - biorbd::utils::String tata3(text.substr (0,idxSlash)); + utils::String tata3(text.substr (0,idxSlash)); patch(i) = (std::stoi(text.substr (0,idxSlash)) - 1); } file.getline(text); // Ignore last element if it is a 4 vertex based @@ -1841,23 +1843,23 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileObj( #ifdef MODULE_VTP_FILES_READER #include "tinyxml.h" -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( - const biorbd::utils::Path &path) +rigidbody::Mesh Reader::readMeshFileVtp( + const utils::Path &path) { // Read an opensim formatted mesh file // Read the file #ifdef _WIN32 - biorbd::utils::String filepath( biorbd::utils::Path::toWindowsFormat( + utils::String filepath( utils::Path::toWindowsFormat( path.absolutePath()).c_str()); #else - biorbd::utils::String filepath( path.absolutePath().c_str() ); + utils::String filepath( path.absolutePath().c_str() ); #endif TiXmlDocument doc(filepath); - biorbd::utils::Error::check(doc.LoadFile(), "Failed to load file " + filepath); + utils::Error::check(doc.LoadFile(), "Failed to load file " + filepath); TiXmlHandle hDoc(&doc); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh mesh; + rigidbody::Mesh mesh; mesh.setPath(path); // Navigate up to VTKFile/PolyData/Piece @@ -1869,11 +1871,11 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( polydata.ToElement()->QueryIntAttribute("NumberOfPoints", &numberOfPoints); polydata.ToElement()->QueryIntAttribute("NumberOfPolys", &NumberOfPolys); - biorbd::utils::String field; + utils::String field; // Get the points { - biorbd::utils::String points( + utils::String points( polydata.ChildElement("Points", 0) .ChildElement("DataArray", 0).Element()->GetText()); std::stringstream ss( points ); @@ -1894,13 +1896,13 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( std::stringstream fs( field ); fs >> z; } - mesh.addPoint(biorbd::utils::Vector3d(x, y, z)); + mesh.addPoint(utils::Vector3d(x, y, z)); } } // Get the patches { - biorbd::utils::String polys( + utils::String polys( polydata.ChildElement("Polys", 0) .ChildElement("DataArray", 0).Element()->GetText()); std::stringstream ss( polys ); @@ -1928,19 +1930,19 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh biorbd::Reader::readMeshFileVtp( return mesh; } -void biorbd::Reader::readVector3d( - biorbd::utils::IfStream &file, - const std::map &variable, - biorbd::utils::Vector3d &vector) +void Reader::readVector3d( + utils::IfStream &file, + const std::map &variable, + utils::Vector3d &vector) { for (unsigned int i=0; i<3; ++i) { file.read(vector(i), variable); } } -void biorbd::Reader::readMatrix33( - biorbd::utils::IfStream &file, - const std::map &variable, +void Reader::readMatrix33( + utils::IfStream &file, + const std::map &variable, RigidBodyDynamics::Math::Matrix3d &matrix) { for (unsigned int i=0; i<3; ++i) { @@ -1950,11 +1952,11 @@ void biorbd::Reader::readMatrix33( } } -void biorbd::Reader::readRtMatrix( - biorbd::utils::IfStream& file, - const std::map& variable, +void Reader::readRtMatrix( + utils::IfStream& file, + const std::map& variable, bool RTinMatrix, - biorbd::utils::RotoTrans &RT) + utils::RotoTrans &RT) { if (RTinMatrix) { // Matrix 4x4 // Counter for classification (Compteur pour classification) @@ -1964,9 +1966,9 @@ void biorbd::Reader::readRtMatrix( } } } else { - biorbd::utils::String seq("xyz"); - biorbd::utils::Vector3d rot(0, 0, 0); - biorbd::utils::Vector3d trans(0, 0, 0); + utils::String seq("xyz"); + utils::Vector3d rot(0, 0, 0); + utils::Vector3d trans(0, 0, 0); // Transcribe the rotations for (unsigned int i=0; i<3; ++i) { file.read(rot(i)); @@ -1977,26 +1979,26 @@ void biorbd::Reader::readRtMatrix( for (unsigned int i=0; i<3; ++i) { file.read(trans(i)); } - RT = biorbd::utils::RotoTrans(rot, trans, seq); + RT = utils::RotoTrans(rot, trans, seq); } } #endif // MODULE_VTP_FILES_READER -std::vector> - biorbd::Reader::readViconMarkerFile(const biorbd::utils::Path &path, +std::vector> + Reader::readViconMarkerFile(const utils::Path &path, int nFramesToGet) { // Read file #ifdef _WIN32 - biorbd::utils::IfStream file( - biorbd::utils::Path::toWindowsFormat( + utils::IfStream file( + utils::Path::toWindowsFormat( path.absolutePath()).c_str(), std::ios::in); #else - biorbd::utils::IfStream file( + utils::IfStream file( path.absolutePath().c_str(), std::ios::in); #endif - biorbd::utils::String t; + utils::String t; // Get the acquisition frequency @@ -2016,9 +2018,9 @@ std::vector> idx_end.push_back(static_cast(t.find(",", idx_tp+1))); } // Keep the names between the separators - std::vector MarkersInFile; + std::vector MarkersInFile; for (unsigned int i=0; i rigidbody::Contacts::contactNames() +std::vector rigidbody::Contacts::contactNames() { - std::vector names; + std::vector names; for (auto name : RigidBodyDynamics::ConstraintSet::name) { names.push_back(name); } return names; } -biorbd::utils::String rigidbody::Contacts::contactName(unsigned int i) +utils::String rigidbody::Contacts::contactName(unsigned int i) { - biorbd::utils::Error::check(i<*m_nbreConstraint, + utils::Error::check(i<*m_nbreConstraint, "Idx for contact names is too high.."); return RigidBodyDynamics::ConstraintSet::name[i]; } -std::vector +std::vector rigidbody::Contacts::constraintsInGlobal( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) @@ -159,7 +159,7 @@ rigidbody::Contacts::constraintsInGlobal( #endif // Output variable - std::vector tp; + std::vector tp; // On each control, apply the rotation and save the position @@ -177,7 +177,7 @@ rigidbody::Contacts::constraintsInGlobal( return tp; } -biorbd::utils::Vector rigidbody::Contacts::getForce() const +utils::Vector rigidbody::Contacts::getForce() const { - return static_cast(this->force); + return static_cast(this->force); } diff --git a/src/RigidBody/GeneralizedAcceleration.cpp b/src/RigidBody/GeneralizedAcceleration.cpp index b880c9a7..a4eef1a0 100644 --- a/src/RigidBody/GeneralizedAcceleration.cpp +++ b/src/RigidBody/GeneralizedAcceleration.cpp @@ -12,28 +12,28 @@ rigidbody::GeneralizedAcceleration::GeneralizedAcceleration() rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( unsigned int nQddot) : - biorbd::utils::Vector(nQddot) + utils::Vector(nQddot) { } rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : - biorbd::utils::Vector(j.nbQ()) + utils::Vector(j.nbQ()) { } rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const rigidbody::GeneralizedAcceleration &Q) : - biorbd::utils::Vector(Q) + utils::Vector(Q) { } rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const RigidBodyDynamics::Math::VectorNd &v) : - biorbd::utils::Vector (v) + utils::Vector (v) { } @@ -42,7 +42,7 @@ rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( const casadi::MX &v) : - biorbd::utils::Vector(v) + utils::Vector(v) { } @@ -56,9 +56,9 @@ rigidbody::GeneralizedAcceleration::~GeneralizedAcceleration() } void rigidbody::GeneralizedAcceleration::operator=( - const biorbd::utils::Vector &other) + const utils::Vector &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH @@ -66,13 +66,13 @@ void rigidbody::GeneralizedAcceleration::operator=( void rigidbody::GeneralizedAcceleration::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } void rigidbody::GeneralizedAcceleration::operator=( const casadi::MX &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #endif diff --git a/src/RigidBody/GeneralizedCoordinates.cpp b/src/RigidBody/GeneralizedCoordinates.cpp index 550e76f1..9382cba4 100644 --- a/src/RigidBody/GeneralizedCoordinates.cpp +++ b/src/RigidBody/GeneralizedCoordinates.cpp @@ -12,28 +12,28 @@ rigidbody::GeneralizedCoordinates::GeneralizedCoordinates() rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( unsigned int nbQ) : - biorbd::utils::Vector(nbQ) + utils::Vector(nbQ) { } rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : - biorbd::utils::Vector(j.nbQ()) + utils::Vector(j.nbQ()) { } rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const rigidbody::GeneralizedCoordinates &Q) : - biorbd::utils::Vector(Q) + utils::Vector(Q) { } rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const RigidBodyDynamics::Math::VectorNd &v) : - biorbd::utils::Vector (v) + utils::Vector (v) { } @@ -42,7 +42,7 @@ rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( const casadi::MX &v) : - biorbd::utils::Vector(v) + utils::Vector(v) { } @@ -55,9 +55,9 @@ rigidbody::GeneralizedCoordinates::~GeneralizedCoordinates() } void rigidbody::GeneralizedCoordinates::operator=( - const biorbd::utils::Vector &other) + const utils::Vector &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH @@ -65,13 +65,13 @@ void rigidbody::GeneralizedCoordinates::operator=( void rigidbody::GeneralizedCoordinates::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } void rigidbody::GeneralizedCoordinates::operator=( const casadi::MX &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #endif diff --git a/src/RigidBody/GeneralizedTorque.cpp b/src/RigidBody/GeneralizedTorque.cpp index b8dc3983..01575322 100644 --- a/src/RigidBody/GeneralizedTorque.cpp +++ b/src/RigidBody/GeneralizedTorque.cpp @@ -12,28 +12,28 @@ rigidbody::GeneralizedTorque::GeneralizedTorque() rigidbody::GeneralizedTorque::GeneralizedTorque( unsigned int nTorque) : - biorbd::utils::Vector(nTorque) + utils::Vector(nTorque) { } rigidbody::GeneralizedTorque::GeneralizedTorque( const rigidbody::Joints &j) : - biorbd::utils::Vector(j.nbGeneralizedTorque()) + utils::Vector(j.nbGeneralizedTorque()) { } rigidbody::GeneralizedTorque::GeneralizedTorque( const rigidbody::GeneralizedTorque &other) : - biorbd::utils::Vector(other) + utils::Vector(other) { } rigidbody::GeneralizedTorque::GeneralizedTorque( const RigidBodyDynamics::Math::VectorNd &v) : - biorbd::utils::Vector (v) + utils::Vector (v) { } @@ -42,7 +42,7 @@ rigidbody::GeneralizedTorque::GeneralizedTorque( rigidbody::GeneralizedTorque::GeneralizedTorque( const casadi::MX &v) : - biorbd::utils::Vector(v) + utils::Vector(v) { } @@ -50,9 +50,9 @@ rigidbody::GeneralizedTorque::GeneralizedTorque( #endif void rigidbody::GeneralizedTorque::operator=( - const biorbd::utils::Vector &other) + const utils::Vector &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH @@ -60,13 +60,13 @@ void rigidbody::GeneralizedTorque::operator=( void rigidbody::GeneralizedTorque::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } void rigidbody::GeneralizedTorque::operator=( const casadi::MX &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #endif diff --git a/src/RigidBody/GeneralizedVelocity.cpp b/src/RigidBody/GeneralizedVelocity.cpp index aa9b1f8f..e65bde07 100644 --- a/src/RigidBody/GeneralizedVelocity.cpp +++ b/src/RigidBody/GeneralizedVelocity.cpp @@ -12,28 +12,28 @@ rigidbody::GeneralizedVelocity::GeneralizedVelocity() rigidbody::GeneralizedVelocity::GeneralizedVelocity( unsigned int nbQdot) : - biorbd::utils::Vector(nbQdot) + utils::Vector(nbQdot) { } rigidbody::GeneralizedVelocity::GeneralizedVelocity( const rigidbody::Joints &j) : - biorbd::utils::Vector(j.nbQdot()) + utils::Vector(j.nbQdot()) { } rigidbody::GeneralizedVelocity::GeneralizedVelocity( const rigidbody::GeneralizedVelocity &Q) : - biorbd::utils::Vector(Q) + utils::Vector(Q) { } rigidbody::GeneralizedVelocity::GeneralizedVelocity( const RigidBodyDynamics::Math::VectorNd &v) : - biorbd::utils::Vector (v) + utils::Vector (v) { } @@ -42,7 +42,7 @@ rigidbody::GeneralizedVelocity::GeneralizedVelocity( rigidbody::GeneralizedVelocity::GeneralizedVelocity( const casadi::MX &v) : - biorbd::utils::Vector(v) + utils::Vector(v) { } @@ -56,9 +56,9 @@ rigidbody::GeneralizedVelocity::~GeneralizedVelocity() } void rigidbody::GeneralizedVelocity::operator=( - const biorbd::utils::Vector &other) + const utils::Vector &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH @@ -66,13 +66,13 @@ void rigidbody::GeneralizedVelocity::operator=( void rigidbody::GeneralizedVelocity::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } void rigidbody::GeneralizedVelocity::operator=( const casadi::MX &other) { - this->biorbd::utils::Vector::operator=(other); + this->utils::Vector::operator=(other); } #endif diff --git a/src/RigidBody/IMU.cpp b/src/RigidBody/IMU.cpp index 665ac641..ed96885f 100644 --- a/src/RigidBody/IMU.cpp +++ b/src/RigidBody/IMU.cpp @@ -8,7 +8,7 @@ using namespace biorbd::BIORBD_MATH_NAMESPACE; rigidbody::IMU::IMU( bool isTechnical, bool isAnatomical) : - biorbd::utils::RotoTransNode(), + utils::RotoTransNode(), m_technical(std::make_shared(isTechnical)), m_anatomical(std::make_shared(isAnatomical)) { @@ -16,10 +16,10 @@ rigidbody::IMU::IMU( } rigidbody::IMU::IMU( - const biorbd::utils::RotoTransNode &RotoTrans, + const utils::RotoTransNode &RotoTrans, bool isTechnical, bool isAnatomical) : - biorbd::utils::RotoTransNode(RotoTrans), + utils::RotoTransNode(RotoTrans), m_technical(std::make_shared(isTechnical)), m_anatomical(std::make_shared(isAnatomical)) { @@ -30,7 +30,7 @@ rigidbody::IMU::IMU( rigidbody::IMU::IMU( const rigidbody::IMU &imu) : - biorbd::utils::RotoTransNode (imu), + utils::RotoTransNode (imu), m_technical(std::make_shared(*imu.m_technical)), m_anatomical(std::make_shared(*imu.m_anatomical)) { @@ -41,9 +41,9 @@ rigidbody::IMU rigidbody::IMU::operator*( const rigidbody::IMU &other) const { return rigidbody::IMU( - biorbd::utils::RotoTransNode( - this->biorbd::utils::RotoTransNode::operator*(other), - this->biorbd::utils::Node::name(), + utils::RotoTransNode( + this->utils::RotoTransNode::operator*(other), + this->utils::Node::name(), this->parent()), this->isTechnical() && other.isTechnical(), this->isAnatomical() && other.isAnatomical()); @@ -60,7 +60,7 @@ rigidbody::IMU rigidbody::IMU::DeepCopy() const void rigidbody::IMU::DeepCopy(const IMU &other) { - biorbd::utils::RotoTransNode::DeepCopy(other); + utils::RotoTransNode::DeepCopy(other); *m_technical = *other.m_technical; *m_anatomical = *other.m_anatomical; } diff --git a/src/RigidBody/IMUs.cpp b/src/RigidBody/IMUs.cpp index d5988ccd..3c9e62b7 100644 --- a/src/RigidBody/IMUs.cpp +++ b/src/RigidBody/IMUs.cpp @@ -53,7 +53,7 @@ void rigidbody::IMUs::addIMU( // Add a new marker to the existing pool of markers void rigidbody::IMUs::addIMU( - const biorbd::utils::RotoTransNode &RotoTrans, + const utils::RotoTransNode &RotoTrans, bool technical, bool anatomical) { @@ -73,7 +73,7 @@ const std::vector& rigidbody::IMUs::IMU() const } std::vector rigidbody::IMUs::IMU( - const biorbd::utils::String& segmentName) + const utils::String& segmentName) { std::vector pos; for (unsigned int i=0; i rigidbody::IMUs::segmentIMU( (*this); // Segment name to find - biorbd::utils::String name(model.segment(idx).name()); + utils::String name(model.segment(idx).name()); std::vector pos; for (unsigned int i=0; i rigidbody::IMUs::segmentIMU( } // Get the Jacobian of the markers -std::vector rigidbody::IMUs::IMUJacobian( +std::vector rigidbody::IMUs::IMUJacobian( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -207,7 +207,7 @@ std::vector rigidbody::IMUs::IMUJacobian( } // Get the Jacobian of the technical markers -std::vector +std::vector rigidbody::IMUs::TechnicalIMUJacobian( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) @@ -217,7 +217,7 @@ rigidbody::IMUs::TechnicalIMUJacobian( // Protected function -std::vector rigidbody::IMUs::IMUJacobian( +std::vector rigidbody::IMUs::IMUJacobian( const rigidbody::GeneralizedCoordinates &Q, bool updateKin, bool lookForTechnical) @@ -226,7 +226,7 @@ std::vector rigidbody::IMUs::IMUJacobian( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast (*this); - std::vector G; + std::vector G; for (unsigned int idx=0; idx rigidbody::IMUs::IMUJacobian( } unsigned int id = model.GetBodyId(node.parent().c_str()); - biorbd::utils::Matrix G_tp(biorbd::utils::Matrix::Zero(9,model.dof_count)); + utils::Matrix G_tp(utils::Matrix::Zero(9,model.dof_count)); // Calculate the Jacobian of this Tag model.CalcMatRotJacobian(Q, id, node.rot(), G_tp, updateKin); // False for speed @@ -271,36 +271,36 @@ unsigned int rigidbody::IMUs::nbAnatIMUs() return nbAnat; } -std::vector rigidbody::IMUs::IMUsNames() +std::vector rigidbody::IMUs::IMUsNames() { // Extract the name of all the markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i rigidbody::IMUs::technicalIMUsNames() +std::vector rigidbody::IMUs::technicalIMUsNames() { // Extract the names of all the technical markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i +std::vector rigidbody::IMUs::anatomicalIMUsNames() { // Extract the names of all the anatomical markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i(0)), m_nRotAQuat(std::make_shared(0)), m_isKinematicsComputed(std::make_shared(false)), - m_totalMass(std::make_shared(0)) + m_totalMass(std::make_shared(0)) { // Redefining gravity so it is on z by default - this->gravity = biorbd::utils::Vector3d (0, 0, -9.81); + this->gravity = utils::Vector3d (0, 0, -9.81); } rigidbody::Joints::Joints(const rigidbody::Joints &other) : @@ -95,9 +95,9 @@ unsigned int rigidbody::Joints::nbDof() const return *m_nbDof; } -std::vector rigidbody::Joints::nameDof() const +std::vector rigidbody::Joints::nameDof() const { - std::vector names; + std::vector names; for (unsigned int i = 0; i < nbSegment(); ++i) { for (unsigned int j = 0; j < segment(i).nbDof(); ++j) { names.push_back(segment(i).name() + "_" + segment(i).nameDof(j)); @@ -129,21 +129,21 @@ unsigned int rigidbody::Joints::nbRoot() const return *m_nbRoot; } -biorbd::utils::Scalar rigidbody::Joints::mass() const +utils::Scalar rigidbody::Joints::mass() const { return *m_totalMass; } unsigned int rigidbody::Joints::AddSegment( - const biorbd::utils::String &segmentName, - const biorbd::utils::String &parentName, - const biorbd::utils::String &translationSequence, - const biorbd::utils::String &rotationSequence, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &segmentName, + const utils::String &parentName, + const utils::String &translationSequence, + const utils::String &rotationSequence, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const rigidbody::SegmentCharacteristics& characteristics, - const biorbd::utils::RotoTrans& referenceFrame, + const utils::RotoTrans& referenceFrame, int forcePlates) { rigidbody::Segment tp( @@ -172,14 +172,14 @@ unsigned int rigidbody::Joints::AddSegment( return 0; } unsigned int rigidbody::Joints::AddSegment( - const biorbd::utils::String &segmentName, - const biorbd::utils::String &parentName, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &segmentName, + const utils::String &parentName, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const rigidbody::SegmentCharacteristics& characteristics, - const biorbd::utils::RotoTrans& referenceFrame, + const utils::RotoTrans& referenceFrame, int forcePlates) { rigidbody::Segment tp( @@ -200,13 +200,13 @@ unsigned int rigidbody::Joints::AddSegment( return 0; } -biorbd::utils::Vector3d rigidbody::Joints::getGravity() const +utils::Vector3d rigidbody::Joints::getGravity() const { return gravity; } void rigidbody::Joints::setGravity( - const biorbd::utils::Vector3d &newGravity) + const utils::Vector3d &newGravity) { gravity = newGravity; } @@ -215,7 +215,7 @@ void rigidbody::Joints::updateSegmentCharacteristics( unsigned int idx, const rigidbody::SegmentCharacteristics& characteristics) { - biorbd::utils::Error::check(idx < m_segments->size(), + utils::Error::check(idx < m_segments->size(), "Asked for a wrong segment (out of range)"); (*m_segments)[idx].updateCharacteristics(*this, characteristics); } @@ -223,13 +223,13 @@ void rigidbody::Joints::updateSegmentCharacteristics( const rigidbody::Segment& rigidbody::Joints::segment( unsigned int idx) const { - biorbd::utils::Error::check(idx < m_segments->size(), + utils::Error::check(idx < m_segments->size(), "Asked for a wrong segment (out of range)"); return (*m_segments)[idx]; } const rigidbody::Segment &rigidbody::Joints::segment( - const biorbd::utils::String & name) const + const utils::String & name) const { return segment(static_cast(GetBodyBiorbdId(name.c_str()))); } @@ -241,11 +241,11 @@ unsigned int rigidbody::Joints::nbSegment() const std::vector rigidbody::Joints::dispatchedForce( - std::vector> &spatialVector, + std::vector> &spatialVector, unsigned int frame) const { // Iterator on the force table - std::vector + std::vector sv2; // Gather in the same table the values at the same instant of different platforms for (auto vec : spatialVector) { sv2.push_back(vec[frame]); @@ -257,14 +257,14 @@ rigidbody::Joints::dispatchedForce( std::vector rigidbody::Joints::dispatchedForce( - std::vector &sv) + std::vector &sv) const // a spatialVector per platform { // Output table std::vector sv_out; // Null Spatial vector nul to fill the final table - biorbd::utils::SpatialVector sv_zero(0.,0.,0.,0.,0.,0.); + utils::SpatialVector sv_zero(0.,0.,0.,0.,0.,0.); sv_out.push_back(sv_zero); // The first one is associated with the universe // Dispatch the forces @@ -290,7 +290,7 @@ const // a spatialVector per platform return sv_out; } -int rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String +int rigidbody::Joints::GetBodyBiorbdId(const utils::String &segmentName) const { for (int i=0; i(m_segments->size()); ++i) @@ -300,32 +300,32 @@ int rigidbody::Joints::GetBodyBiorbdId(const biorbd::utils::String return -1; } -std::vector rigidbody::Joints::allGlobalJCS( +std::vector rigidbody::Joints::allGlobalJCS( const rigidbody::GeneralizedCoordinates &Q) { UpdateKinematicsCustom (&Q, nullptr, nullptr); return allGlobalJCS(); } -std::vector rigidbody::Joints::allGlobalJCS() +std::vector rigidbody::Joints::allGlobalJCS() const { - std::vector out; + std::vector out; for (unsigned int i=0; isize(); ++i) { out.push_back(globalJCS(i)); } return out; } -biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( +utils::RotoTrans rigidbody::Joints::globalJCS( const rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::String &name) + const utils::String &name) { UpdateKinematicsCustom (&Q, nullptr, nullptr); return globalJCS(name); } -biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( +utils::RotoTrans rigidbody::Joints::globalJCS( const rigidbody::GeneralizedCoordinates &Q, unsigned int idx) { @@ -334,22 +334,22 @@ biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( return globalJCS(idx); } -biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( - const biorbd::utils::String &name) const +utils::RotoTrans rigidbody::Joints::globalJCS( + const utils::String &name) const { return globalJCS(static_cast(GetBodyBiorbdId(name))); } -biorbd::utils::RotoTrans rigidbody::Joints::globalJCS( +utils::RotoTrans rigidbody::Joints::globalJCS( unsigned int idx) const { return CalcBodyWorldTransformation((*m_segments)[idx].id()); } -std::vector rigidbody::Joints::localJCS() +std::vector rigidbody::Joints::localJCS() const { - std::vector out; + std::vector out; for (unsigned int i=0; isize(); ++i) { out.push_back(localJCS(i)); @@ -357,12 +357,12 @@ const return out; } -biorbd::utils::RotoTrans rigidbody::Joints::localJCS( - const biorbd::utils::String &name) const +utils::RotoTrans rigidbody::Joints::localJCS( + const utils::String &name) const { return localJCS(static_cast(GetBodyBiorbdId(name.c_str()))); } -biorbd::utils::RotoTrans rigidbody::Joints::localJCS( +utils::RotoTrans rigidbody::Joints::localJCS( const unsigned int idx) const { return (*m_segments)[idx].localJCS(); @@ -388,7 +388,7 @@ rigidbody::Joints::projectPoint( dynamic_cast(*this); // Sécurité - biorbd::utils::Error::check(marks.nbMarkers() == v.size(), + utils::Error::check(marks.nbMarkers() == v.size(), "Number of marker must be equal to number of Vector3d"); std::vector out; @@ -409,9 +409,9 @@ rigidbody::Joints::projectPoint( rigidbody::NodeSegment rigidbody::Joints::projectPoint( const rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::Vector3d &v, + const utils::Vector3d &v, int segmentIdx, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -422,7 +422,7 @@ rigidbody::NodeSegment rigidbody::Joints::projectPoint( } // Create a marker - const biorbd::utils::String& segmentName(segment(static_cast + const utils::String& segmentName(segment(static_cast (segmentIdx)).name()); rigidbody::NodeSegment node( v.applyRT(globalJCS( static_cast(segmentIdx)).transpose()), "tp", @@ -443,7 +443,7 @@ rigidbody::NodeSegment rigidbody::Joints::projectPoint( updateKin); } -biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( +utils::Matrix rigidbody::Joints::projectPointJacobian( const rigidbody::GeneralizedCoordinates &Q, rigidbody::NodeSegment node, bool updateKin) @@ -464,9 +464,9 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( if (node.nbAxesToRemove() != 0) { // Jacobian of the marker node.applyRT(globalJCS(node.parent()).transpose()); - biorbd::utils::Matrix G_tp(marks.markersJacobian(Q, node.parent(), - biorbd::utils::Vector3d(0,0,0), updateKin)); - biorbd::utils::Matrix JCor(biorbd::utils::Matrix::Zero(9,nbQ())); + utils::Matrix G_tp(marks.markersJacobian(Q, node.parent(), + utils::Vector3d(0,0,0), updateKin)); + utils::Matrix JCor(utils::Matrix::Zero(9,nbQ())); CalcMatRotJacobian(Q, GetBodyId(node.parent().c_str()), RigidBodyDynamics::Math::Matrix3d::Identity(), JCor, updateKin); for (unsigned int n=0; n<3; ++n) @@ -477,15 +477,15 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( return G_tp; } else { // Return the value - return biorbd::utils::Matrix::Zero(3,nbQ()); + return utils::Matrix::Zero(3,nbQ()); } } -biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( +utils::Matrix rigidbody::Joints::projectPointJacobian( const rigidbody::GeneralizedCoordinates &Q, const utils::Vector3d &v, int segmentIdx, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, bool updateKin) { // Find the point @@ -496,7 +496,7 @@ biorbd::utils::Matrix rigidbody::Joints::projectPointJacobian( return projectPointJacobian(Q, p, updateKin); } -std::vector +std::vector rigidbody::Joints::projectPointJacobian( const rigidbody::GeneralizedCoordinates &Q, const std::vector &v, @@ -507,7 +507,7 @@ rigidbody::Joints::projectPointJacobian( updateKin)); // Calculate the Jacobian if the point is not projected - std::vector G; + std::vector G; for (unsigned int i=0; i= this->fixed_body_discriminator) { unsigned int fbody_id = segmentIdx - this->fixed_body_discriminator; unsigned int parent_id = this->mFixedBodies[fbody_id].mMovableParent; - biorbd::utils::RotoTrans parentRT( + utils::RotoTrans parentRT( this->X_base[parent_id].E.transpose(), this->X_base[parent_id].r); - biorbd::utils::RotoTrans bodyRT( + utils::RotoTrans bodyRT( this->mFixedBodies[fbody_id].mParentTransform.E.transpose(), this->mFixedBodies[fbody_id].mParentTransform.r); - const biorbd::utils::RotoTrans& transfo_tp = parentRT * bodyRT; + const utils::RotoTrans& transfo_tp = parentRT * bodyRT; return RigidBodyDynamics::Math::SpatialTransform (transfo_tp.rot(), transfo_tp.trans()); } @@ -555,14 +555,14 @@ rigidbody::Joints::CalcBodyWorldTransformation( this->X_base[segmentIdx].E.transpose(), this->X_base[segmentIdx].r); } -biorbd::utils::Vector3d rigidbody::Joints::CoM( +utils::Vector3d rigidbody::Joints::CoM( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { // For each segment, find the CoM (CoM = sum(segment_mass * pos_com_seg) / total mass) const std::vector& com_segment(CoMbySegment(Q, updateKin)); - biorbd::utils::Vector3d com(0, 0, 0); + utils::Vector3d com(0, 0, 0); for (unsigned int i=0; idof_count)); + utils::Matrix Jac(utils::Matrix(3,this->dof_count)); for (auto segment : *m_segments) { Jac.setZero(); RigidBodyDynamics::CalcPointJacobian( @@ -622,7 +622,7 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMdot( // Return the velocity of CoM return com_dot; } -biorbd::utils::Vector3d rigidbody::Joints::CoMddot( +utils::Vector3d rigidbody::Joints::CoMddot( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, @@ -631,7 +631,7 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMddot( #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif - biorbd::utils::Scalar mass; + utils::Scalar mass; RigidBodyDynamics::Math::Vector3d com, com_ddot; RigidBodyDynamics::Utils::CalcCenterOfMass( *this, Q, Qdot, &Qddot, mass, com, nullptr, &com_ddot, @@ -642,7 +642,7 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMddot( return com_ddot; } -biorbd::utils::Matrix rigidbody::Joints::CoMJacobian( +utils::Matrix rigidbody::Joints::CoMJacobian( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { @@ -651,10 +651,10 @@ biorbd::utils::Matrix rigidbody::Joints::CoMJacobian( #endif // Total jacobian - biorbd::utils::Matrix JacTotal(biorbd::utils::Matrix::Zero(3,this->dof_count)); + utils::Matrix JacTotal(utils::Matrix::Zero(3,this->dof_count)); // CoMdot = sum(mass_seg * Jacobian * qdot)/mass total - biorbd::utils::Matrix Jac(biorbd::utils::Matrix::Zero(3,this->dof_count)); + utils::Matrix Jac(utils::Matrix::Zero(3,this->dof_count)); for (auto segment : *m_segments) { Jac.setZero(); RigidBodyDynamics::CalcPointJacobian( @@ -685,12 +685,12 @@ rigidbody::Joints::CoMbySegment( return out; } -biorbd::utils::Matrix rigidbody::Joints::CoMbySegmentInMatrix( +utils::Matrix rigidbody::Joints::CoMbySegmentInMatrix( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { std::vector allCoM(CoMbySegment(Q, updateKin)); - biorbd::utils::Matrix CoMs(3, allCoM.size()); + utils::Matrix CoMs(3, allCoM.size()); for (unsigned int i=0; isize(), + utils::Error::check(idx < m_segments->size(), "Choosen segment doesn't exist"); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -714,12 +714,12 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMbySegment( } -std::vector rigidbody::Joints::CoMdotBySegment( +std::vector rigidbody::Joints::CoMdotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin) { - std::vector out; + std::vector out; for (unsigned int i=0; isize(); ++i) { out.push_back(CoMdotBySegment(Q,Qdot,i,updateKin)); updateKin = false; @@ -728,14 +728,14 @@ std::vector rigidbody::Joints::CoMdotBySegment( } -biorbd::utils::Vector3d rigidbody::Joints::CoMdotBySegment( +utils::Vector3d rigidbody::Joints::CoMdotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const unsigned int idx, bool updateKin) { // Position of the center of mass of segment i - biorbd::utils::Error::check(idx < m_segments->size(), + utils::Error::check(idx < m_segments->size(), "Choosen segment doesn't exist"); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -746,14 +746,14 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMdotBySegment( } -std::vector +std::vector rigidbody::Joints::CoMddotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { - std::vector out; + std::vector out; for (unsigned int i=0; isize(); ++i) { out.push_back(CoMddotBySegment(Q,Qdot,Qddot,i,updateKin)); updateKin = false; @@ -762,14 +762,14 @@ rigidbody::Joints::CoMddotBySegment( } -biorbd::utils::Vector3d rigidbody::Joints::CoMddotBySegment( +utils::Vector3d rigidbody::Joints::CoMddotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin) { - biorbd::utils::Error::check(idx < m_segments->size(), + utils::Error::check(idx < m_segments->size(), "Choosen segment doesn't exist"); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -779,7 +779,7 @@ biorbd::utils::Vector3d rigidbody::Joints::CoMddotBySegment( (*m_segments)[idx].characteristics().mCenterOfMass,updateKin); } -std::vector> +std::vector> rigidbody::Joints::meshPoints( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) @@ -790,10 +790,10 @@ std::vector> if (updateKin) { UpdateKinematicsCustom (&Q); } - std::vector> v; + std::vector> v; // Find the position of the segments - const std::vector& RT(allGlobalJCS()); + const std::vector& RT(allGlobalJCS()); // For all the segments for (unsigned int i=0; i> return v; } -std::vector rigidbody::Joints::meshPoints( +std::vector rigidbody::Joints::meshPoints( const rigidbody::GeneralizedCoordinates &Q, unsigned int i, bool updateKin) @@ -815,12 +815,12 @@ std::vector rigidbody::Joints::meshPoints( } // Find the position of the segments - const std::vector& RT(allGlobalJCS()); + const std::vector& RT(allGlobalJCS()); return meshPoints(RT,i); } -std::vector +std::vector rigidbody::Joints::meshPointsInMatrix( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) @@ -831,13 +831,13 @@ rigidbody::Joints::meshPointsInMatrix( if (updateKin) { UpdateKinematicsCustom (&Q); } - const std::vector& RT(allGlobalJCS()); + const std::vector& RT(allGlobalJCS()); - std::vector all_points; + std::vector all_points; for (unsigned int i=0; isize(); ++i) { - biorbd::utils::Matrix mat(3, mesh(i).nbVertex()); + utils::Matrix mat(3, mesh(i).nbVertex()); for (unsigned int j=0; j rigidbody::Joints::meshPoints( - const std::vector &RT, +std::vector rigidbody::Joints::meshPoints( + const std::vector &RT, unsigned int i) const { // Gather the position of the meshings - std::vector v; + std::vector v; for (unsigned int j=0; j +std::vector rigidbody::Joints::CalcSegmentsAngularMomentum ( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, @@ -944,7 +944,7 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( updateKin = true; #endif - biorbd::utils::Scalar mass; + utils::Scalar mass; RigidBodyDynamics::Math::Vector3d com; RigidBodyDynamics::Utils::CalcCenterOfMass ( *this, Q, Qdot, nullptr, mass, com, nullptr, @@ -952,7 +952,7 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( RigidBodyDynamics::Math::SpatialTransform X_to_COM ( RigidBodyDynamics::Math::Xtrans(com)); - std::vector h_segment; + std::vector h_segment; for (unsigned int i = 1; i < this->mBodies.size(); i++) { this->Ic[i] = this->I[i]; this->hc[i] = this->Ic[i].toMatrix() * this->v[i]; @@ -967,14 +967,14 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( } while (this->lambda[j]!=0); } h = X_to_COM.applyAdjoint (h); - h_segment.push_back(biorbd::utils::Vector3d(h[0],h[1],h[2])); + h_segment.push_back(utils::Vector3d(h[0],h[1],h[2])); } return h_segment; } -std::vector +std::vector rigidbody::Joints::CalcSegmentsAngularMomentum ( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, @@ -985,7 +985,7 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( updateKin = true; #endif - biorbd::utils::Scalar mass; + utils::Scalar mass; RigidBodyDynamics::Math::Vector3d com; RigidBodyDynamics::Utils::CalcCenterOfMass (*this, Q, Qdot, &Qddot, mass, com, nullptr, nullptr, nullptr, nullptr, @@ -993,7 +993,7 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( RigidBodyDynamics::Math::SpatialTransform X_to_COM ( RigidBodyDynamics::Math::Xtrans(com)); - std::vector h_segment; + std::vector h_segment; for (unsigned int i = 1; i < this->mBodies.size(); i++) { this->Ic[i] = this->I[i]; this->hc[i] = this->Ic[i].toMatrix() * this->v[i]; @@ -1008,7 +1008,7 @@ rigidbody::Joints::CalcSegmentsAngularMomentum ( } while (this->lambda[j]!=0); } h = X_to_COM.applyAdjoint (h); - h_segment.push_back(biorbd::utils::Vector3d(h[0],h[1],h[2])); + h_segment.push_back(utils::Vector3d(h[0],h[1],h[2])); } @@ -1037,7 +1037,7 @@ rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( const rigidbody::Segment& segment_i = segment(i); if (segment_i.isRotationAQuaternion()) { // Extraire le quaternion - biorbd::utils::Quaternion quat_tp( + utils::Quaternion quat_tp( Q(Q.size()-*m_nRotAQuat+cmpQuat), Q.block(cmpDof+segment_i.nbDofTrans(), 0, 3, 1), k_stab); @@ -1068,7 +1068,7 @@ rigidbody::GeneralizedTorque rigidbody::Joints::InverseDynamics( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, - std::vector* f_ext) + std::vector* f_ext) { rigidbody::GeneralizedTorque Tau(nbGeneralizedTorque()); if (f_ext) { @@ -1084,7 +1084,7 @@ rigidbody::GeneralizedTorque rigidbody::Joints::InverseDynamics( rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, - std::vector* f_ext) + std::vector* f_ext) { rigidbody::GeneralizedTorque Tau(*this); if (f_ext) { @@ -1102,7 +1102,7 @@ rigidbody::Joints::ForwardDynamics( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, const rigidbody::GeneralizedTorque &Tau, - std::vector* f_ext) + std::vector* f_ext) { #ifdef BIORBD_USE_CASADI_MATH UpdateKinematicsCustom(&Q, &QDot); @@ -1125,7 +1125,7 @@ rigidbody::Joints::ForwardDynamicsConstraintsDirect( const rigidbody::GeneralizedVelocity &QDot, const rigidbody::GeneralizedTorque &Tau, rigidbody::Contacts &CS, - std::vector *f_ext) + std::vector *f_ext) { #ifdef BIORBD_USE_CASADI_MATH UpdateKinematicsCustom(&Q, &QDot); @@ -1144,12 +1144,12 @@ rigidbody::Joints::ForwardDynamicsConstraintsDirect( return QDDot; } -biorbd::utils::Vector +utils::Vector rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, const rigidbody::GeneralizedTorque &Tau, - std::vector *f_ext) + std::vector *f_ext) { rigidbody::Contacts CS = dynamic_cast (this)->getConstraints(); @@ -1162,7 +1162,7 @@ rigidbody::Joints::ForwardDynamicsConstraintsDirect( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, const rigidbody::GeneralizedTorque &Tau, - std::vector *f_ext) + std::vector *f_ext) { rigidbody::Contacts CS = dynamic_cast @@ -1191,15 +1191,15 @@ rigidbody::Joints::ComputeConstraintImpulsesDirect( } unsigned int rigidbody::Joints::getDofIndex( - const biorbd::utils::String& segmentName, - const biorbd::utils::String& dofName) + const utils::String& segmentName, + const utils::String& dofName) { unsigned int idx = 0; unsigned int iB = 0; bool found = false; while (1) { - biorbd::utils::Error::check(iB!=m_segments->size(), "Segment not found"); + utils::Error::check(iB!=m_segments->size(), "Segment not found"); if (segmentName.compare( (*m_segments)[iB].name() ) ) { idx += (*m_segments)[iB].nbDof(); @@ -1212,7 +1212,7 @@ unsigned int rigidbody::Joints::getDofIndex( ++iB; } - biorbd::utils::Error::check(found, "Dof not found"); + utils::Error::check(found, "Dof not found"); return idx; } @@ -1246,10 +1246,10 @@ void rigidbody::Joints::CalcMatRotJacobian( assert (G.rows() == 9 && G.cols() == this->qdot_size ); - std::vector axes; - axes.push_back(biorbd::utils::Vector3d(1,0,0)); - axes.push_back(biorbd::utils::Vector3d(0,1,0)); - axes.push_back(biorbd::utils::Vector3d(0,0,1)); + std::vector axes; + axes.push_back(utils::Vector3d(1,0,0)); + axes.push_back(utils::Vector3d(0,1,0)); + axes.push_back(utils::Vector3d(0,0,1)); for (unsigned int iAxes=0; iAxes<3; ++iAxes) { RigidBodyDynamics::Math::Matrix3d bodyMatRot ( RigidBodyDynamics::CalcBodyWorldOrientation (*this, Q, segmentIdx, @@ -1281,7 +1281,7 @@ void rigidbody::Joints::CalcMatRotJacobian( #endif { RigidBodyDynamics::Math::SpatialTransform X_base = this->X_base[j]; - X_base.r = biorbd::utils::Vector3d(0,0, + X_base.r = utils::Vector3d(0,0, 0); // Remove all concept of translation (only keep the rotation matrix) if (this->mJoints[j].mDoFCount == 3) { @@ -1306,23 +1306,23 @@ void rigidbody::Joints::checkGeneralizedDimensions( { #ifndef SKIP_ASSERT if (Q) { - biorbd::utils::Error::check( + utils::Error::check( Q->size() == nbQ(), "Wrong size for the Generalized Coordiates"); } if (Qdot) { - biorbd::utils::Error::check( + utils::Error::check( Qdot->size() == nbQdot(), "Wrong size for the Generalized Velocities"); } if (Qddot) { - biorbd::utils::Error::check( + utils::Error::check( Qddot->size() == nbQddot(), "Wrong size for the Generalized Accelerations"); } if (torque) { - biorbd::utils::Error::check( + utils::Error::check( torque->size() == nbGeneralizedTorque(), "Wrong size for the Generalized Torques"); } diff --git a/src/RigidBody/Markers.cpp b/src/RigidBody/Markers.cpp index d9527d56..438c6f2c 100644 --- a/src/RigidBody/Markers.cpp +++ b/src/RigidBody/Markers.cpp @@ -50,11 +50,11 @@ void rigidbody::Markers::DeepCopy( // Add a new marker to the markers pool void rigidbody::Markers::addMarker( const rigidbody::NodeSegment &pos, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, + const utils::String &name, + const utils::String &parentName, bool technical, bool anatomical, - const biorbd::utils::String& axesToRemove, + const utils::String& axesToRemove, int id) { rigidbody::NodeSegment tp(pos, name, parentName, technical, anatomical, @@ -69,7 +69,7 @@ const rigidbody::NodeSegment &rigidbody::Markers::marker( } std::vector rigidbody::Markers::marker( - const biorbd::utils::String& name) const + const utils::String& name) const { std::vector pos; for (unsigned int i=0; i pos; for (unsigned int i=0; i(*this); // Name of the segment to find - const biorbd::utils::String& name(model.segment(idxSegment).name()); + const utils::String& name(model.segment(idxSegment).name()); unsigned int n = 0; for (unsigned int i=0; i rigidbody::Markers::markersJacobian( +std::vector rigidbody::Markers::markersJacobian( const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin) @@ -375,7 +375,7 @@ std::vector rigidbody::Markers::markersJacobian( return markersJacobian(Q, removeAxis, updateKin, false); } -std::vector +std::vector rigidbody::Markers::technicalMarkersJacobian( const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, @@ -385,9 +385,9 @@ rigidbody::Markers::technicalMarkersJacobian( } // Get the Jacobian of the technical markers -biorbd::utils::Matrix rigidbody::Markers::markersJacobian( +utils::Matrix rigidbody::Markers::markersJacobian( const rigidbody::GeneralizedCoordinates &Q, - const biorbd::utils::String& parentName, + const utils::String& parentName, const rigidbody::NodeSegment& p, bool updateKin) { @@ -397,7 +397,7 @@ biorbd::utils::Matrix rigidbody::Markers::markersJacobian( #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif - biorbd::utils::Matrix G(biorbd::utils::Matrix::Zero(3, model.nbQ()));; + utils::Matrix G(utils::Matrix::Zero(3, model.nbQ()));; // Calculate the Jacobien of this Tag unsigned int id = model.GetBodyId(parentName.c_str()); @@ -445,7 +445,7 @@ bool rigidbody::Markers::inverseKinematics( #endif // Get the Jacobian of the technical markers -std::vector rigidbody::Markers::markersJacobian( +std::vector rigidbody::Markers::markersJacobian( const rigidbody::GeneralizedCoordinates &Q, bool removeAxis, bool updateKin, @@ -458,7 +458,7 @@ std::vector rigidbody::Markers::markersJacobian( updateKin = true; #endif - std::vector G; + std::vector G; for (unsigned int idx=0; idx rigidbody::Markers::markersJacobian( } unsigned int id = model.GetBodyId(node.parent().c_str()); - const biorbd::utils::Vector3d& pos(marker(idx, removeAxis)); - biorbd::utils::Matrix G_tp(biorbd::utils::Matrix::Zero(3,model.nbQ())); + const utils::Vector3d& pos(marker(idx, removeAxis)); + utils::Matrix G_tp(utils::Matrix::Zero(3,model.nbQ())); // Calculate the Jacobian of this Tag RigidBodyDynamics::CalcPointJacobian(model, Q, id, pos, G_tp, updateKin); @@ -505,7 +505,7 @@ unsigned int rigidbody::Markers::nbTechnicalMarkers( unsigned int nTechMarkers = 0; // Name of the segment to find - const biorbd::utils::String& name(model.segment(idxSegment).name()); + const utils::String& name(model.segment(idxSegment).name()); if (nTechMarkers == 0) // If the function has never been called before for (auto mark : *m_marks) @@ -529,39 +529,39 @@ unsigned int rigidbody::Markers::nbAnatomicalMarkers() return nAnatMarkers; } -std::vector rigidbody::Markers::markerNames() +std::vector rigidbody::Markers::markerNames() const { // Extract the name of all the markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i +std::vector rigidbody::Markers::technicalMarkerNames() const { // Extract the name of all the technical markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i +std::vector rigidbody::Markers::anatomicalMarkerNames() const { // Extract the names of all the anatomical markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i>()), + m_vertex(std::make_shared>()), m_faces(std::make_shared>()), - m_pathFile(std::make_shared()) + m_pathFile(std::make_shared()) { } -rigidbody::Mesh::Mesh(const std::vector &other) +rigidbody::Mesh::Mesh(const std::vector &other) : - m_vertex(std::make_shared>(other)), + m_vertex(std::make_shared>(other)), m_faces(std::make_shared>()), - m_pathFile(std::make_shared()) + m_pathFile(std::make_shared()) { } -rigidbody::Mesh::Mesh(const std::vector +rigidbody::Mesh::Mesh(const std::vector &vertex, const std::vector & faces) : - m_vertex(std::make_shared>(vertex)), + m_vertex(std::make_shared>(vertex)), m_faces(std::make_shared>(faces)), - m_pathFile(std::make_shared()) + m_pathFile(std::make_shared()) { } @@ -54,11 +54,11 @@ void rigidbody::Mesh::DeepCopy(const rigidbody::Mesh &other) *m_pathFile = other.m_pathFile->DeepCopy(); } -void rigidbody::Mesh::addPoint(const biorbd::utils::Vector3d &node) +void rigidbody::Mesh::addPoint(const utils::Vector3d &node) { m_vertex->push_back(node); } -const biorbd::utils::Vector3d &rigidbody::Mesh::point( +const utils::Vector3d &rigidbody::Mesh::point( unsigned int idx) const { return (*m_vertex)[idx]; @@ -91,12 +91,12 @@ const rigidbody::MeshFace &rigidbody::Mesh::face( return (*m_faces)[idx]; } -void rigidbody::Mesh::setPath(const biorbd::utils::Path& path) +void rigidbody::Mesh::setPath(const utils::Path& path) { *m_pathFile = path; } -const biorbd::utils::Path &rigidbody::Mesh::path() const +const utils::Path &rigidbody::Mesh::path() const { return *m_pathFile; } diff --git a/src/RigidBody/MeshFace.cpp b/src/RigidBody/MeshFace.cpp index cab907a1..097cbc3d 100644 --- a/src/RigidBody/MeshFace.cpp +++ b/src/RigidBody/MeshFace.cpp @@ -29,9 +29,9 @@ int &rigidbody::MeshFace::operator()(unsigned int idx) return (*m_face)[idx]; } -biorbd::utils::Vector3d rigidbody::MeshFace::faceAsDouble() +utils::Vector3d rigidbody::MeshFace::faceAsDouble() { - return biorbd::utils::Vector3d(static_cast((*m_face)[0]), + return utils::Vector3d(static_cast((*m_face)[0]), static_cast((*m_face)[1]), static_cast((*m_face)[2])); } diff --git a/src/RigidBody/NodeSegment.cpp b/src/RigidBody/NodeSegment.cpp index a0b03b82..f5fb10f7 100644 --- a/src/RigidBody/NodeSegment.cpp +++ b/src/RigidBody/NodeSegment.cpp @@ -6,7 +6,7 @@ using namespace biorbd::BIORBD_MATH_NAMESPACE; rigidbody::NodeSegment::NodeSegment() : - biorbd::utils::Vector3d(0, 0, 0), + utils::Vector3d(0, 0, 0), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), m_technical(std::make_shared(true)), @@ -17,10 +17,10 @@ rigidbody::NodeSegment::NodeSegment() : } rigidbody::NodeSegment::NodeSegment( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z) : - biorbd::utils::Vector3d(x, y, z), + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z) : + utils::Vector3d(x, y, z), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), m_technical(std::make_shared(true)), @@ -30,9 +30,9 @@ rigidbody::NodeSegment::NodeSegment( setType(); } -rigidbody::NodeSegment::NodeSegment(const biorbd::utils::Vector3d +rigidbody::NodeSegment::NodeSegment(const utils::Vector3d &other) : - biorbd::utils::Vector3d(other), + utils::Vector3d(other), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), m_technical(std::make_shared(true)), @@ -43,16 +43,16 @@ rigidbody::NodeSegment::NodeSegment(const biorbd::utils::Vector3d } rigidbody::NodeSegment::NodeSegment( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName, bool isTechnical, bool isAnatomical, - const biorbd::utils::String &axesToRemove, + const utils::String &axesToRemove, int parentID) : - biorbd::utils::Vector3d(x, y, z, name, parentName), + utils::Vector3d(x, y, z, name, parentName), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), m_technical(std::make_shared(isTechnical)), @@ -63,14 +63,14 @@ rigidbody::NodeSegment::NodeSegment( } rigidbody::NodeSegment::NodeSegment( - const biorbd::utils::Vector3d &node, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, + const utils::Vector3d &node, + const utils::String &name, + const utils::String &parentName, bool isTechnical, bool isAnatomical, - const biorbd::utils::String& axesToRemove, // Axis to remove + const utils::String& axesToRemove, // Axis to remove int parentID) : - biorbd::utils::Vector3d(node, name, parentName), + utils::Vector3d(node, name, parentName), m_axesRemoved(std::make_shared>(3)), m_nbAxesToRemove(std::make_shared(0)), m_technical(std::make_shared(isTechnical)), @@ -91,7 +91,7 @@ rigidbody::NodeSegment rigidbody::NodeSegment::DeepCopy() const void rigidbody::NodeSegment::DeepCopy(const rigidbody::NodeSegment&other) { - biorbd::utils::Node::DeepCopy(other); + utils::Node::DeepCopy(other); *m_axesRemoved = *other.m_axesRemoved; *m_nbAxesToRemove = *other.m_nbAxesToRemove; *m_technical = *other.m_technical; @@ -146,20 +146,20 @@ int rigidbody::NodeSegment::nbAxesToRemove() const void rigidbody::NodeSegment::setType() { - *m_typeOfNode = biorbd::utils::NODE_TYPE::BONE_POINT; + *m_typeOfNode = utils::NODE_TYPE::BONE_POINT; } void rigidbody::NodeSegment::addAxesToRemove(unsigned int axisNumber) { if (axisNumber>2) { - biorbd::utils::Error::raise("Axis must be 0 (\"x\"), 1 (\"y\") or 2 (\"z\")"); + utils::Error::raise("Axis must be 0 (\"x\"), 1 (\"y\") or 2 (\"z\")"); } (*m_axesRemoved)[axisNumber] = true; ++*m_nbAxesToRemove; } void rigidbody::NodeSegment::addAxesToRemove(const - biorbd::utils::String& s) + utils::String& s) { for (unsigned int i=0; i& axes) + std::vector& axes) { for (unsigned int i=0; i>()) + m_RTs(std::make_shared>()) { //ctor } @@ -49,14 +49,14 @@ void rigidbody::RotoTransNodes::DeepCopy(const void rigidbody::RotoTransNodes::addRT() { - m_RTs->push_back(biorbd::utils::RotoTransNode()); + m_RTs->push_back(utils::RotoTransNode()); } // Add a new marker to the existing pool of markers void rigidbody::RotoTransNodes::addRT( - const biorbd::utils::RotoTransNode &RotoTrans) + const utils::RotoTransNode &RotoTrans) { - m_RTs->push_back(biorbd::utils::RotoTransNode(RotoTrans)); + m_RTs->push_back(utils::RotoTransNode(RotoTrans)); } unsigned int rigidbody::RotoTransNodes::nbRTs() const @@ -70,17 +70,17 @@ unsigned int rigidbody::RotoTransNodes::size() const } // Get the markers in the global reference -const std::vector& +const std::vector& rigidbody::RotoTransNodes::RTs() const { return *m_RTs; } -std::vector +std::vector rigidbody::RotoTransNodes::RTs( - const biorbd::utils::String& segmentName) + const utils::String& segmentName) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i +std::vector rigidbody::RotoTransNodes::RTs( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) { - std::vector pos; + std::vector pos; for (unsigned int i=0; i(model.GetBodyBiorbdId( node.parent())); return model.globalJCS(id) * node; } -std::vector +std::vector rigidbody::RotoTransNodes::segmentRTs( const rigidbody::GeneralizedCoordinates &Q, unsigned int idx, @@ -144,9 +144,9 @@ rigidbody::RotoTransNodes::segmentRTs( (*this); // Segment name to find - biorbd::utils::String name(model.segment(idx).name()); + utils::String name(model.segment(idx).name()); - std::vector pos; + std::vector pos; for (unsigned int i=0; i +std::vector rigidbody::RotoTransNodes::RTsJacobian( const rigidbody::GeneralizedCoordinates &Q, bool updateKin) @@ -169,14 +169,14 @@ rigidbody::RotoTransNodes::RTsJacobian( #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif - std::vector G; + std::vector G; for (unsigned int idx=0; idx rigidbody::RotoTransNodes::RTsNames() +std::vector rigidbody::RotoTransNodes::RTsNames() { // Extract the name of all the markers of a model - std::vector names; + std::vector names; for (unsigned int i=0; i(-1)), m_idxPF (std::make_shared(-1)), m_cor(std::make_shared()), - m_seqT(std::make_shared()), - m_seqR(std::make_shared()), - m_QRanges(std::make_shared>()), - m_QDotRanges(std::make_shared>()), - m_QDDotRanges(std::make_shared>()), + m_seqT(std::make_shared()), + m_seqR(std::make_shared()), + m_QRanges(std::make_shared>()), + m_QDotRanges(std::make_shared>()), + m_QDDotRanges(std::make_shared>()), m_nbDof(std::make_shared(0)), m_nbQdot(std::make_shared(0)), m_nbQddot(std::make_shared(0)), @@ -36,7 +36,7 @@ rigidbody::Segment::Segment() : m_idxDof(std::make_shared>()), m_sequenceTrans(std::make_shared>()), m_sequenceRot(std::make_shared>()), - m_nameDof(std::make_shared>()), + m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), m_characteristics( std::make_shared()), @@ -48,26 +48,26 @@ rigidbody::Segment::Segment() : rigidbody::Segment::Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName, - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const utils::String &name, + const utils::String &parentName, + const utils::String &seqT, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const rigidbody::SegmentCharacteristics& characteristics, const RigidBodyDynamics::Math::SpatialTransform& cor, int PF) : - biorbd::utils::Node(name, parentName), + utils::Node(name, parentName), m_idxInModel(std::make_shared(-1)), m_idxPF (std::make_shared(PF)), m_cor(std::make_shared(cor)), - m_seqT(std::make_shared(seqT)), - m_seqR(std::make_shared(seqR)), - m_QRanges(std::make_shared>()), - m_QDotRanges(std::make_shared>()), - m_QDDotRanges(std::make_shared>()), + m_seqT(std::make_shared(seqT)), + m_seqR(std::make_shared(seqR)), + m_QRanges(std::make_shared>()), + m_QDotRanges(std::make_shared>()), + m_QDDotRanges(std::make_shared>()), m_nbDof(std::make_shared(0)), m_nbQdot(std::make_shared(0)), m_nbQddot(std::make_shared(0)), @@ -81,7 +81,7 @@ rigidbody::Segment::Segment( m_idxDof(std::make_shared>()), m_sequenceTrans(std::make_shared>()), m_sequenceRot(std::make_shared>()), - m_nameDof(std::make_shared>()), + m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), m_characteristics(std::make_shared (characteristics)), @@ -96,13 +96,13 @@ rigidbody::Segment::Segment( } rigidbody::Segment::Segment( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &name, // Name of segment - const biorbd::utils::String &parentName, // Name of segment - const biorbd::utils::String + const utils::String &name, // Name of segment + const utils::String &parentName, // Name of segment + const utils::String &seqR, // Cardan sequence to classify the rotation DoF - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges, const rigidbody::SegmentCharacteristics& characteristics, // Mass, Center of mass of segment, Inertia of segment, etc. const RigidBodyDynamics::Math::SpatialTransform& @@ -110,15 +110,15 @@ rigidbody::Segment::Segment( int PF): // Force platform number - biorbd::utils::Node(name, parentName), + utils::Node(name, parentName), m_idxInModel(std::make_shared(-1)), m_idxPF (std::make_shared(PF)), m_cor(std::make_shared(cor)), - m_seqT(std::make_shared()), - m_seqR(std::make_shared(seqR)), - m_QRanges(std::make_shared>()), - m_QDotRanges(std::make_shared>()), - m_QDDotRanges(std::make_shared>()), + m_seqT(std::make_shared()), + m_seqR(std::make_shared(seqR)), + m_QRanges(std::make_shared>()), + m_QDotRanges(std::make_shared>()), + m_QDDotRanges(std::make_shared>()), m_nbDof(std::make_shared(0)), m_nbQdot(std::make_shared(0)), m_nbQddot(std::make_shared(0)), @@ -132,7 +132,7 @@ rigidbody::Segment::Segment( m_idxDof(std::make_shared>()), m_sequenceTrans(std::make_shared>()), m_sequenceRot(std::make_shared>()), - m_nameDof(std::make_shared>()), + m_nameDof(std::make_shared>()), m_dofPosition(std::make_shared>()), m_characteristics(std::make_shared (characteristics)), @@ -156,7 +156,7 @@ rigidbody::Segment rigidbody::Segment::DeepCopy() const void rigidbody::Segment::DeepCopy(const rigidbody::Segment&other) { - biorbd::utils::Node::DeepCopy(other); + utils::Node::DeepCopy(other); *m_idxInModel = *other.m_idxInModel; *m_idxPF = *other.m_idxPF; *m_cor = *other.m_cor; @@ -205,7 +205,7 @@ bool rigidbody::Segment::isRotationAQuaternion() const void rigidbody::Segment::setType() { - *m_typeOfNode = biorbd::utils::NODE_TYPE::SEGMENT; + *m_typeOfNode = utils::NODE_TYPE::SEGMENT; } unsigned int rigidbody::Segment::id() const @@ -256,44 +256,44 @@ void rigidbody::Segment::setPF(int idx) *m_idxPF = idx; } -const biorbd::utils::String &rigidbody::Segment::nameDof( +const utils::String &rigidbody::Segment::nameDof( const unsigned int i) const { // Return the number of DoF of the segment - biorbd::utils::Error::check(i<*m_nbDofTrue, + utils::Error::check(i<*m_nbDofTrue, "Dof ouside N dof for this segment"); return (*m_nameDof)[i]; } -const biorbd::utils::String& rigidbody::Segment::seqT() const +const utils::String& rigidbody::Segment::seqT() const { return *m_seqT; } -const biorbd::utils::String& rigidbody::Segment::seqR() const +const utils::String& rigidbody::Segment::seqR() const { return *m_seqR; } -const std::vector& +const std::vector& rigidbody::Segment::QRanges() const { return *m_QRanges; } -const std::vector& +const std::vector& rigidbody::Segment::QDotRanges() const { return *m_QDotRanges; } -const std::vector& +const std::vector& rigidbody::Segment::QDDotRanges() const { return *m_QDDotRanges; } -biorbd::utils::RotoTrans rigidbody::Segment::localJCS() const +utils::RotoTrans rigidbody::Segment::localJCS() const { return RigidBodyDynamics::Math::SpatialTransform(m_cor->E.transpose(), m_cor->r); @@ -323,11 +323,11 @@ rigidbody::Segment::characteristics() const void rigidbody::Segment::setDofs( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR, - const std::vector& QRanges, - const std::vector& QDotRanges, - const std::vector& QDDotRanges) + const utils::String &seqT, + const utils::String &seqR, + const std::vector& QRanges, + const std::vector& QDotRanges, + const std::vector& QDDotRanges) { determineIfRotIsQuaternion(seqR); setSequence(seqT, seqR); @@ -337,21 +337,21 @@ void rigidbody::Segment::setDofs( if (*m_isQuaternion) { nRot = 0; } - biorbd::utils::Error::check( + utils::Error::check( QRanges.size() == 0 || QRanges.size() == seqT.length() + nRot || (QRanges.size() == 4 && m_isQuaternion), "QRanges and number of dof must be equal"); *m_QRanges = QRanges; - biorbd::utils::Error::check( + utils::Error::check( QDotRanges.size() == 0 || QDotRanges.size() == seqT.length() + nRot || (QDotRanges.size() == 3 && m_isQuaternion), "QDotRanges and number of dof must be equal"); *m_QDotRanges = QDotRanges; - biorbd::utils::Error::check( + utils::Error::check( QDDotRanges.size() == 0 || QDDotRanges.size() == seqT.length() + nRot || (QDDotRanges.size() == 3 && m_isQuaternion), @@ -361,7 +361,7 @@ void rigidbody::Segment::setDofs( } void rigidbody::Segment::determineIfRotIsQuaternion( - const biorbd::utils::String &seqR) + const utils::String &seqR) { if (!seqR.tolower().compare("q")) { *m_isQuaternion = true; @@ -371,8 +371,8 @@ void rigidbody::Segment::determineIfRotIsQuaternion( // Member functions void rigidbody::Segment::str2numSequence( - const biorbd::utils::String &seqT, - const biorbd::utils::String &seqR) + const utils::String &seqT, + const utils::String &seqR) { m_sequenceTrans->clear(); m_sequenceTrans->resize(*m_nbDofTrans); @@ -395,16 +395,16 @@ void rigidbody::Segment::str2numSequence( for (unsigned int i=0; i<*m_nbDofRot; ++i) { (*m_nameDof)[*m_nbDofTrans+i] = "Rot" + seqR(i).toupper(); } - biorbd::utils::String xyz("XYZW"); + utils::String xyz("XYZW"); for (unsigned int i=0; i<*m_nbDofQuat; ++i) { - (*m_nameDof)[*m_nbDofTrans + *m_nbDofRot + i] = biorbd::utils::String("Quat") + + (*m_nameDof)[*m_nbDofTrans + *m_nbDofRot + i] = utils::String("Quat") + xyz.substr(i, 1); } } void rigidbody::Segment::str2numSequence( std::vector& sequenceInteger, - const biorbd::utils::String &sequenceText) + const utils::String &sequenceText) { for (unsigned int i=0; i(seqT.length()), @@ -491,10 +491,10 @@ void rigidbody::Segment::setDofCharacteristicsOnLastBody() void rigidbody::Segment::setJointAxis() { // Definition of the rotation axis - biorbd::utils::Vector3d axis[3]; - axis[0] = biorbd::utils::Vector3d(1,0,0); // axe x - axis[1] = biorbd::utils::Vector3d(0,1,0); // axe y - axis[2] = biorbd::utils::Vector3d(0,0,1); // axe z + utils::Vector3d axis[3]; + axis[0] = utils::Vector3d(1,0,0); // axe x + axis[1] = utils::Vector3d(0,1,0); // axe y + axis[2] = utils::Vector3d(0,0,1); // axe z // Declaration of DoFs in translation m_dof->clear(); @@ -571,7 +571,7 @@ void rigidbody::Segment::setJoints( } unsigned int rigidbody::Segment::getDofIdx( - const biorbd::utils::String &dofName) const + const utils::String &dofName) const { unsigned int idx(INT_MAX); @@ -585,7 +585,7 @@ unsigned int rigidbody::Segment::getDofIdx( } - biorbd::utils::Error::check(found, + utils::Error::check(found, "Type should be \"Rot\" or \"Trans\" and axis " "should be \"X\", \"Y\" or \"Z\", e.g. " "\"RotY\" for Rotation around y or \"TransX\" " diff --git a/src/RigidBody/SegmentCharacteristics.cpp b/src/RigidBody/SegmentCharacteristics.cpp index 90c05888..a608d533 100644 --- a/src/RigidBody/SegmentCharacteristics.cpp +++ b/src/RigidBody/SegmentCharacteristics.cpp @@ -10,28 +10,28 @@ using namespace biorbd::BIORBD_MATH_NAMESPACE; rigidbody::SegmentCharacteristics::SegmentCharacteristics() : Body(), - m_length(std::make_shared(0)), + m_length(std::make_shared(0)), m_mesh(std::make_shared()) { } rigidbody::SegmentCharacteristics::SegmentCharacteristics( - const biorbd::utils::Scalar &mass, - const biorbd::utils::Vector3d &com, + const utils::Scalar &mass, + const utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia) : Body(mass, com, inertia), - m_length(std::make_shared(0)), + m_length(std::make_shared(0)), m_mesh(std::make_shared()) { } rigidbody::SegmentCharacteristics::SegmentCharacteristics( - const biorbd::utils::Scalar &mass, - const biorbd::utils::Vector3d &com, + const utils::Scalar &mass, + const utils::Vector3d &com, const RigidBodyDynamics::Math::Matrix3d &inertia, const rigidbody::Mesh &mesh) : Body(mass, com, inertia), - m_length(std::make_shared(0)), + m_length(std::make_shared(0)), m_mesh(std::make_shared(mesh)) { @@ -54,34 +54,34 @@ void rigidbody::SegmentCharacteristics::DeepCopy( } void rigidbody::SegmentCharacteristics::setLength( - const biorbd::utils::Scalar& val) + const utils::Scalar& val) { *m_length = val; } -biorbd::utils::Scalar rigidbody::SegmentCharacteristics::length() const +utils::Scalar rigidbody::SegmentCharacteristics::length() const { return *m_length; } void rigidbody::SegmentCharacteristics::setMass( - const biorbd::utils::Scalar &newMass) + const utils::Scalar &newMass) { mMass = newMass; } -biorbd::utils::Scalar rigidbody::SegmentCharacteristics::mass() const +utils::Scalar rigidbody::SegmentCharacteristics::mass() const { return mMass; } -biorbd::utils::Vector3d rigidbody::SegmentCharacteristics::CoM() const +utils::Vector3d rigidbody::SegmentCharacteristics::CoM() const { return mCenterOfMass; } void rigidbody::SegmentCharacteristics::setCoM( - const biorbd::utils::Vector3d &com) + const utils::Vector3d &com) { mCenterOfMass = com; } diff --git a/src/Utils/Benchmark.cpp b/src/Utils/Benchmark.cpp index c48a17e6..62b2057f 100644 --- a/src/Utils/Benchmark.cpp +++ b/src/Utils/Benchmark.cpp @@ -4,16 +4,18 @@ #include "Utils/Timer.h" #include "Utils/String.h" -biorbd::utils::Benchmark::Benchmark() : - m_timers(std::map()), - m_counts(std::map()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Benchmark::Benchmark() : + m_timers(std::map()), + m_counts(std::map()) { } -void biorbd::utils::Benchmark::startTimer( - const biorbd::utils::String& name, +void utils::Benchmark::startTimer( + const utils::String& name, bool force) { if (force) { @@ -23,43 +25,43 @@ void biorbd::utils::Benchmark::startTimer( } } -double biorbd::utils::Benchmark::getLap( - const biorbd::utils::String& name) +double utils::Benchmark::getLap( + const utils::String& name) { return m_timers[name].getLap(); } -double biorbd::utils::Benchmark::stopTimer( - const biorbd::utils::String& name) +double utils::Benchmark::stopTimer( + const utils::String& name) { return m_timers[name].stop(); } -void biorbd::utils::Benchmark::pauseTimer( - const biorbd::utils::String& name) +void utils::Benchmark::pauseTimer( + const utils::String& name) { m_timers[name].pause(); } -void biorbd::utils::Benchmark::resumeTimer( - const biorbd::utils::String& name) +void utils::Benchmark::resumeTimer( + const utils::String& name) { m_timers[name].resume(); } -void biorbd::utils::Benchmark::addTimer( - const biorbd::utils::String& name) +void utils::Benchmark::addTimer( + const utils::String& name) { ++(m_counts[name]); } -int biorbd::utils::Benchmark::getTimerIdx( - const biorbd::utils::String& name) +int utils::Benchmark::getTimerIdx( + const utils::String& name) { return m_counts[name]; } -void biorbd::utils::Benchmark::wasteTime( +void utils::Benchmark::wasteTime( double seconds) { // Wait for seconds ask doing dummy stuff diff --git a/src/Utils/Equation.cpp b/src/Utils/Equation.cpp index 8e0d440c..7898ff80 100644 --- a/src/Utils/Equation.cpp +++ b/src/Utils/Equation.cpp @@ -8,11 +8,13 @@ #include #include "Utils/Error.h" -std::vector -biorbd::utils::Equation::prepareMathSymbols() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +std::vector +utils::Equation::prepareMathSymbols() { // Classify in the order of priority of the operations - std::vector symbols; + std::vector symbols; symbols.push_back("("); symbols.push_back(")"); symbols.push_back("e"); @@ -24,42 +26,42 @@ biorbd::utils::Equation::prepareMathSymbols() return symbols; } -biorbd::utils::Equation::Equation() : - biorbd::utils::String("") +utils::Equation::Equation() : + utils::String("") { } -biorbd::utils::Equation::Equation(const char *c) : - biorbd::utils::String(c) +utils::Equation::Equation(const char *c) : + utils::String(c) { } -biorbd::utils::Equation::Equation(const biorbd::utils::String &s) : - biorbd::utils::String(s) +utils::Equation::Equation(const utils::String &s) : + utils::String(s) { } -biorbd::utils::Equation::Equation(const std::basic_string &c) : - biorbd::utils::String(c) +utils::Equation::Equation(const std::basic_string &c) : + utils::String(c) { } -std::vector biorbd::utils::Equation::splitIntoEquation( - biorbd::utils::Equation wholeEq, - const std::map& variables) +std::vector utils::Equation::splitIntoEquation( + utils::Equation wholeEq, + const std::map& variables) { // output variable - std::vector eq; + std::vector eq; // Replace the variables by a number replaceVar(wholeEq, variables); // Declaration of the arithmetic markers - const std::vector& symbols(prepareMathSymbols()); + const std::vector& symbols(prepareMathSymbols()); // All along the string while (1) { @@ -91,7 +93,7 @@ std::vector biorbd::utils::Equation::splitIntoEquation( } else if (firstIdx == 0) { if (!wholeEq(0).compare("-")) { // If the equation starts with "-" - std::vector tp(splitIntoEquation(wholeEq.substr(1), + std::vector tp(splitIntoEquation(wholeEq.substr(1), variables)); if (eq.size() != 0 && !eq[eq.size()-1].compare("e") ) { // special case of scientific notation (1e-x) @@ -100,7 +102,7 @@ std::vector biorbd::utils::Equation::splitIntoEquation( } else if (!tp[0].compare("(")) { // Resolve now the parenthese to apply the minus after size_t idx = wholeEq.find_first_of(")"); - biorbd::utils::Equation newWholeEq(wholeEq.substr(2, idx-2)); + utils::Equation newWholeEq(wholeEq.substr(2, idx-2)); double res(-1*evaluateEquation(splitIntoEquation(newWholeEq, variables))); wholeEq = to_string(res) + wholeEq.substr(idx+1); } else { @@ -110,7 +112,7 @@ std::vector biorbd::utils::Equation::splitIntoEquation( } } else if (!wholeEq(0).compare("+")) { // If the equation starts with + - std::vector tp(splitIntoEquation(wholeEq.substr(1), + std::vector tp(splitIntoEquation(wholeEq.substr(1), variables)); if (eq.size() != 0) { // If there's nothing before, it's false in all cases 1ex eq.push_back("+"); @@ -138,8 +140,8 @@ std::vector biorbd::utils::Equation::splitIntoEquation( return eq; } -void biorbd::utils::Equation::replaceCste( - std::vector &eq) +void utils::Equation::replaceCste( + std::vector &eq) { for (unsigned int i=0; i& variables) +void utils::Equation::replaceVar( + utils::Equation &eq, + const std::map& variables) { for (auto var : variables) while (eq.find(var.first) != std::string::npos) { @@ -162,14 +164,14 @@ void biorbd::utils::Equation::replaceVar( } -double biorbd::utils::Equation::evaluateEquation( - std::vector wholeEq) +double utils::Equation::evaluateEquation( + std::vector wholeEq) { return evaluateEquation(wholeEq,0); } -double biorbd::utils::Equation::evaluateEquation( - std::vector eq, +double utils::Equation::evaluateEquation( + std::vector eq, unsigned int math) { // If everything was done @@ -178,8 +180,8 @@ double biorbd::utils::Equation::evaluateEquation( } // Declaration of the arithmetic markers - const std::vector& symbols(prepareMathSymbols()); - std::vector eq2; + const std::vector& symbols(prepareMathSymbols()); + std::vector eq2; bool continuer(true); for (unsigned int j=0; j eq_tp; + std::vector eq_tp; bool foundIdx(false); int cmpValues(0); unsigned int cmpOpen(0); @@ -213,7 +215,7 @@ double biorbd::utils::Equation::evaluateEquation( eq_tp.push_back(eq[k]); ++cmpValues; } - biorbd::utils::Error::check(foundIdx, "You must close brackets!"); + utils::Error::check(foundIdx, "You must close brackets!"); eq2.push_back(to_string(evaluateEquation(eq_tp))); j+=static_cast(cmpValues); @@ -254,15 +256,15 @@ double biorbd::utils::Equation::evaluateEquation( } } -double biorbd::utils::Equation::evaluateEquation( - biorbd::utils::Equation wholeEq, - const std::map& variables) +double utils::Equation::evaluateEquation( + utils::Equation wholeEq, + const std::map& variables) { return evaluateEquation(splitIntoEquation(wholeEq, variables)); } -double biorbd::utils::Equation::evaluateEquation( - biorbd::utils::Equation wholeEq) +double utils::Equation::evaluateEquation( + utils::Equation wholeEq) { - std::map dumb; + std::map dumb; return evaluateEquation(splitIntoEquation(wholeEq, dumb)); } diff --git a/src/Utils/Error.cpp b/src/Utils/Error.cpp index 46474d63..cc03a108 100644 --- a/src/Utils/Error.cpp +++ b/src/Utils/Error.cpp @@ -1,15 +1,17 @@ #define BIORBD_API_EXPORTS #include "Utils/Error.h" -void biorbd::utils::Error::raise( - const biorbd::utils::String &message) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +void utils::Error::raise( + const utils::String &message) { throw std::runtime_error(message); } -void biorbd::utils::Error::check( +void utils::Error::check( bool cond, - const biorbd::utils::String& message) + const utils::String& message) { if (!cond) { throw std::runtime_error(message); @@ -17,9 +19,9 @@ void biorbd::utils::Error::check( } -void biorbd::utils::Error::warning( +void utils::Error::warning( bool cond, - const biorbd::utils::String& message) + const utils::String& message) { if (!cond) { std::cout << "Warning: " << message << std::endl; diff --git a/src/Utils/IfStream.cpp b/src/Utils/IfStream.cpp index 5c673de7..c2823b24 100644 --- a/src/Utils/IfStream.cpp +++ b/src/Utils/IfStream.cpp @@ -6,30 +6,32 @@ #include "Utils/Error.h" #include "Utils/Equation.h" +using namespace biorbd::BIORBD_MATH_NAMESPACE; + // Constructor -biorbd::utils::IfStream::IfStream() : +utils::IfStream::IfStream() : m_isOpen(std::make_shared(false)), m_ifs(std::make_shared()), - m_path(std::make_shared()) + m_path(std::make_shared()) { setlocale(LC_ALL, "C"); } -biorbd::utils::IfStream::IfStream( - const biorbd::utils::Path& path, +utils::IfStream::IfStream( + const utils::Path& path, std::ios_base::openmode mode = std::ios_base::in ) : m_isOpen(std::make_shared(false)), m_ifs(std::make_shared()), - m_path(std::make_shared(path)) + m_path(std::make_shared(path)) { open(m_path->absolutePath().c_str(), mode); setlocale(LC_ALL, "C"); } -biorbd::utils::IfStream::IfStream( +utils::IfStream::IfStream( const char* path, std::ios_base::openmode mode = std::ios_base::in ) : m_isOpen(std::make_shared(false)), m_ifs(std::make_shared()), - m_path(std::make_shared(path)) + m_path(std::make_shared(path)) { open(m_path->absolutePath().c_str(), mode); setlocale(LC_ALL, "C"); @@ -37,11 +39,11 @@ biorbd::utils::IfStream::IfStream( // Open a file -bool biorbd::utils::IfStream::open( - const biorbd::utils::Path& path, +bool utils::IfStream::open( + const utils::Path& path, std::ios_base::openmode mode = std::ios_base::in ) { - biorbd::utils::Error::check(path.isFileExist(), + utils::Error::check(path.isFileExist(), path.absolutePath() + " could not be loaded"); *m_ifs = std::ifstream(path.absolutePath().c_str(), mode); *m_isOpen = true; @@ -49,37 +51,37 @@ bool biorbd::utils::IfStream::open( } // Read a file -bool biorbd::utils::IfStream::readSpecificTag( - const biorbd::utils::String& tag, - biorbd::utils::String& text) +bool utils::IfStream::readSpecificTag( + const utils::String& tag, + utils::String& text) { reachSpecificTag(tag); read(text); return true; } -bool biorbd::utils::IfStream::reachSpecificTag(const biorbd::utils::String& tag) +bool utils::IfStream::reachSpecificTag(const utils::String& tag) { - biorbd::utils::String text; + utils::String text; while (read(text)) if (!text.tolower().compare(tag)) { return true; } - biorbd::utils::String outMessage(tag + + utils::String outMessage(tag + " parameter could not be found in Data file.."); - biorbd::utils::Error::raise(outMessage); + utils::Error::raise(outMessage); #ifdef _WIN32 // It is impossible to get here, but it's better to have a return for the compiler return false; #endif } -int biorbd::utils::IfStream::countTagsInAConsecutiveLines( - const biorbd::utils::String &tag) +int utils::IfStream::countTagsInAConsecutiveLines( + const utils::String &tag) { // Remember where we were in the file std::streamoff positionInFile(m_ifs->tellg()); - biorbd::utils::String text; + utils::String text; int nMarkers(0); // Read the first word of the line @@ -97,7 +99,7 @@ int biorbd::utils::IfStream::countTagsInAConsecutiveLines( return nMarkers; } -bool biorbd::utils::IfStream::read(biorbd::utils::String& text) +bool utils::IfStream::read(utils::String& text) { bool out(*m_ifs >> text); @@ -120,96 +122,96 @@ bool biorbd::utils::IfStream::read(biorbd::utils::String& text) } return out; } -bool biorbd::utils::IfStream::readAWord(biorbd::utils::String& text) +bool utils::IfStream::readAWord(utils::String& text) { bool out(*m_ifs >> text); return out; } -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( double& val) { - std::map dumb; + std::map dumb; return read(val, dumb); } #ifdef BIORBD_USE_CASADI_MATH -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( RBDLCasadiMath::MX_Xd_SubMatrix val) { - std::map dumb; + std::map dumb; return read(val, dumb); } #endif -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( double& result, - const std::map &variables) + const std::map &variables) { - biorbd::utils::Equation tp; + utils::Equation tp; bool out(read(tp)); // Manage in case of an equation try { - result = biorbd::utils::Equation::evaluateEquation(tp, variables); + result = utils::Equation::evaluateEquation(tp, variables); } catch (std::runtime_error) { - biorbd::utils::Error::raise("The following expression cannot be parsed properly: \"" + utils::Error::raise("The following expression cannot be parsed properly: \"" + tp + "\""); } return out; } #ifdef BIORBD_USE_CASADI_MATH -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( RBDLCasadiMath::MX_Xd_SubMatrix result, - const std::map &variables) + const std::map &variables) { - biorbd::utils::Equation tp; + utils::Equation tp; bool out(read(tp)); // Manage in case of an equation try { - result = biorbd::utils::Equation::evaluateEquation(tp, variables); + result = utils::Equation::evaluateEquation(tp, variables); } catch (std::runtime_error) { - biorbd::utils::Error::raise("The following expression cannot be parsed properly: \"" + utils::Error::raise("The following expression cannot be parsed properly: \"" + tp + "\""); } return out; } #endif -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( int& val) { - biorbd::utils::String tp; + utils::String tp; bool out(read(tp)); val = std::stoi(tp); return out; } -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( unsigned int& val) { - biorbd::utils::String tp; + utils::String tp; bool out(read(tp)); val = static_cast(std::stoul(tp)); return out; } -bool biorbd::utils::IfStream::read( +bool utils::IfStream::read( bool& val) { - biorbd::utils::String tp; + utils::String tp; bool out(read(tp)); val = std::stoi(tp) != 0; return out; } // Read the entire line -void biorbd::utils::IfStream::getline(biorbd::utils::String& text) +void utils::IfStream::getline(utils::String& text) { std::getline(*m_ifs, text); } // Close the file -bool biorbd::utils::IfStream::close() +bool utils::IfStream::close() { m_ifs->close(); return 1; } -bool biorbd::utils::IfStream::eof() +bool utils::IfStream::eof() { return m_ifs->eof(); } diff --git a/src/Utils/Matrix.cpp b/src/Utils/Matrix.cpp index c898be2a..b9beae0e 100644 --- a/src/Utils/Matrix.cpp +++ b/src/Utils/Matrix.cpp @@ -3,7 +3,9 @@ #include "Utils/Vector.h" -biorbd::utils::Matrix::Matrix() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Matrix::Matrix() : RigidBodyDynamics::Math::MatrixNd() { @@ -12,34 +14,34 @@ biorbd::utils::Matrix::Matrix() : #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Matrix::Matrix( - const biorbd::utils::Matrix& v) : +utils::Matrix::Matrix( + const utils::Matrix& v) : RigidBodyDynamics::Math::MatrixNd (v) { } -biorbd::utils::Matrix::Matrix( +utils::Matrix::Matrix( const RigidBodyDynamics::Math::MatrixNd &v) : RigidBodyDynamics::Math::MatrixNd (v) { } -biorbd::utils::Matrix::Matrix( +utils::Matrix::Matrix( const RBDLCasadiMath::MX_Xd_SubMatrix &m) : RigidBodyDynamics::Math::VectorNd (m) { } -void biorbd::utils::Matrix::operator=( - const biorbd::utils::Matrix &other) +void utils::Matrix::operator=( + const utils::Matrix &other) { this->MX_Xd_dynamic::operator=(other); } -void biorbd::utils::Matrix::operator=( +void utils::Matrix::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix& other) { this->MX_Xd_dynamic::operator=(other); @@ -48,7 +50,7 @@ void biorbd::utils::Matrix::operator=( #endif -biorbd::utils::Matrix::Matrix( +utils::Matrix::Matrix( unsigned int nbRows, unsigned int nbCols) : RigidBodyDynamics::Math::MatrixNd(nbRows, nbCols) diff --git a/src/Utils/Node.cpp b/src/Utils/Node.cpp index 52a860e8..15f93828 100644 --- a/src/Utils/Node.cpp +++ b/src/Utils/Node.cpp @@ -3,17 +3,19 @@ #include "Utils/String.h" -biorbd::utils::Node::Node() : - m_name(std::make_shared("")), - m_parentName(std::make_shared("")), - m_typeOfNode(std::make_shared - (biorbd::utils::NODE_TYPE::NO_NODE_TYPE)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Node::Node() : + m_name(std::make_shared("")), + m_parentName(std::make_shared("")), + m_typeOfNode(std::make_shared + (utils::NODE_TYPE::NO_NODE_TYPE)) { } -biorbd::utils::Node::Node( - const biorbd::utils::Node &other) +utils::Node::Node( + const utils::Node &other) { // Shallow copy m_name = other.m_name; @@ -21,61 +23,61 @@ biorbd::utils::Node::Node( m_typeOfNode = other.m_typeOfNode; } -biorbd::utils::Node::Node(const biorbd::utils::String &name) : - m_name(std::make_shared(name)), - m_parentName(std::make_shared("")), - m_typeOfNode(std::make_shared - (biorbd::utils::NODE_TYPE::NO_NODE_TYPE)) +utils::Node::Node(const utils::String &name) : + m_name(std::make_shared(name)), + m_parentName(std::make_shared("")), + m_typeOfNode(std::make_shared + (utils::NODE_TYPE::NO_NODE_TYPE)) { } -biorbd::utils::Node::Node( - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - m_name(std::make_shared(name)), - m_parentName(std::make_shared(parentName)), - m_typeOfNode(std::make_shared - (biorbd::utils::NODE_TYPE::NO_NODE_TYPE)) +utils::Node::Node( + const utils::String &name, + const utils::String &parentName) : + m_name(std::make_shared(name)), + m_parentName(std::make_shared(parentName)), + m_typeOfNode(std::make_shared + (utils::NODE_TYPE::NO_NODE_TYPE)) { } -biorbd::utils::Node::~Node() +utils::Node::~Node() { } -void biorbd::utils::Node::DeepCopy( - const biorbd::utils::Node &other) +void utils::Node::DeepCopy( + const utils::Node &other) { *m_name = *other.m_name; *m_parentName = *other.m_parentName; *m_typeOfNode = *other.m_typeOfNode; } -void biorbd::utils::Node::setName(const biorbd::utils::String &name) +void utils::Node::setName(const utils::String &name) { *m_name = name; } -const biorbd::utils::String &biorbd::utils::Node::name() const +const utils::String &utils::Node::name() const { return *m_name; } -const biorbd::utils::String& biorbd::utils::Node::parent() const +const utils::String& utils::Node::parent() const { return *m_parentName; } -void biorbd::utils::Node::setParent( - const biorbd::utils::String &name) +void utils::Node::setParent( + const utils::String &name) { *m_parentName = name; } -biorbd::utils::NODE_TYPE biorbd::utils::Node::typeOfNode() const +utils::NODE_TYPE utils::Node::typeOfNode() const { return *m_typeOfNode; } diff --git a/src/Utils/Path.cpp b/src/Utils/Path.cpp index 0bd691bc..c430b9a3 100644 --- a/src/Utils/Path.cpp +++ b/src/Utils/Path.cpp @@ -16,60 +16,62 @@ #include #endif -biorbd::utils::Path::Path() : - m_originalPath(std::make_shared()), - m_folder(std::make_shared()), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Path::Path() : + m_originalPath(std::make_shared()), + m_folder(std::make_shared()), m_isFolderAbsolute(std::make_shared()), - m_filename(std::make_shared()), - m_extension(std::make_shared()) + m_filename(std::make_shared()), + m_extension(std::make_shared()) { } -biorbd::utils::Path::Path( +utils::Path::Path( const char *path) : - m_originalPath(std::make_shared(path)), - m_folder(std::make_shared()), + m_originalPath(std::make_shared(path)), + m_folder(std::make_shared()), m_isFolderAbsolute(std::make_shared()), - m_filename(std::make_shared()), - m_extension(std::make_shared()) + m_filename(std::make_shared()), + m_extension(std::make_shared()) { parseFileName(*m_originalPath, *m_folder, *m_filename, *m_extension); setIsFolderAbsolute(); } -biorbd::utils::Path::Path( - const biorbd::utils::String &path) : - m_originalPath(std::make_shared(path)), - m_folder(std::make_shared()), +utils::Path::Path( + const utils::String &path) : + m_originalPath(std::make_shared(path)), + m_folder(std::make_shared()), m_isFolderAbsolute(std::make_shared()), - m_filename(std::make_shared()), - m_extension(std::make_shared()) + m_filename(std::make_shared()), + m_extension(std::make_shared()) { parseFileName(*m_originalPath, *m_folder, *m_filename, *m_extension); setIsFolderAbsolute(); } -biorbd::utils::Path::Path( +utils::Path::Path( const std::basic_string &path) : - m_originalPath(std::make_shared(path)), - m_folder(std::make_shared()), + m_originalPath(std::make_shared(path)), + m_folder(std::make_shared()), m_isFolderAbsolute(std::make_shared()), - m_filename(std::make_shared()), - m_extension(std::make_shared()) + m_filename(std::make_shared()), + m_extension(std::make_shared()) { parseFileName(*m_originalPath, *m_folder, *m_filename, *m_extension); setIsFolderAbsolute(); } -biorbd::utils::Path biorbd::utils::Path::DeepCopy() const +utils::Path utils::Path::DeepCopy() const { - biorbd::utils::Path copy; + utils::Path copy; copy.DeepCopy(*this); return copy; } -void biorbd::utils::Path::DeepCopy( +void utils::Path::DeepCopy( const Path &other) { *m_originalPath = *other.m_originalPath; @@ -79,17 +81,17 @@ void biorbd::utils::Path::DeepCopy( *m_extension = *other.m_extension; } -bool biorbd::utils::Path::isFileExist() const +bool utils::Path::isFileExist() const { return isFileExist(absolutePath()); } -bool biorbd::utils::Path::isFileExist( +bool utils::Path::isFileExist( const Path& path) { return isFileExist(path.absolutePath()); } -bool biorbd::utils::Path::isFileExist( - const biorbd::utils::String& path) +bool utils::Path::isFileExist( + const utils::String& path) { if (FILE *file = fopen( #ifdef _WIN32 @@ -105,7 +107,7 @@ bool biorbd::utils::Path::isFileExist( } } -bool biorbd::utils::Path::isFileReadable() const +bool utils::Path::isFileReadable() const { std::ifstream fichier( #ifdef _WIN32 @@ -119,19 +121,19 @@ bool biorbd::utils::Path::isFileReadable() const return isOpen; } -bool biorbd::utils::Path::isFolderExist() const +bool utils::Path::isFolderExist() const { return isFolderExist(*this); } -bool biorbd::utils::Path::isFolderExist( +bool utils::Path::isFolderExist( const Path &path) { return isFolderExist(path.folder()); } -bool biorbd::utils::Path::isFolderExist( - const biorbd::utils::String & path) +bool utils::Path::isFolderExist( + const utils::String & path) { #ifdef _WIN32 if (GetFileAttributesA(toWindowsFormat(path).c_str()) @@ -156,13 +158,13 @@ bool biorbd::utils::Path::isFolderExist( } -void biorbd::utils::Path::parseFileName( - const biorbd::utils::String &path, - biorbd::utils::String &folder, - biorbd::utils::String &filename, - biorbd::utils::String &extension) +void utils::Path::parseFileName( + const utils::String &path, + utils::String &folder, + utils::String &filename, + utils::String &extension) { - biorbd::utils::String pathSep(toUnixFormat(path)); + utils::String pathSep(toUnixFormat(path)); size_t sepPos(pathSep.rfind("/")); @@ -192,26 +194,26 @@ void biorbd::utils::Path::parseFileName( filename = pathSep.substr(sepPos+1, ext- sepPos-1); } -biorbd::utils::String biorbd::utils::Path::relativePath() const +utils::String utils::Path::relativePath() const { return relativePath(*this, currentDir()); } -biorbd::utils::String biorbd::utils::Path::relativePath( - const biorbd::utils::String& relativeTo) const +utils::String utils::Path::relativePath( + const utils::String& relativeTo) const { return relativePath(*this, relativeTo); } -biorbd::utils::String biorbd::utils::Path::relativePath( - const biorbd::utils::Path &path, - const biorbd::utils::String &relativeTo) +utils::String utils::Path::relativePath( + const utils::Path &path, + const utils::String &relativeTo) { - biorbd::utils::String me(path.absolutePath()); - biorbd::utils::String currentDir(relativeTo); + utils::String me(path.absolutePath()); + utils::String currentDir(relativeTo); - biorbd::utils::String meFirstPart(""); - biorbd::utils::String currentDirFirstPart(""); + utils::String meFirstPart(""); + utils::String currentDirFirstPart(""); // Set the separator to the 0 position size_t sepMe = std::string::npos; @@ -233,7 +235,7 @@ biorbd::utils::String biorbd::utils::Path::relativePath( // we still can advance to find de closest relative part } while(!meFirstPart.compare(currentDirFirstPart)); - biorbd::utils::String outPath; + utils::String outPath; while (currentDir.compare("")) { // Tant que currentDir n'est pas vide, reculer // Trouver le prochain séparateur @@ -262,22 +264,22 @@ biorbd::utils::String biorbd::utils::Path::relativePath( return outPath; } -biorbd::utils::String biorbd::utils::Path::absoluteFolder( - const biorbd::utils::Path &path) +utils::String utils::Path::absoluteFolder( + const utils::Path &path) { if (*path.m_isFolderAbsolute) { return path.folder(); } - biorbd::utils::String base; + utils::String base; #ifdef _WIN32 - biorbd::utils::String current(currentDir()); + utils::String current(currentDir()); std::smatch matches; if (std::regex_search(current, matches, std::regex("^([A-Z]):[\\/].*$"))) { base = matches[1].str() + ":/"; } else { - biorbd::utils::Error::raise("I could not find the current drive to estimate the path"); + utils::Error::raise("I could not find the current drive to estimate the path"); } #else base = "/"; @@ -285,7 +287,7 @@ biorbd::utils::String biorbd::utils::Path::absoluteFolder( return base + relativePath(path, base); } -biorbd::utils::String biorbd::utils::Path::absoluteFolder() const +utils::String utils::Path::absoluteFolder() const { if (*m_isFolderAbsolute) { return *m_folder; @@ -294,7 +296,7 @@ biorbd::utils::String biorbd::utils::Path::absoluteFolder() const } } -biorbd::utils::String biorbd::utils::Path::absolutePath() const +utils::String utils::Path::absolutePath() const { if (m_filename->compare("")) { if (m_extension->compare("")) { @@ -307,10 +309,10 @@ biorbd::utils::String biorbd::utils::Path::absolutePath() const } } -biorbd::utils::String biorbd::utils::Path::toUnixFormat( - const biorbd::utils::String& path) +utils::String utils::Path::toUnixFormat( + const utils::String& path) { - biorbd::utils::String pathOut(path); + utils::String pathOut(path); // Depending on the string origin, "\\" is either the character "\" // escaped or the character "\" written twice. Test for both @@ -330,10 +332,10 @@ biorbd::utils::String biorbd::utils::Path::toUnixFormat( return pathOut; } -biorbd::utils::String biorbd::utils::Path::toWindowsFormat( - const biorbd::utils::String &path) +utils::String utils::Path::toWindowsFormat( + const utils::String &path) { - biorbd::utils::String pathOut(path); + utils::String pathOut(path); size_t pos(pathOut.rfind("/")); while (pos != std::string::npos) { pathOut.replace(pos, 1, "\\\\"); @@ -342,43 +344,43 @@ biorbd::utils::String biorbd::utils::Path::toWindowsFormat( return pathOut; } -const biorbd::utils::String &biorbd::utils::Path::originalPath() const +const utils::String &utils::Path::originalPath() const { return *m_originalPath; } -const biorbd::utils::String &biorbd::utils::Path::folder() const +const utils::String &utils::Path::folder() const { return *m_folder; } -void biorbd::utils::Path::setFilename( - const biorbd::utils::String& name) +void utils::Path::setFilename( + const utils::String& name) { *m_filename = name; } -const biorbd::utils::String& biorbd::utils::Path::filename() const +const utils::String& utils::Path::filename() const { return *m_filename; } -void biorbd::utils::Path::setExtension( - const biorbd::utils::String &ext) +void utils::Path::setExtension( + const utils::String &ext) { *m_extension = ext; } -const biorbd::utils::String& biorbd::utils::Path::extension() const +const utils::String& utils::Path::extension() const { return *m_extension; } -void biorbd::utils::Path::setIsFolderAbsolute() +void utils::Path::setIsFolderAbsolute() { - biorbd::utils::String base; + utils::String base; #ifdef _WIN32 - biorbd::utils::String current(*m_folder); + utils::String current(*m_folder); std::smatch matches; if (std::regex_search(current, matches, std::regex("^([A-Z]):[\\/].*$"))) { @@ -397,23 +399,23 @@ void biorbd::utils::Path::setIsFolderAbsolute() #endif } -biorbd::utils::String biorbd::utils::Path::currentDir() +utils::String utils::Path::currentDir() { char buff[FILENAME_MAX]; #ifdef _WIN32 - biorbd::utils::Error::check(_getcwd(buff, FILENAME_MAX), + utils::Error::check(_getcwd(buff, FILENAME_MAX), "Could not find the current directory"); #else - biorbd::utils::Error::check(getcwd(buff, FILENAME_MAX), + utils::Error::check(getcwd(buff, FILENAME_MAX), "Could not find the current directory"); #endif return toUnixFormat(buff) + "/"; } -void biorbd::utils::Path::createFolder() const +void utils::Path::createFolder() const { - const biorbd::utils::String& tp(folder()); - biorbd::utils::String tp2(tp); + const utils::String& tp(folder()); + utils::String tp2(tp); size_t sep = std::string::npos; size_t sepTrack = 0; @@ -428,7 +430,7 @@ void biorbd::utils::Path::createFolder() const // Séparer la première et la dernière partie if (!isFolderExist( - static_cast( + static_cast( tp.substr(0, sepTrack)))) { #ifdef _WIN32 _mkdir(toWindowsFormat(tp.substr(0, sepTrack)).c_str()); diff --git a/src/Utils/Quaternion.cpp b/src/Utils/Quaternion.cpp index 6aeb27b6..bed47829 100644 --- a/src/Utils/Quaternion.cpp +++ b/src/Utils/Quaternion.cpp @@ -7,7 +7,9 @@ #include "Utils/Error.h" #include "Utils/Rotation.h" -biorbd::utils::Quaternion::Quaternion ( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Quaternion::Quaternion ( double kStabilizer) : RigidBodyDynamics::Math::Vector4d (1, 0, 0, 0), m_Kstab(kStabilizer) @@ -15,15 +17,15 @@ biorbd::utils::Quaternion::Quaternion ( } -biorbd::utils::Quaternion::Quaternion( - const biorbd::utils::Quaternion &other) : +utils::Quaternion::Quaternion( + const utils::Quaternion &other) : RigidBodyDynamics::Math::Vector4d (other), m_Kstab(other.m_Kstab) { } -biorbd::utils::Quaternion::Quaternion( +utils::Quaternion::Quaternion( const RigidBodyDynamics::Math::Vector4d &vec4, double kStabilizer) : RigidBodyDynamics::Math::Vector4d (vec4), @@ -32,11 +34,11 @@ biorbd::utils::Quaternion::Quaternion( } -biorbd::utils::Quaternion::Quaternion ( - const biorbd::utils::Scalar& w, - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, +utils::Quaternion::Quaternion ( + const utils::Scalar& w, + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, double kStabilizer) : RigidBodyDynamics::Math::Vector4d(w, x, y, z), m_Kstab(kStabilizer) @@ -44,9 +46,9 @@ biorbd::utils::Quaternion::Quaternion ( } -biorbd::utils::Quaternion::Quaternion ( - const biorbd::utils::Scalar& w, - const biorbd::utils::Vector3d &vec3, +utils::Quaternion::Quaternion ( + const utils::Scalar& w, + const utils::Vector3d &vec3, double kStabilizer) : RigidBodyDynamics::Math::Vector4d(w, vec3[0], vec3[1], vec3[2]), m_Kstab(kStabilizer) @@ -54,37 +56,37 @@ biorbd::utils::Quaternion::Quaternion ( } -biorbd::utils::Scalar biorbd::utils::Quaternion::w() const +utils::Scalar utils::Quaternion::w() const { return (*this)(0); } -biorbd::utils::Scalar biorbd::utils::Quaternion::x() const +utils::Scalar utils::Quaternion::x() const { return (*this)(1); } -biorbd::utils::Scalar biorbd::utils::Quaternion::y() const +utils::Scalar utils::Quaternion::y() const { return (*this)(2); } -biorbd::utils::Scalar biorbd::utils::Quaternion::z() const +utils::Scalar utils::Quaternion::z() const { return (*this)(3); } -void biorbd::utils::Quaternion::setKStab(double newKStab) +void utils::Quaternion::setKStab(double newKStab) { m_Kstab = newKStab; } -double biorbd::utils::Quaternion::kStab() const +double utils::Quaternion::kStab() const { return m_Kstab; } -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*( - const biorbd::utils::Quaternion& q) const +utils::Quaternion utils::Quaternion::operator*( + const utils::Quaternion& q) const { - return biorbd::utils::Quaternion ( + return utils::Quaternion ( (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] - (*this)[3] * q[3], (*this)[0] * q[1] + (*this)[1] * q[0] + (*this)[2] * q[3] - (*this)[3] * q[2], (*this)[0] * q[2] + (*this)[2] * q[0] + (*this)[3] * q[1] - (*this)[1] * q[3], @@ -92,86 +94,86 @@ biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*( (this->m_Kstab + q.m_Kstab) / 2); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*( - const biorbd::utils::Scalar& scalar) const +utils::Quaternion utils::Quaternion::operator*( + const utils::Scalar& scalar) const { - return biorbd::utils::Quaternion ( + return utils::Quaternion ( this->RigidBodyDynamics::Math::Vector4d::operator*(scalar), this->m_Kstab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*( +utils::Quaternion utils::Quaternion::operator*( float scalar) const { - return biorbd::utils::Quaternion ( + return utils::Quaternion ( this->RigidBodyDynamics::Math::Vector4d::operator*( static_cast(scalar)), this->m_Kstab); } #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*( +utils::Quaternion utils::Quaternion::operator*( double scalar) const { - return biorbd::utils::Quaternion ( + return utils::Quaternion ( this->RigidBodyDynamics::Math::Vector4d::operator*(scalar), this->m_Kstab); } #endif -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator+( - const biorbd::utils::Quaternion& other) const +utils::Quaternion utils::Quaternion::operator+( + const utils::Quaternion& other) const { - return biorbd::utils::Quaternion( + return utils::Quaternion( this->RigidBodyDynamics::Math::Vector4d::operator+(other), (this->m_Kstab + other.m_Kstab) / 2); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::operator-( - const biorbd::utils::Quaternion& other) const +utils::Quaternion utils::Quaternion::operator-( + const utils::Quaternion& other) const { - return biorbd::utils::Quaternion( + return utils::Quaternion( this->RigidBodyDynamics::Math::Vector4d::operator-(other), (this->m_Kstab + other.m_Kstab) / 2); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromGLRotate( - const biorbd::utils::Scalar& angle, - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, +utils::Quaternion utils::Quaternion::fromGLRotate( + const utils::Scalar& angle, + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, double kStab) { - biorbd::utils::Scalar angle_copy(angle); - biorbd::utils::Scalar st = std::sin (angle_copy * M_PI / 360.); - return biorbd::utils::Quaternion ( + utils::Scalar angle_copy(angle); + utils::Scalar st = std::sin (angle_copy * M_PI / 360.); + return utils::Quaternion ( std::cos (angle_copy * M_PI / 360.), st * x, st * y, st * z, kStab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromAxisAngle( - const biorbd::utils::Scalar& angle, - const biorbd::utils::Vector3d &axis, +utils::Quaternion utils::Quaternion::fromAxisAngle( + const utils::Scalar& angle, + const utils::Vector3d &axis, double kStab) { - biorbd::utils::Scalar angle_copy(angle); - biorbd::utils::Scalar d = axis.norm(); - biorbd::utils::Scalar s2 = std::sin (angle_copy * 0.5) / d; - return biorbd::utils::Quaternion ( + utils::Scalar angle_copy(angle); + utils::Scalar d = axis.norm(); + utils::Scalar s2 = std::sin (angle_copy * 0.5) / d; + return utils::Quaternion ( std::cos(angle_copy * 0.5), axis[0] * s2, axis[1] * s2, axis[2] * s2, kStab ); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix( - const biorbd::utils::RotoTrans &rt, +utils::Quaternion utils::Quaternion::fromMatrix( + const utils::RotoTrans &rt, double kStab) { return fromMatrix(rt.rot(), kStab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix( - const biorbd::utils::Rotation &mat, +utils::Quaternion utils::Quaternion::fromMatrix( + const utils::Rotation &mat, double kStab) { - biorbd::utils::Scalar w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; + utils::Scalar w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; return Quaternion ( w, (mat(2,1) - mat(1,2)) / (w * 4.), @@ -180,52 +182,52 @@ biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix( kStab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromZYXAngles( - const biorbd::utils::Vector3d &zyx_angles, +utils::Quaternion utils::Quaternion::fromZYXAngles( + const utils::Vector3d &zyx_angles, double kStab) { - return fromAxisAngle (zyx_angles[2], biorbd::utils::Vector3d (0., 0., 1.), + return fromAxisAngle (zyx_angles[2], utils::Vector3d (0., 0., 1.), kStab) - * fromAxisAngle (zyx_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab) - * fromAxisAngle (zyx_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab); + * fromAxisAngle (zyx_angles[1], utils::Vector3d (0., 1., 0.), kStab) + * fromAxisAngle (zyx_angles[0], utils::Vector3d (1., 0., 0.), kStab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromYXZAngles( - const biorbd::utils::Vector3d &yxz_angles, +utils::Quaternion utils::Quaternion::fromYXZAngles( + const utils::Vector3d &yxz_angles, double kStab) { - return fromAxisAngle (yxz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), + return fromAxisAngle (yxz_angles[1], utils::Vector3d (0., 1., 0.), kStab) - * fromAxisAngle (yxz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab) - * fromAxisAngle (yxz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab); + * fromAxisAngle (yxz_angles[0], utils::Vector3d (1., 0., 0.), kStab) + * fromAxisAngle (yxz_angles[2], utils::Vector3d (0., 0., 1.), kStab); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::fromXYZAngles( - const biorbd::utils::Vector3d &xyz_angles, +utils::Quaternion utils::Quaternion::fromXYZAngles( + const utils::Vector3d &xyz_angles, double kStab) { - return fromAxisAngle (xyz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), + return fromAxisAngle (xyz_angles[0], utils::Vector3d (1., 0., 0.), kStab) - * fromAxisAngle (xyz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab) - * fromAxisAngle (xyz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab); + * fromAxisAngle (xyz_angles[1], utils::Vector3d (0., 1., 0.), kStab) + * fromAxisAngle (xyz_angles[2], utils::Vector3d (0., 0., 1.), kStab); } -biorbd::utils::Rotation biorbd::utils::Quaternion::toMatrix( +utils::Rotation utils::Quaternion::toMatrix( bool skipAsserts) const { #ifndef BIORBD_USE_CASADI_MATH if (!skipAsserts) { - biorbd::utils::Error::check(fabs(this->squaredNorm() - 1.) < 1e-10, + utils::Error::check(fabs(this->squaredNorm() - 1.) < 1e-10, "The Quaternion norm is not equal to one"); } #endif - biorbd::utils::Scalar w = (*this)[0]; - biorbd::utils::Scalar x = (*this)[1]; - biorbd::utils::Scalar y = (*this)[2]; - biorbd::utils::Scalar z = (*this)[3]; - biorbd::utils::Rotation out = biorbd::utils::Rotation( + utils::Scalar w = (*this)[0]; + utils::Scalar x = (*this)[1]; + utils::Scalar y = (*this)[2]; + utils::Scalar z = (*this)[3]; + utils::Rotation out = utils::Rotation( 1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y, 2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x, 2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y); @@ -233,19 +235,19 @@ biorbd::utils::Rotation biorbd::utils::Quaternion::toMatrix( } #ifndef BIORBD_USE_CASADI_MATH -biorbd::utils::Quaternion biorbd::utils::Quaternion::slerp( +utils::Quaternion utils::Quaternion::slerp( double alpha, - const biorbd::utils::Quaternion &quat) const + const utils::Quaternion &quat) const { // check whether one of the two has 0 length - biorbd::utils::Scalar s = std::sqrt (squaredNorm() * quat.squaredNorm()); + utils::Scalar s = std::sqrt (squaredNorm() * quat.squaredNorm()); // division by 0.f is unhealthy! #ifndef BIORBD_USE_CASADI_MATH assert (s != 0.); #endif - biorbd::utils::Scalar angle = acos (dot(quat) / s); + utils::Scalar angle = acos (dot(quat) / s); #ifdef BIORBD_USE_CASADI_MATH if (true) { #else @@ -254,9 +256,9 @@ biorbd::utils::Quaternion biorbd::utils::Quaternion::slerp( return *this; } - biorbd::utils::Scalar d = 1. / std::sin (angle); - biorbd::utils::Scalar p0 = std::sin ((1. - alpha) * angle); - biorbd::utils::Scalar p1 = std::sin (alpha * angle); + utils::Scalar d = 1. / std::sin (angle); + utils::Scalar p0 = std::sin ((1. - alpha) * angle); + utils::Scalar p1 = std::sin (alpha * angle); #ifdef BIORBD_USE_CASADI_MATH return Quaternion(casadi::MX::if_else( @@ -275,38 +277,38 @@ biorbd::utils::Quaternion biorbd::utils::Quaternion::slerp( } #endif -biorbd::utils::Quaternion biorbd::utils::Quaternion::conjugate() const +utils::Quaternion utils::Quaternion::conjugate() const { - return biorbd::utils::Quaternion ( + return utils::Quaternion ( (*this)[0], -(*this)[1],-(*this)[2],-(*this)[3], this->kStab() ); } -biorbd::utils::Quaternion biorbd::utils::Quaternion::timeStep( - const biorbd::utils::Vector3d &omega, +utils::Quaternion utils::Quaternion::timeStep( + const utils::Vector3d &omega, double dt) { - biorbd::utils::Scalar omega_norm = omega.norm(); + utils::Scalar omega_norm = omega.norm(); return fromAxisAngle ( dt * omega_norm, omega / omega_norm, this->m_Kstab) * (*this); } -biorbd::utils::Vector3d biorbd::utils::Quaternion::rotate( - const biorbd::utils::Vector3d &vec) const +utils::Vector3d utils::Quaternion::rotate( + const utils::Vector3d &vec) const { - biorbd::utils::Quaternion vec_quat (0., vec); + utils::Quaternion vec_quat (0., vec); - biorbd::utils::Quaternion res_quat(vec_quat * (*this)); + utils::Quaternion res_quat(vec_quat * (*this)); res_quat = conjugate() * res_quat; - return biorbd::utils::Vector3d(res_quat[1], res_quat[2], res_quat[3]); + return utils::Vector3d(res_quat[1], res_quat[2], res_quat[3]); } #include -biorbd::utils::Quaternion biorbd::utils::Quaternion::omegaToQDot( - const biorbd::utils::Vector3d &omega) const +utils::Quaternion utils::Quaternion::omegaToQDot( + const utils::Vector3d &omega) const { RigidBodyDynamics::Math::MatrixNd m(4, 3); m(0, 0) = -(*this)[1]; @@ -322,17 +324,17 @@ biorbd::utils::Quaternion biorbd::utils::Quaternion::omegaToQDot( m(3, 1) = (*this)[1]; m(3, 2) = (*this)[0]; - return biorbd::utils::Quaternion(0.5 * m * omega, this->m_Kstab); + return utils::Quaternion(0.5 * m * omega, this->m_Kstab); } -biorbd::utils::Vector3d biorbd::utils::Quaternion::eulerDotToOmega( - const biorbd::utils::Vector3d &eulerDot, - const biorbd::utils::Vector3d &euler, - const biorbd::utils::String& seq) +utils::Vector3d utils::Quaternion::eulerDotToOmega( + const utils::Vector3d &eulerDot, + const utils::Vector3d &euler, + const utils::String& seq) { - biorbd::utils::Vector3d w; - biorbd::utils::Scalar dph, dth, dps, ph, th, ps; + utils::Vector3d w; + utils::Scalar dph, dth, dps, ph, th, ps; dph = eulerDot[0]; dth = eulerDot[1]; dps = eulerDot[2]; @@ -344,25 +346,25 @@ biorbd::utils::Vector3d biorbd::utils::Quaternion::eulerDotToOmega( w[1] = dth*std::cos(ps) - dph*std::cos(th)*std::sin(ps); w[2] = dph*std::sin(th) + dps; } else { - biorbd::utils::Error::raise("Angle sequence is either nor implemented or not recognized"); + utils::Error::raise("Angle sequence is either nor implemented or not recognized"); } return w; } -void biorbd::utils::Quaternion::derivate( - const biorbd::utils::Vector &w) +void utils::Quaternion::derivate( + const utils::Vector &w) { // Création du quaternion de "préproduit vectoriel" #ifdef BIORBD_USE_CASADI_MATH - biorbd::utils::Scalar qw = (*this)(0); - biorbd::utils::Scalar qx = (*this)(1); - biorbd::utils::Scalar qy = (*this)(2); - biorbd::utils::Scalar qz = (*this)(3); + utils::Scalar qw = (*this)(0); + utils::Scalar qx = (*this)(1); + utils::Scalar qy = (*this)(2); + utils::Scalar qz = (*this)(3); #else - biorbd::utils::Scalar& qw = (*this)(0); - biorbd::utils::Scalar& qx = (*this)(1); - biorbd::utils::Scalar& qy = (*this)(2); - biorbd::utils::Scalar& qz = (*this)(3); + utils::Scalar& qw = (*this)(0); + utils::Scalar& qx = (*this)(1); + utils::Scalar& qy = (*this)(2); + utils::Scalar& qz = (*this)(3); #endif RigidBodyDynamics::Math::Matrix4d Q = RigidBodyDynamics::Math::Matrix4d( @@ -382,11 +384,11 @@ void biorbd::utils::Quaternion::derivate( qy = newQuat[2]; qz = newQuat[3]; #ifdef BIORBD_USE_CASADI_MATH - *this = biorbd::utils::Quaternion(qw, qx, qy, qz, m_Kstab); + *this = utils::Quaternion(qw, qx, qy, qz, m_Kstab); #endif } -void biorbd::utils::Quaternion::normalize() +void utils::Quaternion::normalize() { *this = *this / this->norm(); } diff --git a/src/Utils/Range.cpp b/src/Utils/Range.cpp index a4bcf19b..7c454a68 100644 --- a/src/Utils/Range.cpp +++ b/src/Utils/Range.cpp @@ -1,7 +1,9 @@ #define BIORBD_API_EXPORTS #include "Utils/Range.h" -biorbd::utils::Range::Range( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Range::Range( double min, double max) : m_min(std::make_shared(min)), @@ -10,39 +12,39 @@ biorbd::utils::Range::Range( } -biorbd::utils::Range -biorbd::utils::Range::DeepCopy() const +utils::Range +utils::Range::DeepCopy() const { - biorbd::utils::Range copy; + utils::Range copy; copy.DeepCopy(*this); return copy; } -void biorbd::utils::Range::DeepCopy( - const biorbd::utils::Range &other) +void utils::Range::DeepCopy( + const utils::Range &other) { *m_min = *other.m_min; *m_max = *other.m_max; } -void biorbd::utils::Range::setMin( +void utils::Range::setMin( double min) { *m_min = min; } -double biorbd::utils::Range::min() const +double utils::Range::min() const { return *m_min; } -void biorbd::utils::Range::setMax( +void utils::Range::setMax( double max) { *m_max = max; } -double biorbd::utils::Range::max() const +double utils::Range::max() const { return *m_max; } diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 67d9c939..0dcac383 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -9,7 +9,9 @@ #include "RigidBody/NodeSegment.h" -biorbd::utils::Rotation::Rotation( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Rotation::Rotation( const RigidBodyDynamics::Math::Matrix3d& matrix) : RigidBodyDynamics::Math::Matrix3d(matrix) { @@ -18,7 +20,7 @@ biorbd::utils::Rotation::Rotation( #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Rotation::Rotation( +utils::Rotation::Rotation( const RigidBodyDynamics::Math::MatrixNd &m) : RigidBodyDynamics::Math::Matrix3d(m) { @@ -27,61 +29,61 @@ biorbd::utils::Rotation::Rotation( #endif -biorbd::utils::Rotation::Rotation( - const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, - const biorbd::utils::Scalar& v02, - const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, - const biorbd::utils::Scalar& v12, - const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, - const biorbd::utils::Scalar& v22) : +utils::Rotation::Rotation( + const utils::Scalar& v00, const utils::Scalar& v01, + const utils::Scalar& v02, + const utils::Scalar& v10, const utils::Scalar& v11, + const utils::Scalar& v12, + const utils::Scalar& v20, const utils::Scalar& v21, + const utils::Scalar& v22) : RigidBodyDynamics::Math::Matrix3d (v00, v01, v02, v10, v11, v12, v20, v21, v22) { } -biorbd::utils::Rotation::Rotation( - const biorbd::utils::Vector& rotation, - const biorbd::utils::String& rotationSequence) : +utils::Rotation::Rotation( + const utils::Vector& rotation, + const utils::String& rotationSequence) : RigidBodyDynamics::Math::Matrix3d(fromEulerAngles(rotation, rotationSequence)) { } -biorbd::utils::Rotation::Rotation( +utils::Rotation::Rotation( const RigidBodyDynamics::Math::SpatialTransform &st) : RigidBodyDynamics::Math::Matrix3d(st.E) { } -biorbd::utils::Vector3d biorbd::utils::Rotation::axe(unsigned int idx) const +utils::Vector3d utils::Rotation::axe(unsigned int idx) const { - biorbd::utils::Error::check(idx<=2, "Axis must be between 0 and 2 included"); + utils::Error::check(idx<=2, "Axis must be between 0 and 2 included"); return this->block(0,idx, 3, 1); } -biorbd::utils::Rotation biorbd::utils::Rotation::fromSpatialTransform( +utils::Rotation utils::Rotation::fromSpatialTransform( const RigidBodyDynamics::Math::SpatialTransform& st) { return st.E; } -biorbd::utils::Rotation biorbd::utils::Rotation::fromEulerAngles( - const biorbd::utils::Vector &rot, - const biorbd::utils::String& seq) +utils::Rotation utils::Rotation::fromEulerAngles( + const utils::Vector &rot, + const utils::String& seq) { // Check for size consistency - biorbd::utils::Error::check( + utils::Error::check( seq.length() == static_cast(rot.rows()), "Rotation and sequence of rotation must be the same length"); - biorbd::utils::Rotation out; + utils::Rotation out; out.setIdentity(); // Set the actual rotation matrix to this RigidBodyDynamics::Math::Matrix3d tp; for (unsigned int i=0; i - &axis1markers, - const std::pair - &axis2markers, - const std::pair& axesNames, - const biorbd::utils::String &axisToRecalculate) +utils::Matrix utils::Rotation::fromMarkersNonNormalized( + const std::pair &axis1markers, + const std::pair &axis2markers, + const std::pair& axesNames, + const utils::String &axisToRecalculate) { if (!axesNames.first.compare("") || !axesNames.second.compare("")) { - biorbd::utils::Error::raise("axesNames must be defined with a pair of \"x\", \"y\" or \"z\""); + utils::Error::raise("axesNames must be defined with a pair of \"x\", \"y\" or \"z\""); } // Figure out where to put the axes @@ -162,7 +162,7 @@ biorbd::utils::Matrix biorbd::utils::Rotation::fromMarkersNonNormalized( } // Get the system of axis XYZ - std::vector axes(3); + std::vector axes(3); axes[map[0]] = axis1markers.second - axis1markers.first; axes[map[1]] = axis2markers.second - axis2markers.first; axes[map[2]] = axes[toMultiply[0]].cross(axes[toMultiply[1]]); @@ -177,20 +177,20 @@ biorbd::utils::Matrix biorbd::utils::Rotation::fromMarkersNonNormalized( } // Organize them in a non-normalized matrix - biorbd::utils::Matrix r_out(3, 3); + utils::Matrix r_out(3, 3); for (unsigned int i=0; i<3; ++i) { r_out.block(0, i, 3, 1) = axes[i]; } return r_out; } -biorbd::utils::Rotation biorbd::utils::Rotation::fromMarkers( - const std::pair &axis1markers, - const std::pair &axis2markers, - const std::pair& axesNames, - const biorbd::utils::String &axisToRecalculate) +utils::Rotation utils::Rotation::fromMarkers( + const std::pair &axis1markers, + const std::pair &axis2markers, + const std::pair& axesNames, + const utils::String &axisToRecalculate) { - biorbd::utils::Matrix r_out( + utils::Matrix r_out( fromMarkersNonNormalized(axis1markers, axis2markers, axesNames, axisToRecalculate)); @@ -203,15 +203,15 @@ biorbd::utils::Rotation biorbd::utils::Rotation::fromMarkers( return r_out; } -biorbd::utils::Vector biorbd::utils::Rotation::toEulerAngles( - const biorbd::utils::Rotation &r, - const biorbd::utils::String &seq) +utils::Vector utils::Rotation::toEulerAngles( + const utils::Rotation &r, + const utils::String &seq) { - biorbd::utils::Vector v; + utils::Vector v; if (!seq.compare("zyzz")) { - v = biorbd::utils::Vector(3); + v = utils::Vector(3); } else { - v = biorbd::utils::Vector(static_cast(seq.length())); + v = utils::Vector(static_cast(seq.length())); } if (!seq.compare("x")) { @@ -275,15 +275,15 @@ biorbd::utils::Vector biorbd::utils::Rotation::toEulerAngles( v[1] = std::acos(r(2,2)); // y v[2] = std::atan2(r(2,1), -r(2,0)) + v[0]; // z+z } else { - biorbd::utils::Error::raise("Angle sequence is not recognized"); + utils::Error::raise("Angle sequence is not recognized"); } return v; } #ifndef BIORBD_USE_CASADI_MATH -biorbd::utils::Rotation biorbd::utils::Rotation::mean( - const std::vector & mToMean) +utils::Rotation utils::Rotation::mean( + const std::vector & mToMean) { RigidBodyDynamics::Math::Matrix3d m_tp; m_tp.setZero(); @@ -299,25 +299,25 @@ biorbd::utils::Rotation biorbd::utils::Rotation::mean( m_tp, Eigen::ComputeFullU | Eigen::ComputeFullV); // Normalize the matrix - biorbd::utils::Rotation m_out(svd.matrixU() * svd.matrixV().transpose()); + utils::Rotation m_out(svd.matrixU() * svd.matrixV().transpose()); return m_out; } #endif -void biorbd::utils::Rotation::checkUnitary() +void utils::Rotation::checkUnitary() { #ifndef BIORBD_USE_CASADI_MATH #ifndef SKIP_ASSERT double sqrtNorm = static_cast(this->squaredNorm()); - biorbd::utils::Error::check(fabs(sqrtNorm - 3.) < 1e-4, - biorbd::utils::String("The Rotation matrix square norm is equal to ") + utils::Error::check(fabs(sqrtNorm - 3.) < 1e-4, + utils::String("The Rotation matrix square norm is equal to ") + sqrtNorm + ", but should be equal to 3"); #endif #endif } -std::ostream &operator<<(std::ostream &os, const biorbd::utils::Rotation &a) +std::ostream &operator<<(std::ostream &os, const utils::Rotation &a) { os << a.block(0,0,3,3); return os; diff --git a/src/Utils/RotoTrans.cpp b/src/Utils/RotoTrans.cpp index bc970149..dbe46637 100644 --- a/src/Utils/RotoTrans.cpp +++ b/src/Utils/RotoTrans.cpp @@ -9,26 +9,28 @@ #include "RigidBody/NodeSegment.h" -biorbd::utils::RotoTrans::RotoTrans( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::RotoTrans::RotoTrans( const RigidBodyDynamics::Math::Matrix4d& matrix) : RigidBodyDynamics::Math::Matrix4d(matrix) { checkUnitary(); } -biorbd::utils::RotoTrans::RotoTrans( - const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, - const biorbd::utils::Scalar& v02, - const biorbd::utils::Scalar& v03, - const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, - const biorbd::utils::Scalar& v12, - const biorbd::utils::Scalar& v13, - const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, - const biorbd::utils::Scalar& v22, - const biorbd::utils::Scalar& v23, - const biorbd::utils::Scalar& v30, const biorbd::utils::Scalar& v31, - const biorbd::utils::Scalar& v32, - const biorbd::utils::Scalar& v33) : +utils::RotoTrans::RotoTrans( + const utils::Scalar& v00, const utils::Scalar& v01, + const utils::Scalar& v02, + const utils::Scalar& v03, + const utils::Scalar& v10, const utils::Scalar& v11, + const utils::Scalar& v12, + const utils::Scalar& v13, + const utils::Scalar& v20, const utils::Scalar& v21, + const utils::Scalar& v22, + const utils::Scalar& v23, + const utils::Scalar& v30, const utils::Scalar& v31, + const utils::Scalar& v32, + const utils::Scalar& v33) : RigidBodyDynamics::Math::Matrix4d (v00, v01, v02, v03, v10, v11, v12, v13, v20, v21, v22, v23, @@ -37,45 +39,45 @@ biorbd::utils::RotoTrans::RotoTrans( checkUnitary(); } -biorbd::utils::RotoTrans::RotoTrans( - const biorbd::utils::Rotation& rot) : +utils::RotoTrans::RotoTrans( + const utils::Rotation& rot) : RigidBodyDynamics::Math::Matrix4d(combineRotAndTrans(rot, - biorbd::utils::Vector3d::Zero())) + utils::Vector3d::Zero())) { } -biorbd::utils::RotoTrans::RotoTrans( - const biorbd::utils::Rotation& rot, - const biorbd::utils::Vector3d& trans) : +utils::RotoTrans::RotoTrans( + const utils::Rotation& rot, + const utils::Vector3d& trans) : RigidBodyDynamics::Math::Matrix4d(combineRotAndTrans(rot,trans)) { } -biorbd::utils::RotoTrans::RotoTrans( - const biorbd::utils::Vector& rotation, - const biorbd::utils::Vector3d& translation, - const biorbd::utils::String& rotationSequence) : +utils::RotoTrans::RotoTrans( + const utils::Vector& rotation, + const utils::Vector3d& translation, + const utils::String& rotationSequence) : RigidBodyDynamics::Math::Matrix4d(fromEulerAngles(rotation, translation, rotationSequence)) { } -biorbd::utils::RotoTrans::RotoTrans( +utils::RotoTrans::RotoTrans( const RigidBodyDynamics::Math::SpatialTransform& st) : RigidBodyDynamics::Math::Matrix4d(fromSpatialTransform(st)) { } -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::fromMarkers( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment& origin, - const std::pair &axis1markers, - const std::pair &axis2markers, - const std::pair& axesNames, - const biorbd::utils::String &axisToRecalculate) +utils::RotoTrans utils::RotoTrans::fromMarkers( + const rigidbody::NodeSegment& origin, + const std::pair &axis1markers, + const std::pair &axis2markers, + const std::pair& axesNames, + const utils::String &axisToRecalculate) { RotoTrans rt_out; rt_out.block(0, 0, 3, 3) = Rotation::fromMarkers(axis1markers, axis2markers, @@ -84,16 +86,16 @@ biorbd::utils::RotoTrans biorbd::utils::RotoTrans::fromMarkers( return rt_out; } -biorbd::utils::Vector3d biorbd::utils::RotoTrans::axe(unsigned int idx) const +utils::Vector3d utils::RotoTrans::axe(unsigned int idx) const { - biorbd::utils::Error::check( + utils::Error::check( idx<=2, "Axis must be between 0 and 2 included"); return rot().block(0,idx,3,1); } -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::transpose() const +utils::RotoTrans utils::RotoTrans::transpose() const { - biorbd::utils::RotoTrans tp; + utils::RotoTrans tp; tp.block(0, 0, 3, 3) = block(0, 0, 3, 3).transpose(); tp.block(0, 3, 3, 1) = -tp.block(0, 0, 3, 3) * block(0, 3, 3, 1); tp.block(3, 0, 1, 4) = RigidBodyDynamics::Math::Vector4d(0, 0, 0, @@ -101,21 +103,21 @@ biorbd::utils::RotoTrans biorbd::utils::RotoTrans::transpose() const return tp; } -biorbd::utils::Vector3d biorbd::utils::RotoTrans::trans() const +utils::Vector3d utils::RotoTrans::trans() const { return this->block<3, 1>(0,3); } -biorbd::utils::Rotation biorbd::utils::RotoTrans::rot() const +utils::Rotation utils::RotoTrans::rot() const { return this->block<3, 3>(0,0); } -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::combineRotAndTrans( - const biorbd::utils::Rotation& rot, - const biorbd::utils::Vector3d& trans) +utils::RotoTrans utils::RotoTrans::combineRotAndTrans( + const utils::Rotation& rot, + const utils::Vector3d& trans) { - biorbd::utils::RotoTrans out; + utils::RotoTrans out; out.block(0,0,3,3) = rot; out.block(0,3,3,1) = trans; out.block(3, 0, 1, 4) = RigidBodyDynamics::Math::Vector4d(0, 0, 0, @@ -123,36 +125,36 @@ biorbd::utils::RotoTrans biorbd::utils::RotoTrans::combineRotAndTrans( return out; } -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::fromSpatialTransform( +utils::RotoTrans utils::RotoTrans::fromSpatialTransform( const RigidBodyDynamics::Math::SpatialTransform& st) { return combineRotAndTrans(st.E,st.r); } -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::fromEulerAngles( - const biorbd::utils::Vector& rot, - const biorbd::utils::Vector3d& trans, - const biorbd::utils::String& seq) +utils::RotoTrans utils::RotoTrans::fromEulerAngles( + const utils::Vector& rot, + const utils::Vector3d& trans, + const utils::String& seq) { - biorbd::utils::Rotation rot_mat(biorbd::utils::Rotation::fromEulerAngles(rot, + utils::Rotation rot_mat(utils::Rotation::fromEulerAngles(rot, seq)); - biorbd::utils::RotoTrans out; + utils::RotoTrans out; out.block(0, 0, 3, 3) = rot_mat; out.block(0, 3, 3, 1) = trans; return out; } -biorbd::utils::Vector biorbd::utils::RotoTrans::toEulerAngles( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::String &seq) +utils::Vector utils::RotoTrans::toEulerAngles( + const utils::RotoTrans& rt, + const utils::String &seq) { - return biorbd::utils::Rotation::toEulerAngles(rt.block<3, 3>(0, 0), seq); + return utils::Rotation::toEulerAngles(rt.block<3, 3>(0, 0), seq); } #ifndef BIORBD_USE_CASADI_MATH -biorbd::utils::RotoTrans biorbd::utils::RotoTrans::mean( +utils::RotoTrans utils::RotoTrans::mean( const std::vector & mToMean) { // The translation part is just the actual mean @@ -164,18 +166,18 @@ biorbd::utils::RotoTrans biorbd::utils::RotoTrans::mean( v_tp = v_tp/mToMean.size(); // The rotation part should call the proper way implemented in Rotation - std::vector rotations; + std::vector rotations; for (unsigned int i=0; i(0, 0)); } - biorbd::utils::RotoTrans m_out( - biorbd::utils::Rotation::mean(rotations), v_tp); + utils::RotoTrans m_out( + utils::Rotation::mean(rotations), v_tp); return m_out; } #endif -RigidBodyDynamics::Math::Vector4d biorbd::utils::RotoTrans::expand3dTo4d( - const biorbd::utils::Vector3d &v1) +RigidBodyDynamics::Math::Vector4d utils::RotoTrans::expand3dTo4d( + const utils::Vector3d &v1) { RigidBodyDynamics::Math::Vector4d v2; v2.block(0,0,3,1) = v1; @@ -183,20 +185,20 @@ RigidBodyDynamics::Math::Vector4d biorbd::utils::RotoTrans::expand3dTo4d( return v2; } -void biorbd::utils::RotoTrans::checkUnitary() +void utils::RotoTrans::checkUnitary() { #ifndef BIORBD_USE_CASADI_MATH #ifndef SKIP_ASSERT this->rot(); // Automatically cast the test for the rotation part - biorbd::utils::Error::check(this->block(3, 0, 1, 4).sum() == 1., + utils::Error::check(this->block(3, 0, 1, 4).sum() == 1., "Last row of the RotoTrans should be (0,0,0,1"); - biorbd::utils::Error::check((*this)(3, 3) == 1., + utils::Error::check((*this)(3, 3) == 1., "Last row of the RotoTrans should be (0,0,0,1"); #endif #endif } -std::ostream &operator<<(std::ostream &os, const biorbd::utils::RotoTrans &a) +std::ostream &operator<<(std::ostream &os, const utils::RotoTrans &a) { os << a.block(0,0,4,4); return os; diff --git a/src/Utils/RotoTransNode.cpp b/src/Utils/RotoTransNode.cpp index 93ce4c9e..e1c41f76 100644 --- a/src/Utils/RotoTransNode.cpp +++ b/src/Utils/RotoTransNode.cpp @@ -3,59 +3,61 @@ #include "Utils/String.h" -biorbd::utils::RotoTransNode::RotoTransNode() : - biorbd::utils::RotoTrans(), - biorbd::utils::Node() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::RotoTransNode::RotoTransNode() : + utils::RotoTrans(), + utils::Node() { setType(); } -biorbd::utils::RotoTransNode::RotoTransNode( +utils::RotoTransNode::RotoTransNode( const RotoTrans &rt, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::utils::RotoTrans(rt), - biorbd::utils::Node(name, parentName) + const utils::String &name, + const utils::String &parentName) : + utils::RotoTrans(rt), + utils::Node(name, parentName) { setType(); } -biorbd::utils::RotoTransNode biorbd::utils::RotoTransNode::DeepCopy() const +utils::RotoTransNode utils::RotoTransNode::DeepCopy() const { - biorbd::utils::RotoTransNode copy; + utils::RotoTransNode copy; copy.DeepCopy(*this); return copy; } -void biorbd::utils::RotoTransNode::DeepCopy(const RotoTransNode &other) +void utils::RotoTransNode::DeepCopy(const RotoTransNode &other) { - this->biorbd::utils::RotoTrans::operator=(other); - biorbd::utils::Node::DeepCopy(other); + this->utils::RotoTrans::operator=(other); + utils::Node::DeepCopy(other); } -void biorbd::utils::RotoTransNode::setType() +void utils::RotoTransNode::setType() { - *m_typeOfNode = biorbd::utils::NODE_TYPE::ROTOTRANS; + *m_typeOfNode = utils::NODE_TYPE::ROTOTRANS; } -void biorbd::utils::RotoTransNode::operator=( - const biorbd::utils::RotoTrans &other) +void utils::RotoTransNode::operator=( + const utils::RotoTrans &other) { *this = RotoTransNode(other, "", ""); } -biorbd::utils::RotoTrans biorbd::utils::RotoTransNode::operator*( - const biorbd::utils::RotoTransNode& other) const +utils::RotoTrans utils::RotoTransNode::operator*( + const utils::RotoTransNode& other) const { - return this->biorbd::utils::RotoTrans::operator*(other); + return this->utils::RotoTrans::operator*(other); } -biorbd::utils::RotoTransNode biorbd::utils::operator*( - const biorbd::utils::RotoTrans &other, - const biorbd::utils::RotoTransNode &me) +utils::RotoTransNode utils::operator*( + const utils::RotoTrans &other, + const utils::RotoTransNode &me) { - return biorbd::utils::RotoTransNode( + return utils::RotoTransNode( other.operator*(me), - me.biorbd::utils::Node::name(), + me.utils::Node::name(), me.parent()); } diff --git a/src/Utils/Scalar.cpp b/src/Utils/Scalar.cpp index 23310ae6..c6b85448 100644 --- a/src/Utils/Scalar.cpp +++ b/src/Utils/Scalar.cpp @@ -4,24 +4,26 @@ #ifdef BIORBD_USE_CASADI_MATH #include "Utils/Error.h" -biorbd::utils::Scalar::Scalar() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Scalar::Scalar() : RigidBodyDynamics::Math::Scalar () { } -biorbd::utils::Scalar::Scalar( +utils::Scalar::Scalar( double val) : RigidBodyDynamics::Math::Scalar (val) { } -biorbd::utils::Scalar::Scalar( +utils::Scalar::Scalar( const casadi::MX &val) : RigidBodyDynamics::Math::Scalar (val) { - biorbd::utils::Error::check( + utils::Error::check( val.rows() == 1 && val.columns() == 1, "Scalar must be a MX 1x1"); } diff --git a/src/Utils/SpatialVector.cpp b/src/Utils/SpatialVector.cpp index 4a18ffe4..33f1b887 100644 --- a/src/Utils/SpatialVector.cpp +++ b/src/Utils/SpatialVector.cpp @@ -5,20 +5,22 @@ #include "Utils/String.h" #include "Utils/Vector3d.h" -biorbd::utils::SpatialVector::SpatialVector() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::SpatialVector::SpatialVector() : RigidBodyDynamics::Math::SpatialVector() { } -biorbd::utils::SpatialVector::SpatialVector( - const biorbd::utils::SpatialVector& other) : +utils::SpatialVector::SpatialVector( + const utils::SpatialVector& other) : RigidBodyDynamics::Math::SpatialVector (other) { } -biorbd::utils::SpatialVector::SpatialVector( +utils::SpatialVector::SpatialVector( Scalar v1, Scalar v2, Scalar v3, Scalar v4, Scalar v5, Scalar v6) : RigidBodyDynamics::Math::SpatialVector (v1, v2, v3, v4, v5, v6) @@ -28,14 +30,14 @@ biorbd::utils::SpatialVector::SpatialVector( #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::SpatialVector::SpatialVector( +utils::SpatialVector::SpatialVector( const casadi::MX &v) : RigidBodyDynamics::Math::SpatialVector(v) { } -biorbd::utils::SpatialVector::SpatialVector( +utils::SpatialVector::SpatialVector( const RBDLCasadiMath::MX_Xd_SubMatrix &m) : RigidBodyDynamics::Math::SpatialVector (m) { @@ -44,21 +46,21 @@ biorbd::utils::SpatialVector::SpatialVector( #endif -void biorbd::utils::SpatialVector::operator=( - const biorbd::utils::SpatialVector &other) +void utils::SpatialVector::operator=( + const utils::SpatialVector &other) { this->RigidBodyDynamics::Math::SpatialVector::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH -void biorbd::utils::SpatialVector::operator=( +void utils::SpatialVector::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix& other) { this->RigidBodyDynamics::Math::SpatialVector::operator=(other); } -void biorbd::utils::SpatialVector::operator=( +void utils::SpatialVector::operator=( const casadi::MX &other) { this->RigidBodyDynamics::Math::SpatialVector::operator=(other); diff --git a/src/Utils/String.cpp b/src/Utils/String.cpp index 2e57643b..c49b837c 100644 --- a/src/Utils/String.cpp +++ b/src/Utils/String.cpp @@ -7,35 +7,37 @@ #include #include "Utils/Error.h" -biorbd::utils::String::String() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::String::String() : std::string("") { } -biorbd::utils::String::String( +utils::String::String( const char *text) : std::string(text) { } -biorbd::utils::String::String( +utils::String::String( const String &text) : std::string(text) { } -biorbd::utils::String::String( +utils::String::String( const std::basic_string &text) : std::string(text) { } -biorbd::utils::String &biorbd::utils::String::operator=( - const biorbd::utils::String &other) +utils::String &utils::String::operator=( + const utils::String &other) { if (this==&other) { // check for self-assigment return *this; @@ -45,7 +47,7 @@ biorbd::utils::String &biorbd::utils::String::operator=( return *this; } -biorbd::utils::String biorbd::utils::String::operator+( +utils::String utils::String::operator+( const char *text) { String tp = *this; @@ -53,28 +55,28 @@ biorbd::utils::String biorbd::utils::String::operator+( return tp; } -biorbd::utils::String biorbd::utils::String::operator+( +utils::String utils::String::operator+( double val) { return *this + to_string(val); } -biorbd::utils::String biorbd::utils::String::operator+( +utils::String utils::String::operator+( unsigned int val) { return *this + std::to_string(val); } -biorbd::utils::String biorbd::utils::String::operator+( +utils::String utils::String::operator+( int val) { return *this + std::to_string(val); } -biorbd::utils::String biorbd::utils::String::operator()( +utils::String utils::String::operator()( unsigned int idx) const { - biorbd::utils::Error::check(idxlength(), + utils::Error::check(idxlength(), "Index for string out of range"); char out[2]; out[0] = (*this)[idx]; @@ -82,56 +84,56 @@ biorbd::utils::String biorbd::utils::String::operator()( return out; } -biorbd::utils::String biorbd::utils::String::operator()( +utils::String utils::String::operator()( unsigned int startIdx, unsigned int lastIdx) const { - biorbd::utils::Error::check((startIdxlength() + utils::Error::check((startIdxlength() || lastIdxlength()), "Index for string out of range"); - biorbd::utils::Error::check(lastIdx>startIdx, + utils::Error::check(lastIdx>startIdx, "Second argument should be higher than first!"); char *out = static_cast(malloc(lastIdx-startIdx+2*sizeof(char))); for (unsigned int k=0; k 0 && out.back() == trailTag[0]) { out.pop_back(); @@ -163,7 +165,7 @@ biorbd::utils::String biorbd::utils::String::removeTrailing( return out; } -std::ostream &operator<<(std::ostream &os, const biorbd::utils::String &a) +std::ostream &operator<<(std::ostream &os, const utils::String &a) { os << a.c_str(); return os; diff --git a/src/Utils/Timer.cpp b/src/Utils/Timer.cpp index 76d2b629..799ccfa1 100644 --- a/src/Utils/Timer.cpp +++ b/src/Utils/Timer.cpp @@ -1,7 +1,9 @@ #define BIORBD_API_EXPORTS #include "Utils/Timer.h" -biorbd::utils::Timer::Timer(bool startNow) : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Timer::Timer(bool startNow) : m_isStarted(false), m_isPaused(false), m_start(), @@ -13,7 +15,7 @@ biorbd::utils::Timer::Timer(bool startNow) : } } -void biorbd::utils::Timer::start() +void utils::Timer::start() { m_start = std::clock(); m_totalPauseTime = 0; @@ -21,12 +23,12 @@ void biorbd::utils::Timer::start() m_isStarted = true; } // Start a timer -bool biorbd::utils::Timer::isStarted() +bool utils::Timer::isStarted() { return m_isStarted; } -void biorbd::utils::Timer::pause() +void utils::Timer::pause() { if (!m_isPaused) { m_isPaused = true; @@ -34,7 +36,7 @@ void biorbd::utils::Timer::pause() } } -void biorbd::utils::Timer::resume() +void utils::Timer::resume() { if (!m_isStarted) { start(); @@ -46,7 +48,7 @@ void biorbd::utils::Timer::resume() } } -double biorbd::utils::Timer::getLap() +double utils::Timer::getLap() { addPauseTime(); @@ -57,7 +59,7 @@ double biorbd::utils::Timer::getLap() } } -double biorbd::utils::Timer::stop() +double utils::Timer::stop() { if (m_isStarted) { m_isStarted = false; @@ -67,7 +69,7 @@ double biorbd::utils::Timer::stop() } } -void biorbd::utils::Timer::addPauseTime() +void utils::Timer::addPauseTime() { if (m_isPaused) { m_totalPauseTime += getTime(m_pauseTime); @@ -75,7 +77,7 @@ void biorbd::utils::Timer::addPauseTime() } } -double biorbd::utils::Timer::getTime(const std::clock_t& timer) +double utils::Timer::getTime(const std::clock_t& timer) { return static_cast(std::clock() - timer) / CLOCKS_PER_SEC; } diff --git a/src/Utils/Vector.cpp b/src/Utils/Vector.cpp index 1dbb6c86..d317c4dc 100644 --- a/src/Utils/Vector.cpp +++ b/src/Utils/Vector.cpp @@ -5,35 +5,37 @@ #include "Utils/String.h" #include "Utils/Vector3d.h" -biorbd::utils::Vector::Vector() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Vector::Vector() : RigidBodyDynamics::Math::VectorNd() { } -biorbd::utils::Vector::Vector( +utils::Vector::Vector( unsigned int size) : RigidBodyDynamics::Math::VectorNd(size) { } -biorbd::utils::Vector::Vector( - const biorbd::utils::Vector& other) : +utils::Vector::Vector( + const utils::Vector& other) : RigidBodyDynamics::Math::VectorNd (other) { } -biorbd::utils::Vector::Vector( +utils::Vector::Vector( const RigidBodyDynamics::Math::VectorNd &other) : RigidBodyDynamics::Math::VectorNd (other) { } -biorbd::utils::Vector::Vector( - const biorbd::utils::Vector3d& other) : +utils::Vector::Vector( + const utils::Vector3d& other) : RigidBodyDynamics::Math::VectorNd (other) { @@ -41,13 +43,13 @@ biorbd::utils::Vector::Vector( #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Vector::Vector(const casadi::MX &other) : +utils::Vector::Vector(const casadi::MX &other) : RigidBodyDynamics::Math::VectorNd(other) { } -biorbd::utils::Vector::Vector( +utils::Vector::Vector( const RBDLCasadiMath::MX_Xd_SubMatrix &other) : RigidBodyDynamics::Math::VectorNd (other) { @@ -56,21 +58,21 @@ biorbd::utils::Vector::Vector( #endif -biorbd::utils::Scalar biorbd::utils::Vector::norm( +utils::Scalar utils::Vector::norm( unsigned int p, bool skipRoot) const { - biorbd::utils::Error::check(p >= 2, "p must be superior or equal to 2"); + utils::Error::check(p >= 2, "p must be superior or equal to 2"); if (p == 2) { - biorbd::utils::Scalar n = dot(*this); + utils::Scalar n = dot(*this); if (skipRoot) { return n; } else { return std::sqrt(n); } } else { - biorbd::utils::Scalar res(0); + utils::Scalar res(0); for(unsigned int i=0; i < size(); ++i) { res += std::pow(fabs((*this)[i]), p); } @@ -82,47 +84,47 @@ biorbd::utils::Scalar biorbd::utils::Vector::norm( } } -biorbd::utils::Vector biorbd::utils::Vector::normGradient( +utils::Vector utils::Vector::normGradient( unsigned int p, bool skipRoot) { - biorbd::utils::Error::check(p >= 2, "p must be superior or equal to 2"); + utils::Error::check(p >= 2, "p must be superior or equal to 2"); if (p == 2) { if (skipRoot) { - return biorbd::utils::Vector(*this * 2.); + return utils::Vector(*this * 2.); } else { - return biorbd::utils::Vector(*this * 1.0/norm(2)); + return utils::Vector(*this * 1.0/norm(2)); } } else { - biorbd::utils::Vector res(static_cast(size())); + utils::Vector res(static_cast(size())); double normalized(std::pow(norm(), p-1)); for (unsigned int i=0; i 2"); + utils::Error::raise("skip root not implemented for p > 2"); } return res; } } -void biorbd::utils::Vector::operator=( - const biorbd::utils::Vector &other) +void utils::Vector::operator=( + const utils::Vector &other) { this->RigidBodyDynamics::Math::VectorNd::operator=(other); } #ifdef BIORBD_USE_CASADI_MATH -void biorbd::utils::Vector::operator=( +void utils::Vector::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix& other) { this->MX_Xd_dynamic::operator=(other); } -void biorbd::utils::Vector::operator=( +void utils::Vector::operator=( const casadi::MX &other) { this->MX_Xd_dynamic::operator=(other); diff --git a/src/Utils/Vector3d.cpp b/src/Utils/Vector3d.cpp index 43cb2390..06efa15d 100644 --- a/src/Utils/Vector3d.cpp +++ b/src/Utils/Vector3d.cpp @@ -4,75 +4,77 @@ #include "Utils/RotoTrans.h" #include "Utils/Vector.h" -biorbd::utils::Vector3d::Vector3d() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +utils::Vector3d::Vector3d() : RigidBodyDynamics::Math::Vector3d (RigidBodyDynamics::Math::Vector3d::Zero()), - biorbd::utils::Node () + utils::Node () { setType(); } -biorbd::utils::Vector3d::Vector3d( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z) : +utils::Vector3d::Vector3d( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z) : RigidBodyDynamics::Math::Vector3d (x, y, z), - biorbd::utils::Node () + utils::Node () { setType(); } -biorbd::utils::Vector3d::Vector3d( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : +utils::Vector3d::Vector3d( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName) : RigidBodyDynamics::Math::Vector3d (x, y, z), - biorbd::utils::Node (name, parentName) + utils::Node (name, parentName) { setType(); } -biorbd::utils::Vector3d::Vector3d( - const biorbd::utils::Vector3d vec, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : +utils::Vector3d::Vector3d( + const utils::Vector3d vec, + const utils::String &name, + const utils::String &parentName) : RigidBodyDynamics::Math::Vector3d (vec), Node(name, parentName) { } -biorbd::utils::Vector3d::Vector3d( +utils::Vector3d::Vector3d( const RigidBodyDynamics::Math::Vector3d &other) : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]), - biorbd::utils::Node () + utils::Node () { setType(); } -biorbd::utils::Vector3d::Vector3d( +utils::Vector3d::Vector3d( const RigidBodyDynamics::Math::VectorNd &other) : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]), - biorbd::utils::Node () + utils::Node () { setType(); } -biorbd::utils::Vector3d::Vector3d( +utils::Vector3d::Vector3d( const RigidBodyDynamics::Math::Vector4d &other) : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]), - biorbd::utils::Node () + utils::Node () { setType(); } #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Vector3d::Vector3d( +utils::Vector3d::Vector3d( const RBDLCasadiMath::MX_Xd_SubMatrix &other) : RigidBodyDynamics::Math::Vector3d(other), - biorbd::utils::Node () + utils::Node () { setType(); } @@ -80,21 +82,21 @@ biorbd::utils::Vector3d::Vector3d( #endif -biorbd::utils::Vector3d biorbd::utils::Vector3d::DeepCopy() const +utils::Vector3d utils::Vector3d::DeepCopy() const { - biorbd::utils::Vector3d copy; + utils::Vector3d copy; copy.DeepCopy(*this); return copy; } -void biorbd::utils::Vector3d::DeepCopy(const Vector3d &other) +void utils::Vector3d::DeepCopy(const Vector3d &other) { this->RigidBodyDynamics::Math::Vector3d::operator=(other); - biorbd::utils::Node::DeepCopy(other); + utils::Node::DeepCopy(other); } -biorbd::utils::Vector3d biorbd::utils::Vector3d::applyRT( - const biorbd::utils::RotoTrans &rt) const +utils::Vector3d utils::Vector3d::applyRT( + const utils::RotoTrans &rt) const { RigidBodyDynamics::Math::Vector4d v; v.block(0, 0, 3, 1) = *this; @@ -102,8 +104,8 @@ biorbd::utils::Vector3d biorbd::utils::Vector3d::applyRT( return (rt * v).block(0, 0, 3, 1); } -void biorbd::utils::Vector3d::applyRT( - const biorbd::utils::RotoTrans &rt) +void utils::Vector3d::applyRT( + const utils::RotoTrans &rt) { RigidBodyDynamics::Math::Vector4d v; v.block(0, 0, 3, 1) = *this; @@ -111,8 +113,8 @@ void biorbd::utils::Vector3d::applyRT( setPosition((rt * v).block(0, 0, 3, 1)); } -void biorbd::utils::Vector3d::setPosition( - const biorbd::utils::Vector3d& v) +void utils::Vector3d::setPosition( + const utils::Vector3d& v) { #ifdef BIORBD_USE_CASADI_MATH (*this)(0) = v(0); @@ -123,36 +125,36 @@ void biorbd::utils::Vector3d::setPosition( #endif } -void biorbd::utils::Vector3d::setType() +void utils::Vector3d::setType() { - *m_typeOfNode = biorbd::utils::VECTOR3D; + *m_typeOfNode = utils::VECTOR3D; } #ifdef BIORBD_USE_CASADI_MATH -biorbd::utils::Scalar biorbd::utils::Vector3d::x() const +utils::Scalar utils::Vector3d::x() const { return (*this)[0]; } -biorbd::utils::Scalar biorbd::utils::Vector3d::y() const +utils::Scalar utils::Vector3d::y() const { return (*this)[1]; } -biorbd::utils::Scalar biorbd::utils::Vector3d::z() const +utils::Scalar utils::Vector3d::z() const { return (*this)[2]; } -void biorbd::utils::Vector3d::operator=( +void utils::Vector3d::operator=( const RBDLCasadiMath::MX_Xd_SubMatrix &other) { - *this = biorbd::utils::Vector3d(other); + *this = utils::Vector3d(other); } -void biorbd::utils::Vector3d::operator=(const RigidBodyDynamics::Math::Vector4d& +void utils::Vector3d::operator=(const RigidBodyDynamics::Math::Vector4d& other) { - *this = biorbd::utils::Vector3d(other); + *this = utils::Vector3d(other); } #endif From 8f3fadbb9d9f1ab045166ac563b7a8f72be6b6cf Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 11:21:48 -0400 Subject: [PATCH 04/14] Actuators in namespace --- include/Actuators/Actuator.h | 17 +- include/Actuators/ActuatorConstant.h | 19 +- include/Actuators/ActuatorEnums.h | 5 +- include/Actuators/ActuatorGauss3p.h | 77 ++++---- include/Actuators/ActuatorGauss6p.h | 95 ++++----- include/Actuators/ActuatorLinear.h | 29 +-- include/Actuators/ActuatorSigmoidGauss3p.h | 49 ++--- include/Actuators/Actuators.h | 39 ++-- include/BiorbdModel.h | 2 +- src/Actuators/Actuator.cpp | 44 +++-- src/Actuators/ActuatorConstant.cpp | 42 ++-- src/Actuators/ActuatorGauss3p.cpp | 170 +++++++++-------- src/Actuators/ActuatorGauss6p.cpp | 198 +++++++++---------- src/Actuators/ActuatorLinear.cpp | 58 +++--- src/Actuators/ActuatorSigmoidGauss3p.cpp | 106 ++++++----- src/Actuators/Actuators.cpp | 212 +++++++++++---------- src/ModelReader.cpp | 14 +- 17 files changed, 607 insertions(+), 569 deletions(-) diff --git a/include/Actuators/Actuator.h b/include/Actuators/Actuator.h index 57a24fa2..3ff31069 100644 --- a/include/Actuators/Actuator.h +++ b/include/Actuators/Actuator.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -33,7 +35,7 @@ class BIORBD_API Actuator /// \param other The other actuator /// Actuator( - const biorbd::actuator::Actuator& other); + const Actuator& other); /// /// \brief Construct actuator @@ -53,7 +55,7 @@ class BIORBD_API Actuator Actuator( int direction, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Destroy class properly @@ -65,7 +67,7 @@ class BIORBD_API Actuator /// \param other The actuator to copy /// void DeepCopy( - const biorbd::actuator::Actuator& other); + const Actuator& other); /// /// \brief Return the index of the DoF associated with actuator @@ -83,13 +85,13 @@ class BIORBD_API Actuator /// \brief Return the type of the actuator /// \return The type of the actuator /// - biorbd::actuator::TYPE type() const; + TYPE type() const; /// /// \brief Return the maximal torque /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax() = 0; + virtual utils::Scalar torqueMax() = 0; protected: /// @@ -97,16 +99,17 @@ class BIORBD_API Actuator /// virtual void setType() = 0; - std::shared_ptr m_type; ///< The type of the actuator + std::shared_ptr m_type; ///< The type of the actuator std::shared_ptr m_direction; ///< The direction of the actuator (+1 or -1) - std::shared_ptr + std::shared_ptr m_jointName; ///< Name of the parent joint std::shared_ptr m_dofIdx;///< Index of the DoF associated with the actuator }; +} } } diff --git a/include/Actuators/ActuatorConstant.h b/include/Actuators/ActuatorConstant.h index f5bdb098..ce2695cf 100644 --- a/include/Actuators/ActuatorConstant.h +++ b/include/Actuators/ActuatorConstant.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace actuator { @@ -25,7 +27,7 @@ class BIORBD_API ActuatorConstant : public Actuator /// \param other The other constant actuator /// ActuatorConstant( - const biorbd::actuator::ActuatorConstant& other); + const ActuatorConstant& other); /// /// \brief Construct a constant actuator @@ -35,7 +37,7 @@ class BIORBD_API ActuatorConstant : public Actuator /// ActuatorConstant( int direction, - const biorbd::utils::Scalar& Tmax, + const utils::Scalar& Tmax, unsigned int dofIdx); /// @@ -47,39 +49,40 @@ class BIORBD_API ActuatorConstant : public Actuator /// ActuatorConstant( int direction, - const biorbd::utils::Scalar& Tmax, + const utils::Scalar& Tmax, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Deep copy of the constant actuator /// \return A copy of the constant actuator /// - biorbd::actuator::ActuatorConstant DeepCopy() const; + ActuatorConstant DeepCopy() const; /// /// \brief Deep copy of the constant actuator to another constant actuator /// \param other The constant actuator to copy /// void DeepCopy( - const biorbd::actuator::ActuatorConstant& other); + const ActuatorConstant& other); /// /// \brief Return the maximal torque /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax(); + virtual utils::Scalar torqueMax(); protected: /// /// \brief Set the type of the constant actuator /// virtual void setType(); - std::shared_ptr + std::shared_ptr m_Tmax; ///< Maximal torque that can be done }; +} } } diff --git a/include/Actuators/ActuatorEnums.h b/include/Actuators/ActuatorEnums.h index f2033a88..7d7a51b3 100644 --- a/include/Actuators/ActuatorEnums.h +++ b/include/Actuators/ActuatorEnums.h @@ -3,6 +3,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace actuator { @@ -23,7 +25,7 @@ enum TYPE { /// \param type The type to convert to string /// \return The name of the type /// -inline const char* TYPE_toStr(biorbd::actuator::TYPE type) +inline const char* TYPE_toStr(TYPE type) { switch (type) { case CONSTANT: @@ -42,6 +44,7 @@ inline const char* TYPE_toStr(biorbd::actuator::TYPE type) } +} } } diff --git a/include/Actuators/ActuatorGauss3p.h b/include/Actuators/ActuatorGauss3p.h index 65682f76..74426c81 100644 --- a/include/Actuators/ActuatorGauss3p.h +++ b/include/Actuators/ActuatorGauss3p.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -33,7 +35,7 @@ class BIORBD_API ActuatorGauss3p : public Actuator /// \param other The other Gauss3p actuator to copy /// ActuatorGauss3p( - const biorbd::actuator::ActuatorGauss3p& other); + const ActuatorGauss3p& other); /// /// \brief Construct Gauss3p actuator @@ -51,15 +53,15 @@ class BIORBD_API ActuatorGauss3p : public Actuator /// ActuatorGauss3p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx); /// @@ -79,17 +81,17 @@ class BIORBD_API ActuatorGauss3p : public Actuator /// ActuatorGauss3p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Destroy the class properly @@ -100,21 +102,21 @@ class BIORBD_API ActuatorGauss3p : public Actuator /// \brief Deep copy of the Gauss3p actuator /// \return A deep copy of the Gauss3p actuator /// - biorbd::actuator::ActuatorGauss3p DeepCopy() const; + ActuatorGauss3p DeepCopy() const; /// /// \brief Deep copy of the Gauss3p actuator from another Gauss3p actuator /// \param other The Gauss3p actuator to copy /// void DeepCopy( - const biorbd::actuator::ActuatorGauss3p& other); + const ActuatorGauss3p& other); /// /// \brief Return the maximal torque (invalid) /// \return The maximal torque /// torqueMax for ActuatorGauss3p must be called with Q and Qdot /// - virtual biorbd::utils::Scalar torqueMax(); + virtual utils::Scalar torqueMax(); /// /// \brief Return the maximal torque at a given Q and Qdot @@ -122,9 +124,9 @@ class BIORBD_API ActuatorGauss3p : public Actuator /// \param Qdot The generalized velocities of the actuator /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + virtual utils::Scalar torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot); protected: /// @@ -134,32 +136,33 @@ class BIORBD_API ActuatorGauss3p : public Actuator // For informations on these parameters, see Monique Iris Jackson's these from page 54 // Angular/velocity relationship - std::shared_ptr + std::shared_ptr m_k; ///< Ratio of slope of the eccentric and concentric phases - std::shared_ptr + std::shared_ptr m_Tmax; ///< Maximum torque in the eccentric phase - std::shared_ptr + std::shared_ptr m_T0; ///< Maximum torque isometric - std::shared_ptr + std::shared_ptr m_wmax; ///< Maximum angular velocity above which torque cannot be produced - std::shared_ptr + std::shared_ptr m_wc; ///< Angular velocity of the vertical asymptote of the concentric hyperbola // Activation/velocity relationship - std::shared_ptr + std::shared_ptr m_amax; ///< Maximum activation level (set to 1) - std::shared_ptr m_amin; ///< Low plateau level - std::shared_ptr + std::shared_ptr m_amin; ///< Low plateau level + std::shared_ptr m_wr; ///< 1/10 de la distance amax/amin - std::shared_ptr m_w1; ///< Mid point plateau + std::shared_ptr m_w1; ///< Mid point plateau // Torque/angle relationship - std::shared_ptr + std::shared_ptr m_r; ///< Width of the gaussian curve - std::shared_ptr m_qopt; ///< Optimal position + std::shared_ptr m_qopt; ///< Optimal position }; +} } } diff --git a/include/Actuators/ActuatorGauss6p.h b/include/Actuators/ActuatorGauss6p.h index 64df20be..84cee9e3 100644 --- a/include/Actuators/ActuatorGauss6p.h +++ b/include/Actuators/ActuatorGauss6p.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -32,7 +34,7 @@ class BIORBD_API ActuatorGauss6p : public Actuator /// \param other The other Gauss6p actuator to copy /// ActuatorGauss6p( - const biorbd::actuator::ActuatorGauss6p& other); + const ActuatorGauss6p& other); /// /// \brief Construct Gauss6p actuator @@ -53,18 +55,18 @@ class BIORBD_API ActuatorGauss6p : public Actuator /// ActuatorGauss6p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, - const biorbd::utils::Scalar& facteur, - const biorbd::utils::Scalar& r2, - const biorbd::utils::Scalar& qopt2, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, + const utils::Scalar& facteur, + const utils::Scalar& r2, + const utils::Scalar& qopt2, unsigned int dofIdx); /// @@ -87,20 +89,20 @@ class BIORBD_API ActuatorGauss6p : public Actuator /// ActuatorGauss6p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, - const biorbd::utils::Scalar& facteur, - const biorbd::utils::Scalar& r2, - const biorbd::utils::Scalar& qopt2, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, + const utils::Scalar& facteur, + const utils::Scalar& r2, + const utils::Scalar& qopt2, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Destroy the class properly @@ -111,21 +113,21 @@ class BIORBD_API ActuatorGauss6p : public Actuator /// \brief Deep copy of the Gauss6p actuator /// \return A deep copy of the Gauss6p actuator /// - biorbd::actuator::ActuatorGauss6p DeepCopy() const; + ActuatorGauss6p DeepCopy() const; /// /// \brief Deep copy of the Gauss 3p actuator from another Gauss6p actuator /// \param other The Gauss6p actuator to copy /// void DeepCopy( - const biorbd::actuator::ActuatorGauss6p& other); + const ActuatorGauss6p& other); /// /// \brief Return the maximal torque (invalid) /// \return The maximal torque /// torqueMax for ActuatorGauss6p must be called with Q and Qdot /// - virtual biorbd::utils::Scalar torqueMax(); + virtual utils::Scalar torqueMax(); /// /// \brief Return the maximal torque at a given Q and Qdot @@ -133,9 +135,9 @@ class BIORBD_API ActuatorGauss6p : public Actuator /// \param Qdot The generalized velocities of the actuator /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + virtual utils::Scalar torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot); protected: /// @@ -145,36 +147,37 @@ class BIORBD_API ActuatorGauss6p : public Actuator // For informations on these parameters, see Monique Iris Jackson's these from page 54 // Angular/velocity relationship - std::shared_ptr + std::shared_ptr m_k; ///< Ratio of slope of the eccentric and concentric phases - std::shared_ptr + std::shared_ptr m_Tmax; ///< Maximum torque in the eccentric phase - std::shared_ptr + std::shared_ptr m_T0; ///< Maximum torque isometric - std::shared_ptr + std::shared_ptr m_wmax; ///< Maximum angular velocity above which torque cannot be produced - std::shared_ptr + std::shared_ptr m_wc; ///< Angular velocity of the vertical asymptote of the concentric hyperbola // Activation/velocity relationship - std::shared_ptr + std::shared_ptr m_amax; ///< Maximum activation level (set to 1) - std::shared_ptr m_amin; ///< Low plateau level - std::shared_ptr + std::shared_ptr m_amin; ///< Low plateau level + std::shared_ptr m_wr; ///< 1/10 of the distance amax/amin - std::shared_ptr m_w1; ///< Mid point plateau + std::shared_ptr m_w1; ///< Mid point plateau // Torque/angle relationship - std::shared_ptr + std::shared_ptr m_r; ///< width of the 1st gaussian curve - std::shared_ptr m_qopt; ///< 1st Optimal position - std::shared_ptr m_facteur; ///< Factor of the 6p - std::shared_ptr + std::shared_ptr m_qopt; ///< 1st Optimal position + std::shared_ptr m_facteur; ///< Factor of the 6p + std::shared_ptr m_r2; ///< width of the 2nd gaussian curve - std::shared_ptr m_qopt2; ///< 2nd Optimal position + std::shared_ptr m_qopt2; ///< 2nd Optimal position }; +} } } diff --git a/include/Actuators/ActuatorLinear.h b/include/Actuators/ActuatorLinear.h index 2e9e2349..ad6c08cf 100644 --- a/include/Actuators/ActuatorLinear.h +++ b/include/Actuators/ActuatorLinear.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -30,7 +32,7 @@ class BIORBD_API ActuatorLinear : public Actuator /// \param other The other linear actuator /// ActuatorLinear( - const biorbd::actuator::ActuatorLinear& other); + const ActuatorLinear& other); /// /// \brief Construct a linear actuator @@ -41,8 +43,8 @@ class BIORBD_API ActuatorLinear : public Actuator /// ActuatorLinear( int direction, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& slope, + const utils::Scalar& T0, + const utils::Scalar& slope, unsigned int dofIdx); /// @@ -56,10 +58,10 @@ class BIORBD_API ActuatorLinear : public Actuator ActuatorLinear( int direction, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& slope, + const utils::Scalar& T0, + const utils::Scalar& slope, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Destroy the class properly @@ -70,29 +72,29 @@ class BIORBD_API ActuatorLinear : public Actuator /// \brief Deep copy of the linear actuator /// \return A deep copy of the linear actuator /// - biorbd::actuator::ActuatorLinear DeepCopy() const; + ActuatorLinear DeepCopy() const; /// /// \brief Deep copy of the linear actuator from another linear actuator /// \param other The linear actuator to copy /// void DeepCopy( - const biorbd::actuator::ActuatorLinear& other); + const ActuatorLinear& other); /// /// \brief Return the maximal torque (invalid) /// \return The maximal torque /// torqueMax for ActuatorLinear must be called with Q /// - virtual biorbd::utils::Scalar torqueMax(); + virtual utils::Scalar torqueMax(); /// /// \brief Return the maximal torque at a given Q /// \param Q The generalized coordinates of the actuator /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q) const; + virtual utils::Scalar torqueMax( + const rigidbody::GeneralizedCoordinates &Q) const; protected: @@ -102,11 +104,12 @@ class BIORBD_API ActuatorLinear : public Actuator virtual void setType(); // mx+b - std::shared_ptr m_m; ///< Slope - std::shared_ptr m_b; ///< Torque at zero + std::shared_ptr m_m; ///< Slope + std::shared_ptr m_b; ///< Torque at zero }; +} } } diff --git a/include/Actuators/ActuatorSigmoidGauss3p.h b/include/Actuators/ActuatorSigmoidGauss3p.h index 21435021..a100f68d 100644 --- a/include/Actuators/ActuatorSigmoidGauss3p.h +++ b/include/Actuators/ActuatorSigmoidGauss3p.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class GeneralizedCoordinates; @@ -33,7 +35,7 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator /// \param other The other SigmoidGauss3p actuator to copy /// ActuatorSigmoidGauss3p( - const biorbd::actuator::ActuatorSigmoidGauss3p& other); + const ActuatorSigmoidGauss3p& other); /// /// \brief Construct SigmoidGauss3p actuator @@ -47,11 +49,11 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator /// ActuatorSigmoidGauss3p( int direction, - const biorbd::utils::Scalar& theta, - const biorbd::utils::Scalar& lambda, - const biorbd::utils::Scalar& offset, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& theta, + const utils::Scalar& lambda, + const utils::Scalar& offset, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx); /// @@ -67,13 +69,13 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator /// ActuatorSigmoidGauss3p( int direction, - const biorbd::utils::Scalar& theta, - const biorbd::utils::Scalar& lambda, - const biorbd::utils::Scalar& offset, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& theta, + const utils::Scalar& lambda, + const utils::Scalar& offset, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx, - const biorbd::utils::String &jointName); + const utils::String &jointName); /// /// \brief Destroy the class properly @@ -84,21 +86,21 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator /// \brief Deep copy of the SigmoidGauss3p actuator /// \return A deep copy of the SigmoidGauss3p actuator /// - biorbd::actuator::ActuatorSigmoidGauss3p DeepCopy() const; + ActuatorSigmoidGauss3p DeepCopy() const; /// /// \brief Deep copy of the SigmoidGauss3p actuator from another SigmoidGauss3p actuator /// \param other The SigmoidGauss3p actuator to copy /// void DeepCopy( - const biorbd::actuator::ActuatorSigmoidGauss3p& other); + const ActuatorSigmoidGauss3p& other); /// /// \brief Return the maximal torque (invalid) /// \return The maximal torque /// torqueMax for ActuatorSigmoidGauss3p must be called with Q and Qdot /// - virtual biorbd::utils::Scalar torqueMax(); + virtual utils::Scalar torqueMax(); /// /// \brief Return the maximal torque at a given Q and Qdot @@ -106,9 +108,9 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator /// \param Qdot The generalized velocities of the actuator /// \return The maximal torque /// - virtual biorbd::utils::Scalar torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + virtual utils::Scalar torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot); protected: /// @@ -118,18 +120,19 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator // Coefficients needed for the SigmoidGauss3p // Torque/angle relationship for the Gauss3p - std::shared_ptr + std::shared_ptr m_r; ///< Width of the gaussian curve - std::shared_ptr m_qopt; ///< Optimal position + std::shared_ptr m_qopt; ///< Optimal position // Coefficients needed for the Sigmoid - std::shared_ptr + std::shared_ptr m_theta; ///< Amplitude of the Sigmoid - std::shared_ptr + std::shared_ptr m_lambda; ///< Tilt factor of the Sigmoid - std::shared_ptr m_offset; ///< Height of the Sigmoid + std::shared_ptr m_offset; ///< Height of the Sigmoid }; +} } } diff --git a/include/Actuators/Actuators.h b/include/Actuators/Actuators.h index 6c2c3823..c1ce0a5b 100644 --- a/include/Actuators/Actuators.h +++ b/include/Actuators/Actuators.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector; @@ -39,7 +41,7 @@ class BIORBD_API Actuators /// \param other The other actuators /// Actuators( - const biorbd::actuator::Actuators& other); + const Actuators& other); /// /// \brief Destroy actuators class properly @@ -51,14 +53,14 @@ class BIORBD_API Actuators /// \param other The other actuators /// void DeepCopy( - const biorbd::actuator::Actuators& other); + const Actuators& other); /// /// \brief Add an actuator to the set of actuators /// \param a The actuator to add /// void addActuator( - const biorbd::actuator::Actuator &a); + const Actuator &a); /// /// \brief Indicate to biorbd to are done adding actuators, sanity checks are performed @@ -71,10 +73,10 @@ class BIORBD_API Actuators /// \param Qdot The generalized velocities of the actuators /// \return Two vectors of maximal torque /// - std::pair + std::pair torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot); + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot); /// /// \brief Return the maximal generalized torque @@ -83,10 +85,10 @@ class BIORBD_API Actuators /// \param Qdot The generalized velocities of the actuators /// \return The maximal generalized torque /// - biorbd::rigidbody::GeneralizedTorque torqueMax( - const biorbd::utils::Vector &activation, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + rigidbody::GeneralizedTorque torqueMax( + const utils::Vector &activation, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity &Qdot); /// /// \brief Return the generalized torque @@ -95,10 +97,10 @@ class BIORBD_API Actuators /// \param Qdot The generalized velocities of the actuators /// \return The maximal generalized torque /// - biorbd::rigidbody::GeneralizedTorque torque( - const biorbd::utils::Vector &activation, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + rigidbody::GeneralizedTorque torque( + const utils::Vector &activation, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity &Qdot); // Get and set /// @@ -106,7 +108,7 @@ class BIORBD_API Actuators /// \param dof Index of the DoF associated with actuator /// \return The actuator /// - const std::pair, std::shared_ptr>& + const std::pair, std::shared_ptr>& actuator(unsigned int dof); /// @@ -115,7 +117,7 @@ class BIORBD_API Actuators /// \param concentric If the return value is the concentric (true) or eccentric (false) value /// \return The actuator /// - const biorbd::actuator::Actuator& actuator(unsigned int dof, bool concentric); + const Actuator& actuator(unsigned int dof, bool concentric); /// /// \brief Return the toal number of actuators @@ -124,7 +126,7 @@ class BIORBD_API Actuators unsigned int nbActuators() const; protected: - std::shared_ptr, std::shared_ptr>>> + std::shared_ptr, std::shared_ptr>>> m_all; ///> m_isDofSet;///< If DoF all dof are set std::shared_ptr m_isClose; ///< If the set is ready @@ -136,13 +138,14 @@ class BIORBD_API Actuators /// \param Qdot The Generalized velocity /// \return The torque max /// - biorbd::utils::Scalar getTorqueMaxDirection( + utils::Scalar getTorqueMaxDirection( const std::shared_ptr actuator, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot) const; }; +} } } diff --git a/include/BiorbdModel.h b/include/BiorbdModel.h index caf1421d..2bb86f90 100644 --- a/include/BiorbdModel.h +++ b/include/BiorbdModel.h @@ -73,7 +73,7 @@ class BIORBD_API Model : ,public rigidbody::RotoTransNodes ,public rigidbody::Contacts #ifdef MODULE_ACTUATORS - ,public biorbd::actuator::Actuators + ,public actuator::Actuators #endif #ifdef MODULE_MUSCLES ,public biorbd::muscles::Muscles diff --git a/src/Actuators/Actuator.cpp b/src/Actuators/Actuator.cpp index ca0afd2b..b66f3099 100644 --- a/src/Actuators/Actuator.cpp +++ b/src/Actuators/Actuator.cpp @@ -4,18 +4,20 @@ #include "Utils/Error.h" #include "Utils/String.h" -biorbd::actuator::Actuator::Actuator() : - m_type(std::make_shared - (biorbd::actuator::TYPE::NO_TYPE)), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::Actuator::Actuator() : + m_type(std::make_shared + (actuator::TYPE::NO_TYPE)), m_direction(std::make_shared(0)), - m_jointName(std::make_shared("")), + m_jointName(std::make_shared("")), m_dofIdx(std::make_shared(-1)) { } -biorbd::actuator::Actuator::Actuator( - const biorbd::actuator::Actuator &other) : +actuator::Actuator::Actuator( + const actuator::Actuator &other) : m_type(other.m_type), m_direction(other.m_direction), m_jointName(other.m_jointName), @@ -24,38 +26,38 @@ biorbd::actuator::Actuator::Actuator( } -biorbd::actuator::Actuator::Actuator( +actuator::Actuator::Actuator( int direction, unsigned int dofIdx) : - m_type(std::make_shared - (biorbd::actuator::TYPE::NO_TYPE)), + m_type(std::make_shared + (actuator::TYPE::NO_TYPE)), m_direction(std::make_shared(direction)), - m_jointName(std::make_shared("")), + m_jointName(std::make_shared("")), m_dofIdx(std::make_shared(dofIdx)) { } -biorbd::actuator::Actuator::Actuator( +actuator::Actuator::Actuator( int direction, unsigned int dofIdx, - const biorbd::utils::String &jointName) : - m_type(std::make_shared - (biorbd::actuator::TYPE::NO_TYPE)), + const utils::String &jointName) : + m_type(std::make_shared + (actuator::TYPE::NO_TYPE)), m_direction(std::make_shared(direction)), - m_jointName(std::make_shared(jointName)), + m_jointName(std::make_shared(jointName)), m_dofIdx(std::make_shared(dofIdx)) { - biorbd::utils::Error::check(*m_direction==-1 || *m_direction==1, + utils::Error::check(*m_direction==-1 || *m_direction==1, "Direction should be -1 or 1"); } -biorbd::actuator::Actuator::~Actuator() +actuator::Actuator::~Actuator() { } -void biorbd::actuator::Actuator::DeepCopy(const biorbd::actuator::Actuator +void actuator::Actuator::DeepCopy(const actuator::Actuator &other) { *m_type = *other.m_type; @@ -64,17 +66,17 @@ void biorbd::actuator::Actuator::DeepCopy(const biorbd::actuator::Actuator *m_dofIdx = *other.m_dofIdx; } -unsigned int biorbd::actuator::Actuator::index() const +unsigned int actuator::Actuator::index() const { return *m_dofIdx; } -int biorbd::actuator::Actuator::direction() const +int actuator::Actuator::direction() const { return *m_direction; } -biorbd::actuator::TYPE biorbd::actuator::Actuator::type() const +actuator::TYPE actuator::Actuator::type() const { return *m_type; } diff --git a/src/Actuators/ActuatorConstant.cpp b/src/Actuators/ActuatorConstant.cpp index c5dd6c4c..2397747b 100644 --- a/src/Actuators/ActuatorConstant.cpp +++ b/src/Actuators/ActuatorConstant.cpp @@ -4,63 +4,65 @@ #include "Utils/String.h" #include "RigidBody/GeneralizedCoordinates.h" -biorbd::actuator::ActuatorConstant::ActuatorConstant() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::ActuatorConstant::ActuatorConstant() : Actuator(), - m_Tmax(std::make_shared(0)) + m_Tmax(std::make_shared(0)) { setType(); } -biorbd::actuator::ActuatorConstant::ActuatorConstant( - const biorbd::actuator::ActuatorConstant &other) : +actuator::ActuatorConstant::ActuatorConstant( + const actuator::ActuatorConstant &other) : Actuator(other), m_Tmax(other.m_Tmax) { } -biorbd::actuator::ActuatorConstant::ActuatorConstant( +actuator::ActuatorConstant::ActuatorConstant( int direction, - const biorbd::utils::Scalar& Tmax, + const utils::Scalar& Tmax, unsigned int dofIdx) : Actuator(direction, dofIdx), - m_Tmax(std::make_shared(Tmax)) + m_Tmax(std::make_shared(Tmax)) { setType(); } -biorbd::actuator::ActuatorConstant::ActuatorConstant( +actuator::ActuatorConstant::ActuatorConstant( int direction, - const biorbd::utils::Scalar& Tmax, + const utils::Scalar& Tmax, unsigned int dofIdx, - const biorbd::utils::String &jointName) : + const utils::String &jointName) : Actuator(direction, dofIdx, jointName), - m_Tmax(std::make_shared(Tmax)) + m_Tmax(std::make_shared(Tmax)) { setType(); } -biorbd::actuator::ActuatorConstant -biorbd::actuator::ActuatorConstant::DeepCopy() const +actuator::ActuatorConstant +actuator::ActuatorConstant::DeepCopy() const { - biorbd::actuator::ActuatorConstant copy; + actuator::ActuatorConstant copy; copy.DeepCopy(*this); return copy; } -void biorbd::actuator::ActuatorConstant::DeepCopy(const - biorbd::actuator::ActuatorConstant &other) +void actuator::ActuatorConstant::DeepCopy(const + actuator::ActuatorConstant &other) { - biorbd::actuator::Actuator::DeepCopy(other); + actuator::Actuator::DeepCopy(other); *m_Tmax = *other.m_Tmax; } -biorbd::utils::Scalar biorbd::actuator::ActuatorConstant::torqueMax() +utils::Scalar actuator::ActuatorConstant::torqueMax() { return *m_Tmax; } -void biorbd::actuator::ActuatorConstant::setType() +void actuator::ActuatorConstant::setType() { - *m_type = biorbd::actuator::TYPE::CONSTANT; + *m_type = actuator::TYPE::CONSTANT; } diff --git a/src/Actuators/ActuatorGauss3p.cpp b/src/Actuators/ActuatorGauss3p.cpp index e340e7ed..4c608b5b 100644 --- a/src/Actuators/ActuatorGauss3p.cpp +++ b/src/Actuators/ActuatorGauss3p.cpp @@ -5,26 +5,28 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -biorbd::actuator::ActuatorGauss3p::ActuatorGauss3p() : - biorbd::actuator::Actuator(), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(0)), - m_T0(std::make_shared(0)), - m_wmax(std::make_shared(0)), - m_wc(std::make_shared(0)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(0)), - m_wr(std::make_shared(0)), - m_w1(std::make_shared(0)), - m_r(std::make_shared(0)), - m_qopt(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::ActuatorGauss3p::ActuatorGauss3p() : + actuator::Actuator(), + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(0)), + m_T0(std::make_shared(0)), + m_wmax(std::make_shared(0)), + m_wc(std::make_shared(0)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(0)), + m_wr(std::make_shared(0)), + m_w1(std::make_shared(0)), + m_r(std::make_shared(0)), + m_qopt(std::make_shared(0)) { setType(); } -biorbd::actuator::ActuatorGauss3p::ActuatorGauss3p( - const biorbd::actuator::ActuatorGauss3p &other) : - biorbd::actuator::Actuator(other), +actuator::ActuatorGauss3p::ActuatorGauss3p( + const actuator::ActuatorGauss3p &other) : + actuator::Actuator(other), m_k(other.m_k), m_Tmax(other.m_Tmax), m_T0(other.m_T0), @@ -40,80 +42,80 @@ biorbd::actuator::ActuatorGauss3p::ActuatorGauss3p( } -biorbd::actuator::ActuatorGauss3p::ActuatorGauss3p( +actuator::ActuatorGauss3p::ActuatorGauss3p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx) : - biorbd::actuator::Actuator(direction, dofIdx), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(Tmax)), - m_T0(std::make_shared(T0)), - m_wmax(std::make_shared(wmax)), - m_wc(std::make_shared(wc)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(amin)), - m_wr(std::make_shared(wr)), - m_w1(std::make_shared(w1)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)) + actuator::Actuator(direction, dofIdx), + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(Tmax)), + m_T0(std::make_shared(T0)), + m_wmax(std::make_shared(wmax)), + m_wc(std::make_shared(wc)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(amin)), + m_wr(std::make_shared(wr)), + m_w1(std::make_shared(w1)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)) { setType(); } -biorbd::actuator::ActuatorGauss3p::ActuatorGauss3p( +actuator::ActuatorGauss3p::ActuatorGauss3p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx, - const biorbd::utils::String &jointName) : - biorbd::actuator::Actuator(direction, dofIdx, jointName), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(Tmax)), - m_T0(std::make_shared(T0)), - m_wmax(std::make_shared(wmax)), - m_wc(std::make_shared(wc)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(amin)), - m_wr(std::make_shared(wr)), - m_w1(std::make_shared(w1)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)) + const utils::String &jointName) : + actuator::Actuator(direction, dofIdx, jointName), + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(Tmax)), + m_T0(std::make_shared(T0)), + m_wmax(std::make_shared(wmax)), + m_wc(std::make_shared(wc)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(amin)), + m_wr(std::make_shared(wr)), + m_w1(std::make_shared(w1)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)) { setType(); } -biorbd::actuator::ActuatorGauss3p::~ActuatorGauss3p() +actuator::ActuatorGauss3p::~ActuatorGauss3p() { } -biorbd::actuator::ActuatorGauss3p biorbd::actuator::ActuatorGauss3p::DeepCopy() +actuator::ActuatorGauss3p actuator::ActuatorGauss3p::DeepCopy() const { - biorbd::actuator::ActuatorGauss3p copy; + actuator::ActuatorGauss3p copy; copy.DeepCopy(*this); return copy; } -void biorbd::actuator::ActuatorGauss3p::DeepCopy( - const biorbd::actuator::ActuatorGauss3p &other) +void actuator::ActuatorGauss3p::DeepCopy( + const actuator::ActuatorGauss3p &other) { - biorbd::actuator::Actuator::DeepCopy(other); + actuator::Actuator::DeepCopy(other); *m_k = *other.m_k; *m_Tmax = *other.m_Tmax; *m_T0 = *other.m_T0; @@ -127,28 +129,28 @@ void biorbd::actuator::ActuatorGauss3p::DeepCopy( *m_qopt = *other.m_qopt; } -biorbd::utils::Scalar biorbd::actuator::ActuatorGauss3p::torqueMax() +utils::Scalar actuator::ActuatorGauss3p::torqueMax() { - biorbd::utils::Error::raise( + utils::Error::raise( "torqueMax for ActuatorGauss3p must be called with Q and Qdot"); } -biorbd::utils::Scalar biorbd::actuator::ActuatorGauss3p::torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +utils::Scalar actuator::ActuatorGauss3p::torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot) { - biorbd::utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); - biorbd::utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); + utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); + utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); // Tetanic torque max - biorbd::utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax; - biorbd::utils::Scalar C = Tc * (*m_wmax + *m_wc); // concentric - biorbd::utils::Scalar we = + utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax; + utils::Scalar C = Tc * (*m_wmax + *m_wc); // concentric + utils::Scalar we = ( (*m_Tmax - *m_T0) * *m_wmax * *m_wc ) / ( *m_k * *m_T0 * (*m_wmax + *m_wc) ); - biorbd::utils::Scalar E = -( *m_Tmax - *m_T0 ) * we; // excentric + utils::Scalar E = -( *m_Tmax - *m_T0 ) * we; // excentric - biorbd::utils::Scalar Tw; + utils::Scalar Tw; #ifdef BIORBD_USE_CASADI_MATH Tw = casadi::MX::if_else(casadi::MX::ge(speed, 0), C / ( *m_wc + speed ) - Tc, @@ -163,12 +165,12 @@ biorbd::utils::Scalar biorbd::actuator::ActuatorGauss3p::torqueMax( // Differential activation - biorbd::utils::Scalar A = + utils::Scalar A = *m_amin + ( *m_amax - *m_amin ) - / ( 1 + biorbd::utils::Scalar(exp( -(speed - *m_w1) / *m_wr )) ); + / ( 1 + utils::Scalar(exp( -(speed - *m_w1) / *m_wr )) ); // Torque angle - biorbd::utils::Scalar Ta = exp( -(*m_qopt - pos) * (*m_qopt - pos) / + utils::Scalar Ta = exp( -(*m_qopt - pos) * (*m_qopt - pos) / (2 * *m_r * *m_r) ); // Calculation of the max torque @@ -176,7 +178,7 @@ biorbd::utils::Scalar biorbd::actuator::ActuatorGauss3p::torqueMax( } -void biorbd::actuator::ActuatorGauss3p::setType() +void actuator::ActuatorGauss3p::setType() { - *m_type = biorbd::actuator::TYPE::GAUSS3P; + *m_type = actuator::TYPE::GAUSS3P; } diff --git a/src/Actuators/ActuatorGauss6p.cpp b/src/Actuators/ActuatorGauss6p.cpp index 7c710c5d..29adf395 100644 --- a/src/Actuators/ActuatorGauss6p.cpp +++ b/src/Actuators/ActuatorGauss6p.cpp @@ -5,29 +5,31 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -biorbd::actuator::ActuatorGauss6p::ActuatorGauss6p() : - biorbd::actuator::Actuator(), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(0)), - m_T0(std::make_shared(0)), - m_wmax(std::make_shared(0)), - m_wc(std::make_shared(0)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(0)), - m_wr(std::make_shared(0)), - m_w1(std::make_shared(0)), - m_r(std::make_shared(0)), - m_qopt(std::make_shared(0)), - m_facteur(std::make_shared(0)), - m_r2(std::make_shared(0)), - m_qopt2(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::ActuatorGauss6p::ActuatorGauss6p() : + actuator::Actuator(), + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(0)), + m_T0(std::make_shared(0)), + m_wmax(std::make_shared(0)), + m_wc(std::make_shared(0)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(0)), + m_wr(std::make_shared(0)), + m_w1(std::make_shared(0)), + m_r(std::make_shared(0)), + m_qopt(std::make_shared(0)), + m_facteur(std::make_shared(0)), + m_r2(std::make_shared(0)), + m_qopt2(std::make_shared(0)) { setType(); } -biorbd::actuator::ActuatorGauss6p::ActuatorGauss6p( - const biorbd::actuator::ActuatorGauss6p &other) : - biorbd::actuator::Actuator(other), +actuator::ActuatorGauss6p::ActuatorGauss6p( + const actuator::ActuatorGauss6p &other) : + actuator::Actuator(other), m_k(other.m_k), m_Tmax(other.m_Tmax), m_T0(other.m_T0), @@ -45,92 +47,92 @@ biorbd::actuator::ActuatorGauss6p::ActuatorGauss6p( { } -biorbd::actuator::ActuatorGauss6p::ActuatorGauss6p( +actuator::ActuatorGauss6p::ActuatorGauss6p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, - const biorbd::utils::Scalar& facteur, - const biorbd::utils::Scalar& r2, - const biorbd::utils::Scalar& qopt2, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, + const utils::Scalar& facteur, + const utils::Scalar& r2, + const utils::Scalar& qopt2, unsigned int dofIdx) : Actuator(direction, dofIdx), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(Tmax)), - m_T0(std::make_shared(T0)), - m_wmax(std::make_shared(wmax)), - m_wc(std::make_shared(wc)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(amin)), - m_wr(std::make_shared(wr)), - m_w1(std::make_shared(w1)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)), - m_facteur(std::make_shared(facteur)), - m_r2(std::make_shared(r2)), - m_qopt2(std::make_shared(qopt2)) + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(Tmax)), + m_T0(std::make_shared(T0)), + m_wmax(std::make_shared(wmax)), + m_wc(std::make_shared(wc)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(amin)), + m_wr(std::make_shared(wr)), + m_w1(std::make_shared(w1)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)), + m_facteur(std::make_shared(facteur)), + m_r2(std::make_shared(r2)), + m_qopt2(std::make_shared(qopt2)) { setType(); } -biorbd::actuator::ActuatorGauss6p::ActuatorGauss6p( +actuator::ActuatorGauss6p::ActuatorGauss6p( int direction, - const biorbd::utils::Scalar& Tmax, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& wmax, - const biorbd::utils::Scalar& wc, - const biorbd::utils::Scalar& amin, - const biorbd::utils::Scalar& wr, - const biorbd::utils::Scalar& w1, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, - const biorbd::utils::Scalar& facteur, - const biorbd::utils::Scalar& r2, - const biorbd::utils::Scalar& qopt2, + const utils::Scalar& Tmax, + const utils::Scalar& T0, + const utils::Scalar& wmax, + const utils::Scalar& wc, + const utils::Scalar& amin, + const utils::Scalar& wr, + const utils::Scalar& w1, + const utils::Scalar& r, + const utils::Scalar& qopt, + const utils::Scalar& facteur, + const utils::Scalar& r2, + const utils::Scalar& qopt2, unsigned int dofIdx, - const biorbd::utils::String &jointName) : + const utils::String &jointName) : Actuator(direction, dofIdx, jointName), - m_k(std::make_shared(4.3)), - m_Tmax(std::make_shared(Tmax)), - m_T0(std::make_shared(T0)), - m_wmax(std::make_shared(wmax)), - m_wc(std::make_shared(wc)), - m_amax(std::make_shared(1.0)), - m_amin(std::make_shared(amin)), - m_wr(std::make_shared(wr)), - m_w1(std::make_shared(w1)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)), - m_facteur(std::make_shared(facteur)), - m_r2(std::make_shared(r2)), - m_qopt2(std::make_shared(qopt2)) + m_k(std::make_shared(4.3)), + m_Tmax(std::make_shared(Tmax)), + m_T0(std::make_shared(T0)), + m_wmax(std::make_shared(wmax)), + m_wc(std::make_shared(wc)), + m_amax(std::make_shared(1.0)), + m_amin(std::make_shared(amin)), + m_wr(std::make_shared(wr)), + m_w1(std::make_shared(w1)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)), + m_facteur(std::make_shared(facteur)), + m_r2(std::make_shared(r2)), + m_qopt2(std::make_shared(qopt2)) { setType(); } -biorbd::actuator::ActuatorGauss6p::~ActuatorGauss6p() +actuator::ActuatorGauss6p::~ActuatorGauss6p() { } -biorbd::actuator::ActuatorGauss6p biorbd::actuator::ActuatorGauss6p::DeepCopy() +actuator::ActuatorGauss6p actuator::ActuatorGauss6p::DeepCopy() const { - biorbd::actuator::ActuatorGauss6p copy; + actuator::ActuatorGauss6p copy; copy.DeepCopy(*this); return copy; } -void biorbd::actuator::ActuatorGauss6p::DeepCopy( - const biorbd::actuator::ActuatorGauss6p &other) +void actuator::ActuatorGauss6p::DeepCopy( + const actuator::ActuatorGauss6p &other) { - biorbd::actuator::Actuator::DeepCopy(other); + actuator::Actuator::DeepCopy(other); *m_k = *other.m_k; *m_Tmax = *other.m_Tmax; *m_T0 = *other.m_T0; @@ -147,26 +149,26 @@ void biorbd::actuator::ActuatorGauss6p::DeepCopy( *m_qopt2 = *other.m_qopt2; } -biorbd::utils::Scalar biorbd::actuator::ActuatorGauss6p::torqueMax() +utils::Scalar actuator::ActuatorGauss6p::torqueMax() { - biorbd::utils::Error::raise("torqueMax for ActuatorGauss6p must be called with Q and Qdot"); + utils::Error::raise("torqueMax for ActuatorGauss6p must be called with Q and Qdot"); } -biorbd::utils::Scalar biorbd::actuator::ActuatorGauss6p::torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +utils::Scalar actuator::ActuatorGauss6p::torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot) { - biorbd::utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); - biorbd::utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); + utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); + utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); // Tetanic torque max - biorbd::utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax; - biorbd::utils::Scalar C = Tc*(*m_wmax + *m_wc); // concentric - biorbd::utils::Scalar we = ( (*m_Tmax - *m_T0) * *m_wmax * *m_wc ) / + utils::Scalar Tc = *m_T0 * *m_wc / *m_wmax; + utils::Scalar C = Tc*(*m_wmax + *m_wc); // concentric + utils::Scalar we = ( (*m_Tmax - *m_T0) * *m_wmax * *m_wc ) / ( *m_k * *m_T0 * (*m_wmax + *m_wc) ); - biorbd::utils::Scalar E = -( *m_Tmax - *m_T0 ) * we; // eccentric + utils::Scalar E = -( *m_Tmax - *m_T0 ) * we; // eccentric - biorbd::utils::Scalar Tw; + utils::Scalar Tw; #ifdef BIORBD_USE_CASADI_MATH Tw = casadi::MX::if_else(casadi::MX::ge(speed, 0), C / ( *m_wc + speed ) - Tc, @@ -180,21 +182,21 @@ biorbd::utils::Scalar biorbd::actuator::ActuatorGauss6p::torqueMax( #endif // Differential activation - biorbd::utils::Scalar A = + utils::Scalar A = *m_amin + ( *m_amax - *m_amin ) - / ( 1 + biorbd::utils::Scalar(exp( -(speed - *m_w1) / *m_wr )) ); + / ( 1 + utils::Scalar(exp( -(speed - *m_w1) / *m_wr )) ); // Torque angle - biorbd::utils::Scalar Ta = biorbd::utils::Scalar(exp( -(*m_qopt - pos) * + utils::Scalar Ta = utils::Scalar(exp( -(*m_qopt - pos) * (*m_qopt - pos) / (2* *m_r * *m_r ) )) - + *m_facteur * biorbd::utils::Scalar(exp( -(*m_qopt2 - pos) * + + *m_facteur * utils::Scalar(exp( -(*m_qopt2 - pos) * (*m_qopt2 - pos) / (2 * *m_r2 * *m_r2) )); // Calculation of the max torque return Tw * A * Ta; } -void biorbd::actuator::ActuatorGauss6p::setType() +void actuator::ActuatorGauss6p::setType() { - *m_type = biorbd::actuator::TYPE::GAUSS6P; + *m_type = actuator::TYPE::GAUSS6P; } diff --git a/src/Actuators/ActuatorLinear.cpp b/src/Actuators/ActuatorLinear.cpp index ec6edb17..fe2de7b6 100644 --- a/src/Actuators/ActuatorLinear.cpp +++ b/src/Actuators/ActuatorLinear.cpp @@ -4,16 +4,18 @@ #include "Utils/Error.h" #include "RigidBody/GeneralizedCoordinates.h" -biorbd::actuator::ActuatorLinear::ActuatorLinear() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::ActuatorLinear::ActuatorLinear() : Actuator(), - m_m(std::make_shared(0)), - m_b(std::make_shared(0)) + m_m(std::make_shared(0)), + m_b(std::make_shared(0)) { setType(); } -biorbd::actuator::ActuatorLinear::ActuatorLinear( - const biorbd::actuator::ActuatorLinear &other) : +actuator::ActuatorLinear::ActuatorLinear( + const actuator::ActuatorLinear &other) : Actuator(other), m_m(other.m_m), m_b(other.m_b) @@ -21,65 +23,65 @@ biorbd::actuator::ActuatorLinear::ActuatorLinear( } -biorbd::actuator::ActuatorLinear::ActuatorLinear( +actuator::ActuatorLinear::ActuatorLinear( int direction, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& slope, + const utils::Scalar& T0, + const utils::Scalar& slope, unsigned int dofIdx) : Actuator(direction, dofIdx), - m_m(std::make_shared(slope)), - m_b(std::make_shared(T0)) + m_m(std::make_shared(slope)), + m_b(std::make_shared(T0)) { setType(); } -biorbd::actuator::ActuatorLinear::ActuatorLinear( +actuator::ActuatorLinear::ActuatorLinear( int direction, - const biorbd::utils::Scalar& T0, - const biorbd::utils::Scalar& slope, + const utils::Scalar& T0, + const utils::Scalar& slope, unsigned int dofIdx, - const biorbd::utils::String &jointName) : + const utils::String &jointName) : Actuator(direction, dofIdx, jointName), - m_m(std::make_shared(slope)), - m_b(std::make_shared(T0)) + m_m(std::make_shared(slope)), + m_b(std::make_shared(T0)) { setType(); } -biorbd::actuator::ActuatorLinear::~ActuatorLinear() +actuator::ActuatorLinear::~ActuatorLinear() { } -biorbd::actuator::ActuatorLinear biorbd::actuator::ActuatorLinear::DeepCopy() +actuator::ActuatorLinear actuator::ActuatorLinear::DeepCopy() const { - biorbd::actuator::ActuatorLinear copy; + actuator::ActuatorLinear copy; copy.DeepCopy(*this); return copy; } -void biorbd::actuator::ActuatorLinear::DeepCopy(const - biorbd::actuator::ActuatorLinear &other) +void actuator::ActuatorLinear::DeepCopy(const + actuator::ActuatorLinear &other) { - biorbd::actuator::Actuator::DeepCopy(other); + actuator::Actuator::DeepCopy(other); *m_m = *other.m_m; *m_b = *other.m_b; } -biorbd::utils::Scalar biorbd::actuator::ActuatorLinear::torqueMax() +utils::Scalar actuator::ActuatorLinear::torqueMax() { - biorbd::utils::Error::raise("torqueMax for ActuatorLinear must be called with Q and Qdot"); + utils::Error::raise("torqueMax for ActuatorLinear must be called with Q and Qdot"); } -biorbd::utils::Scalar biorbd::actuator::ActuatorLinear::torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q) const +utils::Scalar actuator::ActuatorLinear::torqueMax( + const rigidbody::GeneralizedCoordinates &Q) const { return (Q[*m_dofIdx]*180/M_PI) * *m_m + *m_b; } -void biorbd::actuator::ActuatorLinear::setType() +void actuator::ActuatorLinear::setType() { - *m_type = biorbd::actuator::TYPE::LINEAR; + *m_type = actuator::TYPE::LINEAR; } diff --git a/src/Actuators/ActuatorSigmoidGauss3p.cpp b/src/Actuators/ActuatorSigmoidGauss3p.cpp index 3b1b887f..ac4b500c 100644 --- a/src/Actuators/ActuatorSigmoidGauss3p.cpp +++ b/src/Actuators/ActuatorSigmoidGauss3p.cpp @@ -5,20 +5,22 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -biorbd::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p() : - biorbd::actuator::Actuator(), - m_theta(std::make_shared(0)), - m_lambda(std::make_shared(0)), - m_offset(std::make_shared(0)), - m_r(std::make_shared(0)), - m_qopt(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p() : + actuator::Actuator(), + m_theta(std::make_shared(0)), + m_lambda(std::make_shared(0)), + m_offset(std::make_shared(0)), + m_r(std::make_shared(0)), + m_qopt(std::make_shared(0)) { setType(); } -biorbd::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( - const biorbd::actuator::ActuatorSigmoidGauss3p &other) : - biorbd::actuator::Actuator(other), +actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( + const actuator::ActuatorSigmoidGauss3p &other) : + actuator::Actuator(other), m_theta(other.m_theta), m_lambda(other.m_lambda), m_offset(other.m_offset), @@ -28,60 +30,60 @@ biorbd::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( } -biorbd::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( +actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( int direction, - const biorbd::utils::Scalar& theta, - const biorbd::utils::Scalar& lambda, - const biorbd::utils::Scalar& offset, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& theta, + const utils::Scalar& lambda, + const utils::Scalar& offset, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx) : - biorbd::actuator::Actuator(direction, dofIdx), - m_theta(std::make_shared(theta)), - m_lambda(std::make_shared(lambda)), - m_offset(std::make_shared(offset)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)) + actuator::Actuator(direction, dofIdx), + m_theta(std::make_shared(theta)), + m_lambda(std::make_shared(lambda)), + m_offset(std::make_shared(offset)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)) { setType(); } -biorbd::actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( +actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p( int direction, - const biorbd::utils::Scalar& theta, - const biorbd::utils::Scalar& lambda, - const biorbd::utils::Scalar& offset, - const biorbd::utils::Scalar& r, - const biorbd::utils::Scalar& qopt, + const utils::Scalar& theta, + const utils::Scalar& lambda, + const utils::Scalar& offset, + const utils::Scalar& r, + const utils::Scalar& qopt, unsigned int dofIdx, - const biorbd::utils::String &jointName) : - biorbd::actuator::Actuator(direction, dofIdx, jointName), - m_theta(std::make_shared(theta)), - m_lambda(std::make_shared(lambda)), - m_offset(std::make_shared(offset)), - m_r(std::make_shared(r)), - m_qopt(std::make_shared(qopt)) + const utils::String &jointName) : + actuator::Actuator(direction, dofIdx, jointName), + m_theta(std::make_shared(theta)), + m_lambda(std::make_shared(lambda)), + m_offset(std::make_shared(offset)), + m_r(std::make_shared(r)), + m_qopt(std::make_shared(qopt)) { setType(); } -biorbd::actuator::ActuatorSigmoidGauss3p::~ActuatorSigmoidGauss3p() +actuator::ActuatorSigmoidGauss3p::~ActuatorSigmoidGauss3p() { } -biorbd::actuator::ActuatorSigmoidGauss3p -biorbd::actuator::ActuatorSigmoidGauss3p::DeepCopy() const +actuator::ActuatorSigmoidGauss3p +actuator::ActuatorSigmoidGauss3p::DeepCopy() const { - biorbd::actuator::ActuatorSigmoidGauss3p copy; + actuator::ActuatorSigmoidGauss3p copy; copy.DeepCopy(*this); return copy; } -void biorbd::actuator::ActuatorSigmoidGauss3p::DeepCopy( - const biorbd::actuator::ActuatorSigmoidGauss3p &other) +void actuator::ActuatorSigmoidGauss3p::DeepCopy( + const actuator::ActuatorSigmoidGauss3p &other) { - biorbd::actuator::Actuator::DeepCopy(other); + actuator::Actuator::DeepCopy(other); *m_theta = *other.m_theta; *m_lambda = *other.m_lambda; *m_offset = *other.m_offset; @@ -89,27 +91,27 @@ void biorbd::actuator::ActuatorSigmoidGauss3p::DeepCopy( *m_qopt = *other.m_qopt; } -biorbd::utils::Scalar biorbd::actuator::ActuatorSigmoidGauss3p::torqueMax() +utils::Scalar actuator::ActuatorSigmoidGauss3p::torqueMax() { - biorbd::utils::Error::raise( + utils::Error::raise( "torqueMax for ActuatorSigmoidGauss3p must be called with Q and Qdot"); } -biorbd::utils::Scalar biorbd::actuator::ActuatorSigmoidGauss3p::torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +utils::Scalar actuator::ActuatorSigmoidGauss3p::torqueMax( + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot) { - biorbd::utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); - biorbd::utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); + utils::Scalar pos(Q[*m_dofIdx] * 180/M_PI); + utils::Scalar speed(Qdot[*m_dofIdx] * 180/M_PI); // Getting Tmax of Gauss3p from Sigmoid - biorbd::utils::Scalar Tmax(*m_theta / (1 + exp(*m_lambda * speed)) + *m_offset); + utils::Scalar Tmax(*m_theta / (1 + exp(*m_lambda * speed)) + *m_offset); // Calculation of the max torque return Tmax * exp(-(*m_qopt - pos) * (*m_qopt - pos) / (2 * *m_r * *m_r)); } -void biorbd::actuator::ActuatorSigmoidGauss3p::setType() +void actuator::ActuatorSigmoidGauss3p::setType() { - *m_type = biorbd::actuator::TYPE::SIGMOIDGAUSS3P; + *m_type = actuator::TYPE::SIGMOIDGAUSS3P; } diff --git a/src/Actuators/Actuators.cpp b/src/Actuators/Actuators.cpp index 11659b40..717a34c1 100644 --- a/src/Actuators/Actuators.cpp +++ b/src/Actuators/Actuators.cpp @@ -14,16 +14,18 @@ #include "Actuators/ActuatorConstant.h" #include "Actuators/ActuatorLinear.h" -biorbd::actuator::Actuators::Actuators() : - m_all(std::make_shared, std::shared_ptr>>>()), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +actuator::Actuators::Actuators() : + m_all(std::make_shared, std::shared_ptr>>>()), m_isDofSet(std::make_shared>(1)), m_isClose(std::make_shared(false)) { (*m_isDofSet)[0] = false; } -biorbd::actuator::Actuators::Actuators( - const biorbd::actuator::Actuators& other) : +actuator::Actuators::Actuators( + const actuator::Actuators& other) : m_all(other.m_all), m_isDofSet(other.m_isDofSet), m_isClose(other.m_isClose) @@ -32,65 +34,65 @@ biorbd::actuator::Actuators::Actuators( } -biorbd::actuator::Actuators::~Actuators() +actuator::Actuators::~Actuators() { } -void biorbd::actuator::Actuators::DeepCopy(const biorbd::actuator::Actuators +void actuator::Actuators::DeepCopy(const actuator::Actuators &other) { m_all->resize(other.m_all->size()); for (unsigned int i=0; isize(); ++i) { - if ((*other.m_all)[i].first->type() == biorbd::actuator::TYPE::CONSTANT) - (*m_all)[i].first = std::make_shared( - static_cast( * + if ((*other.m_all)[i].first->type() == actuator::TYPE::CONSTANT) + (*m_all)[i].first = std::make_shared( + static_cast( * (*other.m_all)[i].first) ); - else if ((*other.m_all)[i].first->type() == biorbd::actuator::TYPE::LINEAR) - (*m_all)[i].first = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].first->type() == actuator::TYPE::LINEAR) + (*m_all)[i].first = std::make_shared( + static_cast( * (*other.m_all)[i].first) ); - else if ((*other.m_all)[i].first->type() == biorbd::actuator::TYPE::GAUSS3P) - (*m_all)[i].first = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].first->type() == actuator::TYPE::GAUSS3P) + (*m_all)[i].first = std::make_shared( + static_cast( * (*other.m_all)[i].first) ); - else if ((*other.m_all)[i].first->type() == biorbd::actuator::TYPE::GAUSS6P) - (*m_all)[i].first = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].first->type() == actuator::TYPE::GAUSS6P) + (*m_all)[i].first = std::make_shared( + static_cast( * (*other.m_all)[i].first) ); else if ((*other.m_all)[i].first->type() == - biorbd::actuator::TYPE::SIGMOIDGAUSS3P) - (*m_all)[i].first = std::make_shared( - static_cast( * + actuator::TYPE::SIGMOIDGAUSS3P) + (*m_all)[i].first = std::make_shared( + static_cast( * (*other.m_all)[i].first) ); else - biorbd::utils::Error::raise("Actuator " + biorbd::utils::String( - biorbd::actuator::TYPE_toStr((*other.m_all)[i].first->type())) + utils::Error::raise("Actuator " + utils::String( + actuator::TYPE_toStr((*other.m_all)[i].first->type())) + " in DeepCopy"); - if ((*other.m_all)[i].second->type() == biorbd::actuator::TYPE::CONSTANT) - (*m_all)[i].second = std::make_shared( - static_cast( * + if ((*other.m_all)[i].second->type() == actuator::TYPE::CONSTANT) + (*m_all)[i].second = std::make_shared( + static_cast( * (*other.m_all)[i].second) ); - else if ((*other.m_all)[i].second->type() == biorbd::actuator::TYPE::LINEAR) - (*m_all)[i].second = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].second->type() == actuator::TYPE::LINEAR) + (*m_all)[i].second = std::make_shared( + static_cast( * (*other.m_all)[i].second) ); - else if ((*other.m_all)[i].second->type() == biorbd::actuator::TYPE::GAUSS3P) - (*m_all)[i].second = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].second->type() == actuator::TYPE::GAUSS3P) + (*m_all)[i].second = std::make_shared( + static_cast( * (*other.m_all)[i].second) ); - else if ((*other.m_all)[i].second->type() == biorbd::actuator::TYPE::GAUSS6P) - (*m_all)[i].second = std::make_shared( - static_cast( * + else if ((*other.m_all)[i].second->type() == actuator::TYPE::GAUSS6P) + (*m_all)[i].second = std::make_shared( + static_cast( * (*other.m_all)[i].second) ); else if ((*other.m_all)[i].second->type() == - biorbd::actuator::TYPE::SIGMOIDGAUSS3P) - (*m_all)[i].second = std::make_shared( - static_cast( * + actuator::TYPE::SIGMOIDGAUSS3P) + (*m_all)[i].second = std::make_shared( + static_cast( * (*other.m_all)[i].second) ); else - biorbd::utils::Error::raise("Actuator " + biorbd::utils::String( - biorbd::actuator::TYPE_toStr((*other.m_all)[i].second->type())) + utils::Error::raise("Actuator " + utils::String( + actuator::TYPE_toStr((*other.m_all)[i].second->type())) + " in DeepCopy"); } m_isDofSet->resize(other.m_isDofSet->size()); @@ -100,10 +102,10 @@ void biorbd::actuator::Actuators::DeepCopy(const biorbd::actuator::Actuators *m_isClose = *other.m_isClose; } -void biorbd::actuator::Actuators::addActuator(const biorbd::actuator::Actuator +void actuator::Actuators::addActuator(const actuator::Actuator &act) { - biorbd::utils::Error::check( + utils::Error::check( !*m_isClose, "You can't add actuator after closing the model"); // Assuming that this is also a Joints type (via BiorbdModel) @@ -112,7 +114,7 @@ void biorbd::actuator::Actuators::addActuator(const biorbd::actuator::Actuator // Verify that the target dof is associated to a dof that // already exists in the model - biorbd::utils::Error::check( + utils::Error::check( act.index() - (static_cast(act)); + (*m_all)[idx].first = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2] = true; } else { - (*m_all)[idx].second = std::make_shared - (static_cast(act)); + (*m_all)[idx].second = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2+1] = true; } return; - } else if (act.type() == biorbd::actuator::TYPE::LINEAR) { + } else if (act.type() == actuator::TYPE::LINEAR) { if (act.direction() == 1) { - (*m_all)[idx].first = std::make_shared - (static_cast(act)); + (*m_all)[idx].first = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2] = true; } else { - (*m_all)[idx].second = std::make_shared - (static_cast(act)); + (*m_all)[idx].second = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2+1] = true; } return; - } else if (act.type() == biorbd::actuator::TYPE::GAUSS3P) { + } else if (act.type() == actuator::TYPE::GAUSS3P) { if (act.direction() == 1) { - (*m_all)[idx].first = std::make_shared - (static_cast(act)); + (*m_all)[idx].first = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2] = true; } else { - (*m_all)[idx].second = std::make_shared - (static_cast(act)); + (*m_all)[idx].second = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2+1] = true; } return; - } else if (act.type() == biorbd::actuator::TYPE::GAUSS6P) { + } else if (act.type() == actuator::TYPE::GAUSS6P) { if (act.direction() == 1) { - (*m_all)[idx].first = std::make_shared - (static_cast(act)); + (*m_all)[idx].first = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2] = true; } else { - (*m_all)[idx].second = std::make_shared - (static_cast(act)); + (*m_all)[idx].second = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2+1] = true; } return; - } else if (act.type() == biorbd::actuator::TYPE::SIGMOIDGAUSS3P) { + } else if (act.type() == actuator::TYPE::SIGMOIDGAUSS3P) { if (act.direction() == 1) { - (*m_all)[idx].first = std::make_shared - (static_cast(act)); + (*m_all)[idx].first = std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2] = true; } else { (*m_all)[idx].second = - std::make_shared - (static_cast(act)); + std::make_shared + (static_cast(act)); (*m_isDofSet)[idx*2+1] = true; } return; } else { - biorbd::utils::Error::raise("Actuator type not found"); + utils::Error::raise("Actuator type not found"); } } -void biorbd::actuator::Actuators::closeActuator() +void actuator::Actuators::closeActuator() { // Assuming that this is also a Joints type (via BiorbdModel) const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast(*this); - biorbd::utils::Error::check( + utils::Error::check( model.nbDof()==m_all->size(), "All dof must have their actuators set"); for (unsigned int i=0; isize()*2; ++i) - biorbd::utils::Error::check((*m_isDofSet)[i], + utils::Error::check((*m_isDofSet)[i], "All DoF must have their actuators set " "before closing the model"); *m_isClose = true; } -const std::pair, - std::shared_ptr>& - biorbd::actuator::Actuators::actuator(unsigned int dof) +const std::pair, + std::shared_ptr>& + actuator::Actuators::actuator(unsigned int dof) { - biorbd::utils::Error::check(dof, std::shared_ptr>& + const std::pair, std::shared_ptr>& tp(actuator(dof)); if (concentric) { @@ -229,18 +231,18 @@ const biorbd::actuator::Actuator& biorbd::actuator::Actuators::actuator( } } -unsigned int biorbd::actuator::Actuators::nbActuators() const +unsigned int actuator::Actuators::nbActuators() const { return static_cast(m_all->size()); } -biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torque( - const biorbd::utils::Vector& activation, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +rigidbody::GeneralizedTorque actuator::Actuators::torque( + const utils::Vector& activation, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity &Qdot) { // Calculate the maximal torques - biorbd::rigidbody::GeneralizedTorque GeneralizedTorque( + rigidbody::GeneralizedTorque GeneralizedTorque( torqueMax(activation,Q,Qdot)); // Put the signs @@ -252,22 +254,22 @@ biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torque( } -std::pair -biorbd::actuator::Actuators::torqueMax( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot) +std::pair +actuator::Actuators::torqueMax( + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot) { - biorbd::utils::Error::check(*m_isClose, + utils::Error::check(*m_isClose, "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast(*this); - std::pair + std::pair maxGeneralizedTorque_all = - std::make_pair(biorbd::rigidbody::GeneralizedTorque(model), - biorbd::rigidbody::GeneralizedTorque(model)); + std::make_pair(rigidbody::GeneralizedTorque(model), + rigidbody::GeneralizedTorque(model)); for (unsigned int i=0; i, std::shared_ptr> @@ -298,7 +300,7 @@ biorbd::actuator::Actuators::torqueMax( std::static_pointer_cast (GeneralizedTorque_tp.first)->torqueMax(Q, Qdot); } else { - biorbd::utils::Error::raise("Wrong type (should never get here because of previous safety)"); + utils::Error::raise("Wrong type (should never get here because of previous safety)"); } else // Second if (std::dynamic_pointer_cast (GeneralizedTorque_tp.second)) { @@ -325,7 +327,7 @@ biorbd::actuator::Actuators::torqueMax( std::static_pointer_cast (GeneralizedTorque_tp.second)->torqueMax(Q, Qdot); } else { - biorbd::utils::Error::raise("Wrong type (should never get here because of previous safety)"); + utils::Error::raise("Wrong type (should never get here because of previous safety)"); } } } @@ -334,12 +336,12 @@ biorbd::actuator::Actuators::torqueMax( } -biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torqueMax( +rigidbody::GeneralizedTorque actuator::Actuators::torqueMax( const utils::Vector &activation, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity &Qdot) { - biorbd::utils::Error::check(*m_isClose, + utils::Error::check(*m_isClose, "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) @@ -347,7 +349,7 @@ biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torqueMax( dynamic_cast(*this); // Set qdot to be positive if concentric and negative if excentric - biorbd::rigidbody::GeneralizedVelocity QdotResigned(Qdot); + rigidbody::GeneralizedVelocity QdotResigned(Qdot); for (unsigned int i=0; i actuator, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot) const +utils::Scalar actuator::Actuators::getTorqueMaxDirection( + const std::shared_ptr actuator, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot) const { if (std::dynamic_pointer_cast (actuator)) { return std::static_pointer_cast (actuator)->torqueMax(Q, Qdot); @@ -399,7 +401,7 @@ biorbd::utils::Scalar biorbd::actuator::Actuators::getTorqueMaxDirection( return std::static_pointer_cast (actuator)->torqueMax(Q, Qdot); } else { - biorbd::utils::Error::raise("Wrong type (should never get here because of previous safety)"); + utils::Error::raise("Wrong type (should never get here because of previous safety)"); } } diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp index fac39d7a..7796c822 100644 --- a/src/ModelReader.cpp +++ b/src/ModelReader.cpp @@ -715,24 +715,24 @@ void Reader::readModelFile( } // Verify that everything is there utils::Error::check(isTypeSet!=0, "Actuator type must be defined"); - biorbd::actuator::Actuator* actuator; + actuator::Actuator* actuator; if (!type.tolower().compare("gauss3p")) { utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set && iswmaxSet && iswcSet && isaminSet && iswrSet && isw1Set && isrSet && isqoptSet, "Make sure all parameters are defined"); - actuator = new biorbd::actuator::ActuatorGauss3p(int_direction,Tmax,T0,wmax,wc, + actuator = new actuator::ActuatorGauss3p(int_direction,Tmax,T0,wmax,wc, amin,wr,w1,r,qopt,dofIdx,name); } else if (!type.tolower().compare("constant")) { utils::Error::check(isDofSet && isDirectionSet && isTmaxSet, "Make sure all parameters are defined"); - actuator = new biorbd::actuator::ActuatorConstant(int_direction,Tmax,dofIdx, + actuator = new actuator::ActuatorConstant(int_direction,Tmax,dofIdx, name); } else if (!type.tolower().compare("linear")) { utils::Error::check(isDofSet && isDirectionSet && isSlopeSet && isT0Set, "Make sure all parameters are defined"); - actuator = new biorbd::actuator::ActuatorLinear(int_direction,T0,slope,dofIdx, + actuator = new actuator::ActuatorLinear(int_direction,T0,slope,dofIdx, name); } else if (!type.tolower().compare("gauss6p")) { utils::Error::check(isDofSet && isDirectionSet && isTmaxSet && isT0Set @@ -740,19 +740,19 @@ void Reader::readModelFile( iswrSet && isw1Set && isrSet && isqoptSet && isFacteur6pSet && isr2Set && isqopt2Set, "Make sure all parameters are defined"); - actuator = new biorbd::actuator::ActuatorGauss6p(int_direction,Tmax,T0,wmax,wc, + actuator = new actuator::ActuatorGauss6p(int_direction,Tmax,T0,wmax,wc, amin,wr,w1,r,qopt,facteur6p, r2, qopt2, dofIdx,name); } else if (!type.tolower().compare("sigmoidgauss3p")) { utils::Error::check(isDofSet && isThetaSet && isLambdaSet && isOffsetSet && isrSet && isqoptSet, "Make sure all parameters are defined"); - actuator = new biorbd::actuator::ActuatorSigmoidGauss3p(int_direction, theta, + actuator = new actuator::ActuatorSigmoidGauss3p(int_direction, theta, lambda, offset, r, qopt, dofIdx, name); } else { utils::Error::raise("Actuator do not correspond to an implemented one"); #ifdef _WIN32 - actuator = new biorbd::actuator::ActuatorConstant(int_direction, Tmax, dofIdx, + actuator = new actuator::ActuatorConstant(int_direction, Tmax, dofIdx, name); // Échec de compilation sinon #endif } From f36cd5edee8c11c21d5ae9202f42a80d75e106e2 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 11:52:37 -0400 Subject: [PATCH 05/14] muscles in namespace --- binding/biorbd_muscles.i.in | 64 ++--- include/BiorbdModel.h | 2 +- include/Muscles/Characteristics.h | 89 +++--- include/Muscles/Compound.h | 63 ++--- include/Muscles/FatigueDynamicState.h | 31 +- include/Muscles/FatigueDynamicStateXia.h | 21 +- include/Muscles/FatigueModel.h | 25 +- include/Muscles/FatigueParameters.h | 39 +-- include/Muscles/FatigueState.h | 39 +-- include/Muscles/Geometry.h | 131 +++++---- include/Muscles/HillThelenActiveOnlyType.h | 45 +-- include/Muscles/HillThelenType.h | 47 ++-- include/Muscles/HillThelenTypeFatigable.h | 63 +++-- include/Muscles/HillType.h | 107 +++---- include/Muscles/IdealizedActuator.h | 67 ++--- include/Muscles/Muscle.h | 111 ++++---- include/Muscles/MuscleGroup.h | 103 +++---- include/Muscles/Muscles.h | 101 +++---- include/Muscles/MusclesEnums.h | 9 +- include/Muscles/PathModifiers.h | 15 +- include/Muscles/State.h | 40 +-- include/Muscles/StateDynamics.h | 47 ++-- include/Muscles/StateDynamicsBuchanan.h | 33 ++- include/Muscles/StateDynamicsDeGroote.h | 17 +- include/Muscles/ViaPoint.h | 33 ++- include/Muscles/WrappingHalfCylinder.h | 105 +++---- include/Muscles/WrappingObject.h | 79 +++--- include/Muscles/WrappingSphere.h | 73 ++--- src/ModelReader.cpp | 46 +-- src/Muscles/Characteristics.cpp | 151 +++++----- src/Muscles/Compound.cpp | 66 ++--- src/Muscles/FatigueDynamicState.cpp | 48 ++-- src/Muscles/FatigueDynamicStateXia.cpp | 48 ++-- src/Muscles/FatigueModel.cpp | 56 ++-- src/Muscles/FatigueParameters.cpp | 56 ++-- src/Muscles/FatigueState.cpp | 66 ++--- src/Muscles/Geometry.cpp | 268 +++++++++--------- src/Muscles/HillThelenActiveOnlyType.cpp | 86 +++--- src/Muscles/HillThelenType.cpp | 86 +++--- src/Muscles/HillThelenTypeFatigable.cpp | 118 ++++---- src/Muscles/HillType.cpp | 266 +++++++++--------- src/Muscles/IdealizedActuator.cpp | 106 +++---- src/Muscles/Muscle.cpp | 206 +++++++------- src/Muscles/MuscleGroup.cpp | 312 +++++++++++---------- src/Muscles/Muscles.cpp | 166 +++++------ src/Muscles/PathModifiers.cpp | 60 ++-- src/Muscles/State.cpp | 70 ++--- src/Muscles/StateDynamics.cpp | 92 +++--- src/Muscles/StateDynamicsBuchanan.cpp | 82 +++--- src/Muscles/StateDynamicsDeGroote.cpp | 46 +-- src/Muscles/ViaPoint.cpp | 56 ++-- src/Muscles/WrappingHalfCylinder.cpp | 214 +++++++------- src/Muscles/WrappingObject.cpp | 78 +++--- src/Muscles/WrappingSphere.cpp | 78 +++--- 54 files changed, 2317 insertions(+), 2209 deletions(-) diff --git a/binding/biorbd_muscles.i.in b/binding/biorbd_muscles.i.in index 8540617c..19900fd8 100644 --- a/binding/biorbd_muscles.i.in +++ b/binding/biorbd_muscles.i.in @@ -25,63 +25,63 @@ %} // Instantiate templates -%template(VecBiorbdMuscleGroup) std::vector; -%template(SharedBiorbdMuscle) std::shared_ptr; -%template(VecSharedBiorbdMuscle) std::vector>; -%shared_ptr(biorbd::muscles::State); -%shared_ptr(biorbd::muscles::StateDynamics); -%shared_ptr(biorbd::muscles::StateDynamicsBuchanan); -%template(VecBiorbdMuscleState) std::vector>; -%template(MatBiorbdMuscleState) std::vector>>; -%template(SharedBiorbdMuscleFatigueState) std::shared_ptr; +%template(VecBiorbdMuscleGroup) std::vector; +%template(SharedBiorbdMuscle) std::shared_ptr; +%template(VecSharedBiorbdMuscle) std::vector>; +%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::State); +%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::StateDynamics); +%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::StateDynamicsBuchanan); +%template(VecBiorbdMuscleState) std::vector>; +%template(MatBiorbdMuscleState) std::vector>>; +%template(SharedBiorbdMuscleFatigueState) std::shared_ptr; // extension of muscle casting -%extend biorbd::muscles::IdealizedActuator{ - static biorbd::muscles::IdealizedActuator DeepCopy(const biorbd::muscles::Muscle& other) +%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator{ + static biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) { - biorbd::muscles::IdealizedActuator copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::muscles::HillType{ - static biorbd::muscles::HillType DeepCopy(const biorbd::muscles::Muscle& other) +%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType{ + static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) { - biorbd::muscles::HillType copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::muscles::HillThelenType{ - static biorbd::muscles::HillThelenType DeepCopy(const biorbd::muscles::Muscle& other) +%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType{ + static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) { - biorbd::muscles::HillThelenType copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::muscles::HillThelenTypeFatigable{ - static biorbd::muscles::HillThelenTypeFatigable DeepCopy(const biorbd::muscles::Muscle& other) +%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable{ + static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) { - biorbd::muscles::HillThelenTypeFatigable copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } // extension of muscle fatigueState casting -%extend biorbd::muscles::FatigueDynamicStateXia{ - static biorbd::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::muscles::FatigueState& other) +%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia{ + static biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueState& other) { - biorbd::muscles::FatigueDynamicStateXia copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } - static biorbd::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::muscles::FatigueDynamicState& other) + static biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicState& other) { - biorbd::muscles::FatigueDynamicStateXia copy; - copy.DeepCopy(dynamic_cast(other)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } diff --git a/include/BiorbdModel.h b/include/BiorbdModel.h index 2bb86f90..ed0f8fb6 100644 --- a/include/BiorbdModel.h +++ b/include/BiorbdModel.h @@ -76,7 +76,7 @@ class BIORBD_API Model : ,public actuator::Actuators #endif #ifdef MODULE_MUSCLES - ,public biorbd::muscles::Muscles + ,public muscles::Muscles #endif { public: diff --git a/include/Muscles/Characteristics.h b/include/Muscles/Characteristics.h index 4d038b0c..d1813107 100644 --- a/include/Muscles/Characteristics.h +++ b/include/Muscles/Characteristics.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { class State; @@ -28,7 +30,7 @@ class BIORBD_API Characteristics /// \param other The other characteristics /// Characteristics( - const biorbd::muscles::Characteristics& other); + const Characteristics& other); /// /// \brief Construct characteristics @@ -44,16 +46,16 @@ class BIORBD_API Characteristics /// \param minAct Minimal activation (default: 0.01) /// Characteristics( - const biorbd::utils::Scalar& optLength, - const biorbd::utils::Scalar& fmax, - const biorbd::utils::Scalar& PCSA, - const biorbd::utils::Scalar& tendonSlackLength, - const biorbd::utils::Scalar& pennAngle, - const biorbd::muscles::State& emgMax, - const biorbd::muscles::FatigueParameters& fatigueParameters, - const biorbd::utils::Scalar& torqueAct = 0.01, - const biorbd::utils::Scalar& torqueDeact = 0.04, - const biorbd::utils::Scalar& minAct = 0.01); + const utils::Scalar& optLength, + const utils::Scalar& fmax, + const utils::Scalar& PCSA, + const utils::Scalar& tendonSlackLength, + const utils::Scalar& pennAngle, + const State& emgMax, + const FatigueParameters& fatigueParameters, + const utils::Scalar& torqueAct = 0.01, + const utils::Scalar& torqueDeact = 0.04, + const utils::Scalar& minAct = 0.01); /// /// \brief Destroy the class properly @@ -64,14 +66,14 @@ class BIORBD_API Characteristics /// \brief Deep copy of characteristics /// \return A depp copy of characteristics /// - biorbd::muscles::Characteristics DeepCopy() const; + Characteristics DeepCopy() const; /// /// \brief Deep copy of characteristics from another characteristics /// \param other The characteristics to copy from /// void DeepCopy( - const biorbd::muscles::Characteristics& other); + const Characteristics& other); /// @@ -79,153 +81,154 @@ class BIORBD_API Characteristics /// \param val Value of the length without tension /// void setOptimalLength( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the optimal length at which maximal force occurs /// \return The length without tension /// - virtual const biorbd::utils::Scalar& optimalLength() const; + virtual const utils::Scalar& optimalLength() const; /// /// \brief Set the maximal isometric force /// \param val Value of the maximal isometric force /// virtual void setForceIsoMax( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the maximal isometric force at optimal length /// \return The maximal isometric force /// - const biorbd::utils::Scalar& forceIsoMax() const; + const utils::Scalar& forceIsoMax() const; /// /// \brief Set the tendon slack length /// \param val Value of the tendon slack length /// void setTendonSlackLength( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the tendon slack length /// \return The tendon slack length /// - const biorbd::utils::Scalar& tendonSlackLength() const; + const utils::Scalar& tendonSlackLength() const; /// /// \brief Set the angle of pennation /// \param val Value of the angle of pennation /// void setPennationAngle( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the angle of pennation /// \return The angle of pennation /// - const biorbd::utils::Scalar& pennationAngle() const; + const utils::Scalar& pennationAngle() const; /// /// \brief Set the physiological cross-sectional area of the muscle /// \param val Value of the physiological cross-sectional area of the muscle /// void setPCSA( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the physiological cross-sectional area of the muscle /// \return The physiological cross-sectional area of the muscle /// - const biorbd::utils::Scalar& PCSA() const; + const utils::Scalar& PCSA() const; /// /// \brief Set the minimal activation of the muscle /// \param val The value of the minimal activation of the muscle /// void setMinActivation( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the minimal activation of the muscle /// \return The minimal activation of the muscle /// - const biorbd::utils::Scalar& minActivation() const; + const utils::Scalar& minActivation() const; /// /// \brief Set the time activation constant /// \param val The value of the time activation constant /// void setTorqueActivation( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the time activation constant /// \return The time activation constant /// - const biorbd::utils::Scalar& torqueActivation() const; + const utils::Scalar& torqueActivation() const; /// /// \brief Set the time deactivation constant /// \param val The value of the time deactivation constant /// void setTorqueDeactivation( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the time deactivation constant /// \return The time deactivation constant /// - const biorbd::utils::Scalar& torqueDeactivation() const; + const utils::Scalar& torqueDeactivation() const; /// /// \brief Set the maximal excitation and activation of the muscle /// \param emgMax Value of the maximal excitation and activation of the muscle /// void setStateMax( - const biorbd::muscles::State &emgMax); + const State &emgMax); /// /// \brief Return the maximal excitation and activation of the muscle /// \return The maximal excitation and activation of the muscle /// - const biorbd::muscles::State& stateMax() const; + const State& stateMax() const; /// /// \brief Set the fatigue parameters /// \param fatigueParameters The values of the fatigue parameters /// void setFatigueParameters( - const biorbd::muscles::FatigueParameters& fatigueParameters); + const FatigueParameters& fatigueParameters); /// /// \brief Return the fatigue parameters /// \return The fatigue parameters /// - const biorbd::muscles::FatigueParameters& fatigueParameters() const; + const FatigueParameters& fatigueParameters() const; protected: - std::shared_ptr + std::shared_ptr m_optimalLength; ///< Length without tension - std::shared_ptr + std::shared_ptr m_fIsoMax; ///< Maximal isometric force Force maximale isométrique - std::shared_ptr + std::shared_ptr m_PCSA; ///< Physiological cross-sectional area of the muscle - std::shared_ptr + std::shared_ptr m_tendonSlackLength; ///< Tendon slack length - std::shared_ptr m_pennationAngle; ///< Angle of pennation - std::shared_ptr + std::shared_ptr m_pennationAngle; ///< Angle of pennation + std::shared_ptr m_stateMax; ///< Maximal excitation et activation of the muscle // Parametre d'activation - std::shared_ptr m_minActivation; ///< Minimal activation - std::shared_ptr + std::shared_ptr m_minActivation; ///< Minimal activation + std::shared_ptr m_torqueActivation; ///< Time activation constant - std::shared_ptr + std::shared_ptr m_torqueDeactivation; ///< Time deactivation constant // Fatigue parameters - std::shared_ptr + std::shared_ptr m_fatigueParameters; ///< Fatigue parameters }; +} } } diff --git a/include/Muscles/Compound.h b/include/Muscles/Compound.h index 97ce1eda..002744c7 100644 --- a/include/Muscles/Compound.h +++ b/include/Muscles/Compound.h @@ -9,21 +9,17 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; class Vector3d; } -namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; -} -} - -namespace rigidbody -{ class GeneralizedCoordinates; class GeneralizedVelocity; } @@ -49,7 +45,7 @@ class BIORBD_API Compound /// \param name Name of the compound /// Compound( - const biorbd::utils::String &name); + const utils::String &name); /// /// \brief Construct compound @@ -57,21 +53,21 @@ class BIORBD_API Compound /// \param pathModifiers The set of path modifiers /// Compound( - const biorbd::utils::String& name, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const PathModifiers& pathModifiers); /// /// \brief Construct compound from another muscle /// \param other The muscle to shallow copy /// Compound( - const biorbd::muscles::Compound& other); + const Compound& other); /// /// \brief Construct compound from another muscle /// \param other The muscle to shallow copy /// Compound( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Destroy class properly @@ -83,53 +79,53 @@ class BIORBD_API Compound /// \param other Compound to copy /// void DeepCopy( - const biorbd::muscles::Compound& other); + const Compound& other); /// /// \brief Set the name of a muscle /// \param name Name of the muscle /// - void setName(const biorbd::utils::String& name); + void setName(const utils::String& name); /// /// \brief Return the name of the muscle /// \return The name of the muscle /// - const biorbd::utils::String& name() const; + const utils::String& name() const; /// /// \brief Return the type of the muscle /// \return The type of the muscle /// - biorbd::muscles::MUSCLE_TYPE type() const; + MUSCLE_TYPE type() const; // Wrapping object /// /// \brief Return the path modifier /// \return The path modifier /// - const biorbd::muscles::PathModifiers& pathModifier(); + const PathModifiers& pathModifier(); /// /// \brief Add a path modifier object /// \param wrap Position of the object /// - void addPathObject(biorbd::utils::Vector3d& wrap); + void addPathObject(utils::Vector3d& wrap); /// /// \brief Return the last computed muscle force norm /// \return The last computed muscle force norm /// - virtual const biorbd::utils::Scalar& force(); + virtual const utils::Scalar& force(); /// /// \brief Computes and returns the forces norm from the EMG /// \param emg EMG data /// \return The computed forces from the EMG /// - virtual const biorbd::utils::Scalar& force( - const biorbd::muscles::State& emg) = 0; + virtual const utils::Scalar& force( + const State& emg) = 0; /// /// \brief Return the computed force norm from EMG @@ -140,11 +136,11 @@ class BIORBD_API Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The computed force from EMG /// - virtual const biorbd::utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::muscles::State& emg, + virtual const utils::Scalar& force( + rigidbody::Joints& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const State& emg, int updateKin = 2) = 0; /// @@ -155,18 +151,18 @@ class BIORBD_API Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The computed force from EMG /// - virtual const biorbd::utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::muscles::State& emg, + virtual const utils::Scalar& force( + rigidbody::Joints& model, + const rigidbody::GeneralizedCoordinates& Q, + const State& emg, int updateKin = 2) = 0; protected: - std::shared_ptr m_name; ///< The name of the muscle - std::shared_ptr m_type; ///< The type of muscle - std::shared_ptr + std::shared_ptr m_name; ///< The name of the muscle + std::shared_ptr m_type; ///< The type of muscle + std::shared_ptr m_pathChanger; ///< The set of path modifiers - std::shared_ptr m_force; ///< The last computed force + std::shared_ptr m_force; ///< The last computed force /// /// \brief Set the type of muscle @@ -175,6 +171,7 @@ class BIORBD_API Compound }; +} } } diff --git a/include/Muscles/FatigueDynamicState.h b/include/Muscles/FatigueDynamicState.h index 36c8dabb..25f26c53 100644 --- a/include/Muscles/FatigueDynamicState.h +++ b/include/Muscles/FatigueDynamicState.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { class StateDynamics; @@ -14,7 +16,7 @@ class Characteristics; /// /// \brief Class A state of fatigue that dynamically evolves over the usage of the muscle /// -class BIORBD_API FatigueDynamicState : public biorbd::muscles::FatigueState +class BIORBD_API FatigueDynamicState : public FatigueState { public: /// @@ -24,41 +26,41 @@ class BIORBD_API FatigueDynamicState : public biorbd::muscles::FatigueState /// \param resting Muscle resting /// FatigueDynamicState( - const biorbd::utils::Scalar& active = 0, - const biorbd::utils::Scalar& fatigued = 0, - const biorbd::utils::Scalar& resting = 1); + const utils::Scalar& active = 0, + const utils::Scalar& fatigued = 0, + const utils::Scalar& resting = 1); /// /// \brief Construct the fatigue dynamic state from other state /// \param other The other fatigue state /// FatigueDynamicState( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of a fatigue dynamic state /// \param other The other fatigue state /// void DeepCopy( - const biorbd::muscles::FatigueDynamicState& other); + const FatigueDynamicState& other); /// /// \brief Return the active fibers derivative /// \return The active fibers derivative /// - const biorbd::utils::Scalar& activeFibersDot() const; + const utils::Scalar& activeFibersDot() const; /// /// \brief Return the fatigued fibers derivative /// \return The fatigued fibers derivative /// - const biorbd::utils::Scalar& fatiguedFibersDot() const; + const utils::Scalar& fatiguedFibersDot() const; /// /// \brief Return the resting fibers derivative /// \return The resting fibers derivative /// - const biorbd::utils::Scalar& restingFibersDot() const; + const utils::Scalar& restingFibersDot() const; /// /// \brief Compute the derivative of the current state @@ -66,20 +68,21 @@ class BIORBD_API FatigueDynamicState : public biorbd::muscles::FatigueState /// \param characteristics The muscle characteristics /// virtual void timeDerivativeState( - const biorbd::muscles::StateDynamics &emg, - const biorbd::muscles::Characteristics &characteristics + const StateDynamics &emg, + const Characteristics &characteristics ) = 0; protected: - std::shared_ptr + std::shared_ptr m_activeFibersDot; ///< The active fibers derivative - std::shared_ptr + std::shared_ptr m_fatiguedFibersDot; ///< The fatigued fibers derivative - std::shared_ptr + std::shared_ptr m_restingFibersDot; ///< The resting fibers derivative }; +} } } diff --git a/include/Muscles/FatigueDynamicStateXia.h b/include/Muscles/FatigueDynamicStateXia.h index f1df00a4..0aeda0c2 100644 --- a/include/Muscles/FatigueDynamicStateXia.h +++ b/include/Muscles/FatigueDynamicStateXia.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { @@ -13,7 +15,7 @@ namespace muscles /// \brief Implementation of Xia (https://www.sciencedirect.com/science/article/pii/S0021929008003692) dynamic fatigue model /// class BIORBD_API FatigueDynamicStateXia : public - biorbd::muscles::FatigueDynamicState + FatigueDynamicState { public: /// @@ -23,29 +25,29 @@ class BIORBD_API FatigueDynamicStateXia : public /// \param resting Resting muscle /// FatigueDynamicStateXia( - const biorbd::utils::Scalar& active = 1, - const biorbd::utils::Scalar& fatigued = 0, - const biorbd::utils::Scalar& resting = 0); + const utils::Scalar& active = 1, + const utils::Scalar& fatigued = 0, + const utils::Scalar& resting = 0); /// /// \brief Contruct FatigueDynamicsStateXia from another fatigue state /// \param other The other fatigue state /// FatigueDynamicStateXia( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of the fatigue dynamic state xia /// \return Deep copy of fatigue dynamic state xia /// - biorbd::muscles::FatigueDynamicStateXia DeepCopy() const; + FatigueDynamicStateXia DeepCopy() const; /// /// \brief Deep copy of a fatigue dynamic state xia /// \param other The fatigue dynamic state xia to copy /// void DeepCopy( - const biorbd::muscles::FatigueDynamicStateXia& other); + const FatigueDynamicStateXia& other); /// /// \brief Time derivative state of the current states @@ -53,8 +55,8 @@ class BIORBD_API FatigueDynamicStateXia : public /// \param characteristics The muscle characteristics /// virtual void timeDerivativeState( - const biorbd::muscles::StateDynamics &emg, - const biorbd::muscles::Characteristics &characteristics + const StateDynamics &emg, + const Characteristics &characteristics ); protected: /// @@ -64,6 +66,7 @@ class BIORBD_API FatigueDynamicStateXia : public }; +} } } diff --git a/include/Muscles/FatigueModel.h b/include/Muscles/FatigueModel.h index 9a59cc6a..b0c44508 100644 --- a/include/Muscles/FatigueModel.h +++ b/include/Muscles/FatigueModel.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -30,21 +32,21 @@ class BIORBD_API FatigueModel /// \param dynamicFatigueType The type of fatigue of the muscle /// FatigueModel( - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType); + STATE_FATIGUE_TYPE dynamicFatigueType); /// /// \brief Construct a fatigable muscle model from another muscle /// \param other The other fatigue model /// FatigueModel( - const biorbd::muscles::FatigueModel& other); + const FatigueModel& other); /// /// \brief Construct a fatigable muscle model from another muscle /// \param other The other fatigue model /// FatigueModel( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Destroy class properly @@ -56,14 +58,14 @@ class BIORBD_API FatigueModel /// \param other The other fatigue model to copy /// void DeepCopy( - const biorbd::muscles::FatigueModel& other); + const FatigueModel& other); /// /// \brief Compute the time derivative state /// \param emg EMG data /// virtual void computeTimeDerivativeState( - const biorbd::muscles::StateDynamics& emg); + const StateDynamics& emg); #ifndef BIORBD_USE_CASADI_MATH /// @@ -72,29 +74,30 @@ class BIORBD_API FatigueModel /// \param fatigued /// \param resting virtual void setFatigueState( - const biorbd::utils::Scalar& active, - const biorbd::utils::Scalar& fatigued, - const biorbd::utils::Scalar& resting); + const utils::Scalar& active, + const utils::Scalar& fatigued, + const utils::Scalar& resting); #endif /// /// \brief Return the fatigue state /// \return The fatigue state /// - biorbd::muscles::FatigueState& fatigueState(); + FatigueState& fatigueState(); /// /// \brief Return the fatigue state /// \return The fatigue state /// - const biorbd::muscles::FatigueState& fatigueState() const; + const FatigueState& fatigueState() const; protected: - std::shared_ptr + std::shared_ptr m_fatigueState; ///< The fatigue state }; +} } } diff --git a/include/Muscles/FatigueParameters.h b/include/Muscles/FatigueParameters.h index 31990b67..ee2a815a 100644 --- a/include/Muscles/FatigueParameters.h +++ b/include/Muscles/FatigueParameters.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { @@ -26,85 +28,86 @@ class BIORBD_API FatigueParameters /// \param recoveryFactor The recovery factor /// FatigueParameters( - const biorbd::utils::Scalar& fatigueRate = 0, - const biorbd::utils::Scalar& recoveryRate = 0, - const biorbd::utils::Scalar& developFactor = 0, - const biorbd::utils::Scalar& recoveryFactor = 0); + const utils::Scalar& fatigueRate = 0, + const utils::Scalar& recoveryRate = 0, + const utils::Scalar& developFactor = 0, + const utils::Scalar& recoveryFactor = 0); /// /// \brief Deep copy of the fatigue parameters /// \return Copy of the fatigue parameters /// - biorbd::muscles::FatigueParameters DeepCopy() const; + FatigueParameters DeepCopy() const; /// /// \brief Deep copy of fatigue parameters into another FatigueParameters /// \param other The fatigue parameters to copy /// void DeepCopy( - const biorbd::muscles::FatigueParameters& other); + const FatigueParameters& other); /// /// \brief Set the fatigue rate /// \param fatigueRate The fatigue rate /// void setFatigueRate( - const biorbd::utils::Scalar& fatigueRate); + const utils::Scalar& fatigueRate); // Get and Set /// /// \brief Return the fatigue rate /// \return The fatigue rate /// - const biorbd::utils::Scalar& fatigueRate() const; + const utils::Scalar& fatigueRate() const; /// /// \brief Set the recovery rate /// \param recoveryRate The recovery rate /// void setRecoveryRate( - const biorbd::utils::Scalar& recoveryRate); + const utils::Scalar& recoveryRate); /// /// \brief Return the recovery rate /// \return The recovery rate /// - const biorbd::utils::Scalar& recoveryRate() const; + const utils::Scalar& recoveryRate() const; /// /// \brief Set the develop factor /// \param developFactor The develop factor /// void setDevelopFactor( - const biorbd::utils::Scalar& developFactor); + const utils::Scalar& developFactor); /// /// \brief Return the develop factor /// \return The develop factor /// - const biorbd::utils::Scalar& developFactor() const; + const utils::Scalar& developFactor() const; /// /// \brief Set the recovery factor /// \param recoveryFactor The recovery factor /// void setRecoveryFactor( - const biorbd::utils::Scalar& recoveryFactor); + const utils::Scalar& recoveryFactor); /// /// \brief Return the recovery factor /// \return The recovery factor /// - const biorbd::utils::Scalar& recoveryFactor() const; + const utils::Scalar& recoveryFactor() const; protected: - std::shared_ptr m_fatigueRate; ///< The fatigue rate - std::shared_ptr m_recoveryRate;///< The recovery rate - std::shared_ptr m_developFactor; /// m_recoveryFactor; /// m_fatigueRate; ///< The fatigue rate + std::shared_ptr m_recoveryRate;///< The recovery rate + std::shared_ptr m_developFactor; /// m_recoveryFactor; /// other); + const std::shared_ptr other); /// /// \brief Destroy class properly @@ -51,14 +53,14 @@ class BIORBD_API FatigueState /// \brief Deep copy of the fatigue state /// \return A deep copy of the fatigue state /// - biorbd::muscles::FatigueState DeepCopy() const; + FatigueState DeepCopy() const; /// /// \brief Deep copy of fatigue state in another fatigue state /// \param other The fatigue state to copy /// void DeepCopy( - const biorbd::muscles::FatigueState& other); + const FatigueState& other); #ifndef BIORBD_USE_CASADI_MATH /// @@ -69,9 +71,9 @@ class BIORBD_API FatigueState /// \param turnOffWarnings If the warnings should be OFF or ON. /// virtual void setState( - biorbd::utils::Scalar active, - biorbd::utils::Scalar fatigued, - biorbd::utils::Scalar resting, + utils::Scalar active, + utils::Scalar fatigued, + utils::Scalar resting, bool turnOffWarnings = false); #endif @@ -79,43 +81,44 @@ class BIORBD_API FatigueState /// \brief Return the propotion of active muscle fibers /// \return The propotion of active muscle fibers /// - const biorbd::utils::Scalar& activeFibers() const; + const utils::Scalar& activeFibers() const; /// /// \brief Return the propotion of fatigued muscle fibers /// \return The propotion of fatigued muscle fibers /// - const biorbd::utils::Scalar& fatiguedFibers() const; + const utils::Scalar& fatiguedFibers() const; /// /// \brief Return the propotion of resting muscle fibers /// \return The propotion of resting muscle fibers /// - const biorbd::utils::Scalar& restingFibers() const; + const utils::Scalar& restingFibers() const; /// /// \brief Return the type of muscle fatigue model /// \return The type of muscle fatigue model /// - biorbd::muscles::STATE_FATIGUE_TYPE getType() const; + STATE_FATIGUE_TYPE getType() const; protected: - std::shared_ptr + std::shared_ptr m_activeFibers; ///< Proportion of active muscle fibers - std::shared_ptr + std::shared_ptr m_fatiguedFibers;///< Proportion of fatigued muscle fibers - std::shared_ptr + std::shared_ptr m_restingFibers;///< Proportion of resting muscle fibers /// /// \brief Set the type /// virtual void setType(); - std::shared_ptr + std::shared_ptr m_type; ///< Type of the muscle fatigue model }; +} } } diff --git a/include/Muscles/Geometry.h b/include/Muscles/Geometry.h index dfd6ef2c..5cef3e03 100644 --- a/include/Muscles/Geometry.h +++ b/include/Muscles/Geometry.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Matrix; @@ -15,15 +17,9 @@ class Vector; class Vector3d; } -namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; -} -} - -namespace rigidbody -{ class GeneralizedCoordinates; class GeneralizedVelocity; } @@ -50,21 +46,21 @@ class BIORBD_API Geometry /// \param insertion The insertion node of the muscle /// Geometry( - const biorbd::utils::Vector3d &origin, - const biorbd::utils::Vector3d &insertion); + const utils::Vector3d &origin, + const utils::Vector3d &insertion); /// /// \brief Deep copy of a geometry /// \return A deep copy of a geometry /// - biorbd::muscles::Geometry DeepCopy() const; + Geometry DeepCopy() const; /// /// \brief Deep copy of a geometry from another geometry /// \param other The geometry to copy /// void DeepCopy( - const biorbd::muscles::Geometry& other); + const Geometry& other); /// /// \brief Updates the position and dynamic elements of the muscles. @@ -77,8 +73,8 @@ class BIORBD_API Geometry /// void updateKinematics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates* Q = nullptr, - const biorbd::rigidbody::GeneralizedVelocity* Qdot = nullptr, + const rigidbody::GeneralizedCoordinates* Q = nullptr, + const rigidbody::GeneralizedVelocity* Qdot = nullptr, int updateKin = 2); /// @@ -94,10 +90,10 @@ class BIORBD_API Geometry /// void updateKinematics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::muscles::Characteristics& characteristics, - biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::rigidbody::GeneralizedCoordinates* Q = nullptr, - const biorbd::rigidbody::GeneralizedVelocity* Qdot = nullptr, + const Characteristics& characteristics, + PathModifiers& pathModifiers, + const rigidbody::GeneralizedCoordinates* Q = nullptr, + const rigidbody::GeneralizedVelocity* Qdot = nullptr, int updateKin = 2); /// @@ -109,9 +105,9 @@ class BIORBD_API Geometry /// updateKinematics MUST be called before retreiving data that are dependent on Q and/or Qdot /// void updateKinematics( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity* Qdot = nullptr); + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity* Qdot = nullptr); /// /// \brief Updates the position and dynamic elements of the muscles by hand. @@ -123,36 +119,36 @@ class BIORBD_API Geometry /// updateKinematics MUST be called before retreiving data that are dependent on Q and/or Qdot /// void updateKinematics( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::rigidbody::GeneralizedVelocity* Qdot = nullptr); + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal, + const Characteristics& characteristics, + const rigidbody::GeneralizedVelocity* Qdot = nullptr); /// /// \brief Set the origin position in the local reference frame of the muscle /// \param position The origin position to set /// void setOrigin( - const biorbd::utils::Vector3d &position); + const utils::Vector3d &position); /// /// \brief Return the origin position /// \return The origin position /// - const biorbd::utils::Vector3d& originInLocal() const; + const utils::Vector3d& originInLocal() const; /// /// \brief Set the insertion position /// \param position The insertion position value to set /// void setInsertionInLocal( - const biorbd::utils::Vector3d &position); + const utils::Vector3d &position); /// /// \brief Return the insertion position /// \return The insertion position /// - const biorbd::utils::Vector3d& insertionInLocal() const; + const utils::Vector3d& insertionInLocal() const; /// /// \brief Return the origin position in the global reference @@ -160,7 +156,7 @@ class BIORBD_API Geometry /// /// Geometry (updateKin) must be computed at least once before calling originInLocal() /// - const biorbd::utils::Vector3d& originInGlobal() const; + const utils::Vector3d& originInGlobal() const; /// /// \brief Return the insertion position in the global reference @@ -168,7 +164,7 @@ class BIORBD_API Geometry /// /// Geometry (updateKin) must be computed at least once before calling /// - const biorbd::utils::Vector3d& insertionInGlobal() const; + const utils::Vector3d& insertionInGlobal() const; /// /// \brief Return the muscles points in the global reference @@ -176,57 +172,57 @@ class BIORBD_API Geometry /// /// Geometry (updateKin) must be computed at least once before calling. Return already computed via points /// - const std::vector& musclesPointsInGlobal() const; + const std::vector& musclesPointsInGlobal() const; /// /// \brief Return the previously computed muscle length /// \return The muscle lengh /// - const biorbd::utils::Scalar& length() const; + const utils::Scalar& length() const; /// /// \brief Return the previously computed muscle-tendon length /// \return The muscle-tendon length /// - const biorbd::utils::Scalar& musculoTendonLength() const; + const utils::Scalar& musculoTendonLength() const; /// /// \brief Return the previously computed velocity /// \return The computed velocity /// - const biorbd::utils::Scalar& velocity() const; + const utils::Scalar& velocity() const; /// /// \brief Return the previously computed muscle jacobian /// \return The muscle jacobian /// - const biorbd::utils::Matrix& jacobian() const; + const utils::Matrix& jacobian() const; /// /// \brief Return the gradient of the jacobian for the origin node /// \return The origin's node jacobian /// - biorbd::utils::Matrix jacobianOrigin() const; + utils::Matrix jacobianOrigin() const; /// /// \brief Return the gradient of the jacobian for the insertion node /// \return The insertion's node jacobian /// - biorbd::utils::Matrix jacobianInsertion() const ; + utils::Matrix jacobianInsertion() const ; /// /// \brief Return the gradient of the jacobian for the via specified node /// \brief idxViaPoint the index of the via point (0 being the the origin) /// \return The via point's node jacobian /// - biorbd::utils::Matrix jacobian( + utils::Matrix jacobian( unsigned int idxViaPoint) const; /// /// \brief Return the jacobian length of the muscle /// \return The jacobian length /// - const biorbd::utils::Matrix& jacobianLength() const; + const utils::Matrix& jacobianLength() const; protected: @@ -237,9 +233,9 @@ class BIORBD_API Geometry /// \param pathModifiers The set of path modifiers /// void _updateKinematics( - const biorbd::rigidbody::GeneralizedVelocity *Qdot, - const biorbd::muscles::Characteristics* characteristics = nullptr, - biorbd::muscles::PathModifiers* pathModifiers = nullptr); + const rigidbody::GeneralizedVelocity *Qdot, + const Characteristics* characteristics = nullptr, + PathModifiers* pathModifiers = nullptr); /// /// \brief Updates the kinematics and return the position of the origin node @@ -247,9 +243,9 @@ class BIORBD_API Geometry /// \param Q The generalized coordinates of the model /// \return The origin position in the global reference /// - const biorbd::utils::Vector3d& originInGlobal( + const utils::Vector3d& originInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q); + const rigidbody::GeneralizedCoordinates &Q); /// /// \brief Updates the kinematics and return the position of the insertion node @@ -257,16 +253,16 @@ class BIORBD_API Geometry /// \param Q The generalized coordinates of the model /// \return The origin position in the global reference /// - const biorbd::utils::Vector3d& insertionInGlobal( + const utils::Vector3d& insertionInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q); + const rigidbody::GeneralizedCoordinates &Q); /// /// \brief Set the muscle points in global /// \param ptsInGlobal The new muscle points in global /// void setMusclesPointsInGlobal( - std::vector& ptsInGlobal); + std::vector& ptsInGlobal); /// /// \brief Set the muscle points in global @@ -276,8 +272,8 @@ class BIORBD_API Geometry /// void setMusclesPointsInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - biorbd::muscles::PathModifiers* pathModifiers = nullptr); + const rigidbody::GeneralizedCoordinates& Q, + PathModifiers* pathModifiers = nullptr); /// @@ -286,17 +282,17 @@ class BIORBD_API Geometry /// \param pathModifiers The set of path modifiers /// \return The muscle length /// - const biorbd::utils::Scalar& length( - const biorbd::muscles::Characteristics* characteristics = nullptr, - biorbd::muscles::PathModifiers* pathModifiers = nullptr); + const utils::Scalar& length( + const Characteristics* characteristics = nullptr, + PathModifiers* pathModifiers = nullptr); /// /// \brief Update the kinematics, compute and return the muscle velocity assuming not via points nor wrapping objects /// \param Qdot The generalized velocities /// \return The muscle velocity /// - const biorbd::utils::Scalar& velocity( - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + const utils::Scalar& velocity( + const rigidbody::GeneralizedVelocity &Qdot); /// /// \brief Set the jacobian dimensions @@ -310,7 +306,7 @@ class BIORBD_API Geometry /// \param jaco The jacobian /// void jacobian( - const biorbd::utils::Matrix &jaco); + const utils::Matrix &jaco); /// /// \brief Compute the jacobian @@ -319,7 +315,7 @@ class BIORBD_API Geometry /// void jacobian( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q); + const rigidbody::GeneralizedCoordinates &Q); /// /// \brief Compute the muscle length jacobian @@ -327,27 +323,27 @@ class BIORBD_API Geometry void computeJacobianLength(); // Position des nodes dans le repere local - std::shared_ptr m_origin; ///< Origin node - std::shared_ptr m_insertion; ///< Insertion node + std::shared_ptr m_origin; ///< Origin node + std::shared_ptr m_insertion; ///< Insertion node - std::shared_ptr + std::shared_ptr m_originInGlobal; ///< Position of the origin in the global reference - std::shared_ptr + std::shared_ptr m_insertionInGlobal; ///< Position of the insertion node in the global reference - std::shared_ptr> + std::shared_ptr> m_pointsInGlobal; ///< Position of all the points in the global reference - std::shared_ptr> + std::shared_ptr> m_pointsInLocal; ///< Position of all the points in local - std::shared_ptr m_jacobian; /// + std::shared_ptr m_jacobian; /// m_G; ///< Internal matrix of the jacobian dimension to speed up calculation - std::shared_ptr + std::shared_ptr m_jacobianLength; ///< The muscle length jacobian - std::shared_ptr m_length; ///< Muscle length - std::shared_ptr + std::shared_ptr m_length; ///< Muscle length + std::shared_ptr m_muscleTendonLength; ///< Muscle tendon length - std::shared_ptr + std::shared_ptr m_velocity; ///< Velocity of the muscular elongation std::shared_ptr @@ -359,6 +355,7 @@ class BIORBD_API Geometry }; +} } } diff --git a/include/Muscles/HillThelenActiveOnlyType.h b/include/Muscles/HillThelenActiveOnlyType.h index 1ef722bc..d0b6d1f3 100644 --- a/include/Muscles/HillThelenActiveOnlyType.h +++ b/include/Muscles/HillThelenActiveOnlyType.h @@ -6,13 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Muscle of Hill type augmented by Thelen (https://simtk-confluence.stanford.edu/display/OpenSim/Thelen+2003+Muscle+Model) /// class BIORBD_API HillThelenActiveOnlyType : public - biorbd::muscles::HillThelenType + HillThelenType { public: /// @@ -27,9 +29,9 @@ class BIORBD_API HillThelenActiveOnlyType : public /// \param characteristics The muscle characteristics /// HillThelenActiveOnlyType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics); /// /// \brief Construct a Hill-Thelen-type muscle @@ -39,10 +41,10 @@ class BIORBD_API HillThelenActiveOnlyType : public /// \param emg The muscle dynamic state /// HillThelenActiveOnlyType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const State& emg); /// /// \brief Construct a Hill-Thelen-type muscle @@ -52,10 +54,10 @@ class BIORBD_API HillThelenActiveOnlyType : public /// \param pathModifiers The set of path modifiers /// HillThelenActiveOnlyType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers); /// /// \brief Construct a Hill-Thelen-type muscle @@ -66,37 +68,37 @@ class BIORBD_API HillThelenActiveOnlyType : public /// \param emg The dynamic state /// HillThelenActiveOnlyType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg); /// /// \brief Construct a Hill-Thelen-type muscle from another muscle /// \param other The other muscle /// HillThelenActiveOnlyType( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct a Hill-Thelen-type muscle from another muscle /// \param other The other muscle (pointer) /// HillThelenActiveOnlyType( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of a Hill-Thelen-type muscle /// \return A deep copy of a Hill-Thelen-type muscle /// - biorbd::muscles::HillThelenActiveOnlyType DeepCopy() const; + HillThelenActiveOnlyType DeepCopy() const; /// /// \brief Deep copy of a Hill-Thelen-type muscle in a new Hill-Thelen-type muscle /// \param other The Hill-Thelen-type muscle to copy /// - void DeepCopy(const biorbd::muscles::HillThelenActiveOnlyType& other); + void DeepCopy(const HillThelenActiveOnlyType& other); /// /// \brief Compute the Force-Length of the passive element @@ -116,6 +118,7 @@ class BIORBD_API HillThelenActiveOnlyType : public }; +} } } diff --git a/include/Muscles/HillThelenType.h b/include/Muscles/HillThelenType.h index 8cfcdd24..7e021122 100644 --- a/include/Muscles/HillThelenType.h +++ b/include/Muscles/HillThelenType.h @@ -6,13 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Muscle of Hill type augmented by Thelen /// https://simtk-confluence.stanford.edu/display/OpenSim/Thelen+2003+Muscle+Model /// -class BIORBD_API HillThelenType : public biorbd::muscles::HillType +class BIORBD_API HillThelenType : public HillType { public: /// @@ -27,9 +29,9 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType /// \param characteristics The muscle characteristics /// HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics); /// /// \brief Construct a Hill-Thelen-type muscle @@ -39,10 +41,10 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType /// \param emg The muscle dynamic state /// HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const State& emg); /// /// \brief Construct a Hill-Thelen-type muscle @@ -52,10 +54,10 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType /// \param pathModifiers The set of path modifiers /// HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers); /// /// \brief Construct a Hill-Thelen-type muscle @@ -66,37 +68,37 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType /// \param emg The dynamic state /// HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg); /// /// \brief Construct a Hill-Thelen-type muscle from another muscle /// \param other The other muscle /// HillThelenType( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct a Hill-Thelen-type muscle from another muscle /// \param other The other muscle (pointer) /// HillThelenType( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of a Hill-Thelen-type muscle /// \return A deep copy of a Hill-Thelen-type muscle /// - biorbd::muscles::HillThelenType DeepCopy() const; + HillThelenType DeepCopy() const; /// /// \brief Deep copy of a Hill-Thelen-type muscle in a new Hill-Thelen-type muscle /// \param other The Hill-Thelen-type muscle to copy /// - void DeepCopy(const biorbd::muscles::HillThelenType& other); + void DeepCopy(const HillThelenType& other); /// /// \brief Compute the Force-Length of the passive element @@ -107,7 +109,7 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType /// \brief Compute the Force-Length of the passive element /// \param emg EMG data /// - virtual void computeFlCE(const biorbd::muscles::State &emg); + virtual void computeFlCE(const State &emg); protected: /// @@ -117,6 +119,7 @@ class BIORBD_API HillThelenType : public biorbd::muscles::HillType }; +} } } diff --git a/include/Muscles/HillThelenTypeFatigable.h b/include/Muscles/HillThelenTypeFatigable.h index 2450e47c..aa18a266 100644 --- a/include/Muscles/HillThelenTypeFatigable.h +++ b/include/Muscles/HillThelenTypeFatigable.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -25,7 +27,7 @@ namespace muscles /// recoverFactor = 10 /// class BIORBD_API HillThelenTypeFatigable : public - biorbd::muscles::HillThelenType, public biorbd::muscles::FatigueModel + HillThelenType, public FatigueModel { public: /// @@ -41,11 +43,11 @@ class BIORBD_API HillThelenTypeFatigable : public /// \param dynamicFatigueType The muscle dynamic fatigue type /// HillThelenTypeFatigable( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); /// /// \brief Construct a Hill-Thelen-type fatigable muscle @@ -56,12 +58,12 @@ class BIORBD_API HillThelenTypeFatigable : public /// \param dynamicFatigueType The muscle dynamic fatigue model /// HillThelenTypeFatigable( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const State& emg, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); /// /// \brief Construct a Hill-Thelen-type fatigable muscle @@ -72,12 +74,12 @@ class BIORBD_API HillThelenTypeFatigable : public /// \param dynamicFatigueType The muscle dynamic fatigue model /// HillThelenTypeFatigable( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); /// /// \brief Construct a Hill-Thelen-type fatigable muscle @@ -89,46 +91,46 @@ class BIORBD_API HillThelenTypeFatigable : public /// \param dynamicFatigueType The muscle dynamic fatigue model /// HillThelenTypeFatigable( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); /// /// \brief Construct a Hill-Thelen-type fatigable muscle from another muscle /// \param other The other muscle /// HillThelenTypeFatigable( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct a Hill-Thelen-type muscle from another muscle /// \param other The other muscle (pointer) /// HillThelenTypeFatigable( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of a Hill-Thelen-type fatigable muscle /// \return A deep copy of a Hill-Thelen-type fatigable muscle /// - biorbd::muscles::HillThelenTypeFatigable DeepCopy() const; + HillThelenTypeFatigable DeepCopy() const; /// /// \brief Deep copy of a Hill-Thelen-type fatigable muscle in a new Hill-Thelen-type fatigable muscle /// \param other The Hill-Thelen-type fatigable muscle to copy /// - void DeepCopy(const biorbd::muscles::HillThelenTypeFatigable& other); + void DeepCopy(const HillThelenTypeFatigable& other); /// /// \brief Compute the Force-Length of the contractile element /// \param emg EMG data /// - virtual void computeFlCE(const biorbd::muscles::State &emg); + virtual void computeFlCE(const State &emg); protected: /// @@ -138,6 +140,7 @@ class BIORBD_API HillThelenTypeFatigable : public }; +} } } diff --git a/include/Muscles/HillType.h b/include/Muscles/HillType.h index 000684bd..191d4336 100644 --- a/include/Muscles/HillType.h +++ b/include/Muscles/HillType.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { @@ -24,7 +26,7 @@ namespace muscles /// - \f$damping = 0.1\f$ /// - \f$maxShorteningSpeed = 10.0\f$ /// -class BIORBD_API HillType : public biorbd::muscles::Muscle +class BIORBD_API HillType : public Muscle { public: /// @@ -39,9 +41,9 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param characteristics The muscle characteristics /// HillType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics); /// /// \brief Construct a Hill-type muscle @@ -51,10 +53,10 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param emg The muscle dynamic state /// HillType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const State& emg); /// /// \brief Construct a Hill-type muscle @@ -64,10 +66,10 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param pathModifiers The set of path modifiers /// HillType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers); /// /// \brief Construct a Hill-type muscle @@ -78,46 +80,46 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param emg The dynamic state /// HillType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg); /// /// \brief Construct a Hill-type muscle from another muscle /// \param other The other muscle /// HillType( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct a Hill-type muscle from another muscle /// \param other THe other muscle /// HillType( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of a Hill-type muscle /// \return A deep copy of a Hill-type muscle /// - biorbd::muscles::HillType DeepCopy() const; + HillType DeepCopy() const; /// /// \brief Deep copy of a Hill-type muscle in a new Hill-type muscle /// \param other The Hill-type to copy /// void DeepCopy( - const biorbd::muscles::HillType& other); + const HillType& other); /// /// \brief Return the muscle force vector at origin and insertion /// \param emg The EMG data /// \return The force vector at origin and insertion /// - virtual const biorbd::utils::Scalar& force( - const biorbd::muscles::State& emg); + virtual const utils::Scalar& force( + const State& emg); /// /// \brief Return the muscle force vector at origin and insertion @@ -128,11 +130,11 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The force vector at origin and insertion /// - virtual const biorbd::utils::Scalar& force( + virtual const utils::Scalar& force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::muscles::State& emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const State& emg, int updateKin = 2); /// @@ -143,36 +145,36 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The force vector at origin and insertion /// - virtual const biorbd::utils::Scalar& force( + virtual const utils::Scalar& force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::muscles::State& emg, + const rigidbody::GeneralizedCoordinates& Q, + const State& emg, int updateKin = 2); /// /// \brief Return the Force-Length of the contractile element /// \return The Force-Length of the contractile element /// - const biorbd::utils::Scalar& FlCE( - const biorbd::muscles::State& emg); + const utils::Scalar& FlCE( + const State& emg); /// /// \brief Return the Force-Length of the passive element /// \return The Force-Length of the passive element /// - const biorbd::utils::Scalar& FlPE(); + const utils::Scalar& FlPE(); /// /// \brief Return the Force-Velocity of the contractile element /// \return The Force-Velocity of the contractile element /// - const biorbd::utils::Scalar& FvCE(); + const utils::Scalar& FvCE(); /// /// \brief Return the muscle damping (spring force) /// \return The muscle damping /// - const biorbd::utils::Scalar& damping(); + const utils::Scalar& damping(); protected: /// @@ -189,7 +191,7 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \brief Compute the Force-length of the contractile element /// virtual void computeFlCE( - const biorbd::muscles::State &emg); + const State &emg); /// /// \brief Compute the Force-Velocity of the contractile element @@ -205,48 +207,49 @@ class BIORBD_API HillType : public biorbd::muscles::Muscle /// \brief Function allowing modification of the way the multiplication is done in computeForce(EMG) /// \param emg The EMG data /// \return The force from activation - virtual biorbd::utils::Scalar getForceFromActivation( - const biorbd::muscles::State &emg); + virtual utils::Scalar getForceFromActivation( + const State &emg); /// /// \brief Normalize the EMG data /// \param emg EMG data /// virtual void normalizeEmg( - biorbd::muscles::State& emg); + State& emg); // Attributs intermédiaires lors du calcul de la force - std::shared_ptr + std::shared_ptr m_damping; ///< Muscle damping (spring force) - std::shared_ptr + std::shared_ptr m_FlCE; /// + std::shared_ptr m_FlPE; ///< Force-Length of the passive element - std::shared_ptr + std::shared_ptr m_FvCE; /// + std::shared_ptr m_cste_FlCE_1; ///< constant 1 used in the FlCE - std::shared_ptr + std::shared_ptr m_cste_FlCE_2; ///< constant 2 used in the FlCE - std::shared_ptr + std::shared_ptr m_cste_FvCE_1; ///< constant 1 used in the FvCE - std::shared_ptr + std::shared_ptr m_cste_FvCE_2; ///< constant 2 used in the FvCE - std::shared_ptr + std::shared_ptr m_cste_FlPE_1; ///< constant 1 used in the FlPE - std::shared_ptr + std::shared_ptr m_cste_FlPE_2; ///< constant 2 used in the FlPE - std::shared_ptr + std::shared_ptr m_cste_eccentricForceMultiplier; ///< Constant used for ForceVelocity - std::shared_ptr + std::shared_ptr m_cste_damping; ///< parameters used in damping - std::shared_ptr + std::shared_ptr m_cste_maxShorteningSpeed; ///< Maximal velocity of shortening }; +} } } diff --git a/include/Muscles/IdealizedActuator.h b/include/Muscles/IdealizedActuator.h index 0e5f9325..3c8fc61a 100644 --- a/include/Muscles/IdealizedActuator.h +++ b/include/Muscles/IdealizedActuator.h @@ -6,13 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Muscle that has a constant maximal force /// -class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle +class BIORBD_API IdealizedActuator : public Muscle { public: /// @@ -27,9 +29,9 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param characteristics The muscle characteristics /// IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics); /// /// \brief Construct an idealized actuator @@ -39,10 +41,10 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param emg The muscle dynamic state /// IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const State& emg); /// /// \brief Construct an idealized actuator @@ -52,10 +54,10 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param pathModifiers The set of path modifiers /// IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers); /// /// \brief Construct an idealized actuator @@ -66,45 +68,45 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param emg The dynamic state /// IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg); /// /// \brief Construct an idealized actuator from another muscle /// \param other The other muscle /// IdealizedActuator( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct an idealized actuator from another muscle /// \param other The other muscle /// IdealizedActuator( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Deep copy of an idealized actuator /// \return A deep copy of an idealized actuator /// - biorbd::muscles::IdealizedActuator DeepCopy() const; + IdealizedActuator DeepCopy() const; /// /// \brief Deep copy of an idealized actuator from another idealized actuator /// \param other The idealized actuator to copy /// - void DeepCopy(const biorbd::muscles::IdealizedActuator& other); + void DeepCopy(const IdealizedActuator& other); /// /// \brief Return the force /// \param emg EMG data /// \return The force /// - virtual const biorbd::utils::Scalar& force( - const biorbd::muscles::State& emg); + virtual const utils::Scalar& force( + const State& emg); /// /// \brief Return the muscle force vector at origin and insertion @@ -115,11 +117,11 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The force vector at origin and insertion /// - virtual const biorbd::utils::Scalar& force( + virtual const utils::Scalar& force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::muscles::State& emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const State& emg, int updateKin = 2); /// @@ -130,18 +132,18 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The force vector at origin and insertion /// - virtual const biorbd::utils::Scalar& force( + virtual const utils::Scalar& force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::muscles::State& emg, + const rigidbody::GeneralizedCoordinates& Q, + const State& emg, int updateKin = 2); protected: /// /// \brief Function allowing modification of the way the multiplication is done in computeForce(EMG) /// \param emg The EMG data /// \return The force from activation - virtual biorbd::utils::Scalar getForceFromActivation( - const biorbd::muscles::State &emg); + virtual utils::Scalar getForceFromActivation( + const State &emg); /// /// \brief Set the type to Idealized_actuator @@ -150,6 +152,7 @@ class BIORBD_API IdealizedActuator : public biorbd::muscles::Muscle }; +} } } diff --git a/include/Muscles/Muscle.h b/include/Muscles/Muscle.h index fbc9b2c9..9c8cc1a6 100644 --- a/include/Muscles/Muscle.h +++ b/include/Muscles/Muscle.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Matrix; @@ -23,7 +25,7 @@ class Muscles; /// /// \brief Base class of all muscle /// -class BIORBD_API Muscle : public biorbd::muscles::Compound +class BIORBD_API Muscle : public Compound { friend Muscles; @@ -40,9 +42,9 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param characteristics Muscle characteristics from an initial state /// Muscle( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& position, - const biorbd::muscles::Characteristics& characteristics); + const utils::String& name, + const Geometry& position, + const Characteristics& characteristics); /// /// \brief Construct a muscle @@ -52,10 +54,10 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param emg Dynamic state /// Muscle( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& position, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& position, + const Characteristics& characteristics, + const State& emg); /// /// \brief Construct a muscle @@ -65,10 +67,10 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param pathModifiers The path modifiers /// Muscle( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& position, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers); + const utils::String& name, + const Geometry& position, + const Characteristics& characteristics, + const PathModifiers& pathModifiers); /// /// \brief Construct a muscle @@ -79,25 +81,25 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param emg The dynamic state /// Muscle( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& position, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg); + const utils::String& name, + const Geometry& position, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + const State& emg); /// /// \brief Construct a muscle from another muscle /// \param other The other muscle /// Muscle( - const biorbd::muscles::Muscle& other); + const Muscle& other); /// /// \brief Construct a muscle from another muscle /// \param other The other muscle /// Muscle( - const std::shared_ptr other); + const std::shared_ptr other); /// /// \brief Destroy class properly @@ -109,7 +111,7 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param other The muscle to copy /// void DeepCopy( - const biorbd::muscles::Muscle& other); + const Muscle& other); // Get and set @@ -120,9 +122,9 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The length of the muscle /// - const biorbd::utils::Scalar& length( + const utils::Scalar& length( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, int updateKin = 2); /// @@ -132,9 +134,9 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// \return The musculo tendon length /// - const biorbd::utils::Scalar& musculoTendonLength( + const utils::Scalar& musculoTendonLength( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, int updateKin = 2); /// @@ -145,10 +147,10 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) //// \return The velocity of the muscle /// - const biorbd::utils::Scalar& velocity( + const utils::Scalar& velocity( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, bool updateKin = true); /// @@ -159,7 +161,7 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// void updateOrientations( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, int updateKin = 2); /// @@ -171,8 +173,8 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// void updateOrientations( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, int updateKin = 2); /// @@ -181,8 +183,8 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param jacoPointsInGlobal The jacobian matrix /// void updateOrientations( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal); + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal); /// /// \brief Update by hand the position of the origin and insertion nodes of the muscle @@ -191,35 +193,35 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param Qdot The genelized velocities /// void updateOrientations( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity &Qdot); + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity &Qdot); /// /// \brief Set the position of all the points attached to the muscle (0 being the origin) /// \param positions New value of the position /// void setPosition( - const biorbd::muscles::Geometry &positions); + const Geometry &positions); /// /// \brief Return the position of all the points attached to the muscle (0 being the origin) /// \return The positions /// - const biorbd::muscles::Geometry& position() const; + const Geometry& position() const; /// /// \brief Set the muscle characteristics /// \param characteristics New value of the muscle characteristics /// void setCharacteristics( - const biorbd::muscles::Characteristics &characteristics); + const Characteristics &characteristics); /// /// \brief Return the muscle characteristics /// \return The muscle characteristics /// - const biorbd::muscles::Characteristics& characteristics() const; + const Characteristics& characteristics() const; /// /// \brief Return the muscle points in global reference frame @@ -227,41 +229,41 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param Q The generalized coordinates /// \return The muscle points in global reference frame /// - const std::vector& musclesPointsInGlobal( + const std::vector& musclesPointsInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q); + const rigidbody::GeneralizedCoordinates &Q); /// /// \brief Return the previously computed muscle points in global reference frame /// \return The muscle points in global reference frame /// - const std::vector& musclesPointsInGlobal() const; + const std::vector& musclesPointsInGlobal() const; /// /// \brief Set the maximal isometric force /// \param forceMax The force to set /// void setForceIsoMax( - const biorbd::utils::Scalar& forceMax); + const utils::Scalar& forceMax); /// /// \brief Set the dynamic state /// \param emg The dynamic state value /// void setState( - const biorbd::muscles::State &emg); + const State &emg); /// /// \brief Return the dynamic state /// \return The dynamic state /// - const biorbd::muscles::State& state() const; + const State& state() const; /// /// \brief Return the dynamic state /// \return The dynamic state /// - biorbd::muscles::State& state(); + State& state(); /// /// \brief Return the activation time derivative @@ -269,8 +271,8 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param alreadyNormalized If the emg is already normalized /// \return The activation time derivative /// - const biorbd::utils::Scalar& activationDot( - const biorbd::muscles::State& state, + const utils::Scalar& activationDot( + const State& state, bool alreadyNormalized = false) const; protected: /// @@ -278,24 +280,25 @@ class BIORBD_API Muscle : public biorbd::muscles::Compound /// \param emg EMG data /// virtual void computeForce( - const biorbd::muscles::State &emg); + const State &emg); /// /// \brief Function allowing modification of the way the multiplication is done in computeForce(EMG) /// \param emg The EMG data /// \return The force from activation /// - virtual biorbd::utils::Scalar getForceFromActivation(const - biorbd::muscles::State &emg) = 0; + virtual utils::Scalar getForceFromActivation(const + State &emg) = 0; - std::shared_ptr + std::shared_ptr m_position; ///< The position of all the nodes of the muscle (0 being the origin and last being insertion - std::shared_ptr + std::shared_ptr m_characteristics; ///< The muscle characteristics - std::shared_ptr m_state; ///< The dynamic state + std::shared_ptr m_state; ///< The dynamic state }; +} } } diff --git a/include/Muscles/MuscleGroup.h b/include/Muscles/MuscleGroup.h index 7412eae0..ea823d9c 100644 --- a/include/Muscles/MuscleGroup.h +++ b/include/Muscles/MuscleGroup.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -35,7 +37,7 @@ class BIORBD_API MuscleGroup /// \param other The other muscle group /// MuscleGroup( - const biorbd::muscles::MuscleGroup& other); + const MuscleGroup& other); /// /// \brief Construct a muscle group @@ -44,9 +46,9 @@ class BIORBD_API MuscleGroup /// \param insertionName The segment name where the insertion lies /// MuscleGroup( - const biorbd::utils::String &name, - const biorbd::utils::String &originName, - const biorbd::utils::String &insertionName); + const utils::String &name, + const utils::String &originName, + const utils::String &insertionName); /// /// \brief Destroy class properly @@ -57,14 +59,14 @@ class BIORBD_API MuscleGroup /// \brief Deep copy of a muscle group /// \return A deep copy of a muscle group /// - biorbd::muscles::MuscleGroup DeepCopy() const; + MuscleGroup DeepCopy() const; /// /// \brief Deep copy of a muscle group in new muscle group /// \param other The muscle group to copy /// void DeepCopy( - const biorbd::muscles::MuscleGroup& other); + const MuscleGroup& other); #ifndef SWIG /// @@ -77,14 +79,14 @@ class BIORBD_API MuscleGroup /// \param dynamicFatigueType The dynamic state fatigue type /// virtual void addMuscle( - const biorbd::utils::String& name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - biorbd::muscles::STATE_TYPE stateType = - biorbd::muscles::STATE_TYPE::NO_STATE_TYPE, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + const utils::String& name, + MUSCLE_TYPE type, + const Geometry& geometry, + const Characteristics& characteristics, + STATE_TYPE stateType = + STATE_TYPE::NO_STATE_TYPE, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); /// /// \brief To add a muscle to the group @@ -95,11 +97,11 @@ class BIORBD_API MuscleGroup /// \param dynamicFatigueType The dynamic state fatigue type /// virtual void addMuscle( - const biorbd::utils::String& name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType); + const utils::String& name, + MUSCLE_TYPE type, + const Geometry& geometry, + const Characteristics& characteristics, + STATE_FATIGUE_TYPE dynamicFatigueType); /// /// \brief To add a muscle to the group @@ -112,15 +114,15 @@ class BIORBD_API MuscleGroup /// \param dynamicFatigueType The dynamic state fatigue type /// virtual void addMuscle( - const biorbd::utils::String& name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - biorbd::muscles::STATE_TYPE stateType = - biorbd::muscles::STATE_TYPE::NO_STATE_TYPE, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType = - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + const utils::String& name, + MUSCLE_TYPE type, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + STATE_TYPE stateType = + STATE_TYPE::NO_STATE_TYPE, + STATE_FATIGUE_TYPE dynamicFatigueType = + STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); /// /// \brief To add a muscle to the group @@ -132,12 +134,12 @@ class BIORBD_API MuscleGroup /// \param dynamicFatigueType The dynamic state fatigue type /// virtual void addMuscle( - const biorbd::utils::String& name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType); + const utils::String& name, + MUSCLE_TYPE type, + const Geometry& geometry, + const Characteristics& characteristics, + const PathModifiers& pathModifiers, + STATE_FATIGUE_TYPE dynamicFatigueType); #endif /// @@ -145,7 +147,7 @@ class BIORBD_API MuscleGroup /// \param muscle The muscle to add /// virtual void addMuscle( - const biorbd::muscles::Muscle &muscle); + const Muscle &muscle); /// /// \brief Return the number of muscles in the group @@ -157,20 +159,20 @@ class BIORBD_API MuscleGroup /// \brief Return the muscles in the group /// \return The muscles /// - std::vector>& muscles(); + std::vector>& muscles(); /// /// \brief Return the muscles in the group /// \return The muscles /// - const std::vector>& muscles() const; + const std::vector>& muscles() const; /// /// \brief Return the muscle of a specific index in the group /// \param idx The muscle index /// \return The muscle of a specific index /// - biorbd::muscles::Muscle& muscle( + Muscle& muscle( unsigned int idx); /// @@ -178,7 +180,7 @@ class BIORBD_API MuscleGroup /// \param idx The muscle index /// \return The muscle of a specific index /// - const biorbd::muscles::Muscle& muscle( + const Muscle& muscle( unsigned int idx) const; /// @@ -187,56 +189,57 @@ class BIORBD_API MuscleGroup /// \return The muscle index /// int muscleID( - const biorbd::utils::String& name); + const utils::String& name); /// /// \brief Set the name of the muscle group /// \param name The name of the muscle group /// void setName( - const biorbd::utils::String& name); + const utils::String& name); /// /// \brief Return the name of the muscle group /// \return The name of the muscle /// - const biorbd::utils::String& name() const; + const utils::String& name() const; /// /// \brief Set the origin segment name where the origin lies /// \param name The origin segment name where the origin lies /// void setOrigin( - const biorbd::utils::String& name); + const utils::String& name); /// /// \brief Return the origin segment name /// \return The origin segment name /// - const biorbd::utils::String& origin() const; + const utils::String& origin() const; /// /// \brief Set the insertion segment name where the origin lies /// \param name The insertion segment name where the origin lies /// void setInsertion( - const biorbd::utils::String& name); + const utils::String& name); /// /// \brief Return the insertion segment name /// \return The insertion segment name /// - const biorbd::utils::String& insertion() const; + const utils::String& insertion() const; protected: - std::shared_ptr>> + std::shared_ptr>> m_mus; ///< The set of muscles - std::shared_ptr m_name; ///< The muscle group name - std::shared_ptr m_originName; /// m_insertName; ///< The insertion name + std::shared_ptr m_name; ///< The muscle group name + std::shared_ptr m_originName; /// m_insertName; ///< The insertion name }; +} } } diff --git a/include/Muscles/Muscles.h b/include/Muscles/Muscles.h index 62c24312..9aa1ea08 100644 --- a/include/Muscles/Muscles.h +++ b/include/Muscles/Muscles.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -44,7 +46,7 @@ class BIORBD_API Muscles /// \brief Construct muscles from other muscles /// \param other The other muscles /// - Muscles(const biorbd::muscles::Muscles& other); + Muscles(const Muscles& other); /// /// \brief Destroy class properly @@ -55,14 +57,14 @@ class BIORBD_API Muscles /// \brief Deep copy of muscles /// \return A deep copy of muscles /// - biorbd::muscles::Muscles DeepCopy() const; + Muscles DeepCopy() const; /// /// \brief Deep copy of muscles into another mucles /// \param other The muscles to copy /// void DeepCopy( - const biorbd::muscles::Muscles& other); + const Muscles& other); /// /// \brief Add a muscle group to the set @@ -71,9 +73,9 @@ class BIORBD_API Muscles /// \param insertionName The insertion segment name where the origin lies /// void addMuscleGroup( - const biorbd::utils::String &name, - const biorbd::utils::String &originName, - const biorbd::utils::String &insertionName); + const utils::String &name, + const utils::String &originName, + const utils::String &insertionName); /// /// \brief Return the group ID @@ -81,21 +83,21 @@ class BIORBD_API Muscles /// \return The group ID (returns -1 if not found) /// int getMuscleGroupId( - const biorbd::utils::String &name) const; + const utils::String &name) const; /// /// \brief Returns all the muscles. It sorts the muscles by group /// \return All the muscle /// - const std::vector> muscles() const; + const std::vector> muscles() const; /// /// \brief Returns a specific muscle sorted by muscles() /// \param idx The muscle index /// \return The muscle /// - const biorbd::muscles::Muscle& muscle( + const Muscle& muscle( unsigned int idx) const; /// @@ -103,26 +105,26 @@ class BIORBD_API Muscles /// respective group name /// \return All the muscle names /// - std::vector muscleNames() const; + std::vector muscleNames() const; /// /// \brief Return the muscle groups /// \return The muscle groups /// - std::vector& muscleGroups(); + std::vector& muscleGroups(); /// /// \brief Return the muscle groups /// \return The muscle groups /// - const std::vector& muscleGroups() const; + const std::vector& muscleGroups() const; /// /// \brief Return the muscle group of specific index /// \param idx The index of the muscle group to return /// \return A muscle group /// - biorbd::muscles::MuscleGroup& muscleGroup( + MuscleGroup& muscleGroup( unsigned int idx); /// @@ -130,7 +132,7 @@ class BIORBD_API Muscles /// \param idx The index of the muscle group to return /// \return A muscle group /// - const biorbd::muscles::MuscleGroup& muscleGroup( + const MuscleGroup& muscleGroup( unsigned int idx) const; /// @@ -138,8 +140,8 @@ class BIORBD_API Muscles /// \param name The name of the muscle group to return /// \return A muscle group /// - const biorbd::muscles::MuscleGroup& muscleGroup( - const biorbd::utils::String& name) const; + const MuscleGroup& muscleGroup( + const utils::String& name) const; /// /// \brief Update all the muscles (positions, jacobian, etc.) @@ -147,7 +149,7 @@ class BIORBD_API Muscles /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// void updateMuscles( - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, bool updateKin); /// @@ -157,8 +159,8 @@ class BIORBD_API Muscles /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// void updateMuscles( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot, bool updateKin); /// @@ -167,8 +169,8 @@ class BIORBD_API Muscles /// \param jacoPointsInGlobal The jacobian points in global reference frame /// void updateMuscles( - std::vector>& musclePointsInGlobal, - std::vector& jacoPointsInGlobal); + std::vector>& musclePointsInGlobal, + std::vector& jacoPointsInGlobal); /// /// \brief Update by hand all the muscles (positions, jacobian, velocity, etc.) @@ -177,9 +179,9 @@ class BIORBD_API Muscles /// \param QDot The generalized velocities /// void updateMuscles( - std::vector>& musclePointsInGlobal, - std::vector& jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity& QDot); + std::vector>& musclePointsInGlobal, + std::vector& jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity& QDot); /// /// \brief Get the vector of state that must be used to update states @@ -188,7 +190,7 @@ class BIORBD_API Muscles /// Note: creating your own vector of state is possible. However, it /// will override the state type that is associated with the muscle /// - std::vector> stateSet(); + std::vector> stateSet(); /// /// \brief Compute the muscular joint torque @@ -202,8 +204,8 @@ class BIORBD_API Muscles /// /// Warning: This function assumes that muscles are already updated (via `updateMuscles`) /// - biorbd::rigidbody::GeneralizedTorque muscularJointTorque( - const biorbd::utils::Vector& F); + rigidbody::GeneralizedTorque muscularJointTorque( + const utils::Vector& F); /// /// \brief Compute the muscular joint torque @@ -220,10 +222,10 @@ class BIORBD_API Muscles /// /// Warning: This function assumes that muscles are already updated (via `updateMuscles`) /// - biorbd::rigidbody::GeneralizedTorque muscularJointTorque( - const biorbd::utils::Vector& F, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot); + rigidbody::GeneralizedTorque muscularJointTorque( + const utils::Vector& F, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot); /// /// \brief Compute the muscular joint torque @@ -238,8 +240,8 @@ class BIORBD_API Muscles /// /// Warning: This function assumes that muscles are already updated (via `updateMuscles`) /// - biorbd::rigidbody::GeneralizedTorque muscularJointTorque( - const std::vector>& emg); + rigidbody::GeneralizedTorque muscularJointTorque( + const std::vector>& emg); /// /// \brief Compute the muscular joint torque @@ -253,10 +255,10 @@ class BIORBD_API Muscles /// /// where \f$J\f$ is the muscle lengths jacobian and \f$F\f$ is the force vector of all the muscles /// - biorbd::rigidbody::GeneralizedTorque muscularJointTorque( - const std::vector>& emg, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot); + rigidbody::GeneralizedTorque muscularJointTorque( + const std::vector>& emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot); /// /// \brief Interface that returns in a vector all the activations dot @@ -264,23 +266,23 @@ class BIORBD_API Muscles /// \param areadyNormalized If the states are already normalized /// \return All the activations dot /// - biorbd::utils::Vector activationDot( - const std::vector>& states, + utils::Vector activationDot( + const std::vector>& states, bool areadyNormalized = true); /// /// \brief Return the previously computed muscle length jacobian /// \return The muscle length jacobian /// - biorbd::utils::Matrix musclesLengthJacobian(); + utils::Matrix musclesLengthJacobian(); /// /// \brief Compute and return the muscle length Jacobian /// \param Q The generalized coordinates /// \return The muscle length Jacobian /// - biorbd::utils::Matrix musclesLengthJacobian( - const biorbd::rigidbody::GeneralizedCoordinates& Q); + utils::Matrix musclesLengthJacobian( + const rigidbody::GeneralizedCoordinates& Q); /// /// \brief Compute and return the muscle forces @@ -289,8 +291,8 @@ class BIORBD_API Muscles /// /// Warning: This function assumes that muscles are already updated (via `updateMuscles`) /// - biorbd::utils::Vector muscleForces( - const std::vector>& emg); + utils::Vector muscleForces( + const std::vector>& emg); /// /// \brief Compute and return the muscle forces @@ -299,10 +301,10 @@ class BIORBD_API Muscles /// \param QDot The generalized velocities /// \return The muscle forces /// - biorbd::utils::Vector muscleForces( - const std::vector>& emg, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot); + utils::Vector muscleForces( + const std::vector>& emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot); /// /// \brief Return the total number of muscle groups @@ -323,11 +325,12 @@ class BIORBD_API Muscles unsigned int nbMuscles() const; protected: - std::shared_ptr> + std::shared_ptr> m_mus; ///< Holder for muscle groups }; +} } } diff --git a/include/Muscles/MusclesEnums.h b/include/Muscles/MusclesEnums.h index 8edc2773..8364e57e 100644 --- a/include/Muscles/MusclesEnums.h +++ b/include/Muscles/MusclesEnums.h @@ -3,6 +3,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { @@ -23,7 +25,7 @@ enum MUSCLE_TYPE { /// \param type The type to convert to string /// \return The name of the type /// -inline const char* MUSCLE_TYPE_toStr(biorbd::muscles::MUSCLE_TYPE type) +inline const char* MUSCLE_TYPE_toStr(MUSCLE_TYPE type) { switch (type) { case IDEALIZED_ACTUATOR: @@ -57,7 +59,7 @@ enum STATE_TYPE { /// \param type The type to convert to string /// \return The name of the type /// -inline const char* STATE_TYPE_toStr(biorbd::muscles::STATE_TYPE type) +inline const char* STATE_TYPE_toStr(STATE_TYPE type) { switch (type) { case SIMPLE_STATE: @@ -76,7 +78,7 @@ enum STATE_FATIGUE_TYPE { DYNAMIC_XIA, NO_FATIGUE_STATE_TYPE }; -inline const char* STATE_FATIGUE_TYPE_toStr(biorbd::muscles::STATE_FATIGUE_TYPE +inline const char* STATE_FATIGUE_TYPE_toStr(STATE_FATIGUE_TYPE type) { switch (type) { @@ -89,6 +91,7 @@ inline const char* STATE_FATIGUE_TYPE_toStr(biorbd::muscles::STATE_FATIGUE_TYPE } } +} } } diff --git a/include/Muscles/PathModifiers.h b/include/Muscles/PathModifiers.h index 6ddde937..1f042100 100644 --- a/include/Muscles/PathModifiers.h +++ b/include/Muscles/PathModifiers.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Vector3d; @@ -29,21 +31,21 @@ class BIORBD_API PathModifiers /// \brief Deep copy of path changers /// \return A deep copy of path changers /// - biorbd::muscles::PathModifiers DeepCopy() const; + PathModifiers DeepCopy() const; /// /// \brief Deep copy of path changers from another path changers /// \param other THe path changers to copy /// void DeepCopy( - const biorbd::muscles::PathModifiers& other); + const PathModifiers& other); /// /// \brief Add a wrapping or a via point to the set of path modifiers /// \param object The wrapping or via point to add /// void addPathChanger( - biorbd::utils::Vector3d& object); + utils::Vector3d& object); // Set and get /// @@ -69,17 +71,17 @@ class BIORBD_API PathModifiers /// \param idx Index of the object /// \return The object at a specific index /// - biorbd::utils::Vector3d& object(unsigned int idx); + utils::Vector3d& object(unsigned int idx); /// /// \brief Return the object at a specific index in the set /// \param idx Index of the object /// \return The object at a specific index /// - const biorbd::utils::Vector3d& object(unsigned int idx) const; + const utils::Vector3d& object(unsigned int idx) const; protected: - std::shared_ptr>> + std::shared_ptr>> m_obj; ///< set of objects std::shared_ptr m_nbWraps; ///< Number of wrapping object in the set @@ -89,6 +91,7 @@ class BIORBD_API PathModifiers }; +} } } diff --git a/include/Muscles/State.h b/include/Muscles/State.h index 358f3712..e23a4c90 100644 --- a/include/Muscles/State.h +++ b/include/Muscles/State.h @@ -8,7 +8,8 @@ namespace biorbd { - +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// @@ -23,15 +24,15 @@ class BIORBD_API State /// \param activation The muscle activation /// State( - const biorbd::utils::Scalar& excitation = 0, - const biorbd::utils::Scalar& activation = 0); + const utils::Scalar& excitation = 0, + const utils::Scalar& activation = 0); /// /// \brief Construct a muscle state from another state /// \param other The other state /// State( - const biorbd::muscles::State& other); + const State& other); /// /// \brief Destroy class properly @@ -42,14 +43,14 @@ class BIORBD_API State /// \brief Deep copy of state /// \return A deep copy of state /// - biorbd::muscles::State DeepCopy() const; + State DeepCopy() const; /// /// \brief Deep copy of state into another state /// \param other The state to copy /// void DeepCopy( - const biorbd::muscles::State& other); + const State& other); /// /// \brief Set the muscle excitation @@ -60,14 +61,14 @@ class BIORBD_API State /// it changes it to 0 anyway, but doesn't send a warning saying it. /// virtual void setExcitation( - const biorbd::utils::Scalar& val, + const utils::Scalar& val, bool turnOffWarnings = false); /// /// \brief Return the muscle excitation /// \return The muscle excitation /// - const biorbd::utils::Scalar& excitation() const; + const utils::Scalar& excitation() const; /// /// \brief Compute and return the normalized excitation @@ -77,8 +78,8 @@ class BIORBD_API State /// /// Even when the warning is ON, the computation is performed anyway /// - const biorbd::utils::Scalar& normalizeExcitation( - const biorbd::muscles::State &emgMax, + const utils::Scalar& normalizeExcitation( + const State &emgMax, bool turnOffWarnings = false); /// @@ -86,13 +87,13 @@ class BIORBD_API State /// \param val Value of the normalized excitation to set /// void setExcitationNorm( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the previously normalized excitation /// \return The normalized excitation /// - const biorbd::utils::Scalar& excitationNorm() const; + const utils::Scalar& excitationNorm() const; /// /// \brief Set the muscle activation @@ -106,34 +107,35 @@ class BIORBD_API State /// it changes it to 1 anyway, but doesn't send a warning saying it. /// virtual void setActivation( - const biorbd::utils::Scalar& val, + const utils::Scalar& val, bool turnOffWarnings = false); /// /// \brief Return the muscle activation /// \return The muscle activation /// - const biorbd::utils::Scalar& activation() const; + const utils::Scalar& activation() const; /// /// \brief Return the state type /// \return The state type /// - biorbd::muscles::STATE_TYPE type() const; + STATE_TYPE type() const; protected: /// /// \brief Set the type to simple_state /// virtual void setType(); - std::shared_ptr m_stateType;///< The state type - std::shared_ptr m_excitation;///< The muscle excitation - std::shared_ptr + std::shared_ptr m_stateType;///< The state type + std::shared_ptr m_excitation;///< The muscle excitation + std::shared_ptr m_excitationNorm; ///< The normalized excitation - std::shared_ptr m_activation;///< The muscle activation + std::shared_ptr m_activation;///< The muscle activation }; +} } } diff --git a/include/Muscles/StateDynamics.h b/include/Muscles/StateDynamics.h index 3ca38e8a..119580dd 100644 --- a/include/Muscles/StateDynamics.h +++ b/include/Muscles/StateDynamics.h @@ -6,13 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { class Characteristics; /// /// \brief EMG with the capability to compute the time derivative /// -class BIORBD_API StateDynamics : public biorbd::muscles::State +class BIORBD_API StateDynamics : public State { public: /// @@ -21,15 +23,15 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// \param activation The muscle activation /// StateDynamics( - const biorbd::utils::Scalar& excitation = 0, - const biorbd::utils::Scalar& activation = 0); + const utils::Scalar& excitation = 0, + const utils::Scalar& activation = 0); /// /// \brief Construct a state dynamics from another state dynamics /// \param other The other state dynamics /// StateDynamics( - const biorbd::muscles::State& other); + const State& other); /// /// \brief Destroy class properly @@ -40,14 +42,14 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// \brief Deep copy of state dynamics /// \return A deep copy of state dynamics /// - biorbd::muscles::StateDynamics DeepCopy() const; + StateDynamics DeepCopy() const; /// /// \brief Deep copy of state dynamics into another state dynamics /// \param other The state dynamics to copy /// void DeepCopy( - const biorbd::muscles::StateDynamics& other); + const StateDynamics& other); /// /// \brief Set the muscle excitation @@ -58,14 +60,14 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// it changes it to 0 anyway, but doesn't send a warning saying it. /// virtual void setExcitation( - const biorbd::utils::Scalar& val, + const utils::Scalar& val, bool turnOffWarnings = false); /// /// \brief Return the previous activation /// \return The previous activation /// - const biorbd::utils::Scalar& previousExcitation() const; + const utils::Scalar& previousExcitation() const; /// /// \brief Set the muscle activation @@ -79,14 +81,14 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// it changes it to 1 anyway, but doesn't send a warning saying it. /// virtual void setActivation( - const biorbd::utils::Scalar& val, + const utils::Scalar& val, bool turnOffWarnings = false); /// /// \brief Return the previous activation /// \return The previous activation /// - const biorbd::utils::Scalar& previousActivation() const; + const utils::Scalar& previousActivation() const; /// /// \brief Compute and return the activation time derivative from the excitation and activation @@ -96,9 +98,9 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// \param alreadyNormalized If already normalized /// \return The activation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeActivation( - const biorbd::utils::Scalar& excitation, - const biorbd::utils::Scalar& activation, + virtual const utils::Scalar& timeDerivativeActivation( + const utils::Scalar& excitation, + const utils::Scalar& activation, const Characteristics& characteristics, bool alreadyNormalized = false); @@ -109,9 +111,9 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// \param alreadyNormalized If already normalized /// \return The activation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeActivation( - const biorbd::muscles::State& emg, - const biorbd::muscles::Characteristics& characteristics, + virtual const utils::Scalar& timeDerivativeActivation( + const State& emg, + const Characteristics& characteristics, bool alreadyNormalized = false); /// @@ -120,27 +122,28 @@ class BIORBD_API StateDynamics : public biorbd::muscles::State /// \param alreadyNormalized If already normalized /// \return The activation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeActivation( - const biorbd::muscles::Characteristics& characteristics, + virtual const utils::Scalar& timeDerivativeActivation( + const Characteristics& characteristics, bool alreadyNormalized = false); /// /// \brief Return the previously computed activation time derivative /// \return The activation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeActivation(); + virtual const utils::Scalar& timeDerivativeActivation(); protected: virtual void setType(); - std::shared_ptr + std::shared_ptr m_previousExcitation; ///< The previous excitation - std::shared_ptr + std::shared_ptr m_previousActivation; /// + std::shared_ptr m_activationDot;///< The activation velocity }; +} } } diff --git a/include/Muscles/StateDynamicsBuchanan.h b/include/Muscles/StateDynamicsBuchanan.h index 366daa65..8e20ed5a 100644 --- a/include/Muscles/StateDynamicsBuchanan.h +++ b/include/Muscles/StateDynamicsBuchanan.h @@ -6,12 +6,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Time derivative of activation as described by Buchanan (https://www.sciencedirect.com/science/article/pii/S0021929003001520) /// -class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics +class BIORBD_API StateDynamicsBuchanan : public StateDynamics { public: /// @@ -20,15 +22,15 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// \param excitation The muscle excitation /// StateDynamicsBuchanan( - const biorbd::utils::Scalar& neuralCommand = 0, - const biorbd::utils::Scalar& excitation = 0); + const utils::Scalar& neuralCommand = 0, + const utils::Scalar& excitation = 0); /// /// \brief Construct a state dynamics Buchanan from another state dynamics Buchanan /// \param other The other state dynamics Buchanan /// StateDynamicsBuchanan( - const biorbd::muscles::State& other); + const State& other); /// /// \brief Destroy class properly @@ -39,14 +41,14 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// \brief Deep copy of state dynamics Buchanan /// \return A deep copy of state dynamics Buchanan /// - biorbd::muscles::StateDynamicsBuchanan DeepCopy() const; + StateDynamicsBuchanan DeepCopy() const; /// /// \brief Deep copy of state dynamics Buchanan into another state dynamics Buchanan /// \param other The state dynamics Buchanan to copy /// void DeepCopy( - const biorbd::muscles::StateDynamicsBuchanan& other); + const StateDynamicsBuchanan& other); /// /// \brief Compute and return the excitation velocity @@ -54,7 +56,7 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// \param alreadyNormalized If already normalized /// \return The excitation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeExcitation( + virtual const utils::Scalar& timeDerivativeExcitation( const Characteristics &characteristics, bool alreadyNormalized); @@ -64,7 +66,7 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// \param turnOffWarnings If the warning should be silenced /// virtual void setExcitation( - const biorbd::utils::Scalar& val, + const utils::Scalar& val, bool turnOffWarnings = false); /// @@ -72,27 +74,27 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// \param val Value of the neural command /// virtual void setNeuralCommand( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Set the shape factor /// \param shape_factor Value of the shape factor /// void shapeFactor( - const biorbd::utils::Scalar& shape_factor); + const utils::Scalar& shape_factor); /// /// \brief Return the shape factor /// \return The shape factor /// - const biorbd::utils::Scalar& shapeFactor() const; + const utils::Scalar& shapeFactor() const; /// /// \brief Set the muscle activation /// \param notUsed the activation is computed from the neuralCommand and excitation /// virtual void setActivation( - const biorbd::utils::Scalar& notUsed, + const utils::Scalar& notUsed, bool turnOffWarnings = false); protected: @@ -101,15 +103,16 @@ class BIORBD_API StateDynamicsBuchanan : public biorbd::muscles::StateDynamics /// virtual void setType(); - std::shared_ptr + std::shared_ptr m_neuralCommand; ///< The muscle neural command - std::shared_ptr + std::shared_ptr m_shapeFactor; ///< The shape factor (Buchanan2004, march 22nd, 2018) - std::shared_ptr + std::shared_ptr m_excitationDot; ///< The excitation velocity }; +} } } diff --git a/include/Muscles/StateDynamicsDeGroote.h b/include/Muscles/StateDynamicsDeGroote.h index e68cbd38..90a9cba2 100644 --- a/include/Muscles/StateDynamicsDeGroote.h +++ b/include/Muscles/StateDynamicsDeGroote.h @@ -6,12 +6,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief EMG with the capability to compute the time derivative /// -class BIORBD_API StateDynamicsDeGroote : public biorbd::muscles::StateDynamics +class BIORBD_API StateDynamicsDeGroote : public StateDynamics { public: /// @@ -20,28 +22,28 @@ class BIORBD_API StateDynamicsDeGroote : public biorbd::muscles::StateDynamics /// \param activation The muscle activation /// StateDynamicsDeGroote( - const biorbd::utils::Scalar& excitation = 0, - const biorbd::utils::Scalar& activation = 0); + const utils::Scalar& excitation = 0, + const utils::Scalar& activation = 0); /// /// \brief Construct a state dynamics from another state dynamics /// \param other The other state dynamics /// StateDynamicsDeGroote( - const biorbd::muscles::StateDynamicsDeGroote& other); + const StateDynamicsDeGroote& other); /// /// \brief Deep copy of state dynamics /// \return A deep copy of state dynamics /// - biorbd::muscles::StateDynamicsDeGroote DeepCopy() const; + StateDynamicsDeGroote DeepCopy() const; /// /// \brief Deep copy of state dynamics into another state dynamics /// \param other The state dynamics to copy /// void DeepCopy( - const biorbd::muscles::StateDynamicsDeGroote& other); + const StateDynamicsDeGroote& other); /// /// \brief Compute and return the activation time derivative @@ -49,7 +51,7 @@ class BIORBD_API StateDynamicsDeGroote : public biorbd::muscles::StateDynamics /// \param alreadyNormalized If already normalized /// \return The activation time derivative /// - virtual const biorbd::utils::Scalar& timeDerivativeActivation( + virtual const utils::Scalar& timeDerivativeActivation( const Characteristics& characteristics, bool alreadyNormalized = false); @@ -58,6 +60,7 @@ class BIORBD_API StateDynamicsDeGroote : public biorbd::muscles::StateDynamics }; +} } } diff --git a/include/Muscles/ViaPoint.h b/include/Muscles/ViaPoint.h index d69855c6..127a0c89 100644 --- a/include/Muscles/ViaPoint.h +++ b/include/Muscles/ViaPoint.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; @@ -16,7 +18,7 @@ namespace muscles /// /// \brief Via point of a muscle /// -class BIORBD_API ViaPoint : public biorbd::utils::Vector3d +class BIORBD_API ViaPoint : public utils::Vector3d { public: /// @@ -31,9 +33,9 @@ class BIORBD_API ViaPoint : public biorbd::utils::Vector3d /// \param z Z-Component of the ViaPoint /// ViaPoint( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z); /// /// \brief Construct ViaPoint @@ -44,50 +46,51 @@ class BIORBD_API ViaPoint : public biorbd::utils::Vector3d /// \param parentName The name of the parent segment /// ViaPoint( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName); /// /// \brief Construct ViaPoint from a vector /// \param other The vector /// ViaPoint( - const biorbd::utils::Vector3d& other); + const utils::Vector3d& other); /// /// \brief Construct ViaPoint from another ViaPoint /// \param other The other ViaPoint /// ViaPoint( - const biorbd::muscles::ViaPoint& other); + const ViaPoint& other); /// /// \brief Deep copy of a ViaPoint /// \return A deep copy of a ViaPoint /// - biorbd::muscles::ViaPoint DeepCopy() const; + ViaPoint DeepCopy() const; /// /// \brief Deep copy of a ViaPoint into another ViaPoint /// \param other The ViaPoint to copy /// - void DeepCopy(const biorbd::muscles::ViaPoint& other); + void DeepCopy(const ViaPoint& other); /// /// \brief To be able to use operator "=" when assigning a ViaPoint to a 3D node /// \param other The 3D node to assign to ViaPoint /// template - biorbd::muscles::ViaPoint& operator=(const biorbd::utils::Vector3d& other) + ViaPoint& operator=(const utils::Vector3d& other) { - this->biorbd::utils::Vector3d::operator=(other); + this->utils::Vector3d::operator=(other); return *this; } }; +} } } diff --git a/include/Muscles/WrappingHalfCylinder.h b/include/Muscles/WrappingHalfCylinder.h index 464402ac..98be86d9 100644 --- a/include/Muscles/WrappingHalfCylinder.h +++ b/include/Muscles/WrappingHalfCylinder.h @@ -6,13 +6,15 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Half cylinder of infinite length (the length only affect the graphical /// representation) object that makes the muscle to wrap around /// -class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject +class BIORBD_API WrappingHalfCylinder : public WrappingObject { public: /// @@ -24,14 +26,14 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \brief Construct a wrapping half cylinder /// WrappingHalfCylinder( - const biorbd::utils::Vector3d& other); + const utils::Vector3d& other); #ifndef SWIG /// /// \brief Construct a wrapping half cylinder /// WrappingHalfCylinder( - const biorbd::utils::Vector3d* other); + const utils::Vector3d* other); #endif /// @@ -41,9 +43,9 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param length The Length of the half cylinder /// WrappingHalfCylinder( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::Scalar& radius, - const biorbd::utils::Scalar& length); + const utils::RotoTrans& rt, + const utils::Scalar& radius, + const utils::Scalar& length); /// /// \brief Construct a wrapping half cylinder @@ -54,24 +56,24 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param parentName The parent name segment /// WrappingHalfCylinder( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::Scalar& radius, - const biorbd::utils::Scalar& length, - const biorbd::utils::String& name, - const biorbd::utils::String& parentName); + const utils::RotoTrans& rt, + const utils::Scalar& radius, + const utils::Scalar& length, + const utils::String& name, + const utils::String& parentName); /// /// \brief Deep copy of the wrapping half cylinder /// \return A deep copy of the wrapping half cylinder /// - biorbd::muscles::WrappingHalfCylinder DeepCopy() const; + WrappingHalfCylinder DeepCopy() const; /// /// \brief Deep copy of the wrapping half cylinder in another wrapping half cylinder /// \param other The wrapping half cylinder to copy /// void DeepCopy( - const biorbd::muscles::WrappingHalfCylinder& other); + const WrappingHalfCylinder& other); /// /// \brief From the position of the half cylinder, return the 2 locations where the muscle leaves the wrapping object @@ -83,12 +85,12 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param length Length of the muscle (ignored if no value is provided) /// void wrapPoints( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* length = nullptr); + const utils::RotoTrans& rt, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* length = nullptr); /// /// \brief From the position of the half cylinder, return the 2 locations where the muscle leaves the wrapping object @@ -102,12 +104,12 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// void wrapPoints( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* length = nullptr) ; + const rigidbody::GeneralizedCoordinates& Q, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* length = nullptr) ; /// /// \brief Returns the previously computed 2 locations where the muscle leaves the wrapping object @@ -116,9 +118,9 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param length Length of the muscle (ignored if no value is provided) /// void wrapPoints( - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* length = nullptr); + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* length = nullptr); /// /// \brief Return the RotoTrans matrix of the half cylinder @@ -127,9 +129,9 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param updateKin If the kinematics should be computed /// \return The RotoTrans matrix of the half cylinder /// - virtual const biorbd::utils::RotoTrans& RT( + virtual const utils::RotoTrans& RT( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, bool updateKin = true); /// @@ -137,32 +139,32 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param val Value of the diameter to set /// void setRadius( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the radius of the half cylinder /// \return The radius of the half cylinder /// - biorbd::utils::Scalar radius() const; + utils::Scalar radius() const; /// /// \brief Return the diamter of the half cylinder /// \return The diamter of the half cylinder /// - biorbd::utils::Scalar diameter() const; + utils::Scalar diameter() const; /// /// \brief Set the length of the half cylinder /// \param val Value of the to set /// void setLength( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the length of the half cylinder /// \return The length of the half cylinder /// - const biorbd::utils::Scalar& length() const; + const utils::Scalar& length() const; protected: #ifndef SWIG @@ -178,13 +180,13 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param p2 Point 2 /// NodeMusclePair( - const biorbd::utils::Vector3d &p1, - const biorbd::utils::Vector3d &p2) : - m_p1(std::make_shared(p1)), - m_p2(std::make_shared(p2)) + const utils::Vector3d &p1, + const utils::Vector3d &p2) : + m_p1(std::make_shared(p1)), + m_p2(std::make_shared(p2)) {} - std::shared_ptr m_p1; ///< Point 1 - std::shared_ptr m_p2; ///< Point 2 + std::shared_ptr m_p1; ///< Point 1 + std::shared_ptr m_p2; ///< Point 2 }; #endif @@ -194,8 +196,8 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param p_tan The point tangent /// void findTangentToCircle( - const biorbd::utils::Vector3d& p, - biorbd::utils::Vector3d& p_tan) const; + const utils::Vector3d& p, + utils::Vector3d& p_tan) const; /// /// \brief Select between a set of nodes which ones to keep @@ -204,7 +206,7 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// void selectTangents( const NodeMusclePair&p, - biorbd::utils::Vector3d& p_tan) const; + utils::Vector3d& p_tan) const; /// /// \brief Find the height of both points @@ -233,25 +235,26 @@ class BIORBD_API WrappingHalfCylinder : public biorbd::muscles::WrappingObject /// \param p the muscle node pair /// \return The muscle lengh on the half cylinder /// - biorbd::utils::Scalar computeLength( + utils::Scalar computeLength( const NodeMusclePair &p) const; - std::shared_ptr + std::shared_ptr m_radius; ///< Diameter of the half cylinder diametre du cylindre - std::shared_ptr + std::shared_ptr m_length; ///< Length of the half cylinder - std::shared_ptr + std::shared_ptr m_RTtoParent; /// + std::shared_ptr m_p1Wrap; ///< First point of contact with the wrap - std::shared_ptr + std::shared_ptr m_p2Wrap; ///< Second point of contact with the wrap - std::shared_ptr m_lengthAroundWrap + std::shared_ptr m_lengthAroundWrap ; ///< Length between p1 and p2 }; +} } } diff --git a/include/Muscles/WrappingObject.h b/include/Muscles/WrappingObject.h index 1dea0ce5..322d930a 100644 --- a/include/Muscles/WrappingObject.h +++ b/include/Muscles/WrappingObject.h @@ -7,20 +7,16 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class String; } -namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class Joints; -} -} - -namespace rigidbody -{ class GeneralizedCoordinates; } @@ -29,7 +25,7 @@ namespace muscles /// /// \brief Base class for the wrapping objects /// -class BIORBD_API WrappingObject : public biorbd::utils::Vector3d +class BIORBD_API WrappingObject : public utils::Vector3d { public: /// @@ -44,9 +40,9 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param z Z-Component of the wrapping object /// WrappingObject( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z); /// /// \brief Construct a wrapping object /// \param x X-Component of the wrapping object @@ -56,18 +52,18 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param parentName Name of the parent segment /// WrappingObject( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName); /// /// \brief Construct a wrapping object /// \param other Eigen vector /// WrappingObject( - const biorbd::utils::Vector3d& other); + const utils::Vector3d& other); /// /// \brief Construct a wrapping object @@ -76,16 +72,16 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param parentName Name of the parent segment /// WrappingObject( - const biorbd::utils::Vector3d& other, - const biorbd::utils::String& name, - const biorbd::utils::String& parentName); + const utils::Vector3d& other, + const utils::String& name, + const utils::String& parentName); /// /// \brief Deep copy of the wrapping ibject in another wrapping object /// \param other The wrapping object to copy /// void DeepCopy( - const biorbd::muscles::WrappingObject& other); + const WrappingObject& other); /// /// \brief From the position of the wrapping object, return the 2 locations where the muscle leaves the wrapping object @@ -97,12 +93,12 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param muscleLength Length of the muscle (ignored if no value is provided) /// virtual void wrapPoints( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* muscleLength = nullptr) = 0 + const utils::RotoTrans& rt, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* muscleLength = nullptr) = 0 ; // Premier et dernier points musculaire /// @@ -117,12 +113,12 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// virtual void wrapPoints( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* muscleLength = nullptr) = + const rigidbody::GeneralizedCoordinates& Q, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* muscleLength = nullptr) = 0; // Premier et dernier points musculaire /// @@ -132,9 +128,9 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param muscleLength Length of the muscle (ignored if no value is provided) /// virtual void wrapPoints( - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar* muscleLength = nullptr) = + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar* muscleLength = nullptr) = 0; // Assume un appel déja faits /// @@ -144,32 +140,33 @@ class BIORBD_API WrappingObject : public biorbd::utils::Vector3d /// \param updateKin If the kinematics should be computed /// \return The RotoTrans matrix of the wrapping object /// - virtual const biorbd::utils::RotoTrans& RT( + virtual const utils::RotoTrans& RT( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, bool updateKin = true) = 0; /// /// \brief Return the RotoTrans matrix of the wrapping object /// \return The RotoTrans matrix of the wrapping object /// - const biorbd::utils::RotoTrans& RT() const; + const utils::RotoTrans& RT() const; #ifndef SWIG /// /// \brief To be able to use the equal "=" operator to define wrapping object /// \param other The 3d node to define the wrapping object - biorbd::muscles::WrappingObject& operator=(const biorbd::utils::Vector3d& other) + WrappingObject& operator=(const utils::Vector3d& other) { - this->biorbd::utils::Vector3d::operator=(other); + this->utils::Vector3d::operator=(other); return *this; } #endif protected: - std::shared_ptr + std::shared_ptr m_RT; ///< RotoTrans matrix of the wrapping object }; +} } } diff --git a/include/Muscles/WrappingSphere.h b/include/Muscles/WrappingSphere.h index 614ddf8b..39beda3d 100644 --- a/include/Muscles/WrappingSphere.h +++ b/include/Muscles/WrappingSphere.h @@ -6,12 +6,14 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace muscles { /// /// \brief Sphere object that makes the muscle to wrap around /// -class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject +class BIORBD_API WrappingSphere : public WrappingObject { public: @@ -28,10 +30,10 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \param diameter Diameter of the sphere /// WrappingSphere( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::Scalar& diameter); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::Scalar& diameter); /// /// \brief Construct a wrapping sphere @@ -43,12 +45,12 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \param parentName Name of the parent segment /// WrappingSphere( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::Scalar& diameter, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName); + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::Scalar& diameter, + const utils::String &name, + const utils::String &parentName); /// /// \brief Construct a wrapping sphere @@ -56,52 +58,52 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \param diameter Diameter of the sphere /// WrappingSphere( - const biorbd::utils::Vector3d &v, - const biorbd::utils::Scalar& diameter); + const utils::Vector3d &v, + const utils::Scalar& diameter); /// /// \brief Deep copy of the wrapping sphere /// \return A deep copy of the wrapping sphere /// - biorbd::muscles::WrappingSphere DeepCopy() const; + WrappingSphere DeepCopy() const; /// /// \brief Deep copy of the wrapping sphere in another wrapping sphere /// \param other The wrapping sphere to copy /// void DeepCopy( - const biorbd::muscles::WrappingSphere& other); + const WrappingSphere& other); /// /// \brief Not yet implemented /// virtual void wrapPoints( - const biorbd::utils::RotoTrans&, - const biorbd::utils::Vector3d&, - const biorbd::utils::Vector3d&, - biorbd::utils::Vector3d&, - biorbd::utils::Vector3d&, - biorbd::utils::Scalar* = nullptr) {} + const utils::RotoTrans&, + const utils::Vector3d&, + const utils::Vector3d&, + utils::Vector3d&, + utils::Vector3d&, + utils::Scalar* = nullptr) {} /// /// \brief Not yet implemented /// virtual void wrapPoints( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints&, - const biorbd::rigidbody::GeneralizedCoordinates&, - const biorbd::utils::Vector3d&, - const biorbd::utils::Vector3d&, - biorbd::utils::Vector3d&, - biorbd::utils::Vector3d&, - biorbd::utils::Scalar* = nullptr) {} + const rigidbody::GeneralizedCoordinates&, + const utils::Vector3d&, + const utils::Vector3d&, + utils::Vector3d&, + utils::Vector3d&, + utils::Scalar* = nullptr) {} /// /// \brief Not yet implemented /// virtual void wrapPoints( - biorbd::utils::Vector3d&, - biorbd::utils::Vector3d&, - biorbd::utils::Scalar* = nullptr) {} + utils::Vector3d&, + utils::Vector3d&, + utils::Scalar* = nullptr) {} /// /// \brief Return the RotoTrans matrix of the sphere @@ -110,9 +112,9 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \param updateKin If the kinematics should be computed /// \return The RotoTrans matrix of the sphere /// - const biorbd::utils::RotoTrans& RT( + const utils::RotoTrans& RT( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates&Q , + const rigidbody::GeneralizedCoordinates&Q , bool updateKin = true); /// @@ -120,20 +122,21 @@ class BIORBD_API WrappingSphere : public biorbd::muscles::WrappingObject /// \param val Value of the diameter /// void setDiameter( - const biorbd::utils::Scalar& val); + const utils::Scalar& val); /// /// \brief Return the diameter of the wrapping sphere /// \return The diameter of the wrapping sphere /// - const biorbd::utils::Scalar& diameter() const; + const utils::Scalar& diameter() const; protected: - std::shared_ptr + std::shared_ptr m_dia; ///< Diameter of the wrapping sphere }; +} } } diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp index 7796c822..1985b561 100644 --- a/src/ModelReader.cpp +++ b/src/ModelReader.cpp @@ -797,11 +797,11 @@ void Reader::readModelFile( utils::String name; file.read(name); // Name of the muscle // Declaration of the variables - biorbd::muscles::MUSCLE_TYPE type(biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE); - biorbd::muscles::STATE_TYPE stateType( - biorbd::muscles::STATE_TYPE::NO_STATE_TYPE); - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType( - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + muscles::MUSCLE_TYPE type(muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE); + muscles::STATE_TYPE stateType( + muscles::STATE_TYPE::NO_STATE_TYPE); + muscles::STATE_FATIGUE_TYPE dynamicFatigueType( + muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); utils::String muscleGroup(""); int idxGroup(-1); utils::Vector3d origin_pos(0,0,0); @@ -814,7 +814,7 @@ void Reader::readModelFile( double maxActivation(1); double PCSA(0); double shapeFactor(0); - biorbd::muscles::FatigueParameters fatigueParameters; + muscles::FatigueParameters fatigueParameters; // Read file while(file.read(property_tag) && property_tag.tolower().compare("endmuscle")) { @@ -828,18 +828,18 @@ void Reader::readModelFile( utils::String tp_type; file.read(tp_type); if (!tp_type.tolower().compare("idealizedactuator")) { - type = biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR; + type = muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR; } else if (!tp_type.tolower().compare("hill")) { - type = biorbd::muscles::MUSCLE_TYPE::HILL; + type = muscles::MUSCLE_TYPE::HILL; } else if (!tp_type.tolower().compare("hillthelen") || !tp_type.tolower().compare("thelen")) { - type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN; + type = muscles::MUSCLE_TYPE::HILL_THELEN; } else if (!tp_type.tolower().compare("hillthelenactive") || !tp_type.tolower().compare("thelenactive")) { - type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE; + type = muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE; } else if (!tp_type.tolower().compare("hillthelenfatigable") || !tp_type.tolower().compare("thelenfatigable")) { - type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE; + type = muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE; } else { utils::Error::raise(property_tag + " is not a valid muscle type"); } @@ -847,9 +847,9 @@ void Reader::readModelFile( utils::String tp_state; file.read(tp_state); if (!tp_state.tolower().compare("buchanan")) { - stateType = biorbd::muscles::STATE_TYPE::BUCHANAN; + stateType = muscles::STATE_TYPE::BUCHANAN; } else if (!tp_state.tolower().compare("degroote")) { - stateType = biorbd::muscles::STATE_TYPE::DE_GROOTE; + stateType = muscles::STATE_TYPE::DE_GROOTE; } else { utils::Error::raise(property_tag + " is not a valid muscle state type"); } @@ -876,9 +876,9 @@ void Reader::readModelFile( utils::String tp_fatigue_type; file.read(tp_fatigue_type); if (!tp_fatigue_type.tolower().compare("simple")) { - dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE; + dynamicFatigueType = muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE; } else if (!tp_fatigue_type.tolower().compare("xia")) { - dynamicFatigueType = biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA; + dynamicFatigueType = muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA; } else { utils::Error::raise(tp_fatigue_type + " is not a value fatigue parameter type"); @@ -902,23 +902,23 @@ void Reader::readModelFile( } } utils::Error::check(idxGroup!=-1, "No muscle group was provided!"); - biorbd::muscles::Geometry geo( + muscles::Geometry geo( utils::Vector3d(origin_pos, name + "_origin", model->muscleGroup(static_cast(idxGroup)).origin()), utils::Vector3d(insert_pos, name + "_insertion", model->muscleGroup(static_cast(idxGroup)).insertion())); - biorbd::muscles::State stateMax(maxExcitation, maxActivation); - biorbd::muscles::Characteristics characteristics(optimalLength, maxForce, PCSA, + muscles::State stateMax(maxExcitation, maxActivation); + muscles::Characteristics characteristics(optimalLength, maxForce, PCSA, tendonSlackLength, pennAngle, stateMax, fatigueParameters); model->muscleGroup(static_cast(idxGroup)).addMuscle(name,type,geo, characteristics, - biorbd::muscles::PathModifiers(),stateType,dynamicFatigueType); - if (stateType == biorbd::muscles::STATE_TYPE::BUCHANAN && shapeFactor != 0) { + muscles::PathModifiers(),stateType,dynamicFatigueType); + if (stateType == muscles::STATE_TYPE::BUCHANAN && shapeFactor != 0) { auto& muscleGroup = model->muscleGroup(idxGroup); size_t nMuscInGroup(muscleGroup.nbMuscles()-1); auto& state = muscleGroup.muscle(nMuscInGroup).state(); - static_cast(state).shapeFactor( + static_cast(state).shapeFactor( shapeFactor); } #else // MODULE_MUSCLES @@ -935,7 +935,7 @@ void Reader::readModelFile( utils::String musclegroup(""); int iMuscleGroup(-1); int iMuscle(-1); - biorbd::muscles::ViaPoint position(0,0,0); + muscles::ViaPoint position(0,0,0); // Read file while(file.read(property_tag) @@ -1025,7 +1025,7 @@ void Reader::readModelFile( utils::Error::check(radius > 0.0, "Radius must be defined and positive"); utils::Error::check(length >= 0.0, "Length was must be positive"); - biorbd::muscles::WrappingHalfCylinder cylinder(RT,radius,length,name,parent); + muscles::WrappingHalfCylinder cylinder(RT,radius,length,name,parent); model->muscleGroup(static_cast(iMuscleGroup)).muscle( static_cast(iMuscle)).addPathObject( cylinder); diff --git a/src/Muscles/Characteristics.cpp b/src/Muscles/Characteristics.cpp index ed8773d2..c280e853 100644 --- a/src/Muscles/Characteristics.cpp +++ b/src/Muscles/Characteristics.cpp @@ -4,25 +4,26 @@ #include "Muscles/State.h" #include "Muscles/FatigueParameters.h" -biorbd::muscles::Characteristics::Characteristics() : - m_optimalLength(std::make_shared(0)), - m_fIsoMax(std::make_shared(0)), - m_PCSA(std::make_shared(0)), - m_tendonSlackLength(std::make_shared(0)), - m_pennationAngle(std::make_shared(0)), - m_stateMax(std::make_shared(biorbd::muscles::State(1, - 1))), - m_minActivation(std::make_shared(0.01)), - m_torqueActivation(std::make_shared(0.01)), - m_torqueDeactivation(std::make_shared(0.04)), - m_fatigueParameters(std::make_shared - (biorbd::muscles::FatigueParameters())) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::Characteristics::Characteristics() : + m_optimalLength(std::make_shared(0)), + m_fIsoMax(std::make_shared(0)), + m_PCSA(std::make_shared(0)), + m_tendonSlackLength(std::make_shared(0)), + m_pennationAngle(std::make_shared(0)), + m_stateMax(std::make_shared(muscles::State(1, 1))), + m_minActivation(std::make_shared(0.01)), + m_torqueActivation(std::make_shared(0.01)), + m_torqueDeactivation(std::make_shared(0.04)), + m_fatigueParameters(std::make_shared + (muscles::FatigueParameters())) { } -biorbd::muscles::Characteristics::Characteristics( - const biorbd::muscles::Characteristics &other) : +muscles::Characteristics::Characteristics( + const muscles::Characteristics &other) : m_optimalLength(other.m_optimalLength), m_fIsoMax(other.m_fIsoMax), m_PCSA(other.m_PCSA), @@ -37,47 +38,47 @@ biorbd::muscles::Characteristics::Characteristics( } -biorbd::muscles::Characteristics::Characteristics( - const biorbd::utils::Scalar& optLength, - const biorbd::utils::Scalar& fmax, - const biorbd::utils::Scalar& PCSA, - const biorbd::utils::Scalar& tendonSlackLength, - const biorbd::utils::Scalar& pennAngle, - const biorbd::muscles::State &emgMax, - const biorbd::muscles::FatigueParameters &fatigueParameters, - const biorbd::utils::Scalar& torqueAct, - const biorbd::utils::Scalar& torqueDeact, - const biorbd::utils::Scalar& minAct): - m_optimalLength(std::make_shared(optLength)), - m_fIsoMax(std::make_shared(fmax)), - m_PCSA(std::make_shared(PCSA)), - m_tendonSlackLength(std::make_shared(tendonSlackLength)), - m_pennationAngle(std::make_shared(pennAngle)), - m_stateMax(std::make_shared(emgMax)), - m_minActivation(std::make_shared(minAct)), - m_torqueActivation(std::make_shared(torqueAct)), - m_torqueDeactivation(std::make_shared(torqueDeact)), - m_fatigueParameters(std::make_shared +muscles::Characteristics::Characteristics( + const utils::Scalar& optLength, + const utils::Scalar& fmax, + const utils::Scalar& PCSA, + const utils::Scalar& tendonSlackLength, + const utils::Scalar& pennAngle, + const muscles::State &emgMax, + const muscles::FatigueParameters &fatigueParameters, + const utils::Scalar& torqueAct, + const utils::Scalar& torqueDeact, + const utils::Scalar& minAct): + m_optimalLength(std::make_shared(optLength)), + m_fIsoMax(std::make_shared(fmax)), + m_PCSA(std::make_shared(PCSA)), + m_tendonSlackLength(std::make_shared(tendonSlackLength)), + m_pennationAngle(std::make_shared(pennAngle)), + m_stateMax(std::make_shared(emgMax)), + m_minActivation(std::make_shared(minAct)), + m_torqueActivation(std::make_shared(torqueAct)), + m_torqueDeactivation(std::make_shared(torqueDeact)), + m_fatigueParameters(std::make_shared (fatigueParameters)) { } -biorbd::muscles::Characteristics::~Characteristics() +muscles::Characteristics::~Characteristics() { } -biorbd::muscles::Characteristics biorbd::muscles::Characteristics::DeepCopy() +muscles::Characteristics muscles::Characteristics::DeepCopy() const { - biorbd::muscles::Characteristics copy; + muscles::Characteristics copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::Characteristics::DeepCopy( - const biorbd::muscles::Characteristics &other) +void muscles::Characteristics::DeepCopy( + const muscles::Characteristics &other) { *m_optimalLength = *other.m_optimalLength; *m_fIsoMax = *other.m_fIsoMax; @@ -92,111 +93,111 @@ void biorbd::muscles::Characteristics::DeepCopy( } // Get et Set -void biorbd::muscles::Characteristics::setOptimalLength( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setOptimalLength( + const utils::Scalar& val) { *m_optimalLength = val; } -const biorbd::utils::Scalar& biorbd::muscles::Characteristics::optimalLength() +const utils::Scalar& muscles::Characteristics::optimalLength() const { return *m_optimalLength; } -void biorbd::muscles::Characteristics::setForceIsoMax( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setForceIsoMax( + const utils::Scalar& val) { *m_fIsoMax = val; } -const biorbd::utils::Scalar& biorbd::muscles::Characteristics::forceIsoMax() +const utils::Scalar& muscles::Characteristics::forceIsoMax() const { return *m_fIsoMax; } -void biorbd::muscles::Characteristics::setTendonSlackLength( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setTendonSlackLength( + const utils::Scalar& val) { *m_tendonSlackLength = val; } -const biorbd::utils::Scalar& -biorbd::muscles::Characteristics::tendonSlackLength() const +const utils::Scalar& +muscles::Characteristics::tendonSlackLength() const { return *m_tendonSlackLength; } -void biorbd::muscles::Characteristics::setPennationAngle( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setPennationAngle( + const utils::Scalar& val) { *m_pennationAngle = val; } -const biorbd::utils::Scalar& biorbd::muscles::Characteristics::pennationAngle() +const utils::Scalar& muscles::Characteristics::pennationAngle() const { return *m_pennationAngle; } -void biorbd::muscles::Characteristics::setPCSA( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setPCSA( + const utils::Scalar& val) { *m_PCSA = val; } -const biorbd::utils::Scalar& biorbd::muscles::Characteristics::PCSA() const +const utils::Scalar& muscles::Characteristics::PCSA() const { return *m_PCSA; } -void biorbd::muscles::Characteristics::setMinActivation( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setMinActivation( + const utils::Scalar& val) { *m_minActivation = val; } -const biorbd::utils::Scalar& biorbd::muscles::Characteristics::minActivation() +const utils::Scalar& muscles::Characteristics::minActivation() const { return *m_minActivation; } -void biorbd::muscles::Characteristics::setTorqueActivation( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setTorqueActivation( + const utils::Scalar& val) { *m_torqueActivation = val; } -const biorbd::utils::Scalar& -biorbd::muscles::Characteristics::torqueActivation() const +const utils::Scalar& +muscles::Characteristics::torqueActivation() const { return *m_torqueActivation; } -void biorbd::muscles::Characteristics::setTorqueDeactivation( - const biorbd::utils::Scalar& val) +void muscles::Characteristics::setTorqueDeactivation( + const utils::Scalar& val) { *m_torqueDeactivation = val; } -const biorbd::utils::Scalar& -biorbd::muscles::Characteristics::torqueDeactivation() const +const utils::Scalar& +muscles::Characteristics::torqueDeactivation() const { return *m_torqueDeactivation; } -void biorbd::muscles::Characteristics::setStateMax( - const biorbd::muscles::State &emgMax) +void muscles::Characteristics::setStateMax( + const muscles::State &emgMax) { *m_stateMax = emgMax; } -const biorbd::muscles::State &biorbd::muscles::Characteristics::stateMax() const +const muscles::State &muscles::Characteristics::stateMax() const { return *m_stateMax; } -void biorbd::muscles::Characteristics::setFatigueParameters( - const biorbd::muscles::FatigueParameters &fatigueParameters) +void muscles::Characteristics::setFatigueParameters( + const muscles::FatigueParameters &fatigueParameters) { *m_fatigueParameters = fatigueParameters; } -const biorbd::muscles::FatigueParameters -&biorbd::muscles::Characteristics::fatigueParameters() const +const muscles::FatigueParameters +&muscles::Characteristics::fatigueParameters() const { return *m_fatigueParameters; } diff --git a/src/Muscles/Compound.cpp b/src/Muscles/Compound.cpp index 13d2ed5d..6aacab1a 100644 --- a/src/Muscles/Compound.cpp +++ b/src/Muscles/Compound.cpp @@ -8,40 +8,42 @@ #include "Muscles/PathModifiers.h" #include "Muscles/State.h" -biorbd::muscles::Compound::Compound() : - m_name(std::make_shared("")), - m_type(std::make_shared - (biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), - m_pathChanger(std::make_shared()), - m_force(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::Compound::Compound() : + m_name(std::make_shared("")), + m_type(std::make_shared + (muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), + m_pathChanger(std::make_shared()), + m_force(std::make_shared(0)) { } -biorbd::muscles::Compound::Compound(const biorbd::utils::String &name) : - m_name(std::make_shared(name)), - m_type(std::make_shared - (biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), - m_pathChanger(std::make_shared()), - m_force(std::make_shared(0)) +muscles::Compound::Compound(const utils::String &name) : + m_name(std::make_shared(name)), + m_type(std::make_shared + (muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), + m_pathChanger(std::make_shared()), + m_force(std::make_shared(0)) { } -biorbd::muscles::Compound::Compound( - const biorbd::utils::String &name, - const biorbd::muscles::PathModifiers &pathModifiers) : - m_name(std::make_shared(name)), - m_type(std::make_shared - (biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), - m_pathChanger(std::make_shared(pathModifiers)), - m_force(std::make_shared(0)) +muscles::Compound::Compound( + const utils::String &name, + const muscles::PathModifiers &pathModifiers) : + m_name(std::make_shared(name)), + m_type(std::make_shared + (muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE)), + m_pathChanger(std::make_shared(pathModifiers)), + m_force(std::make_shared(0)) { } -biorbd::muscles::Compound::Compound( - const biorbd::muscles::Compound &other) : +muscles::Compound::Compound( + const muscles::Compound &other) : m_name(other.m_name), m_type(other.m_type), m_pathChanger(other.m_pathChanger), @@ -50,8 +52,8 @@ biorbd::muscles::Compound::Compound( } -biorbd::muscles::Compound::Compound( - std::shared_ptr other) : +muscles::Compound::Compound( + std::shared_ptr other) : m_name(other->m_name), m_type(other->m_type), m_pathChanger(other->m_pathChanger), @@ -60,12 +62,12 @@ biorbd::muscles::Compound::Compound( } -biorbd::muscles::Compound::~Compound() +muscles::Compound::~Compound() { } -void biorbd::muscles::Compound::DeepCopy(const biorbd::muscles::Compound &other) +void muscles::Compound::DeepCopy(const muscles::Compound &other) { *m_name = *other.m_name; *m_type = *other.m_type; @@ -73,32 +75,32 @@ void biorbd::muscles::Compound::DeepCopy(const biorbd::muscles::Compound &other) *m_force = *other.m_force; } -const biorbd::utils::String &biorbd::muscles::Compound::name() const +const utils::String &muscles::Compound::name() const { return *m_name; } -void biorbd::muscles::Compound::setName(const biorbd::utils::String &name) +void muscles::Compound::setName(const utils::String &name) { *m_name = name; } -biorbd::muscles::MUSCLE_TYPE biorbd::muscles::Compound::type() const +muscles::MUSCLE_TYPE muscles::Compound::type() const { return *m_type; } -const biorbd::muscles::PathModifiers &biorbd::muscles::Compound::pathModifier() +const muscles::PathModifiers &muscles::Compound::pathModifier() { return *m_pathChanger; } -void biorbd::muscles::Compound::addPathObject(biorbd::utils::Vector3d &wrap) +void muscles::Compound::addPathObject(utils::Vector3d &wrap) { m_pathChanger->addPathChanger(wrap); } -const biorbd::utils::Scalar& biorbd::muscles::Compound::force() +const utils::Scalar& muscles::Compound::force() { return *m_force; } diff --git a/src/Muscles/FatigueDynamicState.cpp b/src/Muscles/FatigueDynamicState.cpp index b382d5e4..1f0d3745 100644 --- a/src/Muscles/FatigueDynamicState.cpp +++ b/src/Muscles/FatigueDynamicState.cpp @@ -3,55 +3,57 @@ #include "Utils/Error.h" -biorbd::muscles::FatigueDynamicState::FatigueDynamicState( - const biorbd::utils::Scalar& active, - const biorbd::utils::Scalar& fatigued, - const biorbd::utils::Scalar& resting) : - biorbd::muscles::FatigueState(active,fatigued,resting), - m_activeFibersDot(std::make_shared(0)), - m_fatiguedFibersDot(std::make_shared(0)), - m_restingFibersDot(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::FatigueDynamicState::FatigueDynamicState( + const utils::Scalar& active, + const utils::Scalar& fatigued, + const utils::Scalar& resting) : + muscles::FatigueState(active,fatigued,resting), + m_activeFibersDot(std::make_shared(0)), + m_fatiguedFibersDot(std::make_shared(0)), + m_restingFibersDot(std::make_shared(0)) { setType(); } -biorbd::muscles::FatigueDynamicState::FatigueDynamicState( - const std::shared_ptr other) : - biorbd::muscles::FatigueState(other) +muscles::FatigueDynamicState::FatigueDynamicState( + const std::shared_ptr other) : + muscles::FatigueState(other) { - std::shared_ptr muscle_tp( - std::dynamic_pointer_cast(other)); + std::shared_ptr muscle_tp( + std::dynamic_pointer_cast(other)); if (!muscle_tp) { - biorbd::utils::Error::raise("This is not a dynamically fatigable muscle"); + utils::Error::raise("This is not a dynamically fatigable muscle"); } m_activeFibersDot = muscle_tp->m_activeFibersDot; m_fatiguedFibersDot = muscle_tp->m_fatiguedFibersDot; m_restingFibersDot = muscle_tp->m_restingFibersDot; } -void biorbd::muscles::FatigueDynamicState::DeepCopy(const - biorbd::muscles::FatigueDynamicState &other) +void muscles::FatigueDynamicState::DeepCopy(const + muscles::FatigueDynamicState &other) { - biorbd::muscles::FatigueState::DeepCopy(other); + muscles::FatigueState::DeepCopy(other); *m_activeFibersDot = *other.m_activeFibersDot; *m_fatiguedFibersDot = *other.m_fatiguedFibersDot; *m_restingFibersDot = *other.m_restingFibersDot; } -const biorbd::utils::Scalar& -biorbd::muscles::FatigueDynamicState::activeFibersDot() const +const utils::Scalar& +muscles::FatigueDynamicState::activeFibersDot() const { return *m_activeFibersDot; } -const biorbd::utils::Scalar& -biorbd::muscles::FatigueDynamicState::fatiguedFibersDot() const +const utils::Scalar& +muscles::FatigueDynamicState::fatiguedFibersDot() const { return *m_fatiguedFibersDot; } -const biorbd::utils::Scalar& -biorbd::muscles::FatigueDynamicState::restingFibersDot() const +const utils::Scalar& +muscles::FatigueDynamicState::restingFibersDot() const { return *m_restingFibersDot; } diff --git a/src/Muscles/FatigueDynamicStateXia.cpp b/src/Muscles/FatigueDynamicStateXia.cpp index 341f96d9..71276557 100644 --- a/src/Muscles/FatigueDynamicStateXia.cpp +++ b/src/Muscles/FatigueDynamicStateXia.cpp @@ -7,47 +7,49 @@ #include "Muscles/FatigueParameters.h" #include "Muscles/StateDynamics.h" -biorbd::muscles::FatigueDynamicStateXia::FatigueDynamicStateXia( - const biorbd::utils::Scalar& active, - const biorbd::utils::Scalar& fatigued, - const biorbd::utils::Scalar& resting) : - biorbd::muscles::FatigueDynamicState(active,fatigued,resting) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::FatigueDynamicStateXia::FatigueDynamicStateXia( + const utils::Scalar& active, + const utils::Scalar& fatigued, + const utils::Scalar& resting) : + muscles::FatigueDynamicState(active,fatigued,resting) { setType(); } -biorbd::muscles::FatigueDynamicStateXia::FatigueDynamicStateXia( - const std::shared_ptr other): - biorbd::muscles::FatigueDynamicState(other) +muscles::FatigueDynamicStateXia::FatigueDynamicStateXia( + const std::shared_ptr other): + muscles::FatigueDynamicState(other) { } -biorbd::muscles::FatigueDynamicStateXia -biorbd::muscles::FatigueDynamicStateXia::DeepCopy() const +muscles::FatigueDynamicStateXia +muscles::FatigueDynamicStateXia::DeepCopy() const { - biorbd::muscles::FatigueDynamicStateXia copy; + muscles::FatigueDynamicStateXia copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::FatigueDynamicStateXia::DeepCopy(const - biorbd::muscles::FatigueDynamicStateXia &other) +void muscles::FatigueDynamicStateXia::DeepCopy(const + muscles::FatigueDynamicStateXia &other) { - biorbd::muscles::FatigueDynamicState::DeepCopy(other); + muscles::FatigueDynamicState::DeepCopy(other); } -void biorbd::muscles::FatigueDynamicStateXia::timeDerivativeState( - const biorbd::muscles::StateDynamics &emg, - const biorbd::muscles::Characteristics &characteristics) +void muscles::FatigueDynamicStateXia::timeDerivativeState( + const muscles::StateDynamics &emg, + const muscles::Characteristics &characteristics) { #ifdef BIORBD_USE_CASADI_MATH - biorbd::utils::Error::raise("timeDerivativeState for FatigueDynamicStateXia" + utils::Error::raise("timeDerivativeState for FatigueDynamicStateXia" " is not implemented yet"); #else // Getting the command - biorbd::utils::Scalar targetCommand(emg.activation()); - biorbd::utils::Scalar command(0); + utils::Scalar targetCommand(emg.activation()); + utils::Scalar command(0); if (*m_activeFibers < targetCommand) { if (*m_restingFibers > targetCommand - *m_activeFibers) { command = characteristics.fatigueParameters().developFactor()* @@ -69,13 +71,13 @@ void biorbd::muscles::FatigueDynamicStateXia::timeDerivativeState( *m_activeFibers - characteristics.fatigueParameters().recoveryRate()* *m_fatiguedFibers; - biorbd::utils::Error::check( + utils::Error::check( fabs(*m_activeFibersDot + *m_restingFibersDot + *m_fatiguedFibersDot) <= 1e-7, "Sum of time derivates of fatigue states must be equal to 0"); #endif } -void biorbd::muscles::FatigueDynamicStateXia::setType() +void muscles::FatigueDynamicStateXia::setType() { - *m_type =biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA; + *m_type =muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA; } diff --git a/src/Muscles/FatigueModel.cpp b/src/Muscles/FatigueModel.cpp index 743e1644..ff6ca7eb 100644 --- a/src/Muscles/FatigueModel.cpp +++ b/src/Muscles/FatigueModel.cpp @@ -5,81 +5,83 @@ #include "Muscles/Muscle.h" #include "Muscles/FatigueDynamicStateXia.h" -biorbd::muscles::FatigueModel::FatigueModel( - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::FatigueModel::FatigueModel( + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) { if (dynamicFatigueType == - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE) { - m_fatigueState = std::make_shared(); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE) { + m_fatigueState = std::make_shared(); } else if (dynamicFatigueType == - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA) { - m_fatigueState = std::make_shared(); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA) { + m_fatigueState = std::make_shared(); } else { - biorbd::utils::Error::raise("Wrong muscle fatigue type"); + utils::Error::raise("Wrong muscle fatigue type"); } } -biorbd::muscles::FatigueModel::FatigueModel( - const biorbd::muscles::FatigueModel &other) : +muscles::FatigueModel::FatigueModel( + const muscles::FatigueModel &other) : m_fatigueState(other.m_fatigueState) { } -biorbd::muscles::FatigueModel::FatigueModel( - const std::shared_ptr other) : +muscles::FatigueModel::FatigueModel( + const std::shared_ptr other) : m_fatigueState(other->m_fatigueState) { } -biorbd::muscles::FatigueModel::~FatigueModel() +muscles::FatigueModel::~FatigueModel() { } -void biorbd::muscles::FatigueModel::DeepCopy(const biorbd::muscles::FatigueModel +void muscles::FatigueModel::DeepCopy(const muscles::FatigueModel &other) { *m_fatigueState = other.m_fatigueState->DeepCopy(); } #ifndef BIORBD_USE_CASADI_MATH -void biorbd::muscles::FatigueModel::setFatigueState( - const biorbd::utils::Scalar& active, - const biorbd::utils::Scalar& fatigued, - const biorbd::utils::Scalar& resting) +void muscles::FatigueModel::setFatigueState( + const utils::Scalar& active, + const utils::Scalar& fatigued, + const utils::Scalar& resting) { m_fatigueState->setState(active, fatigued, resting); } #endif -biorbd::muscles::FatigueState& biorbd::muscles::FatigueModel::fatigueState() +muscles::FatigueState& muscles::FatigueModel::fatigueState() { return *m_fatigueState; } -const biorbd::muscles::FatigueState& -biorbd::muscles::FatigueModel::fatigueState() const +const muscles::FatigueState& +muscles::FatigueModel::fatigueState() const { return *m_fatigueState; } -void biorbd::muscles::FatigueModel::computeTimeDerivativeState( - const biorbd::muscles::StateDynamics &emg) +void muscles::FatigueModel::computeTimeDerivativeState( + const muscles::StateDynamics &emg) { - if (std::dynamic_pointer_cast + if (std::dynamic_pointer_cast (m_fatigueState)) { - biorbd::muscles::Muscle* muscle = dynamic_cast(this); + muscles::Muscle* muscle = dynamic_cast(this); if (muscle) { - std::static_pointer_cast + std::static_pointer_cast (m_fatigueState)->timeDerivativeState(emg, muscle->characteristics()); } else { - biorbd::utils::Error::raise("biorbd::muscles::FatigueModel should be a biorbd::muscles::Muscle"); + utils::Error::raise("muscles::FatigueModel should be a muscles::Muscle"); } } else { - biorbd::utils::Error::raise("Type cannot be fatigued"); + utils::Error::raise("Type cannot be fatigued"); } } diff --git a/src/Muscles/FatigueParameters.cpp b/src/Muscles/FatigueParameters.cpp index 1ee0ccc3..5ac1e54d 100644 --- a/src/Muscles/FatigueParameters.cpp +++ b/src/Muscles/FatigueParameters.cpp @@ -1,29 +1,31 @@ #define BIORBD_API_EXPORTS #include "Muscles/FatigueParameters.h" -biorbd::muscles::FatigueParameters::FatigueParameters( - const biorbd::utils::Scalar& _fatigueRate, - const biorbd::utils::Scalar& _recoveryRate, - const biorbd::utils::Scalar& _developFactor, - const biorbd::utils::Scalar& recoveryFactor): - m_fatigueRate(std::make_shared(_fatigueRate)), - m_recoveryRate(std::make_shared(_recoveryRate)), - m_developFactor(std::make_shared(_developFactor)), - m_recoveryFactor(std::make_shared(recoveryFactor)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::FatigueParameters::FatigueParameters( + const utils::Scalar& _fatigueRate, + const utils::Scalar& _recoveryRate, + const utils::Scalar& _developFactor, + const utils::Scalar& recoveryFactor): + m_fatigueRate(std::make_shared(_fatigueRate)), + m_recoveryRate(std::make_shared(_recoveryRate)), + m_developFactor(std::make_shared(_developFactor)), + m_recoveryFactor(std::make_shared(recoveryFactor)) { } -biorbd::muscles::FatigueParameters -biorbd::muscles::FatigueParameters::DeepCopy() const +muscles::FatigueParameters +muscles::FatigueParameters::DeepCopy() const { - biorbd::muscles::FatigueParameters copy; + muscles::FatigueParameters copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::FatigueParameters::DeepCopy(const - biorbd::muscles::FatigueParameters &other) +void muscles::FatigueParameters::DeepCopy(const + muscles::FatigueParameters &other) { *m_fatigueRate = *other.m_fatigueRate; *m_recoveryRate = *other.m_recoveryRate; @@ -31,46 +33,46 @@ void biorbd::muscles::FatigueParameters::DeepCopy(const *m_recoveryFactor = *other.m_recoveryFactor; } -void biorbd::muscles::FatigueParameters::setFatigueRate( - const biorbd::utils::Scalar& fatigueRate) +void muscles::FatigueParameters::setFatigueRate( + const utils::Scalar& fatigueRate) { *m_fatigueRate = fatigueRate; } -const biorbd::utils::Scalar& biorbd::muscles::FatigueParameters::fatigueRate() +const utils::Scalar& muscles::FatigueParameters::fatigueRate() const { return *m_fatigueRate; } -void biorbd::muscles::FatigueParameters::setRecoveryRate( - const biorbd::utils::Scalar& recoveryRate) +void muscles::FatigueParameters::setRecoveryRate( + const utils::Scalar& recoveryRate) { *m_recoveryRate = recoveryRate; } -const biorbd::utils::Scalar& biorbd::muscles::FatigueParameters::recoveryRate() +const utils::Scalar& muscles::FatigueParameters::recoveryRate() const { return *m_recoveryRate; } -void biorbd::muscles::FatigueParameters::setDevelopFactor( - const biorbd::utils::Scalar& developFactor) +void muscles::FatigueParameters::setDevelopFactor( + const utils::Scalar& developFactor) { *m_developFactor = developFactor; } -const biorbd::utils::Scalar& biorbd::muscles::FatigueParameters::developFactor() +const utils::Scalar& muscles::FatigueParameters::developFactor() const { return *m_developFactor; } -void biorbd::muscles::FatigueParameters::setRecoveryFactor( - const biorbd::utils::Scalar& recoveryFactor) +void muscles::FatigueParameters::setRecoveryFactor( + const utils::Scalar& recoveryFactor) { *m_recoveryFactor = recoveryFactor; } -const biorbd::utils::Scalar& -biorbd::muscles::FatigueParameters::recoveryFactor() const +const utils::Scalar& +muscles::FatigueParameters::recoveryFactor() const { return *m_recoveryFactor; } diff --git a/src/Muscles/FatigueState.cpp b/src/Muscles/FatigueState.cpp index 49f03937..fb0849bb 100644 --- a/src/Muscles/FatigueState.cpp +++ b/src/Muscles/FatigueState.cpp @@ -17,19 +17,21 @@ template < typename T > std::string to_string( const T& n ) } #endif -biorbd::muscles::FatigueState::FatigueState( - const biorbd::utils::Scalar& active, - const biorbd::utils::Scalar& fatigued, - const biorbd::utils::Scalar& resting) : - m_activeFibers(std::make_shared(active)), - m_fatiguedFibers(std::make_shared(fatigued)), - m_restingFibers(std::make_shared(resting)), - m_type(std::make_shared()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::FatigueState::FatigueState( + const utils::Scalar& active, + const utils::Scalar& fatigued, + const utils::Scalar& resting) : + m_activeFibers(std::make_shared(active)), + m_fatiguedFibers(std::make_shared(fatigued)), + m_restingFibers(std::make_shared(resting)), + m_type(std::make_shared()) { setType(); } -biorbd::muscles::FatigueState::FatigueState(const biorbd::muscles::FatigueState +muscles::FatigueState::FatigueState(const muscles::FatigueState &other) : m_activeFibers(other.m_activeFibers), m_fatiguedFibers(other.m_fatiguedFibers), @@ -39,8 +41,8 @@ biorbd::muscles::FatigueState::FatigueState(const biorbd::muscles::FatigueState } -biorbd::muscles::FatigueState::FatigueState(const - std::shared_ptr other) : +muscles::FatigueState::FatigueState(const + std::shared_ptr other) : m_activeFibers(other->m_activeFibers), m_fatiguedFibers(other->m_fatiguedFibers), m_restingFibers(other->m_restingFibers), @@ -49,19 +51,19 @@ biorbd::muscles::FatigueState::FatigueState(const } -biorbd::muscles::FatigueState::~FatigueState() +muscles::FatigueState::~FatigueState() { } -biorbd::muscles::FatigueState biorbd::muscles::FatigueState::DeepCopy() const +muscles::FatigueState muscles::FatigueState::DeepCopy() const { - biorbd::muscles::FatigueState copy; + muscles::FatigueState copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::FatigueState::DeepCopy(const biorbd::muscles::FatigueState +void muscles::FatigueState::DeepCopy(const muscles::FatigueState &other) { *m_activeFibers = *other.m_activeFibers; @@ -71,10 +73,10 @@ void biorbd::muscles::FatigueState::DeepCopy(const biorbd::muscles::FatigueState } #ifndef BIORBD_USE_CASADI_MATH -void biorbd::muscles::FatigueState::setState( - biorbd::utils::Scalar active, - biorbd::utils::Scalar fatigued, - biorbd::utils::Scalar resting, +void muscles::FatigueState::setState( + utils::Scalar active, + utils::Scalar fatigued, + utils::Scalar resting, bool turnOffWarnings) { // Sanity check for active fibers @@ -86,7 +88,7 @@ void biorbd::muscles::FatigueState::setState( // if (active < 0) { if (!turnOffWarnings) { - biorbd::utils::Error::warning(0, + utils::Error::warning(0, "Active Fibers Quantity can't be lower than 0, 0 is used then\n" "Previous Active Fibers Quantity before set to 0:" + std::to_string(active)); @@ -95,7 +97,7 @@ void biorbd::muscles::FatigueState::setState( active = 0; } else if (active > 1) { if (!turnOffWarnings) { - biorbd::utils::Error::warning(0, + utils::Error::warning(0, "Active Fibers Quantity can't be higher than 1, 1 is used then\n" "Previous Active Fibers Quantity before set to 1: " + std::to_string(active)); @@ -107,9 +109,9 @@ void biorbd::muscles::FatigueState::setState( // Sanity check for fatigued fibers if (fatigued < 0) { - biorbd::utils::Error::raise("Fatigued Fibers Quantity can't be lower than 0"); + utils::Error::raise("Fatigued Fibers Quantity can't be lower than 0"); } else if (fatigued > 1) { - biorbd::utils::Error::raise("Fatigued Fibers Quantity can't be higher than 1"); + utils::Error::raise("Fatigued Fibers Quantity can't be higher than 1"); } // Sanity check for resting fibers @@ -121,7 +123,7 @@ void biorbd::muscles::FatigueState::setState( // if (resting < 0) { if (!turnOffWarnings) { - biorbd::utils::Error::warning(0, + utils::Error::warning(0, "Resting Fibers Quantity can't be lower than 0, 0 is used then\n" "Previous Resting Fibers Quantity before set to 0: " + std::to_string(resting)); @@ -129,13 +131,13 @@ void biorbd::muscles::FatigueState::setState( active += resting; resting = 0; } else if (resting > 1) { - biorbd::utils::Error::raise( + utils::Error::raise( "Resting Fibers Quantity can't be higher than 1"); } if (fabs(active + fatigued + resting - 1.0) > 0.1) { - biorbd::utils::Error::raise("Sum of the fatigued states must be equal to 1"); + utils::Error::raise("Sum of the fatigued states must be equal to 1"); } *m_activeFibers = active; @@ -144,30 +146,30 @@ void biorbd::muscles::FatigueState::setState( } #endif -const biorbd::utils::Scalar& biorbd::muscles::FatigueState::activeFibers() const +const utils::Scalar& muscles::FatigueState::activeFibers() const { return *m_activeFibers; } -const biorbd::utils::Scalar& biorbd::muscles::FatigueState::fatiguedFibers() +const utils::Scalar& muscles::FatigueState::fatiguedFibers() const { return *m_fatiguedFibers; } -const biorbd::utils::Scalar& biorbd::muscles::FatigueState::restingFibers() +const utils::Scalar& muscles::FatigueState::restingFibers() const { return *m_restingFibers; } -biorbd::muscles::STATE_FATIGUE_TYPE biorbd::muscles::FatigueState::getType() +muscles::STATE_FATIGUE_TYPE muscles::FatigueState::getType() const { return *m_type; } -void biorbd::muscles::FatigueState::setType() +void muscles::FatigueState::setType() { - *m_type = biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE; + *m_type = muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE; } diff --git a/src/Muscles/Geometry.cpp b/src/Muscles/Geometry.cpp index 65b188b0..18d73d21 100644 --- a/src/Muscles/Geometry.cpp +++ b/src/Muscles/Geometry.cpp @@ -16,21 +16,23 @@ #include "Muscles/ViaPoint.h" #include "Muscles/PathModifiers.h" -biorbd::muscles::Geometry::Geometry() : - m_origin(std::make_shared()), - m_insertion(std::make_shared()), - m_originInGlobal(std::make_shared - (biorbd::utils::Vector3d::Zero())), - m_insertionInGlobal(std::make_shared - (biorbd::utils::Vector3d::Zero())), - m_pointsInGlobal(std::make_shared>()), - m_pointsInLocal(std::make_shared>()), - m_jacobian(std::make_shared()), - m_G(std::make_shared()), - m_jacobianLength(std::make_shared()), - m_length(std::make_shared(0)), - m_muscleTendonLength(std::make_shared(0)), - m_velocity(std::make_shared(0)), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::Geometry::Geometry() : + m_origin(std::make_shared()), + m_insertion(std::make_shared()), + m_originInGlobal(std::make_shared + (utils::Vector3d::Zero())), + m_insertionInGlobal(std::make_shared + (utils::Vector3d::Zero())), + m_pointsInGlobal(std::make_shared>()), + m_pointsInLocal(std::make_shared>()), + m_jacobian(std::make_shared()), + m_G(std::make_shared()), + m_jacobianLength(std::make_shared()), + m_length(std::make_shared(0)), + m_muscleTendonLength(std::make_shared(0)), + m_velocity(std::make_shared(0)), m_isGeometryComputed(std::make_shared(false)), m_isVelocityComputed(std::make_shared(false)), m_posAndJacoWereForced(std::make_shared(false)) @@ -38,23 +40,23 @@ biorbd::muscles::Geometry::Geometry() : } -biorbd::muscles::Geometry::Geometry( - const biorbd::utils::Vector3d &origin, - const biorbd::utils::Vector3d &insertion) : - m_origin(std::make_shared(origin)), - m_insertion(std::make_shared(insertion)), - m_originInGlobal(std::make_shared - (biorbd::utils::Vector3d::Zero())), - m_insertionInGlobal(std::make_shared - (biorbd::utils::Vector3d::Zero())), - m_pointsInGlobal(std::make_shared>()), - m_pointsInLocal(std::make_shared>()), - m_jacobian(std::make_shared()), - m_G(std::make_shared()), - m_jacobianLength(std::make_shared()), - m_length(std::make_shared(0)), - m_muscleTendonLength(std::make_shared(0)), - m_velocity(std::make_shared(0)), +muscles::Geometry::Geometry( + const utils::Vector3d &origin, + const utils::Vector3d &insertion) : + m_origin(std::make_shared(origin)), + m_insertion(std::make_shared(insertion)), + m_originInGlobal(std::make_shared + (utils::Vector3d::Zero())), + m_insertionInGlobal(std::make_shared + (utils::Vector3d::Zero())), + m_pointsInGlobal(std::make_shared>()), + m_pointsInLocal(std::make_shared>()), + m_jacobian(std::make_shared()), + m_G(std::make_shared()), + m_jacobianLength(std::make_shared()), + m_length(std::make_shared(0)), + m_muscleTendonLength(std::make_shared(0)), + m_velocity(std::make_shared(0)), m_isGeometryComputed(std::make_shared(false)), m_isVelocityComputed(std::make_shared(false)), m_posAndJacoWereForced(std::make_shared(false)) @@ -62,14 +64,14 @@ biorbd::muscles::Geometry::Geometry( } -biorbd::muscles::Geometry biorbd::muscles::Geometry::DeepCopy() const +muscles::Geometry muscles::Geometry::DeepCopy() const { - biorbd::muscles::Geometry copy; + muscles::Geometry copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::Geometry::DeepCopy(const biorbd::muscles::Geometry &other) +void muscles::Geometry::DeepCopy(const muscles::Geometry &other) { *m_origin = other.m_origin->DeepCopy(); *m_insertion = other.m_insertion->DeepCopy(); @@ -96,14 +98,14 @@ void biorbd::muscles::Geometry::DeepCopy(const biorbd::muscles::Geometry &other) // ------ PUBLIC FUNCTIONS ------ // -void biorbd::muscles::Geometry::updateKinematics( +void muscles::Geometry::updateKinematics( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates *Q, - const biorbd::rigidbody::GeneralizedVelocity *Qdot, + const rigidbody::GeneralizedCoordinates *Q, + const rigidbody::GeneralizedVelocity *Qdot, int updateKin) { if (*m_posAndJacoWereForced) { - biorbd::utils::Error::warning( + utils::Error::warning( false, "Warning, using updateKinematics overrides the" " previously sent position and jacobian"); @@ -128,16 +130,16 @@ void biorbd::muscles::Geometry::updateKinematics( _updateKinematics(Qdot); } -void biorbd::muscles::Geometry::updateKinematics(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints +void muscles::Geometry::updateKinematics(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::muscles::Characteristics& characteristics, - biorbd::muscles::PathModifiers &pathModifiers, - const biorbd::rigidbody::GeneralizedCoordinates *Q, - const biorbd::rigidbody::GeneralizedVelocity *Qdot, + const muscles::Characteristics& characteristics, + muscles::PathModifiers &pathModifiers, + const rigidbody::GeneralizedCoordinates *Q, + const rigidbody::GeneralizedVelocity *Qdot, int updateKin) { if (*m_posAndJacoWereForced) { - biorbd::utils::Error::warning( + utils::Error::warning( false, "Warning, using updateKinematics overrides the" " previously sent position and jacobian"); *m_posAndJacoWereForced = false; @@ -161,10 +163,10 @@ void biorbd::muscles::Geometry::updateKinematics(biorbd::BIORBD_MATH_NAMESPACE:: _updateKinematics(Qdot, &characteristics, &pathModifiers); } -void biorbd::muscles::Geometry::updateKinematics( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity* Qdot) +void muscles::Geometry::updateKinematics( + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity* Qdot) { *m_posAndJacoWereForced = true; @@ -178,11 +180,11 @@ void biorbd::muscles::Geometry::updateKinematics( _updateKinematics(Qdot); } -void biorbd::muscles::Geometry::updateKinematics( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix& jacoPointsInGlobal, - const biorbd::muscles::Characteristics& c, - const biorbd::rigidbody::GeneralizedVelocity* Qdot) +void muscles::Geometry::updateKinematics( + std::vector& musclePointsInGlobal, + utils::Matrix& jacoPointsInGlobal, + const muscles::Characteristics& c, + const rigidbody::GeneralizedVelocity* Qdot) { *m_posAndJacoWereForced = true; @@ -197,120 +199,120 @@ void biorbd::muscles::Geometry::updateKinematics( } // Get and set the positions of the origins and insertions -void biorbd::muscles::Geometry::setOrigin( +void muscles::Geometry::setOrigin( const utils::Vector3d &position) { - if (dynamic_cast(&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 +const utils::Vector3d& muscles::Geometry::originInLocal() const { return *m_origin; } -void biorbd::muscles::Geometry::setInsertionInLocal( +void muscles::Geometry::setInsertionInLocal( const utils::Vector3d &position) { - if (dynamic_cast(&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 utils::Vector3d &muscles::Geometry::insertionInLocal() const { return *m_insertion; } // Position of the muscles in space -const biorbd::utils::Vector3d &biorbd::muscles::Geometry::originInGlobal() const +const utils::Vector3d &muscles::Geometry::originInGlobal() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed at least once before calling originInLocal()"); return *m_originInGlobal; } -const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInGlobal() +const utils::Vector3d &muscles::Geometry::insertionInGlobal() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed at least once before calling insertionInGlobal()"); return *m_insertionInGlobal; } -const std::vector -&biorbd::muscles::Geometry::musclesPointsInGlobal() const +const std::vector +&muscles::Geometry::musclesPointsInGlobal() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed at least once before calling musclesPointsInGlobal()"); return *m_pointsInGlobal; } // Return the length and muscular velocity -const biorbd::utils::Scalar& biorbd::muscles::Geometry::length() const +const utils::Scalar& muscles::Geometry::length() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed at least before calling length()"); return *m_length; } -const biorbd::utils::Scalar& biorbd::muscles::Geometry::musculoTendonLength() +const utils::Scalar& muscles::Geometry::musculoTendonLength() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed at least before calling length()"); return *m_muscleTendonLength; } -const biorbd::utils::Scalar& biorbd::muscles::Geometry::velocity() const +const utils::Scalar& muscles::Geometry::velocity() const { - biorbd::utils::Error::check(*m_isVelocityComputed, + utils::Error::check(*m_isVelocityComputed, "Geometry must be computed before calling velocity()"); return *m_velocity; } // Return the Jacobian -const biorbd::utils::Matrix& biorbd::muscles::Geometry::jacobian() const +const utils::Matrix& muscles::Geometry::jacobian() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed before calling jacobian()"); return *m_jacobian; } // Return the last Jacobian -biorbd::utils::Matrix biorbd::muscles::Geometry::jacobianOrigin() const +utils::Matrix muscles::Geometry::jacobianOrigin() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed before calling jacobianOrigin()"); return m_jacobian->block(0,0,3,m_jacobian->cols()); } -biorbd::utils::Matrix biorbd::muscles::Geometry::jacobianInsertion() const +utils::Matrix muscles::Geometry::jacobianInsertion() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed before calling jacobianInsertion()"); return m_jacobian->block(m_jacobian->rows()-3,0,3,m_jacobian->cols()); } -biorbd::utils::Matrix biorbd::muscles::Geometry::jacobian( +utils::Matrix muscles::Geometry::jacobian( unsigned int idxViaPoint) const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed before calling jacobian(i)"); return m_jacobian->block(3*idxViaPoint,0,3,m_jacobian->cols()); } -const biorbd::utils::Matrix &biorbd::muscles::Geometry::jacobianLength() const +const utils::Matrix &muscles::Geometry::jacobianLength() const { - biorbd::utils::Error::check(*m_isGeometryComputed, + utils::Error::check(*m_isGeometryComputed, "Geometry must be computed before calling jacobianLength()"); return *m_jacobianLength; } // --------------------------------------- // -void biorbd::muscles::Geometry::_updateKinematics( - const biorbd::rigidbody::GeneralizedVelocity* Qdot, - const biorbd::muscles::Characteristics* characteristics, - biorbd::muscles::PathModifiers *pathModifiers) +void muscles::Geometry::_updateKinematics( + const rigidbody::GeneralizedVelocity* Qdot, + const muscles::Characteristics* characteristics, + muscles::PathModifiers *pathModifiers) { // Compute the length and velocities length(characteristics, pathModifiers); @@ -326,9 +328,9 @@ void biorbd::muscles::Geometry::_updateKinematics( } } -const biorbd::utils::Vector3d &biorbd::muscles::Geometry::originInGlobal( +const utils::Vector3d &muscles::Geometry::originInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q) + const rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position m_originInGlobal->block(0,0,3, @@ -337,9 +339,9 @@ const biorbd::utils::Vector3d &biorbd::muscles::Geometry::originInGlobal( return *m_originInGlobal; } -const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInGlobal( +const utils::Vector3d &muscles::Geometry::insertionInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q) + const rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position m_insertionInGlobal->block(0,0,3, @@ -348,19 +350,19 @@ const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInGlobal( return *m_insertionInGlobal; } -void biorbd::muscles::Geometry::setMusclesPointsInGlobal( - std::vector &ptsInGlobal) +void muscles::Geometry::setMusclesPointsInGlobal( + std::vector &ptsInGlobal) { - biorbd::utils::Error::check(ptsInGlobal.size() >= 2, + utils::Error::check(ptsInGlobal.size() >= 2, "ptsInGlobal must at least have an origin and an insertion"); m_pointsInLocal->clear(); // In this mode, we don't need the local, because the Jacobian of the points has to be given as well *m_pointsInGlobal = ptsInGlobal; } -void biorbd::muscles::Geometry::setMusclesPointsInGlobal( +void muscles::Geometry::setMusclesPointsInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - biorbd::muscles::PathModifiers *pathModifiers) + const rigidbody::GeneralizedCoordinates &Q, + muscles::PathModifiers *pathModifiers) { // Output varible (reset to zero) m_pointsInLocal->clear(); @@ -369,38 +371,38 @@ void biorbd::muscles::Geometry::setMusclesPointsInGlobal( // Do not apply on wrapping objects if (pathModifiers->nbWraps()!=0) { // CHECK TO MODIFY BEFOR GOING FORWARD WITH PROJECTS - biorbd::utils::Error::check(pathModifiers->nbVia() == 0, + utils::Error::check(pathModifiers->nbVia() == 0, "Cannot mix wrapping and via points yet") ; - biorbd::utils::Error::check(pathModifiers->nbWraps() < 2, + utils::Error::check(pathModifiers->nbWraps() < 2, "Cannot compute more than one wrapping yet"); // Get the matrix of Rt of the wrap - biorbd::muscles::WrappingObject& w = - static_cast(pathModifiers->object(0)); - const biorbd::utils::RotoTrans& RT = w.RT(model,Q); + muscles::WrappingObject& w = + static_cast(pathModifiers->object(0)); + const utils::RotoTrans& RT = w.RT(model,Q); // Alias - const biorbd::utils::Vector3d& po_mus = originInGlobal(model, + const utils::Vector3d& po_mus = originInGlobal(model, Q); // Origin on bone - const biorbd::utils::Vector3d& pi_mus = insertionInGlobal(model, + const utils::Vector3d& pi_mus = insertionInGlobal(model, Q); // Insertion on bone - biorbd::utils::Vector3d pi_wrap(0, 0, + utils::Vector3d pi_wrap(0, 0, 0); // point on the wrapping related to insertion - biorbd::utils::Vector3d po_wrap(0, 0, + utils::Vector3d po_wrap(0, 0, 0); // point on the wrapping related to origin - biorbd::utils::Scalar a; // Force the computation of the length + utils::Scalar a; // Force the computation of the length w.wrapPoints(RT,po_mus,pi_mus,po_wrap, pi_wrap, &a); // Store the points in local m_pointsInLocal->push_back(originInLocal()); m_pointsInLocal->push_back( - biorbd::utils::Vector3d(RigidBodyDynamics::CalcBodyToBaseCoordinates( + utils::Vector3d(RigidBodyDynamics::CalcBodyToBaseCoordinates( model, Q, model.GetBodyId(w.parent().c_str()),po_wrap, false), "wrap_o", w.parent())); m_pointsInLocal->push_back( - biorbd::utils::Vector3d(RigidBodyDynamics::CalcBodyToBaseCoordinates( + utils::Vector3d(RigidBodyDynamics::CalcBodyToBaseCoordinates( model, Q, model.GetBodyId(w.parent().c_str()),pi_wrap, false), "wrap_i", w.parent())); m_pointsInLocal->push_back(insertionInLocal()); @@ -415,11 +417,11 @@ void biorbd::muscles::Geometry::setMusclesPointsInGlobal( else if (pathModifiers->nbObjects()!=0 && pathModifiers->object(0).typeOfNode() == - biorbd::utils::NODE_TYPE::VIA_POINT) { + utils::NODE_TYPE::VIA_POINT) { m_pointsInLocal->push_back(originInLocal()); m_pointsInGlobal->push_back(originInGlobal(model, Q)); for (unsigned int i=0; inbObjects(); ++i) { - const biorbd::muscles::ViaPoint& node(static_cast + const muscles::ViaPoint& node(static_cast (pathModifiers->object(i))); m_pointsInLocal->push_back(node); m_pointsInGlobal->push_back(RigidBodyDynamics::CalcBodyToBaseCoordinates(model, @@ -435,32 +437,32 @@ void biorbd::muscles::Geometry::setMusclesPointsInGlobal( m_pointsInGlobal->push_back(originInGlobal(model, Q)); m_pointsInGlobal->push_back(insertionInGlobal(model,Q)); } else { - biorbd::utils::Error::raise("Length for this type of object was not implemented"); + utils::Error::raise("Length for this type of object was not implemented"); } // Set the dimension of jacobian setJacobianDimension(model); } -const biorbd::utils::Scalar& biorbd::muscles::Geometry::length( - const biorbd::muscles::Characteristics *characteristics, - biorbd::muscles::PathModifiers *pathModifiers) +const utils::Scalar& muscles::Geometry::length( + const muscles::Characteristics *characteristics, + muscles::PathModifiers *pathModifiers) { *m_muscleTendonLength = 0; // because we can't combine, test the first (0) will let us know all the types if more than one if (pathModifiers != nullptr && pathModifiers->nbWraps()!=0) { - biorbd::utils::Error::check(pathModifiers->nbVia() == 0, + utils::Error::check(pathModifiers->nbVia() == 0, "Cannot mix wrapping and via points yet" ) ; - biorbd::utils::Error::check(pathModifiers->nbWraps() < 2, + utils::Error::check(pathModifiers->nbWraps() < 2, "Cannot compute more than one wrapping yet"); - biorbd::utils::Vector3d pi_wrap(0, 0, + utils::Vector3d pi_wrap(0, 0, 0); // point on the wrapping related to insertion - biorbd::utils::Vector3d po_wrap(0, 0, + utils::Vector3d po_wrap(0, 0, 0); // point on the wrapping related to origin - biorbd::utils::Scalar lengthWrap(0); - static_cast( + utils::Scalar lengthWrap(0); + static_cast( pathModifiers->object(0)).wrapPoints(po_wrap, pi_wrap, &lengthWrap); *m_muscleTendonLength = ((*m_pointsInGlobal)[0] - po_wrap).norm() + // length before the wrap @@ -481,32 +483,32 @@ const biorbd::utils::Scalar& biorbd::muscles::Geometry::length( return *m_length; } -const biorbd::utils::Scalar& biorbd::muscles::Geometry::velocity( - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +const utils::Scalar& muscles::Geometry::velocity( + const rigidbody::GeneralizedVelocity &Qdot) { // Compute the velocity of the muscular elongation *m_velocity = (jacobianLength()*Qdot)[0]; return *m_velocity; } -void biorbd::muscles::Geometry::setJacobianDimension(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints +void muscles::Geometry::setJacobianDimension(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model) { - *m_jacobian = biorbd::utils::Matrix::Zero(static_cast + *m_jacobian = utils::Matrix::Zero(static_cast (m_pointsInLocal->size()*3), model.dof_count); - *m_G = biorbd::utils::Matrix::Zero(3, model.dof_count); + *m_G = utils::Matrix::Zero(3, model.dof_count); } -void biorbd::muscles::Geometry::jacobian(const biorbd::utils::Matrix &jaco) +void muscles::Geometry::jacobian(const utils::Matrix &jaco) { - biorbd::utils::Error::check(jaco.rows()/3 == static_cast + utils::Error::check(jaco.rows()/3 == static_cast (m_pointsInGlobal->size()), "Jacobian is the wrong size"); *m_jacobian = jaco; } -void biorbd::muscles::Geometry::jacobian( +void muscles::Geometry::jacobian( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q) + const rigidbody::GeneralizedCoordinates &Q) { for (unsigned int i=0; isize(); ++i) { m_G->setZero(); @@ -517,12 +519,12 @@ void biorbd::muscles::Geometry::jacobian( } } -void biorbd::muscles::Geometry::computeJacobianLength() +void muscles::Geometry::computeJacobianLength() { - *m_jacobianLength = biorbd::utils::Matrix::Zero(1, m_jacobian->cols()); + *m_jacobianLength = utils::Matrix::Zero(1, m_jacobian->cols()); // jacobian approximates as if there were no wrapping object - const std::vector& p = *m_pointsInGlobal; + const std::vector& p = *m_pointsInGlobal; for (unsigned int i=0; i +muscles::HillThelenActiveOnlyType::HillThelenActiveOnlyType( + const std::shared_ptr other) : - biorbd::muscles::HillThelenType(other) + muscles::HillThelenType(other) { } -biorbd::muscles::HillThelenActiveOnlyType -biorbd::muscles::HillThelenActiveOnlyType::DeepCopy() const +muscles::HillThelenActiveOnlyType +muscles::HillThelenActiveOnlyType::DeepCopy() const { - biorbd::muscles::HillThelenActiveOnlyType copy; + muscles::HillThelenActiveOnlyType copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::HillThelenActiveOnlyType::DeepCopy( - const biorbd::muscles::HillThelenActiveOnlyType &other) +void muscles::HillThelenActiveOnlyType::DeepCopy( + const muscles::HillThelenActiveOnlyType &other) { - biorbd::muscles::HillThelenType::DeepCopy(other); + muscles::HillThelenType::DeepCopy(other); } -void biorbd::muscles::HillThelenActiveOnlyType::computeFlPE() +void muscles::HillThelenActiveOnlyType::computeFlPE() { *m_FlPE = 0; } -void biorbd::muscles::HillThelenActiveOnlyType::computeDamping() +void muscles::HillThelenActiveOnlyType::computeDamping() { *m_damping = 0; } -void biorbd::muscles::HillThelenActiveOnlyType::setType() +void muscles::HillThelenActiveOnlyType::setType() { - *m_type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE; + *m_type = muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE; } diff --git a/src/Muscles/HillThelenType.cpp b/src/Muscles/HillThelenType.cpp index efbaee7d..b2c817b7 100644 --- a/src/Muscles/HillThelenType.cpp +++ b/src/Muscles/HillThelenType.cpp @@ -6,80 +6,82 @@ #include "Muscles/Geometry.h" #include "Muscles/Characteristics.h" -biorbd::muscles::HillThelenType::HillThelenType() : - biorbd::muscles::HillType() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::HillThelenType::HillThelenType() : + muscles::HillType() { setType(); } -biorbd::muscles::HillThelenType::HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics) : - biorbd::muscles::HillType (name, geometry, characteristics) +muscles::HillThelenType::HillThelenType( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics) : + muscles::HillType (name, geometry, characteristics) { setType(); } -biorbd::muscles::HillThelenType::HillThelenType( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::State &emg) : - biorbd::muscles::HillType (name, geometry, characteristics, emg) +muscles::HillThelenType::HillThelenType( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::State &emg) : + muscles::HillType (name, geometry, characteristics, emg) { setType(); } -biorbd::muscles::HillThelenType::HillThelenType( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers) : - biorbd::muscles::HillType (name, geometry, characteristics, pathModifiers) +muscles::HillThelenType::HillThelenType( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers) : + muscles::HillType (name, geometry, characteristics, pathModifiers) { setType(); } -biorbd::muscles::HillThelenType::HillThelenType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - const biorbd::muscles::State& emg) : - biorbd::muscles::HillType (name, geometry, characteristics, pathModifiers, emg) +muscles::HillThelenType::HillThelenType( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics, + const muscles::PathModifiers &pathModifiers, + const muscles::State& emg) : + muscles::HillType (name, geometry, characteristics, pathModifiers, emg) { setType(); } -biorbd::muscles::HillThelenType::HillThelenType( - const biorbd::muscles::Muscle &other) : - biorbd::muscles::HillType (other) +muscles::HillThelenType::HillThelenType( + const muscles::Muscle &other) : + muscles::HillType (other) { } -biorbd::muscles::HillThelenType::HillThelenType( - const std::shared_ptr other) : - biorbd::muscles::HillType(other) +muscles::HillThelenType::HillThelenType( + const std::shared_ptr other) : + muscles::HillType(other) { } -biorbd::muscles::HillThelenType biorbd::muscles::HillThelenType::DeepCopy() +muscles::HillThelenType muscles::HillThelenType::DeepCopy() const { - biorbd::muscles::HillThelenType copy; + muscles::HillThelenType copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::HillThelenType::DeepCopy( - const biorbd::muscles::HillThelenType &other) +void muscles::HillThelenType::DeepCopy( + const muscles::HillThelenType &other) { - biorbd::muscles::HillType::DeepCopy(other); + muscles::HillType::DeepCopy(other); } -void biorbd::muscles::HillThelenType::computeFlPE() +void muscles::HillThelenType::computeFlPE() { #ifdef BIORBD_USE_CASADI_MATH *m_FlPE = casadi::MX::if_else( @@ -101,14 +103,14 @@ void biorbd::muscles::HillThelenType::computeFlPE() #endif } -void biorbd::muscles::HillThelenType::computeFlCE( - const biorbd::muscles::State&) +void muscles::HillThelenType::computeFlCE( + const muscles::State&) { *m_FlCE = exp( -pow(((position().length() / characteristics().optimalLength()) -1), 2 ) / *m_cste_FlCE_2 ); } -void biorbd::muscles::HillThelenType::setType() +void muscles::HillThelenType::setType() { - *m_type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN; + *m_type = muscles::MUSCLE_TYPE::HILL_THELEN; } diff --git a/src/Muscles/HillThelenTypeFatigable.cpp b/src/Muscles/HillThelenTypeFatigable.cpp index a41f142b..05114b04 100644 --- a/src/Muscles/HillThelenTypeFatigable.cpp +++ b/src/Muscles/HillThelenTypeFatigable.cpp @@ -4,105 +4,107 @@ #include "Utils/String.h" #include "Muscles/FatigueState.h" -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable() : - biorbd::muscles::HillThelenType(), - biorbd::muscles::FatigueModel ( - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable() : + muscles::HillThelenType(), + muscles::FatigueModel ( + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE) { setType(); } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : - biorbd::muscles::HillThelenType(name, geometry, characteristics), - biorbd::muscles::FatigueModel (dynamicFatigueType) +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : + muscles::HillThelenType(name, geometry, characteristics), + muscles::FatigueModel (dynamicFatigueType) { setType(); } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::State &emg, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : - biorbd::muscles::HillThelenType(name, geometry, characteristics, emg), - biorbd::muscles::FatigueModel (dynamicFatigueType) +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::State &emg, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : + muscles::HillThelenType(name, geometry, characteristics, emg), + muscles::FatigueModel (dynamicFatigueType) { setType(); } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : - biorbd::muscles::HillThelenType(name, geometry, characteristics, pathModifiers), - biorbd::muscles::FatigueModel (dynamicFatigueType) +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : + muscles::HillThelenType(name, geometry, characteristics, pathModifiers), + muscles::FatigueModel (dynamicFatigueType) { setType(); } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers& pathModifiers, - const biorbd::muscles::State& emg, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : - biorbd::muscles::HillThelenType(name, geometry, characteristics, pathModifiers, +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics, + const muscles::PathModifiers& pathModifiers, + const muscles::State& emg, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) : + muscles::HillThelenType(name, geometry, characteristics, pathModifiers, emg), - biorbd::muscles::FatigueModel (dynamicFatigueType) + muscles::FatigueModel (dynamicFatigueType) { setType(); } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const biorbd::muscles::Muscle &other) : - biorbd::muscles::HillThelenType (other), - biorbd::muscles::FatigueModel ( - dynamic_cast(other)) +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const muscles::Muscle &other) : + muscles::HillThelenType (other), + muscles::FatigueModel ( + dynamic_cast(other)) { } -biorbd::muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( - const std::shared_ptr other) : - biorbd::muscles::HillThelenType (other), - biorbd::muscles::FatigueModel ( - std::dynamic_pointer_cast(other)) +muscles::HillThelenTypeFatigable::HillThelenTypeFatigable( + const std::shared_ptr other) : + muscles::HillThelenType (other), + muscles::FatigueModel ( + std::dynamic_pointer_cast(other)) { } -biorbd::muscles::HillThelenTypeFatigable -biorbd::muscles::HillThelenTypeFatigable::DeepCopy() const +muscles::HillThelenTypeFatigable +muscles::HillThelenTypeFatigable::DeepCopy() const { - biorbd::muscles::HillThelenTypeFatigable copy; + muscles::HillThelenTypeFatigable copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::HillThelenTypeFatigable::DeepCopy( - const biorbd::muscles::HillThelenTypeFatigable &other) +void muscles::HillThelenTypeFatigable::DeepCopy( + const muscles::HillThelenTypeFatigable &other) { - biorbd::muscles::HillThelenType::DeepCopy(other); - biorbd::muscles::FatigueModel::DeepCopy(other); + muscles::HillThelenType::DeepCopy(other); + muscles::FatigueModel::DeepCopy(other); } -void biorbd::muscles::HillThelenTypeFatigable::computeFlCE( - const biorbd::muscles::State &emg) +void muscles::HillThelenTypeFatigable::computeFlCE( + const muscles::State &emg) { - biorbd::muscles::HillThelenType::computeFlCE(emg); + muscles::HillThelenType::computeFlCE(emg); // Do something with m_FlCE and m_characteristics.fatigueParameters *m_FlCE *= m_fatigueState->activeFibers(); } -void biorbd::muscles::HillThelenTypeFatigable::setType() +void muscles::HillThelenTypeFatigable::setType() { - *m_type = biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE; + *m_type = muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE; } diff --git a/src/Muscles/HillType.cpp b/src/Muscles/HillType.cpp index cbf0e4a5..a1128644 100644 --- a/src/Muscles/HillType.cpp +++ b/src/Muscles/HillType.cpp @@ -8,126 +8,128 @@ #include "Muscles/Geometry.h" #include "Muscles/State.h" -biorbd::muscles::HillType::HillType() : - biorbd::muscles::Muscle(), - m_damping(std::make_shared()), - m_FlCE(std::make_shared()), - m_FlPE(std::make_shared()), - m_FvCE(std::make_shared()), - m_cste_FlCE_1(std::make_shared(0.15)), - m_cste_FlCE_2(std::make_shared(0.45)), - m_cste_FvCE_1(std::make_shared(1)), - m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::HillType::HillType() : + muscles::Muscle(), + m_damping(std::make_shared()), + m_FlCE(std::make_shared()), + m_FlPE(std::make_shared()), + m_FvCE(std::make_shared()), + m_cste_FlCE_1(std::make_shared(0.15)), + m_cste_FlCE_2(std::make_shared(0.45)), + m_cste_FvCE_1(std::make_shared(1)), + m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ (1+*m_cste_FvCE_1))), - m_cste_FlPE_1(std::make_shared(10.0)), - m_cste_FlPE_2(std::make_shared(5.0)), - m_cste_eccentricForceMultiplier(std::make_shared(1.8)), - m_cste_damping(std::make_shared(0.1)), - m_cste_maxShorteningSpeed(std::make_shared(10.0)) + m_cste_FlPE_1(std::make_shared(10.0)), + m_cste_FlPE_2(std::make_shared(5.0)), + m_cste_eccentricForceMultiplier(std::make_shared(1.8)), + m_cste_damping(std::make_shared(0.1)), + m_cste_maxShorteningSpeed(std::make_shared(10.0)) { setType(); } -biorbd::muscles::HillType::HillType( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics) : - biorbd::muscles::Muscle(name,geometry,characteristics), - m_damping(std::make_shared()), - m_FlCE(std::make_shared()), - m_FlPE(std::make_shared()), - m_FvCE(std::make_shared()), - m_cste_FlCE_1(std::make_shared(0.15)), - m_cste_FlCE_2(std::make_shared(0.45)), - m_cste_FvCE_1(std::make_shared(1)), - m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ +muscles::HillType::HillType( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics) : + muscles::Muscle(name,geometry,characteristics), + m_damping(std::make_shared()), + m_FlCE(std::make_shared()), + m_FlPE(std::make_shared()), + m_FvCE(std::make_shared()), + m_cste_FlCE_1(std::make_shared(0.15)), + m_cste_FlCE_2(std::make_shared(0.45)), + m_cste_FvCE_1(std::make_shared(1)), + m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ (1+*m_cste_FvCE_1))), - m_cste_FlPE_1(std::make_shared(10.0)), - m_cste_FlPE_2(std::make_shared(5.0)), - m_cste_eccentricForceMultiplier(std::make_shared(1.8)), - m_cste_damping(std::make_shared(0.1)), - m_cste_maxShorteningSpeed(std::make_shared(10.0)) + m_cste_FlPE_1(std::make_shared(10.0)), + m_cste_FlPE_2(std::make_shared(5.0)), + m_cste_eccentricForceMultiplier(std::make_shared(1.8)), + m_cste_damping(std::make_shared(0.1)), + m_cste_maxShorteningSpeed(std::make_shared(10.0)) { setType(); } -biorbd::muscles::HillType::HillType( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::State& emg) : - biorbd::muscles::Muscle(name,geometry,characteristics, emg), - m_damping(std::make_shared()), - m_FlCE(std::make_shared()), - m_FlPE(std::make_shared()), - m_FvCE(std::make_shared()), - m_cste_FlCE_1(std::make_shared(0.15)), - m_cste_FlCE_2(std::make_shared(0.45)), - m_cste_FvCE_1(std::make_shared(1)), - m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ +muscles::HillType::HillType( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::State& emg) : + muscles::Muscle(name,geometry,characteristics, emg), + m_damping(std::make_shared()), + m_FlCE(std::make_shared()), + m_FlPE(std::make_shared()), + m_FvCE(std::make_shared()), + m_cste_FlCE_1(std::make_shared(0.15)), + m_cste_FlCE_2(std::make_shared(0.45)), + m_cste_FvCE_1(std::make_shared(1)), + m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ (1+*m_cste_FvCE_1))), - m_cste_FlPE_1(std::make_shared(10.0)), - m_cste_FlPE_2(std::make_shared(5.0)), - m_cste_eccentricForceMultiplier(std::make_shared(1.8)), - m_cste_damping(std::make_shared(0.1)), - m_cste_maxShorteningSpeed(std::make_shared(10.0)) + m_cste_FlPE_1(std::make_shared(10.0)), + m_cste_FlPE_2(std::make_shared(5.0)), + m_cste_eccentricForceMultiplier(std::make_shared(1.8)), + m_cste_damping(std::make_shared(0.1)), + m_cste_maxShorteningSpeed(std::make_shared(10.0)) { setType(); } -biorbd::muscles::HillType::HillType( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers) : - biorbd::muscles::Muscle(name,geometry,characteristics,pathModifiers), - m_damping(std::make_shared()), - m_FlCE(std::make_shared()), - m_FlPE(std::make_shared()), - m_FvCE(std::make_shared()), - m_cste_FlCE_1(std::make_shared(0.15)), - m_cste_FlCE_2(std::make_shared(0.45)), - m_cste_FvCE_1(std::make_shared(1)), - m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ +muscles::HillType::HillType( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers) : + muscles::Muscle(name,geometry,characteristics,pathModifiers), + m_damping(std::make_shared()), + m_FlCE(std::make_shared()), + m_FlPE(std::make_shared()), + m_FvCE(std::make_shared()), + m_cste_FlCE_1(std::make_shared(0.15)), + m_cste_FlCE_2(std::make_shared(0.45)), + m_cste_FvCE_1(std::make_shared(1)), + m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ (1+*m_cste_FvCE_1))), - m_cste_FlPE_1(std::make_shared(10.0)), - m_cste_FlPE_2(std::make_shared(5.0)), - m_cste_eccentricForceMultiplier(std::make_shared(1.8)), - m_cste_damping(std::make_shared(0.1)), - m_cste_maxShorteningSpeed(std::make_shared(10.0)) + m_cste_FlPE_1(std::make_shared(10.0)), + m_cste_FlPE_2(std::make_shared(5.0)), + m_cste_eccentricForceMultiplier(std::make_shared(1.8)), + m_cste_damping(std::make_shared(0.1)), + m_cste_maxShorteningSpeed(std::make_shared(10.0)) { setType(); } -biorbd::muscles::HillType::HillType( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - const biorbd::muscles::State& state) : - biorbd::muscles::Muscle(name,geometry,characteristics,pathModifiers,state), - m_damping(std::make_shared()), - m_FlCE(std::make_shared()), - m_FlPE(std::make_shared()), - m_FvCE(std::make_shared()), - m_cste_FlCE_1(std::make_shared(0.15)), - m_cste_FlCE_2(std::make_shared(0.45)), - m_cste_FvCE_1(std::make_shared(1)), - m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ +muscles::HillType::HillType( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics, + const muscles::PathModifiers &pathModifiers, + const muscles::State& state) : + muscles::Muscle(name,geometry,characteristics,pathModifiers,state), + m_damping(std::make_shared()), + m_FlCE(std::make_shared()), + m_FlPE(std::make_shared()), + m_FvCE(std::make_shared()), + m_cste_FlCE_1(std::make_shared(0.15)), + m_cste_FlCE_2(std::make_shared(0.45)), + m_cste_FvCE_1(std::make_shared(1)), + m_cste_FvCE_2(std::make_shared(-.33/2 * *m_cste_FvCE_1/ (1+*m_cste_FvCE_1))), - m_cste_FlPE_1(std::make_shared(10.0)), - m_cste_FlPE_2(std::make_shared(5.0)), - m_cste_eccentricForceMultiplier(std::make_shared(1.8)), - m_cste_damping(std::make_shared(0.1)), - m_cste_maxShorteningSpeed(std::make_shared(10.0)) + m_cste_FlPE_1(std::make_shared(10.0)), + m_cste_FlPE_2(std::make_shared(5.0)), + m_cste_eccentricForceMultiplier(std::make_shared(1.8)), + m_cste_damping(std::make_shared(0.1)), + m_cste_maxShorteningSpeed(std::make_shared(10.0)) { setType(); } -biorbd::muscles::HillType::HillType(const biorbd::muscles::Muscle &other) : - biorbd::muscles::Muscle (other) +muscles::HillType::HillType(const muscles::Muscle &other) : + muscles::Muscle (other) { - const biorbd::muscles::HillType & m_tp( - dynamic_cast(other)); + const muscles::HillType & m_tp( + dynamic_cast(other)); m_damping = m_tp.m_damping; m_FlCE = m_tp.m_FlCE; m_FlPE = m_tp.m_FlPE; @@ -143,13 +145,13 @@ biorbd::muscles::HillType::HillType(const biorbd::muscles::Muscle &other) : m_cste_maxShorteningSpeed = m_tp.m_cste_maxShorteningSpeed; } -biorbd::muscles::HillType::HillType( - const std::shared_ptr other) : - biorbd::muscles::Muscle (other) +muscles::HillType::HillType( + const std::shared_ptr other) : + muscles::Muscle (other) { - const std::shared_ptr m_tp( - std::dynamic_pointer_cast(other)); - biorbd::utils::Error::check(m_tp != nullptr, "Muscle must be of a Hill Type"); + const std::shared_ptr m_tp( + std::dynamic_pointer_cast(other)); + utils::Error::check(m_tp != nullptr, "Muscle must be of a Hill Type"); m_damping = m_tp->m_damping; m_FlCE = m_tp->m_FlCE; m_FlPE = m_tp->m_FlPE; @@ -165,16 +167,16 @@ biorbd::muscles::HillType::HillType( m_cste_maxShorteningSpeed = m_tp->m_cste_maxShorteningSpeed; } -biorbd::muscles::HillType biorbd::muscles::HillType::DeepCopy() const +muscles::HillType muscles::HillType::DeepCopy() const { - biorbd::muscles::HillType copy; + muscles::HillType copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::HillType::DeepCopy(const biorbd::muscles::HillType &other) +void muscles::HillType::DeepCopy(const muscles::HillType &other) { - biorbd::muscles::Muscle::DeepCopy(other); + muscles::Muscle::DeepCopy(other); *m_damping = *other.m_damping; *m_FlCE = *other.m_FlCE; *m_FlPE = *other.m_FlPE; @@ -190,8 +192,8 @@ void biorbd::muscles::HillType::DeepCopy(const biorbd::muscles::HillType &other) *m_cste_maxShorteningSpeed = *other.m_cste_maxShorteningSpeed; } -const biorbd::utils::Scalar& biorbd::muscles::HillType::force( - const biorbd::muscles::State& emg) +const utils::Scalar& muscles::HillType::force( + const muscles::State& emg) { // Compute the forces of each element computeFvCE(); @@ -204,11 +206,11 @@ const biorbd::utils::Scalar& biorbd::muscles::HillType::force( return *m_force; } -const biorbd::utils::Scalar& biorbd::muscles::HillType::force( +const utils::Scalar& muscles::HillType::force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::muscles::State &emg, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, + const muscles::State &emg, int updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -220,7 +222,7 @@ const biorbd::utils::Scalar& biorbd::muscles::HillType::force( } else if (updateKin == 2) { updateOrientations(model,Q,Qdot,2); } else { - biorbd::utils::Error::check(updateKin == 0, + utils::Error::check(updateKin == 0, "Wrong level of update in force function"); } @@ -228,49 +230,49 @@ const biorbd::utils::Scalar& biorbd::muscles::HillType::force( return force(emg); } -const biorbd::utils::Scalar& biorbd::muscles::HillType::force( +const utils::Scalar& muscles::HillType::force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, - const biorbd::rigidbody::GeneralizedCoordinates &, - const biorbd::muscles::State &, + const rigidbody::GeneralizedCoordinates &, + const muscles::State &, int) { - biorbd::utils::Error::raise("Hill type needs velocity"); + utils::Error::raise("Hill type needs velocity"); #ifdef _WIN32 return *m_force; // Will never reach here #endif } -const biorbd::utils::Scalar& biorbd::muscles::HillType::FlCE( - const biorbd::muscles::State &EMG) +const utils::Scalar& muscles::HillType::FlCE( + const muscles::State &EMG) { computeFlCE(EMG); return *m_FlCE; } -const biorbd::utils::Scalar& biorbd::muscles::HillType::FlPE() +const utils::Scalar& muscles::HillType::FlPE() { computeFlPE(); return *m_FlPE; } -const biorbd::utils::Scalar& biorbd::muscles::HillType::FvCE() +const utils::Scalar& muscles::HillType::FvCE() { computeFvCE(); return *m_FvCE; } -const biorbd::utils::Scalar& biorbd::muscles::HillType::damping() +const utils::Scalar& muscles::HillType::damping() { computeDamping(); return *m_damping; } -void biorbd::muscles::HillType::setType() +void muscles::HillType::setType() { - *m_type = biorbd::muscles::MUSCLE_TYPE::HILL; + *m_type = muscles::MUSCLE_TYPE::HILL; } -void biorbd::muscles::HillType::computeDamping() +void muscles::HillType::computeDamping() { *m_damping = position().velocity() / @@ -278,7 +280,7 @@ void biorbd::muscles::HillType::computeDamping() *m_cste_damping; } -void biorbd::muscles::HillType::computeFlCE(const biorbd::muscles::State& emg) +void muscles::HillType::computeFlCE(const muscles::State& emg) { *m_FlCE = exp( -pow(( position().length() / m_characteristics->optimalLength() / (*m_cste_FlCE_1* @@ -287,10 +289,10 @@ void biorbd::muscles::HillType::computeFlCE(const biorbd::muscles::State& emg) *m_cste_FlCE_2 ); } -void biorbd::muscles::HillType::computeFvCE() +void muscles::HillType::computeFvCE() { // The relation is different if velocity< 0 or > 0 - biorbd::utils::Scalar v = m_position->velocity(); + utils::Scalar v = m_position->velocity(); #ifdef BIORBD_USE_CASADI_MATH *m_FvCE = casadi::MX::if_else( casadi::MX::le(v, 0), @@ -309,7 +311,7 @@ void biorbd::muscles::HillType::computeFvCE() #endif } -void biorbd::muscles::HillType::computeFlPE() +void muscles::HillType::computeFlPE() { #ifdef BIORBD_USE_CASADI_MATH @@ -327,16 +329,16 @@ void biorbd::muscles::HillType::computeFlPE() #endif } -biorbd::utils::Scalar biorbd::muscles::HillType::getForceFromActivation( - const biorbd::muscles::State &emg) +utils::Scalar muscles::HillType::getForceFromActivation( + const muscles::State &emg) { // Fonction qui permet de modifier la facon dont la multiplication est faite dans computeForce(EMG) return characteristics().forceIsoMax() * (emg.activation() * *m_FlCE * *m_FvCE + *m_FlPE + *m_damping); } -void biorbd::muscles::HillType::normalizeEmg( - biorbd::muscles::State& emg) +void muscles::HillType::normalizeEmg( + muscles::State& emg) { emg.normalizeExcitation(characteristics().stateMax()); } diff --git a/src/Muscles/IdealizedActuator.cpp b/src/Muscles/IdealizedActuator.cpp index e788c9bd..7c5a28b8 100644 --- a/src/Muscles/IdealizedActuator.cpp +++ b/src/Muscles/IdealizedActuator.cpp @@ -5,116 +5,118 @@ #include "Muscles/Characteristics.h" #include "Muscles/State.h" -biorbd::muscles::IdealizedActuator::IdealizedActuator() : - biorbd::muscles::Muscle() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::IdealizedActuator::IdealizedActuator() : + muscles::Muscle() { setType(); } -biorbd::muscles::IdealizedActuator::IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics) : - biorbd::muscles::Muscle(name,geometry,characteristics) +muscles::IdealizedActuator::IdealizedActuator( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics) : + muscles::Muscle(name,geometry,characteristics) { setType(); } -biorbd::muscles::IdealizedActuator::IdealizedActuator( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::State &emg) : - biorbd::muscles::Muscle(name,geometry,characteristics,emg) +muscles::IdealizedActuator::IdealizedActuator( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::State &emg) : + muscles::Muscle(name,geometry,characteristics,emg) { setType(); } -biorbd::muscles::IdealizedActuator::IdealizedActuator( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers) : - biorbd::muscles::Muscle(name,geometry,characteristics, pathModifiers) +muscles::IdealizedActuator::IdealizedActuator( + const utils::String &name, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers) : + muscles::Muscle(name,geometry,characteristics, pathModifiers) { setType(); } -biorbd::muscles::IdealizedActuator::IdealizedActuator( - const biorbd::utils::String& name, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - const biorbd::muscles::State& emg) : - biorbd::muscles::Muscle(name,geometry,characteristics,pathModifiers,emg) +muscles::IdealizedActuator::IdealizedActuator( + const utils::String& name, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics, + const muscles::PathModifiers &pathModifiers, + const muscles::State& emg) : + muscles::Muscle(name,geometry,characteristics,pathModifiers,emg) { setType(); } -biorbd::muscles::IdealizedActuator::IdealizedActuator(const - biorbd::muscles::Muscle &other) : - biorbd::muscles::Muscle (other) +muscles::IdealizedActuator::IdealizedActuator(const + muscles::Muscle &other) : + muscles::Muscle (other) { } -biorbd::muscles::IdealizedActuator::IdealizedActuator(const - std::shared_ptr other) : - biorbd::muscles::Muscle (other) +muscles::IdealizedActuator::IdealizedActuator(const + std::shared_ptr other) : + muscles::Muscle (other) { } -biorbd::muscles::IdealizedActuator -biorbd::muscles::IdealizedActuator::DeepCopy() const +muscles::IdealizedActuator +muscles::IdealizedActuator::DeepCopy() const { - biorbd::muscles::IdealizedActuator copy; + muscles::IdealizedActuator copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::IdealizedActuator::DeepCopy(const - biorbd::muscles::IdealizedActuator &other) +void muscles::IdealizedActuator::DeepCopy(const + muscles::IdealizedActuator &other) { - biorbd::muscles::Muscle::DeepCopy(other); + muscles::Muscle::DeepCopy(other); } -const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( - const biorbd::muscles::State &emg) +const utils::Scalar& muscles::IdealizedActuator::force( + const muscles::State &emg) { computeForce(emg); return *m_force; } -const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( +const utils::Scalar& muscles::IdealizedActuator::force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, - const biorbd::rigidbody::GeneralizedCoordinates &, - const biorbd::rigidbody::GeneralizedVelocity &, - const biorbd::muscles::State &emg, + const rigidbody::GeneralizedCoordinates &, + const rigidbody::GeneralizedVelocity &, + const muscles::State &emg, int) { computeForce(emg); return *m_force; } -const biorbd::utils::Scalar& biorbd::muscles::IdealizedActuator::force( +const utils::Scalar& muscles::IdealizedActuator::force( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, - const biorbd::rigidbody::GeneralizedCoordinates &, - const biorbd::muscles::State &emg, + const rigidbody::GeneralizedCoordinates &, + const muscles::State &emg, int) { computeForce(emg); return *m_force; } -biorbd::utils::Scalar -biorbd::muscles::IdealizedActuator::getForceFromActivation( - const biorbd::muscles::State &emg) +utils::Scalar +muscles::IdealizedActuator::getForceFromActivation( + const muscles::State &emg) { return characteristics().forceIsoMax() * (emg.activation()); } -void biorbd::muscles::IdealizedActuator::setType() +void muscles::IdealizedActuator::setType() { - *m_type = biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR; + *m_type = muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR; } diff --git a/src/Muscles/Muscle.cpp b/src/Muscles/Muscle.cpp index 663f2c88..3732f442 100644 --- a/src/Muscles/Muscle.cpp +++ b/src/Muscles/Muscle.cpp @@ -12,58 +12,60 @@ #include "Muscles/Characteristics.h" #include "Muscles/Geometry.h" -biorbd::muscles::Muscle::Muscle() : - biorbd::muscles::Compound(), - m_position(std::make_shared()), - m_characteristics(std::make_shared()), - m_state(std::make_shared()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::Muscle::Muscle() : + muscles::Compound(), + m_position(std::make_shared()), + m_characteristics(std::make_shared()), + m_state(std::make_shared()) { } -biorbd::muscles::Muscle::Muscle( - const biorbd::utils::String & name, - const biorbd::muscles::Geometry & position, - const biorbd::muscles::Characteristics &characteristics) : - biorbd::muscles::Compound (name), - m_position(std::make_shared(position)), - m_characteristics(std::make_shared +muscles::Muscle::Muscle( + const utils::String & name, + const muscles::Geometry & position, + const muscles::Characteristics &characteristics) : + muscles::Compound (name), + m_position(std::make_shared(position)), + m_characteristics(std::make_shared (characteristics)), - m_state(std::make_shared()) + m_state(std::make_shared()) { } -biorbd::muscles::Muscle::Muscle( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &position, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::State &dynamicState) : - biorbd::muscles::Compound (name), - m_position(std::make_shared(position)), - m_characteristics(std::make_shared +muscles::Muscle::Muscle( + const utils::String &name, + const muscles::Geometry &position, + const muscles::Characteristics &characteristics, + const muscles::State &dynamicState) : + muscles::Compound (name), + m_position(std::make_shared(position)), + m_characteristics(std::make_shared (characteristics)), - m_state(std::make_shared(dynamicState)) + m_state(std::make_shared(dynamicState)) { } -biorbd::muscles::Muscle::Muscle( - const biorbd::utils::String &name, - const biorbd::muscles::Geometry &position, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers) : - biorbd::muscles::Compound (name, pathModifiers), - m_position(std::make_shared(position)), - m_characteristics(std::make_shared +muscles::Muscle::Muscle( + const utils::String &name, + const muscles::Geometry &position, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers) : + muscles::Compound (name, pathModifiers), + m_position(std::make_shared(position)), + m_characteristics(std::make_shared (characteristics)), - m_state(std::make_shared()) + m_state(std::make_shared()) { } -biorbd::muscles::Muscle::Muscle(const biorbd::muscles::Muscle &other) : - biorbd::muscles::Compound (other), +muscles::Muscle::Muscle(const muscles::Muscle &other) : + muscles::Compound (other), m_position(other.m_position), m_characteristics(other.m_characteristics), m_state(other.m_state) @@ -71,9 +73,9 @@ biorbd::muscles::Muscle::Muscle(const biorbd::muscles::Muscle &other) : } -biorbd::muscles::Muscle::Muscle(const std::shared_ptr +muscles::Muscle::Muscle(const std::shared_ptr other) : - biorbd::muscles::Compound (other), + muscles::Compound (other), m_position(other->m_position), m_characteristics(other->m_characteristics), m_state(other->m_state) @@ -81,85 +83,85 @@ biorbd::muscles::Muscle::Muscle(const std::shared_ptr } -biorbd::muscles::Muscle::Muscle(const biorbd::utils::String& name, - const biorbd::muscles::Geometry& g, - const biorbd::muscles::Characteristics& c, - const biorbd::muscles::PathModifiers &pathModifiers, - const biorbd::muscles::State& emg) : - biorbd::muscles::Compound(name,pathModifiers), - m_position(std::make_shared(g)), - m_characteristics(std::make_shared(c)), - m_state(std::make_shared()) +muscles::Muscle::Muscle(const utils::String& name, + const muscles::Geometry& g, + const muscles::Characteristics& c, + const muscles::PathModifiers &pathModifiers, + const muscles::State& emg) : + muscles::Compound(name,pathModifiers), + m_position(std::make_shared(g)), + m_characteristics(std::make_shared(c)), + m_state(std::make_shared()) { setState(emg); - biorbd::utils::Error::check(pathModifiers.nbWraps() <= 1, + utils::Error::check(pathModifiers.nbWraps() <= 1, "Multiple wrapping objects is not implemented yet"); } -biorbd::muscles::Muscle::~Muscle() +muscles::Muscle::~Muscle() { //dtor } -void biorbd::muscles::Muscle::DeepCopy(const biorbd::muscles::Muscle &other) +void muscles::Muscle::DeepCopy(const muscles::Muscle &other) { - this->biorbd::muscles::Compound::DeepCopy(other); + this->muscles::Compound::DeepCopy(other); *m_position = other.m_position->DeepCopy(); *m_characteristics = other.m_characteristics->DeepCopy(); *m_state = other.m_state->DeepCopy(); } -void biorbd::muscles::Muscle::updateOrientations( +void muscles::Muscle::updateOrientations( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, int updateKin) { // Update de la position des insertions et origines m_position->updateKinematics(model,*m_characteristics,*m_pathChanger,&Q,nullptr, updateKin); } -void biorbd::muscles::Muscle::updateOrientations( +void muscles::Muscle::updateOrientations( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, int updateKin) { // Update de la position des insertions et origines m_position->updateKinematics(model,*m_characteristics,*m_pathChanger,&Q,&Qdot, updateKin); } -void biorbd::muscles::Muscle::updateOrientations( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix &jacoPointsInGlobal) +void muscles::Muscle::updateOrientations( + std::vector& musclePointsInGlobal, + utils::Matrix &jacoPointsInGlobal) { // Update de la position des insertions et origines m_position->updateKinematics(musclePointsInGlobal,jacoPointsInGlobal, *m_characteristics,nullptr); } -void biorbd::muscles::Muscle::updateOrientations( - std::vector& musclePointsInGlobal, - biorbd::utils::Matrix &jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity &Qdot) +void muscles::Muscle::updateOrientations( + std::vector& musclePointsInGlobal, + utils::Matrix &jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity &Qdot) { // Update de la position des insertions et origines m_position->updateKinematics(musclePointsInGlobal,jacoPointsInGlobal, *m_characteristics,&Qdot); } -void biorbd::muscles::Muscle::setPosition( - const biorbd::muscles::Geometry &positions) +void muscles::Muscle::setPosition( + const muscles::Geometry &positions) { *m_position = positions; } -const biorbd::muscles::Geometry &biorbd::muscles::Muscle::position() const +const muscles::Geometry &muscles::Muscle::position() const { return *m_position; } -const biorbd::utils::Scalar& biorbd::muscles::Muscle::length( +const utils::Scalar& muscles::Muscle::length( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, int updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -173,9 +175,9 @@ const biorbd::utils::Scalar& biorbd::muscles::Muscle::length( return position().length(); } -const biorbd::utils::Scalar& biorbd::muscles::Muscle::musculoTendonLength( +const utils::Scalar& muscles::Muscle::musculoTendonLength( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &m, - const biorbd::rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedCoordinates &Q, int updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -189,10 +191,10 @@ const biorbd::utils::Scalar& biorbd::muscles::Muscle::musculoTendonLength( return m_position->musculoTendonLength(); } -const biorbd::utils::Scalar& biorbd::muscles::Muscle::velocity( +const utils::Scalar& muscles::Muscle::velocity( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -206,28 +208,28 @@ const biorbd::utils::Scalar& biorbd::muscles::Muscle::velocity( return m_position->velocity(); } -const biorbd::utils::Scalar& biorbd::muscles::Muscle::activationDot( - const biorbd::muscles::State& state, +const utils::Scalar& muscles::Muscle::activationDot( + const muscles::State& state, bool alreadyNormalized) const { - std::shared_ptr state_copy = - std::dynamic_pointer_cast(m_state); - biorbd::utils::Error::check( + std::shared_ptr state_copy = + std::dynamic_pointer_cast(m_state); + utils::Error::check( state_copy != nullptr, "The muscle " + name() + " is not a dynamic muscle"); return state_copy->timeDerivativeActivation( state, characteristics(), alreadyNormalized); } -void biorbd::muscles::Muscle::computeForce(const biorbd::muscles::State &emg) +void muscles::Muscle::computeForce(const muscles::State &emg) { *m_force = getForceFromActivation(emg); } -const std::vector& -biorbd::muscles::Muscle::musclesPointsInGlobal( +const std::vector& +muscles::Muscle::musclesPointsInGlobal( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q) + const rigidbody::GeneralizedCoordinates &Q) { m_position->updateKinematics(model,*m_characteristics,*m_pathChanger,&Q, nullptr); @@ -235,54 +237,54 @@ biorbd::muscles::Muscle::musclesPointsInGlobal( return musclesPointsInGlobal(); } -const std::vector -&biorbd::muscles::Muscle::musclesPointsInGlobal() const +const std::vector +&muscles::Muscle::musclesPointsInGlobal() const { return m_position->musclesPointsInGlobal(); } -void biorbd::muscles::Muscle::setForceIsoMax( - const biorbd::utils::Scalar& forceMax) +void muscles::Muscle::setForceIsoMax( + const utils::Scalar& forceMax) { m_characteristics->setForceIsoMax(forceMax); } -void biorbd::muscles::Muscle::setCharacteristics( - const biorbd::muscles::Characteristics &characteristics) +void muscles::Muscle::setCharacteristics( + const muscles::Characteristics &characteristics) { *m_characteristics = characteristics; } -const biorbd::muscles::Characteristics& -biorbd::muscles::Muscle::characteristics() const +const muscles::Characteristics& +muscles::Muscle::characteristics() const { return *m_characteristics; } // Get and set -void biorbd::muscles::Muscle::setState( - const biorbd::muscles::State &emg) +void muscles::Muscle::setState( + const muscles::State &emg) { - if (emg.type() == biorbd::muscles::STATE_TYPE::BUCHANAN) { - m_state = std::make_shared - (biorbd::muscles::StateDynamicsBuchanan()); - } else if (emg.type() == biorbd::muscles::STATE_TYPE::DE_GROOTE) { - m_state = std::make_shared - (biorbd::muscles::StateDynamicsDeGroote()); - } else if (emg.type() == biorbd::muscles::STATE_TYPE::DYNAMIC) { - m_state = std::make_shared - (biorbd::muscles::StateDynamics()); + if (emg.type() == muscles::STATE_TYPE::BUCHANAN) { + m_state = std::make_shared + (muscles::StateDynamicsBuchanan()); + } else if (emg.type() == muscles::STATE_TYPE::DE_GROOTE) { + m_state = std::make_shared + (muscles::StateDynamicsDeGroote()); + } else if (emg.type() == muscles::STATE_TYPE::DYNAMIC) { + m_state = std::make_shared + (muscles::StateDynamics()); } else { - biorbd::utils::Error::raise(biorbd::utils::String( - biorbd::muscles::STATE_TYPE_toStr( + utils::Error::raise(utils::String( + muscles::STATE_TYPE_toStr( emg.type())) + " is not a valid type for setState"); } *m_state = emg; } -const biorbd::muscles::State& biorbd::muscles::Muscle::state() const +const muscles::State& muscles::Muscle::state() const { return *m_state; } -biorbd::muscles::State& biorbd::muscles::Muscle::state() +muscles::State& muscles::Muscle::state() { return *m_state; } diff --git a/src/Muscles/MuscleGroup.cpp b/src/Muscles/MuscleGroup.cpp index aee1b9dd..b7cb5bf3 100644 --- a/src/Muscles/MuscleGroup.cpp +++ b/src/Muscles/MuscleGroup.cpp @@ -10,16 +10,18 @@ #include "Muscles/StateDynamicsBuchanan.h" #include "Muscles/StateDynamicsDeGroote.h" -biorbd::muscles::MuscleGroup::MuscleGroup() : - m_mus(std::make_shared>>()), - m_name(std::make_shared()), - m_originName(std::make_shared()), - m_insertName(std::make_shared()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::MuscleGroup::MuscleGroup() : + m_mus(std::make_shared>>()), + m_name(std::make_shared()), + m_originName(std::make_shared()), + m_insertName(std::make_shared()) { } -biorbd::muscles::MuscleGroup::MuscleGroup(const biorbd::muscles::MuscleGroup +muscles::MuscleGroup::MuscleGroup(const muscles::MuscleGroup &other) : m_mus(other.m_mus), m_name(other.m_name), @@ -29,56 +31,56 @@ biorbd::muscles::MuscleGroup::MuscleGroup(const biorbd::muscles::MuscleGroup } -biorbd::muscles::MuscleGroup::MuscleGroup( - const biorbd::utils::String &name, - const biorbd::utils::String &originName, - const biorbd::utils::String &insertionName) : - m_mus(std::make_shared>>()), - m_name(std::make_shared(name)), - m_originName(std::make_shared(originName)), - m_insertName(std::make_shared(insertionName)) +muscles::MuscleGroup::MuscleGroup( + const utils::String &name, + const utils::String &originName, + const utils::String &insertionName) : + m_mus(std::make_shared>>()), + m_name(std::make_shared(name)), + m_originName(std::make_shared(originName)), + m_insertName(std::make_shared(insertionName)) { } -biorbd::muscles::MuscleGroup::~MuscleGroup() +muscles::MuscleGroup::~MuscleGroup() { } -biorbd::muscles::MuscleGroup biorbd::muscles::MuscleGroup::DeepCopy() const +muscles::MuscleGroup muscles::MuscleGroup::DeepCopy() const { - biorbd::muscles::MuscleGroup copy; + muscles::MuscleGroup copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::MuscleGroup::DeepCopy(const biorbd::muscles::MuscleGroup +void muscles::MuscleGroup::DeepCopy(const muscles::MuscleGroup &other) { m_mus->resize(other.m_mus->size()); for (unsigned int i=0; isize(); ++i) { if ((*other.m_mus)[i]->type() == - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { - (*m_mus)[i] = std::make_shared(( + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { + (*m_mus)[i] = std::make_shared(( *other.m_mus)[i]); - } else if ((*other.m_mus)[i]->type() == biorbd::muscles::MUSCLE_TYPE::HILL) { - (*m_mus)[i] = std::make_shared((*other.m_mus)[i]); + } else if ((*other.m_mus)[i]->type() == muscles::MUSCLE_TYPE::HILL) { + (*m_mus)[i] = std::make_shared((*other.m_mus)[i]); } else if ((*other.m_mus)[i]->type() == - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) { - (*m_mus)[i] = std::make_shared(( + muscles::MUSCLE_TYPE::HILL_THELEN) { + (*m_mus)[i] = std::make_shared(( *other.m_mus)[i]); } else if ((*other.m_mus)[i]->type() == - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { - (*m_mus)[i] = std::make_shared(( + muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { + (*m_mus)[i] = std::make_shared(( *other.m_mus)[i]); } else if ((*other.m_mus)[i]->type() == - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { - (*m_mus)[i] = std::make_shared(( + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { + (*m_mus)[i] = std::make_shared(( *other.m_mus)[i]); } else { - biorbd::utils::Error::raise("DeepCopy was not prepared to copy " + - biorbd::utils::String( - biorbd::muscles::MUSCLE_TYPE_toStr((*other.m_mus)[i]->type())) + " type"); + utils::Error::raise("DeepCopy was not prepared to copy " + + utils::String( + muscles::MUSCLE_TYPE_toStr((*other.m_mus)[i]->type())) + " type"); } } *m_mus = *other.m_mus; @@ -87,217 +89,217 @@ void biorbd::muscles::MuscleGroup::DeepCopy(const biorbd::muscles::MuscleGroup *m_insertName = *other.m_insertName; } -void biorbd::muscles::MuscleGroup::addMuscle( - const biorbd::utils::String &name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - biorbd::muscles::STATE_TYPE stateType, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) +void muscles::MuscleGroup::addMuscle( + const utils::String &name, + muscles::MUSCLE_TYPE type, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + muscles::STATE_TYPE stateType, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) { - std::shared_ptr muscle; - std::shared_ptr state; + std::shared_ptr muscle; + std::shared_ptr state; // Cast the dynamic type - if (stateType == biorbd::muscles::STATE_TYPE::SIMPLE_STATE) { - state = std::make_shared(); - } else if (stateType == biorbd::muscles::STATE_TYPE::DYNAMIC) { - state = std::make_shared(); - } else if (stateType == biorbd::muscles::STATE_TYPE::BUCHANAN) { - state = std::make_shared(); - } else if (stateType == biorbd::muscles::STATE_TYPE::DE_GROOTE) { - state = std::make_shared(); + if (stateType == muscles::STATE_TYPE::SIMPLE_STATE) { + state = std::make_shared(); + } else if (stateType == muscles::STATE_TYPE::DYNAMIC) { + state = std::make_shared(); + } else if (stateType == muscles::STATE_TYPE::BUCHANAN) { + state = std::make_shared(); + } else if (stateType == muscles::STATE_TYPE::DE_GROOTE) { + state = std::make_shared(); } else { - state = std::make_shared(); + state = std::make_shared(); } - if (type == biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { - muscle = std::make_shared(name,geometry, + if (type == muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { + muscle = std::make_shared(name,geometry, characteristics, *state); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL) { + muscle = std::make_shared(name,geometry, characteristics, *state); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN) { + muscle = std::make_shared(name,geometry, characteristics, *state); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { + muscle = std::make_shared(name, geometry,characteristics, *state); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { + muscle = std::make_shared(name, geometry,characteristics, *state, dynamicFatigueType); } else { - biorbd::utils::Error::raise("Wrong muscle type"); + utils::Error::raise("Wrong muscle type"); } addMuscle(*muscle); } -void biorbd::muscles::MuscleGroup::addMuscle( - const biorbd::utils::String &name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) +void muscles::MuscleGroup::addMuscle( + const utils::String &name, + muscles::MUSCLE_TYPE type, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) { - std::shared_ptr muscle; - if (type == biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { - muscle = std::make_shared(name,geometry, + std::shared_ptr muscle; + if (type == muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { + muscle = std::make_shared(name,geometry, characteristics); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL) { + muscle = std::make_shared(name,geometry, characteristics); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN) { + muscle = std::make_shared(name,geometry, characteristics); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { + muscle = std::make_shared(name, geometry,characteristics); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { + muscle = std::make_shared(name, geometry,characteristics, dynamicFatigueType); } else { - biorbd::utils::Error::raise("Wrong muscle type"); + utils::Error::raise("Wrong muscle type"); } addMuscle(*muscle); } -void biorbd::muscles::MuscleGroup::addMuscle( - const biorbd::utils::String& name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry& geometry, - const biorbd::muscles::Characteristics& characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - biorbd::muscles::STATE_TYPE stateType, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) +void muscles::MuscleGroup::addMuscle( + const utils::String& name, + muscles::MUSCLE_TYPE type, + const muscles::Geometry& geometry, + const muscles::Characteristics& characteristics, + const muscles::PathModifiers &pathModifiers, + muscles::STATE_TYPE stateType, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) { - std::shared_ptr muscle; - std::shared_ptr state; + std::shared_ptr muscle; + std::shared_ptr state; // Cast the dynamic type - if (stateType == biorbd::muscles::STATE_TYPE::SIMPLE_STATE) { - state = std::make_shared(); - } else if (stateType == biorbd::muscles::STATE_TYPE::BUCHANAN) { - state = std::make_shared(); - } else if (stateType == biorbd::muscles::STATE_TYPE::DE_GROOTE) { - state = std::make_shared(); + if (stateType == muscles::STATE_TYPE::SIMPLE_STATE) { + state = std::make_shared(); + } else if (stateType == muscles::STATE_TYPE::BUCHANAN) { + state = std::make_shared(); + } else if (stateType == muscles::STATE_TYPE::DE_GROOTE) { + state = std::make_shared(); } else { - state = std::make_shared(); + state = std::make_shared(); } - if (type == biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) - muscle = std::make_shared + if (type == muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) + muscle = std::make_shared (name,geometry,characteristics,pathModifiers,*state); - else if (type == biorbd::muscles::MUSCLE_TYPE::HILL) - muscle = std::make_shared + else if (type == muscles::MUSCLE_TYPE::HILL) + muscle = std::make_shared (name,geometry,characteristics,pathModifiers,*state); - else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) - muscle = std::make_shared + else if (type == muscles::MUSCLE_TYPE::HILL_THELEN) + muscle = std::make_shared (name,geometry,characteristics,pathModifiers,*state); - else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) - muscle = std::make_shared + else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) + muscle = std::make_shared (name,geometry,characteristics,pathModifiers,*state); - else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) - muscle = std::make_shared( + else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) + muscle = std::make_shared( name,geometry,characteristics,pathModifiers,*state,dynamicFatigueType); else { - biorbd::utils::Error::raise("Wrong muscle type"); + utils::Error::raise("Wrong muscle type"); } addMuscle(*muscle); } -void biorbd::muscles::MuscleGroup::addMuscle( - const biorbd::utils::String &name, - biorbd::muscles::MUSCLE_TYPE type, - const biorbd::muscles::Geometry &geometry, - const biorbd::muscles::Characteristics &characteristics, - const biorbd::muscles::PathModifiers &pathModifiers, - biorbd::muscles::STATE_FATIGUE_TYPE dynamicFatigueType) +void muscles::MuscleGroup::addMuscle( + const utils::String &name, + muscles::MUSCLE_TYPE type, + const muscles::Geometry &geometry, + const muscles::Characteristics &characteristics, + const muscles::PathModifiers &pathModifiers, + muscles::STATE_FATIGUE_TYPE dynamicFatigueType) { - std::shared_ptr muscle; + std::shared_ptr muscle; - if (type == biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { - muscle = std::make_shared(name,geometry, + if (type == muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { + muscle = std::make_shared(name,geometry, characteristics,pathModifiers); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL) { + muscle = std::make_shared(name,geometry, characteristics,pathModifiers); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) { - muscle = std::make_shared(name,geometry, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN) { + muscle = std::make_shared(name,geometry, characteristics,pathModifiers); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { + muscle = std::make_shared(name, geometry,characteristics,pathModifiers); - } else if (type == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { - muscle = std::make_shared(name, + } else if (type == muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { + muscle = std::make_shared(name, geometry,characteristics,pathModifiers, dynamicFatigueType); } else { - biorbd::utils::Error::raise("Wrong muscle type"); + utils::Error::raise("Wrong muscle type"); } addMuscle(*muscle); } -void biorbd::muscles::MuscleGroup::addMuscle( - const biorbd::muscles::Muscle &muscle) +void muscles::MuscleGroup::addMuscle( + const muscles::Muscle &muscle) { - biorbd::utils::Error::check(muscleID(muscle.name()) == -1, + utils::Error::check(muscleID(muscle.name()) == -1, "This muscle name was already defined for this muscle group"); // Add muscle according to its type - if (muscle.type() == biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { - m_mus->push_back(std::make_shared(muscle)); + if (muscle.type() == muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR) { + m_mus->push_back(std::make_shared(muscle)); } else if (muscle.type() == - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { - m_mus->push_back(std::make_shared + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE) { + m_mus->push_back(std::make_shared (muscle)); - } else if (muscle.type() == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN) { - m_mus->push_back(std::make_shared(muscle)); - } else if (muscle.type() == biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { - m_mus->push_back(std::make_shared + } else if (muscle.type() == muscles::MUSCLE_TYPE::HILL_THELEN) { + m_mus->push_back(std::make_shared(muscle)); + } else if (muscle.type() == muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE) { + m_mus->push_back(std::make_shared (muscle)); - } else if (muscle.type() == biorbd::muscles::MUSCLE_TYPE::HILL) { - m_mus->push_back(std::make_shared(muscle)); + } else if (muscle.type() == muscles::MUSCLE_TYPE::HILL) { + m_mus->push_back(std::make_shared(muscle)); } else { - biorbd::utils::Error::raise("Muscle type not found"); + utils::Error::raise("Muscle type not found"); } return; } -biorbd::muscles::Muscle& biorbd::muscles::MuscleGroup::muscle(unsigned int idx) +muscles::Muscle& muscles::MuscleGroup::muscle(unsigned int idx) { - biorbd::utils::Error::check(idx(m_mus->size()); } -std::vector>& - biorbd::muscles::MuscleGroup::muscles() +std::vector>& + muscles::MuscleGroup::muscles() { return *m_mus; } -const std::vector>& - biorbd::muscles::MuscleGroup::muscles() const +const std::vector>& + muscles::MuscleGroup::muscles() const { return *m_mus; } -int biorbd::muscles::MuscleGroup::muscleID(const biorbd::utils::String& +int muscles::MuscleGroup::muscleID(const utils::String& nameToFind) { for (unsigned int i=0; isize(); ++i) { @@ -309,30 +311,30 @@ int biorbd::muscles::MuscleGroup::muscleID(const biorbd::utils::String& return -1; } -void biorbd::muscles::MuscleGroup::setName(const biorbd::utils::String& name) +void muscles::MuscleGroup::setName(const utils::String& name) { *m_name = name; } -const biorbd::utils::String &biorbd::muscles::MuscleGroup::name() const +const utils::String &muscles::MuscleGroup::name() const { return *m_name; } -void biorbd::muscles::MuscleGroup::setOrigin(const biorbd::utils::String& name) +void muscles::MuscleGroup::setOrigin(const utils::String& name) { *m_originName = name; } -const biorbd::utils::String &biorbd::muscles::MuscleGroup::origin() const +const utils::String &muscles::MuscleGroup::origin() const { return *m_originName; } -void biorbd::muscles::MuscleGroup::setInsertion(const biorbd::utils::String& +void muscles::MuscleGroup::setInsertion(const utils::String& name) { *m_insertName = name; } -const biorbd::utils::String &biorbd::muscles::MuscleGroup::insertion() const +const utils::String &muscles::MuscleGroup::insertion() const { return *m_insertName; } diff --git a/src/Muscles/Muscles.cpp b/src/Muscles/Muscles.cpp index ad9e9d31..6e93e2cd 100644 --- a/src/Muscles/Muscles.cpp +++ b/src/Muscles/Muscles.cpp @@ -12,31 +12,33 @@ #include "Muscles/MuscleGroup.h" #include "Muscles/StateDynamics.h" -biorbd::muscles::Muscles::Muscles() : - m_mus(std::make_shared>()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::Muscles::Muscles() : + m_mus(std::make_shared>()) { } -biorbd::muscles::Muscles::Muscles(const biorbd::muscles::Muscles &other) : +muscles::Muscles::Muscles(const muscles::Muscles &other) : m_mus(other.m_mus) { } -biorbd::muscles::Muscles::~Muscles() +muscles::Muscles::~Muscles() { } -biorbd::muscles::Muscles biorbd::muscles::Muscles::DeepCopy() const +muscles::Muscles muscles::Muscles::DeepCopy() const { - biorbd::muscles::Muscles copy; + muscles::Muscles copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::Muscles::DeepCopy(const biorbd::muscles::Muscles &other) +void muscles::Muscles::DeepCopy(const muscles::Muscles &other) { m_mus->resize(other.m_mus->size()); for (unsigned int i=0; isize(); ++i) { @@ -45,20 +47,20 @@ void biorbd::muscles::Muscles::DeepCopy(const biorbd::muscles::Muscles &other) } -void biorbd::muscles::Muscles::addMuscleGroup( - const biorbd::utils::String &name, - const biorbd::utils::String &originName, - const biorbd::utils::String &insertionName) +void muscles::Muscles::addMuscleGroup( + const utils::String &name, + const utils::String &originName, + const utils::String &insertionName) { if (m_mus->size() > 0) { - biorbd::utils::Error::check(getMuscleGroupId(name)==-1, + utils::Error::check(getMuscleGroupId(name)==-1, "Muscle group already defined"); } - m_mus->push_back(biorbd::muscles::MuscleGroup(name, originName, insertionName)); + m_mus->push_back(muscles::MuscleGroup(name, originName, insertionName)); } -int biorbd::muscles::Muscles::getMuscleGroupId(const biorbd::utils::String +int muscles::Muscles::getMuscleGroupId(const utils::String &name) const { for (unsigned int i=0; isize(); ++i) @@ -68,10 +70,10 @@ int biorbd::muscles::Muscles::getMuscleGroupId(const biorbd::utils::String return -1; } -const std::vector> - biorbd::muscles::Muscles::muscles() const +const std::vector> + muscles::Muscles::muscles() const { - std::vector> m; + std::vector> m; for (auto group : muscleGroups()) { for (auto muscle : group.muscles()) { m.push_back(muscle); @@ -80,7 +82,7 @@ const std::vector> return m; } -const biorbd::muscles::Muscle &biorbd::muscles::Muscles::muscle( +const muscles::Muscle &muscles::Muscles::muscle( unsigned int idx) const { for (auto g : muscleGroups()) { @@ -90,12 +92,12 @@ const biorbd::muscles::Muscle &biorbd::muscles::Muscles::muscle( return g.muscle(idx); } } - biorbd::utils::Error::raise("idx is higher than the number of muscles"); + utils::Error::raise("idx is higher than the number of muscles"); } -std::vector biorbd::muscles::Muscles::muscleNames() const +std::vector muscles::Muscles::muscleNames() const { - std::vector names; + std::vector names; for (auto group : muscleGroups()) { for (auto muscle : group.muscles()) { names.push_back(muscle->name()); @@ -104,59 +106,59 @@ std::vector biorbd::muscles::Muscles::muscleNames() const return names; } -std::vector& -biorbd::muscles::Muscles::muscleGroups() +std::vector& +muscles::Muscles::muscleGroups() { return *m_mus; } -const std::vector& -biorbd::muscles::Muscles::muscleGroups() const +const std::vector& +muscles::Muscles::muscleGroups() const { return *m_mus; } -biorbd::muscles::MuscleGroup &biorbd::muscles::Muscles::muscleGroup( +muscles::MuscleGroup &muscles::Muscles::muscleGroup( unsigned int idx) { - biorbd::utils::Error::check(idx(idx)); } // From muscle activation (return muscle force) -biorbd::rigidbody::GeneralizedTorque -biorbd::muscles::Muscles::muscularJointTorque( - const biorbd::utils::Vector &F) +rigidbody::GeneralizedTorque +muscles::Muscles::muscularJointTorque( + const utils::Vector &F) { // Get the Jacobian matrix and get the forces of each muscle - const biorbd::utils::Matrix& jaco(musclesLengthJacobian()); + const utils::Matrix& jaco(musclesLengthJacobian()); // Compute the reaction of the forces on the bodies - return biorbd::rigidbody::GeneralizedTorque( -jaco.transpose() * F ); + return rigidbody::GeneralizedTorque( -jaco.transpose() * F ); } // From Muscular Force -biorbd::rigidbody::GeneralizedTorque -biorbd::muscles::Muscles::muscularJointTorque( - const biorbd::utils::Vector &F, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot) +rigidbody::GeneralizedTorque +muscles::Muscles::muscularJointTorque( + const utils::Vector &F, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot) { // Update the muscular position @@ -166,28 +168,28 @@ biorbd::muscles::Muscles::muscularJointTorque( } // From muscle activation (return muscle force) -biorbd::rigidbody::GeneralizedTorque -biorbd::muscles::Muscles::muscularJointTorque( - const std::vector>& emg) +rigidbody::GeneralizedTorque +muscles::Muscles::muscularJointTorque( + const std::vector>& emg) { return muscularJointTorque(muscleForces(emg)); } // From muscle activation (do not return muscle force) -biorbd::rigidbody::GeneralizedTorque -biorbd::muscles::Muscles::muscularJointTorque( - const std::vector>& emg, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot) +rigidbody::GeneralizedTorque +muscles::Muscles::muscularJointTorque( + const std::vector>& emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot) { return muscularJointTorque(muscleForces(emg, Q, QDot)); } -biorbd::utils::Vector biorbd::muscles::Muscles::activationDot( - const std::vector>& emg, +utils::Vector muscles::Muscles::activationDot( + const std::vector>& emg, bool areadyNormalized) { - biorbd::utils::Vector activationDot(nbMuscleTotal()); + utils::Vector activationDot(nbMuscleTotal()); unsigned int cmp(0); for (unsigned int i=0; i>& emg) +utils::Vector muscles::Muscles::muscleForces( + const std::vector>& emg) { // Output variable - biorbd::utils::Vector forces(nbMuscleTotal()); + utils::Vector forces(nbMuscleTotal()); unsigned int cmpMus(0); for (unsigned int i=0; isize(); ++i) { // muscle group @@ -219,10 +221,10 @@ biorbd::utils::Vector biorbd::muscles::Muscles::muscleForces( return forces; } -biorbd::utils::Vector biorbd::muscles::Muscles::muscleForces( - const std::vector> &emg, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot) +utils::Vector muscles::Muscles::muscleForces( + const std::vector> &emg, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot) { // Update the muscular position updateMuscles(Q, QDot, true); @@ -230,18 +232,18 @@ biorbd::utils::Vector biorbd::muscles::Muscles::muscleForces( return muscleForces(emg); } -unsigned int biorbd::muscles::Muscles::nbMuscleGroups() const +unsigned int muscles::Muscles::nbMuscleGroups() const { return static_cast(m_mus->size()); } -biorbd::utils::Matrix biorbd::muscles::Muscles::musclesLengthJacobian() +utils::Matrix muscles::Muscles::musclesLengthJacobian() { // Assuming that this is also a Joints type (via BiorbdModel) const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast(*this); - biorbd::utils::Matrix tp(nbMuscleTotal(), model.nbDof()); + utils::Matrix tp(nbMuscleTotal(), model.nbDof()); unsigned int cmpMus(0); for (unsigned int i=0; isize(); ++grp) { // muscular group @@ -276,9 +278,9 @@ unsigned int biorbd::muscles::Muscles::nbMuscles() const return total; } -void biorbd::muscles::Muscles::updateMuscles( - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& QDot, +void muscles::Muscles::updateMuscles( + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& QDot, bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) @@ -299,8 +301,8 @@ void biorbd::muscles::Muscles::updateMuscles( updateKinTP=1; } } -void biorbd::muscles::Muscles::updateMuscles( - const biorbd::rigidbody::GeneralizedCoordinates& Q, +void muscles::Muscles::updateMuscles( + const rigidbody::GeneralizedCoordinates& Q, bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) @@ -322,10 +324,10 @@ void biorbd::muscles::Muscles::updateMuscles( updateKinTP=1; } } -void biorbd::muscles::Muscles::updateMuscles( - std::vector>& musclePointsInGlobal, - std::vector &jacoPointsInGlobal, - const biorbd::rigidbody::GeneralizedVelocity& QDot) +void muscles::Muscles::updateMuscles( + std::vector>& musclePointsInGlobal, + std::vector &jacoPointsInGlobal, + const rigidbody::GeneralizedVelocity& QDot) { unsigned int cmpMuscle = 0; for (auto group : *m_mus) // muscle group @@ -336,19 +338,19 @@ void biorbd::muscles::Muscles::updateMuscles( } } -std::vector> - biorbd::muscles::Muscles::stateSet() +std::vector> + muscles::Muscles::stateSet() { - std::vector> out; + std::vector> out; for (unsigned int i=0; i>& musclePointsInGlobal, - std::vector &jacoPointsInGlobal) +void muscles::Muscles::updateMuscles( + std::vector>& musclePointsInGlobal, + std::vector &jacoPointsInGlobal) { // Updater all the muscles unsigned int cmpMuscle = 0; diff --git a/src/Muscles/PathModifiers.cpp b/src/Muscles/PathModifiers.cpp index cecf99e2..6d6d1ba0 100644 --- a/src/Muscles/PathModifiers.cpp +++ b/src/Muscles/PathModifiers.cpp @@ -7,8 +7,10 @@ #include "Muscles/WrappingSphere.h" #include "Muscles/WrappingHalfCylinder.h" -biorbd::muscles::PathModifiers::PathModifiers() : - m_obj(std::make_shared>>()), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::PathModifiers::PathModifiers() : + m_obj(std::make_shared>>()), m_nbWraps(std::make_shared(0)), m_nbVia(std::make_shared(0)), m_totalObjects(std::make_shared(0)) @@ -16,19 +18,19 @@ biorbd::muscles::PathModifiers::PathModifiers() : } -biorbd::muscles::PathModifiers biorbd::muscles::PathModifiers::DeepCopy() const +muscles::PathModifiers muscles::PathModifiers::DeepCopy() const { - biorbd::muscles::PathModifiers copy; + muscles::PathModifiers copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::PathModifiers::DeepCopy(const - biorbd::muscles::PathModifiers &other) +void muscles::PathModifiers::DeepCopy(const + muscles::PathModifiers &other) { m_obj->resize(other.m_obj->size()); for (unsigned int i=0; isize(); ++i) { - (*m_obj)[i] = std::make_shared(); + (*m_obj)[i] = std::make_shared(); *(*m_obj)[i] = (*other.m_obj)[i]->DeepCopy(); } *m_nbWraps = *other.m_nbWraps; @@ -37,64 +39,64 @@ void biorbd::muscles::PathModifiers::DeepCopy(const } // Private method to assing values -void biorbd::muscles::PathModifiers::addPathChanger( - biorbd::utils::Vector3d &object) +void muscles::PathModifiers::addPathChanger( + utils::Vector3d &object) { // Add a muscle to the pool of muscle depending on type - if (object.typeOfNode() == biorbd::utils::NODE_TYPE::WRAPPING_SPHERE) { - biorbd::utils::Error::check(*m_nbVia == 0, + if (object.typeOfNode() == utils::NODE_TYPE::WRAPPING_SPHERE) { + utils::Error::check(*m_nbVia == 0, "Cannot mix via points and wrapping objects yet"); - m_obj->push_back(std::make_shared( - static_cast (object))); + m_obj->push_back(std::make_shared( + static_cast (object))); ++*m_nbWraps; } else if (object.typeOfNode() == - biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { - biorbd::utils::Error::check(*m_nbVia == 0, + utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { + utils::Error::check(*m_nbVia == 0, "Cannot mix via points and wrapping objects yet"); - m_obj->push_back(std::make_shared( - dynamic_cast (object))); + m_obj->push_back(std::make_shared( + dynamic_cast (object))); ++*m_nbWraps; - } else if (object.typeOfNode() == biorbd::utils::NODE_TYPE::VIA_POINT) { - biorbd::utils::Error::check(*m_nbWraps == 0, + } else if (object.typeOfNode() == utils::NODE_TYPE::VIA_POINT) { + utils::Error::check(*m_nbWraps == 0, "Cannot mix via points and wrapping objects yet"); - m_obj->push_back(std::make_shared( - dynamic_cast (object))); + m_obj->push_back(std::make_shared( + dynamic_cast (object))); ++*m_nbVia; } else { - biorbd::utils::Error::raise("Wrapping type not found"); + utils::Error::raise("Wrapping type not found"); } ++*m_totalObjects; } -unsigned int biorbd::muscles::PathModifiers::nbWraps() const +unsigned int muscles::PathModifiers::nbWraps() const { return *m_nbWraps; } -unsigned int biorbd::muscles::PathModifiers::nbVia() const +unsigned int muscles::PathModifiers::nbVia() const { return *m_nbVia; } -unsigned int biorbd::muscles::PathModifiers::nbObjects() const +unsigned int muscles::PathModifiers::nbObjects() const { return *m_totalObjects; } -biorbd::utils::Vector3d& biorbd::muscles::PathModifiers::object( +utils::Vector3d& muscles::PathModifiers::object( unsigned int idx) { - biorbd::utils::Error::check(idx()), - m_excitation(std::make_shared(excitation)), - m_excitationNorm(std::make_shared(0)), - m_activation(std::make_shared(activation)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::State::State( + const utils::Scalar& excitation, + const utils::Scalar& activation) : + m_stateType(std::make_shared()), + m_excitation(std::make_shared(excitation)), + m_excitationNorm(std::make_shared(0)), + m_activation(std::make_shared(activation)) { setType(); } -biorbd::muscles::State::State( - const biorbd::muscles::State &other) : +muscles::State::State( + const muscles::State &other) : m_stateType(other.m_stateType), m_excitation(other.m_excitation), - m_excitationNorm(std::make_shared(0)), + m_excitationNorm(std::make_shared(0)), m_activation(other.m_activation) { } -biorbd::muscles::State::~State() +muscles::State::~State() { //dtor } -biorbd::muscles::State biorbd::muscles::State::DeepCopy() const +muscles::State muscles::State::DeepCopy() const { - biorbd::muscles::State copy; + muscles::State copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::State::DeepCopy(const biorbd::muscles::State &other) +void muscles::State::DeepCopy(const muscles::State &other) { *m_stateType = *other.m_stateType; *m_excitation = *other.m_excitation; @@ -44,8 +46,8 @@ void biorbd::muscles::State::DeepCopy(const biorbd::muscles::State &other) *m_activation = *other.m_activation; } -void biorbd::muscles::State::setExcitation( - const biorbd::utils::Scalar& val, +void muscles::State::setExcitation( + const utils::Scalar& val, bool turnOffWarnings) { @@ -54,7 +56,7 @@ void biorbd::muscles::State::setExcitation( #else if (val<0) { if (!turnOffWarnings) { - biorbd::utils::Error::warning( + utils::Error::warning( 0, "Excitation can't be lower than 0, 0 is used then"); } *m_excitation = 0; @@ -64,19 +66,19 @@ void biorbd::muscles::State::setExcitation( #endif } -const biorbd::utils::Scalar& biorbd::muscles::State::excitation() const +const utils::Scalar& muscles::State::excitation() const { return *m_excitation; } -const biorbd::utils::Scalar& biorbd::muscles::State::normalizeExcitation( - const biorbd::muscles::State &emgMax, +const utils::Scalar& muscles::State::normalizeExcitation( + const muscles::State &emgMax, bool turnOffWarnings) { #ifndef BIORBD_USE_CASADI_MATH if (!turnOffWarnings) { - biorbd::utils::Error::warning( + utils::Error::warning( *m_excitation < emgMax.excitation(), "Excitation is higher than maximal excitation."); } @@ -86,19 +88,19 @@ const biorbd::utils::Scalar& biorbd::muscles::State::normalizeExcitation( return *m_excitationNorm; } -void biorbd::muscles::State::setExcitationNorm( - const biorbd::utils::Scalar& val) +void muscles::State::setExcitationNorm( + const utils::Scalar& val) { *m_excitationNorm = val; } -const biorbd::utils::Scalar& biorbd::muscles::State::excitationNorm() const +const utils::Scalar& muscles::State::excitationNorm() const { return *m_excitationNorm; } -void biorbd::muscles::State::setActivation( - const biorbd::utils::Scalar& val, +void muscles::State::setActivation( + const utils::Scalar& val, bool turnOffWarnings) { #ifdef BIORBD_USE_CASADI_MATH @@ -106,15 +108,15 @@ void biorbd::muscles::State::setActivation( #else if (val <= 0) { if (!turnOffWarnings) { - biorbd::utils::Error::warning( - 0, "Activation is " + biorbd::utils::String::to_string(val) + + utils::Error::warning( + 0, "Activation is " + utils::String::to_string(val) + " but can't be lower than 0, 0 is used then"); } *m_activation = 0; } else if (val >= 1) { if (!turnOffWarnings) { - biorbd::utils::Error::warning( - 0, "Activation " + biorbd::utils::String::to_string(val) + + utils::Error::warning( + 0, "Activation " + utils::String::to_string(val) + " but can't be higher than 1, 1 is used then"); } *m_activation = 1; @@ -124,17 +126,17 @@ void biorbd::muscles::State::setActivation( #endif } -const biorbd::utils::Scalar& biorbd::muscles::State::activation() const +const utils::Scalar& muscles::State::activation() const { return *m_activation; } -biorbd::muscles::STATE_TYPE biorbd::muscles::State::type() const +muscles::STATE_TYPE muscles::State::type() const { return *m_stateType; } -void biorbd::muscles::State::setType() +void muscles::State::setType() { - *m_stateType = biorbd::muscles::STATE_TYPE::SIMPLE_STATE; + *m_stateType = muscles::STATE_TYPE::SIMPLE_STATE; } diff --git a/src/Muscles/StateDynamics.cpp b/src/Muscles/StateDynamics.cpp index e7eb8e06..b465982b 100644 --- a/src/Muscles/StateDynamics.cpp +++ b/src/Muscles/StateDynamics.cpp @@ -5,43 +5,45 @@ #include "Utils/String.h" #include "Muscles/Characteristics.h" -biorbd::muscles::StateDynamics::StateDynamics( - const biorbd::utils::Scalar& excitation, - const biorbd::utils::Scalar& activation) : - biorbd::muscles::State(excitation,activation), - m_previousExcitation(std::make_shared(0)), - m_previousActivation(std::make_shared(0)), - m_activationDot(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StateDynamics::StateDynamics( + const utils::Scalar& excitation, + const utils::Scalar& activation) : + muscles::State(excitation,activation), + m_previousExcitation(std::make_shared(0)), + m_previousActivation(std::make_shared(0)), + m_activationDot(std::make_shared(0)) { setType(); } -biorbd::muscles::StateDynamics::StateDynamics( - const biorbd::muscles::State &other) : - biorbd::muscles::State(other) +muscles::StateDynamics::StateDynamics( + const muscles::State &other) : + muscles::State(other) { - const auto& state = dynamic_cast(other); + const auto& state = dynamic_cast(other); m_previousExcitation = state.m_previousExcitation; m_previousActivation = state.m_previousActivation; m_activationDot = state.m_activationDot; } -biorbd::muscles::StateDynamics::~StateDynamics() +muscles::StateDynamics::~StateDynamics() { //dtor } -biorbd::muscles::StateDynamics biorbd::muscles::StateDynamics::DeepCopy() const +muscles::StateDynamics muscles::StateDynamics::DeepCopy() const { - biorbd::muscles::StateDynamics copy; + muscles::StateDynamics copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::StateDynamics::DeepCopy(const - biorbd::muscles::StateDynamics &other) +void muscles::StateDynamics::DeepCopy(const + muscles::StateDynamics &other) { - biorbd::muscles::State::DeepCopy(other); + muscles::State::DeepCopy(other); *m_excitationNorm = *other.m_excitationNorm; *m_previousExcitation = *other.m_previousExcitation; *m_previousActivation = *other.m_previousActivation; @@ -49,10 +51,10 @@ void biorbd::muscles::StateDynamics::DeepCopy(const } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::timeDerivativeActivation( - const biorbd::muscles::State& emg, - const biorbd::muscles::Characteristics& characteristics, +const utils::Scalar& +muscles::StateDynamics::timeDerivativeActivation( + const muscles::State& emg, + const muscles::Characteristics& characteristics, bool alreadyNormalized) { return timeDerivativeActivation(emg.excitation(), emg.activation(), @@ -60,11 +62,11 @@ biorbd::muscles::StateDynamics::timeDerivativeActivation( } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::timeDerivativeActivation( - const biorbd::utils::Scalar& excitation, - const biorbd::utils::Scalar& activation, - const biorbd::muscles::Characteristics &characteristics, +const utils::Scalar& +muscles::StateDynamics::timeDerivativeActivation( + const utils::Scalar& excitation, + const utils::Scalar& activation, + const muscles::Characteristics &characteristics, bool alreadyNormalized) { setExcitation(excitation); @@ -72,9 +74,9 @@ biorbd::muscles::StateDynamics::timeDerivativeActivation( return timeDerivativeActivation(characteristics, alreadyNormalized); } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::timeDerivativeActivation( - const biorbd::muscles::Characteristics &characteristics, +const utils::Scalar& +muscles::StateDynamics::timeDerivativeActivation( + const muscles::Characteristics &characteristics, bool alreadyNormalized) { // Implémentation de la fonction da/dt = (u-a)/GeneralizedTorque(u,a) @@ -104,7 +106,7 @@ biorbd::muscles::StateDynamics::timeDerivativeActivation( // http://simtk-confluence.stanford.edu:8080/display/OpenSim/First-Order+Activation+Dynamics - biorbd::utils::Scalar num; + utils::Scalar num; if (alreadyNormalized) { num = *m_excitation - *m_activation; // numérateur } else { @@ -112,7 +114,7 @@ biorbd::muscles::StateDynamics::timeDerivativeActivation( *m_activation; // numérateur } - biorbd::utils::Scalar denom; // dénominateur + utils::Scalar denom; // dénominateur #ifdef BIORBD_USE_CASADI_MATH denom = casadi::MX::if_else( casadi::MX::gt(num, 0), @@ -130,41 +132,41 @@ biorbd::muscles::StateDynamics::timeDerivativeActivation( return *m_activationDot; } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::timeDerivativeActivation() +const utils::Scalar& +muscles::StateDynamics::timeDerivativeActivation() { return *m_activationDot; } -void biorbd::muscles::StateDynamics::setExcitation( - const biorbd::utils::Scalar& val, +void muscles::StateDynamics::setExcitation( + const utils::Scalar& val, bool turnOffWarnings) { *m_previousExcitation = *m_excitation; - biorbd::muscles::State::setExcitation(val, turnOffWarnings); + muscles::State::setExcitation(val, turnOffWarnings); } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::previousExcitation() const +const utils::Scalar& +muscles::StateDynamics::previousExcitation() const { return *m_previousExcitation; } -void biorbd::muscles::StateDynamics::setActivation( - const biorbd::utils::Scalar& val, +void muscles::StateDynamics::setActivation( + const utils::Scalar& val, bool turnOffWarnings) { *m_previousActivation = *m_activation; - biorbd::muscles::State::setActivation(val, turnOffWarnings); + muscles::State::setActivation(val, turnOffWarnings); } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamics::previousActivation() const +const utils::Scalar& +muscles::StateDynamics::previousActivation() const { return *m_previousActivation; } -void biorbd::muscles::StateDynamics::setType() +void muscles::StateDynamics::setType() { - *m_stateType = biorbd::muscles::STATE_TYPE::DYNAMIC; + *m_stateType = muscles::STATE_TYPE::DYNAMIC; } diff --git a/src/Muscles/StateDynamicsBuchanan.cpp b/src/Muscles/StateDynamicsBuchanan.cpp index 6c17aa68..1c8a7a8d 100644 --- a/src/Muscles/StateDynamicsBuchanan.cpp +++ b/src/Muscles/StateDynamicsBuchanan.cpp @@ -3,54 +3,56 @@ #include -biorbd::muscles::StateDynamicsBuchanan::StateDynamicsBuchanan( - const biorbd::utils::Scalar& neuralCommand, - const biorbd::utils::Scalar& excitation) : - biorbd::muscles::StateDynamics(excitation,0), - m_neuralCommand(std::make_shared(neuralCommand)), - m_shapeFactor(std::make_shared(-3)), - m_excitationDot(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StateDynamicsBuchanan::StateDynamicsBuchanan( + const utils::Scalar& neuralCommand, + const utils::Scalar& excitation) : + muscles::StateDynamics(excitation,0), + m_neuralCommand(std::make_shared(neuralCommand)), + m_shapeFactor(std::make_shared(-3)), + m_excitationDot(std::make_shared(0)) { setType(); // Update activation - biorbd::muscles::StateDynamicsBuchanan::activation(); + muscles::StateDynamicsBuchanan::activation(); } -biorbd::muscles::StateDynamicsBuchanan::StateDynamicsBuchanan( - const biorbd::muscles::State &other) : - biorbd::muscles::StateDynamics(other) +muscles::StateDynamicsBuchanan::StateDynamicsBuchanan( + const muscles::State &other) : + muscles::StateDynamics(other) { const auto& buchanan = - dynamic_cast(other); + dynamic_cast(other); m_neuralCommand = buchanan.m_neuralCommand; m_shapeFactor = buchanan.m_shapeFactor; m_excitationDot = buchanan.m_excitationDot; } -biorbd::muscles::StateDynamicsBuchanan::~StateDynamicsBuchanan() +muscles::StateDynamicsBuchanan::~StateDynamicsBuchanan() { //dtor } -biorbd::muscles::StateDynamicsBuchanan -biorbd::muscles::StateDynamicsBuchanan::DeepCopy() const +muscles::StateDynamicsBuchanan +muscles::StateDynamicsBuchanan::DeepCopy() const { - biorbd::muscles::StateDynamicsBuchanan copy; + muscles::StateDynamicsBuchanan copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::StateDynamicsBuchanan::DeepCopy( - const biorbd::muscles::StateDynamicsBuchanan &other) +void muscles::StateDynamicsBuchanan::DeepCopy( + const muscles::StateDynamicsBuchanan &other) { - biorbd::muscles::StateDynamics::DeepCopy(other); + muscles::StateDynamics::DeepCopy(other); *m_neuralCommand = *other.m_neuralCommand; *m_shapeFactor = *other.m_shapeFactor; *m_excitationDot = *other.m_excitationDot; } -void biorbd::muscles::StateDynamicsBuchanan::shapeFactor( - const biorbd::utils::Scalar& shape_factor) +void muscles::StateDynamicsBuchanan::shapeFactor( + const utils::Scalar& shape_factor) { *m_shapeFactor = shape_factor; @@ -58,25 +60,25 @@ void biorbd::muscles::StateDynamicsBuchanan::shapeFactor( setActivation(0); } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamicsBuchanan::shapeFactor() const +const utils::Scalar& +muscles::StateDynamicsBuchanan::shapeFactor() const { return *m_shapeFactor; } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamicsBuchanan::timeDerivativeExcitation( - const biorbd::muscles::Characteristics &characteristics, +const utils::Scalar& +muscles::StateDynamicsBuchanan::timeDerivativeExcitation( + const muscles::Characteristics &characteristics, bool alreadyNormalized) { - // Move excitation to activation to use properly biorbd::muscles::StateDynamics::timeDerivativeActivation - biorbd::utils::Scalar activationTp = *m_activation; + // Move excitation to activation to use properly muscles::StateDynamics::timeDerivativeActivation + utils::Scalar activationTp = *m_activation; *m_activation = *m_excitation; - biorbd::utils::Scalar excitationTp = *m_excitation; + utils::Scalar excitationTp = *m_excitation; *m_excitation = *m_neuralCommand; // Compute excitationDot - *m_excitationDot = biorbd::muscles::StateDynamics::timeDerivativeActivation( + *m_excitationDot = muscles::StateDynamics::timeDerivativeActivation( characteristics, alreadyNormalized); // Set back activationDot to 0 (since it is suppose to calculate excitationDot) *m_activationDot = 0; @@ -86,33 +88,33 @@ biorbd::muscles::StateDynamicsBuchanan::timeDerivativeExcitation( return *m_excitationDot; } -void biorbd::muscles::StateDynamicsBuchanan::setExcitation( - const biorbd::utils::Scalar& val, +void muscles::StateDynamicsBuchanan::setExcitation( + const utils::Scalar& val, bool) { - biorbd::muscles::StateDynamics::setExcitation(val); + muscles::StateDynamics::setExcitation(val); // Update activation setActivation(0); } -void biorbd::muscles::StateDynamicsBuchanan::setNeuralCommand( - const biorbd::utils::Scalar& val) +void muscles::StateDynamicsBuchanan::setNeuralCommand( + const utils::Scalar& val) { *m_neuralCommand = val; } -void biorbd::muscles::StateDynamicsBuchanan::setActivation( - const biorbd::utils::Scalar&, +void muscles::StateDynamicsBuchanan::setActivation( + const utils::Scalar&, bool) { - biorbd::utils::Scalar expShapeFactor(exp(*m_shapeFactor)); + utils::Scalar expShapeFactor(exp(*m_shapeFactor)); *m_activation = ( pow(expShapeFactor, *m_excitation) - 1) / (expShapeFactor - 1) ; } -void biorbd::muscles::StateDynamicsBuchanan::setType() +void muscles::StateDynamicsBuchanan::setType() { - *m_stateType = biorbd::muscles::STATE_TYPE::BUCHANAN; + *m_stateType = muscles::STATE_TYPE::BUCHANAN; } diff --git a/src/Muscles/StateDynamicsDeGroote.cpp b/src/Muscles/StateDynamicsDeGroote.cpp index bd9d430b..55d1c2be 100644 --- a/src/Muscles/StateDynamicsDeGroote.cpp +++ b/src/Muscles/StateDynamicsDeGroote.cpp @@ -6,53 +6,55 @@ #include "Utils/String.h" #include "Muscles/Characteristics.h" -biorbd::muscles::StateDynamicsDeGroote::StateDynamicsDeGroote( - const biorbd::utils::Scalar& excitation, - const biorbd::utils::Scalar& activation) : - biorbd::muscles::StateDynamics(excitation,activation) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StateDynamicsDeGroote::StateDynamicsDeGroote( + const utils::Scalar& excitation, + const utils::Scalar& activation) : + muscles::StateDynamics(excitation,activation) { setType(); } -biorbd::muscles::StateDynamicsDeGroote::StateDynamicsDeGroote( - const biorbd::muscles::StateDynamicsDeGroote &other) : - biorbd::muscles::StateDynamics(other) +muscles::StateDynamicsDeGroote::StateDynamicsDeGroote( + const muscles::StateDynamicsDeGroote &other) : + muscles::StateDynamics(other) { } -biorbd::muscles::StateDynamicsDeGroote -biorbd::muscles::StateDynamicsDeGroote::DeepCopy() const +muscles::StateDynamicsDeGroote +muscles::StateDynamicsDeGroote::DeepCopy() const { - biorbd::muscles::StateDynamicsDeGroote copy; + muscles::StateDynamicsDeGroote copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::StateDynamicsDeGroote::DeepCopy(const - biorbd::muscles::StateDynamicsDeGroote &other) +void muscles::StateDynamicsDeGroote::DeepCopy(const + muscles::StateDynamicsDeGroote &other) { - biorbd::muscles::StateDynamics::DeepCopy(other); + muscles::StateDynamics::DeepCopy(other); } -const biorbd::utils::Scalar& -biorbd::muscles::StateDynamicsDeGroote::timeDerivativeActivation( - const biorbd::muscles::Characteristics &characteristics, +const utils::Scalar& +muscles::StateDynamicsDeGroote::timeDerivativeActivation( + const muscles::Characteristics &characteristics, bool alreadyNormalized) { // From DeGroote https://www.ncbi.nlm.nih.gov/pmc/articles/PMC5043004/ // Get activation and excitation and make difference - biorbd::utils::Scalar diff; //(e - a) - biorbd::utils::Scalar f; //activation dynamics + utils::Scalar diff; //(e - a) + utils::Scalar f; //activation dynamics //std::tanh; diff = *m_excitation - *m_activation; f = 0.5 * tanh(0.1*diff); - biorbd::utils::Scalar denom_activation = characteristics.torqueActivation() + utils::Scalar denom_activation = characteristics.torqueActivation() * (0.5+1.5* *m_activation); - biorbd::utils::Scalar denom_deactivation = + utils::Scalar denom_deactivation = characteristics.torqueDeactivation() / (0.5+1.5* *m_activation); *m_activationDot = (((f+0.5)/denom_activation)+((-f + 0.5)/denom_deactivation)) @@ -60,7 +62,7 @@ biorbd::muscles::StateDynamicsDeGroote::timeDerivativeActivation( return *m_activationDot; } -void biorbd::muscles::StateDynamicsDeGroote::setType() +void muscles::StateDynamicsDeGroote::setType() { - *m_stateType = biorbd::muscles::STATE_TYPE::DE_GROOTE; + *m_stateType = muscles::STATE_TYPE::DE_GROOTE; } diff --git a/src/Muscles/ViaPoint.cpp b/src/Muscles/ViaPoint.cpp index f72a18de..f3a4abc9 100644 --- a/src/Muscles/ViaPoint.cpp +++ b/src/Muscles/ViaPoint.cpp @@ -3,53 +3,55 @@ #include "Utils/String.h" -biorbd::muscles::ViaPoint::ViaPoint() : - biorbd::utils::Vector3d() +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::ViaPoint::ViaPoint() : + utils::Vector3d() { - *m_typeOfNode = biorbd::utils::NODE_TYPE::VIA_POINT; + *m_typeOfNode = utils::NODE_TYPE::VIA_POINT; } -biorbd::muscles::ViaPoint::ViaPoint( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z) : - biorbd::utils::Vector3d(x, y, z) +muscles::ViaPoint::ViaPoint( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z) : + utils::Vector3d(x, y, z) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::VIA_POINT; + *m_typeOfNode = utils::NODE_TYPE::VIA_POINT; } -biorbd::muscles::ViaPoint::ViaPoint( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::utils::Vector3d(x, y, z, name, parentName) +muscles::ViaPoint::ViaPoint( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName) : + utils::Vector3d(x, y, z, name, parentName) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::VIA_POINT; + *m_typeOfNode = utils::NODE_TYPE::VIA_POINT; } -biorbd::muscles::ViaPoint::ViaPoint( - const biorbd::utils::Vector3d &other) : - biorbd::utils::Vector3d(other) +muscles::ViaPoint::ViaPoint( + const utils::Vector3d &other) : + utils::Vector3d(other) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::VIA_POINT; + *m_typeOfNode = utils::NODE_TYPE::VIA_POINT; } -biorbd::muscles::ViaPoint::ViaPoint(const biorbd::muscles::ViaPoint &other) : - biorbd::utils::Vector3d(other) +muscles::ViaPoint::ViaPoint(const muscles::ViaPoint &other) : + utils::Vector3d(other) { } -biorbd::muscles::ViaPoint biorbd::muscles::ViaPoint::DeepCopy() const +muscles::ViaPoint muscles::ViaPoint::DeepCopy() const { - biorbd::muscles::ViaPoint copy; + muscles::ViaPoint copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::ViaPoint::DeepCopy(const biorbd::muscles::ViaPoint &other) +void muscles::ViaPoint::DeepCopy(const muscles::ViaPoint &other) { - biorbd::utils::Vector3d::DeepCopy(other); + utils::Vector3d::DeepCopy(other); } diff --git a/src/Muscles/WrappingHalfCylinder.cpp b/src/Muscles/WrappingHalfCylinder.cpp index caf3644f..d113e8c8 100644 --- a/src/Muscles/WrappingHalfCylinder.cpp +++ b/src/Muscles/WrappingHalfCylinder.cpp @@ -5,25 +5,27 @@ #include "Utils/RotoTrans.h" #include "RigidBody/Joints.h" -biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder() : - biorbd::muscles::WrappingObject (), - m_radius(std::make_shared(0)), - m_length(std::make_shared(0)), - m_RTtoParent(std::make_shared()), - m_p1Wrap(std::make_shared()), - m_p2Wrap(std::make_shared()), - m_lengthAroundWrap(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::WrappingHalfCylinder::WrappingHalfCylinder() : + muscles::WrappingObject (), + m_radius(std::make_shared(0)), + m_length(std::make_shared(0)), + m_RTtoParent(std::make_shared()), + m_p1Wrap(std::make_shared()), + m_p2Wrap(std::make_shared()), + m_lengthAroundWrap(std::make_shared(0)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; } -biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( - const biorbd::utils::Vector3d &other): - biorbd::muscles::WrappingObject(other) +muscles::WrappingHalfCylinder::WrappingHalfCylinder( + const utils::Vector3d &other): + muscles::WrappingObject(other) { - biorbd::muscles::WrappingHalfCylinder& otherWrap( - const_cast( - dynamic_cast(other))); + muscles::WrappingHalfCylinder& otherWrap( + const_cast( + dynamic_cast(other))); m_radius = otherWrap.m_radius; m_length = otherWrap.m_length; m_RTtoParent = otherWrap.m_RTtoParent; @@ -32,13 +34,13 @@ biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( m_lengthAroundWrap = otherWrap.m_lengthAroundWrap; } -biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( - const biorbd::utils::Vector3d *other): - biorbd::muscles::WrappingObject(*other) +muscles::WrappingHalfCylinder::WrappingHalfCylinder( + const utils::Vector3d *other): + muscles::WrappingObject(*other) { - biorbd::muscles::WrappingHalfCylinder* otherWrap( - const_cast( - dynamic_cast(other))); + muscles::WrappingHalfCylinder* otherWrap( + const_cast( + dynamic_cast(other))); m_radius = otherWrap->m_radius; m_length = otherWrap->m_length; m_RTtoParent = otherWrap->m_RTtoParent; @@ -47,50 +49,50 @@ biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( m_lengthAroundWrap = otherWrap->m_lengthAroundWrap; } -biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( - const biorbd::utils::RotoTrans &rt, - const biorbd::utils::Scalar& radius, - const biorbd::utils::Scalar& length) : - biorbd::muscles::WrappingObject (rt.trans()), - m_radius(std::make_shared(radius)), - m_length(std::make_shared(length)), - m_RTtoParent(std::make_shared(rt)), - m_p1Wrap(std::make_shared()), - m_p2Wrap(std::make_shared()), - m_lengthAroundWrap(std::make_shared(0)) +muscles::WrappingHalfCylinder::WrappingHalfCylinder( + const utils::RotoTrans &rt, + const utils::Scalar& radius, + const utils::Scalar& length) : + muscles::WrappingObject (rt.trans()), + m_radius(std::make_shared(radius)), + m_length(std::make_shared(length)), + m_RTtoParent(std::make_shared(rt)), + m_p1Wrap(std::make_shared()), + m_p2Wrap(std::make_shared()), + m_lengthAroundWrap(std::make_shared(0)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; } -biorbd::muscles::WrappingHalfCylinder::WrappingHalfCylinder( - const biorbd::utils::RotoTrans &rt, - const biorbd::utils::Scalar& radius, - const biorbd::utils::Scalar& length, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::muscles::WrappingObject (rt.trans(), name, parentName), - m_radius(std::make_shared(radius)), - m_length(std::make_shared(length)), - m_RTtoParent(std::make_shared(rt)), - m_p1Wrap(std::make_shared()), - m_p2Wrap(std::make_shared()), - m_lengthAroundWrap(std::make_shared(0)) +muscles::WrappingHalfCylinder::WrappingHalfCylinder( + const utils::RotoTrans &rt, + const utils::Scalar& radius, + const utils::Scalar& length, + const utils::String &name, + const utils::String &parentName) : + muscles::WrappingObject (rt.trans(), name, parentName), + m_radius(std::make_shared(radius)), + m_length(std::make_shared(length)), + m_RTtoParent(std::make_shared(rt)), + m_p1Wrap(std::make_shared()), + m_p2Wrap(std::make_shared()), + m_lengthAroundWrap(std::make_shared(0)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER; } -biorbd::muscles::WrappingHalfCylinder -biorbd::muscles::WrappingHalfCylinder::DeepCopy() const +muscles::WrappingHalfCylinder +muscles::WrappingHalfCylinder::DeepCopy() const { - biorbd::muscles::WrappingHalfCylinder copy; + muscles::WrappingHalfCylinder copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::WrappingHalfCylinder::DeepCopy(const - biorbd::muscles::WrappingHalfCylinder &other) +void muscles::WrappingHalfCylinder::DeepCopy(const + muscles::WrappingHalfCylinder &other) { - biorbd::muscles::WrappingObject::DeepCopy(other); + muscles::WrappingObject::DeepCopy(other); *m_radius = *other.m_radius; *m_length = *other.m_length; *m_RTtoParent = *other.m_RTtoParent; @@ -99,13 +101,13 @@ void biorbd::muscles::WrappingHalfCylinder::DeepCopy(const *m_lengthAroundWrap = *other.m_lengthAroundWrap; } -void biorbd::muscles::WrappingHalfCylinder::wrapPoints( - const biorbd::utils::RotoTrans& rt, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar *length) +void muscles::WrappingHalfCylinder::wrapPoints( + const utils::RotoTrans& rt, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar *length) { // This function takes the position of the wrapping and finds the location where muscle 1 and 2 leave the wrapping object @@ -115,8 +117,8 @@ void biorbd::muscles::WrappingHalfCylinder::wrapPoints( p_glob.m_p2->applyRT(rt.transpose()); // Find the tangents of these points to the circle (cylinder seen from above) - biorbd::utils::Vector3d p1_tan(0, 0, 0); - biorbd::utils::Vector3d p2_tan(0, 0, 0); + utils::Vector3d p1_tan(0, 0, 0); + utils::Vector3d p2_tan(0, 0, 0); findTangentToCircle(*p_glob.m_p1, p1_tan); findTangentToCircle(*p_glob.m_p2, p2_tan); @@ -146,24 +148,24 @@ void biorbd::muscles::WrappingHalfCylinder::wrapPoints( } } -void biorbd::muscles::WrappingHalfCylinder::wrapPoints( +void muscles::WrappingHalfCylinder::wrapPoints( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::utils::Vector3d& p1_bone, - const biorbd::utils::Vector3d& p2_bone, - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar *length) + const rigidbody::GeneralizedCoordinates& Q, + const utils::Vector3d& p1_bone, + const utils::Vector3d& p2_bone, + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar *length) { // This function takes a model and a position of the model and returns the location where muscle 1 et 2 leave the wrapping object wrapPoints(RT(model,Q), p1_bone, p2_bone, p1, p2, length); } -void biorbd::muscles::WrappingHalfCylinder::wrapPoints( - biorbd::utils::Vector3d& p1, - biorbd::utils::Vector3d& p2, - biorbd::utils::Scalar *length) +void muscles::WrappingHalfCylinder::wrapPoints( + utils::Vector3d& p1, + utils::Vector3d& p2, + utils::Scalar *length) { p1 = *m_p1Wrap; p2 = *m_p2Wrap; @@ -172,9 +174,9 @@ void biorbd::muscles::WrappingHalfCylinder::wrapPoints( } } -const biorbd::utils::RotoTrans& biorbd::muscles::WrappingHalfCylinder::RT( +const utils::RotoTrans& muscles::WrappingHalfCylinder::RT( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedCoordinates& Q, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -189,39 +191,39 @@ const biorbd::utils::RotoTrans& biorbd::muscles::WrappingHalfCylinder::RT( return *m_RT; } -void biorbd::muscles::WrappingHalfCylinder::setRadius( - const biorbd::utils::Scalar &val) +void muscles::WrappingHalfCylinder::setRadius( + const utils::Scalar &val) { *m_radius = val; } -biorbd::utils::Scalar biorbd::muscles::WrappingHalfCylinder::radius() const +utils::Scalar muscles::WrappingHalfCylinder::radius() const { return *m_radius; } -biorbd::utils::Scalar biorbd::muscles::WrappingHalfCylinder::diameter() const +utils::Scalar muscles::WrappingHalfCylinder::diameter() const { return 2 * *m_radius; } -void biorbd::muscles::WrappingHalfCylinder::setLength( - const biorbd::utils::Scalar& val) +void muscles::WrappingHalfCylinder::setLength( + const utils::Scalar& val) { *m_length = val; } -const biorbd::utils::Scalar& biorbd::muscles::WrappingHalfCylinder::length() +const utils::Scalar& muscles::WrappingHalfCylinder::length() const { return *m_length; } -void biorbd::muscles::WrappingHalfCylinder::findTangentToCircle( - const biorbd::utils::Vector3d& p, - biorbd::utils::Vector3d& p_tan) const +void muscles::WrappingHalfCylinder::findTangentToCircle( + const utils::Vector3d& p, + utils::Vector3d& p_tan) const { - biorbd::utils::Scalar p_dot = p.block(0,0,2,1).dot(p.block(0,0,2,1)); + utils::Scalar p_dot = p.block(0,0,2,1).dot(p.block(0,0,2,1)); const RigidBodyDynamics::Math::Vector2d& Q0(radius()*radius()/p_dot*p.block(0,0, 2,1)); @@ -241,9 +243,9 @@ void biorbd::muscles::WrappingHalfCylinder::findTangentToCircle( selectTangents(m, p_tan); } -void biorbd::muscles::WrappingHalfCylinder::selectTangents( +void muscles::WrappingHalfCylinder::selectTangents( const NodeMusclePair &p1, - biorbd::utils::Vector3d &p_tan) const + utils::Vector3d &p_tan) const { #ifdef BIORBD_USE_CASADI_MATH p_tan = casadi::MX::if_else( @@ -258,7 +260,7 @@ void biorbd::muscles::WrappingHalfCylinder::selectTangents( #endif } -bool biorbd::muscles::WrappingHalfCylinder::findVerticalNode( +bool muscles::WrappingHalfCylinder::findVerticalNode( const NodeMusclePair &pointsInGlobal, NodeMusclePair &pointsToWrap) const { @@ -269,9 +271,9 @@ bool biorbd::muscles::WrappingHalfCylinder::findVerticalNode( if (!checkIfWraps(pointsInGlobal, pointsToWrap)) { // If it doesn't pass by the wrap, put NaN and stop for (unsigned int i=0; i<3; ++i) { - (*pointsToWrap.m_p1)(i) = static_cast + (*pointsToWrap.m_p1)(i) = static_cast (static_cast(NAN)); - (*pointsToWrap.m_p2)(i) = static_cast + (*pointsToWrap.m_p2)(i) = static_cast (static_cast(NAN)); } return false; @@ -286,11 +288,11 @@ bool biorbd::muscles::WrappingHalfCylinder::findVerticalNode( // Find the location where the points cross the cylinder // X is the straight line between the two points - biorbd::utils::Vector3d X(*pointsInGlobal.m_p2 - *pointsInGlobal.m_p1); + utils::Vector3d X(*pointsInGlobal.m_p2 - *pointsInGlobal.m_p1); // Z is the empty axis of the cylinder - biorbd::utils::Vector3d Z(0,0,1); + utils::Vector3d Z(0,0,1); - biorbd::utils::Vector3d Y(Z.cross(X)); + utils::Vector3d Y(Z.cross(X)); // Re-compute X for it to be aligned with the cylinder X = Y.cross(Z); // Normalise everything @@ -298,28 +300,28 @@ bool biorbd::muscles::WrappingHalfCylinder::findVerticalNode( Y = Y/Y.norm(); Z = Z/Z.norm(); // Concatenate to get the rotation matrix - biorbd::utils::RotoTrans R(X(0), X(1), X(2), 0, + utils::RotoTrans R(X(0), X(1), X(2), 0, Y(0), Y(1), Y(2), 0, Z(0), Z(1), Z(2), 0, 0, 0, 0, 1); // Turn the points in the R reference - biorbd::utils::Vector3d globA(*pointsInGlobal.m_p1); - biorbd::utils::Vector3d globB(*pointsInGlobal.m_p2); - biorbd::utils::Vector3d wrapA(*pointsToWrap.m_p1); - biorbd::utils::Vector3d wrapB(*pointsToWrap.m_p2); + utils::Vector3d globA(*pointsInGlobal.m_p1); + utils::Vector3d globB(*pointsInGlobal.m_p2); + utils::Vector3d wrapA(*pointsToWrap.m_p1); + utils::Vector3d wrapB(*pointsToWrap.m_p2); globA.applyRT(R); globB.applyRT(R); wrapA.applyRT(R); wrapB.applyRT(R); // The height depends on the relative distance - (*pointsToWrap.m_p1)(2) = biorbd::utils::Scalar(wrapA(0)-globB(0)) / - biorbd::utils::Scalar(globA(0)-globB(0)) * + (*pointsToWrap.m_p1)(2) = utils::Scalar(wrapA(0)-globB(0)) / + utils::Scalar(globA(0)-globB(0)) * ((*pointsInGlobal.m_p1)(2)-(*pointsInGlobal.m_p2)(2)) + (*pointsInGlobal.m_p2)( 2); - (*pointsToWrap.m_p2)(2) = biorbd::utils::Scalar(wrapB(0)-globB(0)) / - biorbd::utils::Scalar(globA(0)-globB(0)) * + (*pointsToWrap.m_p2)(2) = utils::Scalar(wrapB(0)-globB(0)) / + utils::Scalar(globA(0)-globB(0)) * ((*pointsInGlobal.m_p1)(2)-(*pointsInGlobal.m_p2)(2)) + (*pointsInGlobal.m_p2)( 2); @@ -327,7 +329,7 @@ bool biorbd::muscles::WrappingHalfCylinder::findVerticalNode( } #ifndef BIORBD_USE_CASADI_MATH -bool biorbd::muscles::WrappingHalfCylinder::checkIfWraps( +bool muscles::WrappingHalfCylinder::checkIfWraps( const NodeMusclePair &pointsInGlobal, NodeMusclePair &pointsToWrap) const { @@ -374,10 +376,10 @@ bool biorbd::muscles::WrappingHalfCylinder::checkIfWraps( } #endif -biorbd::utils::Scalar biorbd::muscles::WrappingHalfCylinder::computeLength( +utils::Scalar muscles::WrappingHalfCylinder::computeLength( const NodeMusclePair &p) const { - biorbd::utils::Scalar arc = std::acos( ( (*p.m_p1)(0) * (*p.m_p2)(0) + + utils::Scalar arc = std::acos( ( (*p.m_p1)(0) * (*p.m_p2)(0) + (*p.m_p1)(1) * (*p.m_p2)(1)) / std::sqrt( ((*p.m_p1)(0) * (*p.m_p1)(0) + (*p.m_p1)(1) * (*p.m_p1)(1)) * diff --git a/src/Muscles/WrappingObject.cpp b/src/Muscles/WrappingObject.cpp index 925fb1f8..2d799f6e 100644 --- a/src/Muscles/WrappingObject.cpp +++ b/src/Muscles/WrappingObject.cpp @@ -4,67 +4,69 @@ #include "Utils/String.h" #include "Utils/RotoTrans.h" -biorbd::muscles::WrappingObject::WrappingObject() : - biorbd::utils::Vector3d (), - m_RT(std::make_shared()) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::WrappingObject::WrappingObject() : + utils::Vector3d (), + m_RT(std::make_shared()) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_OBJECT; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_OBJECT; } -biorbd::muscles::WrappingObject::WrappingObject( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z) : - biorbd::utils::Vector3d(x, y, z), - m_RT(std::make_shared()) +muscles::WrappingObject::WrappingObject( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z) : + utils::Vector3d(x, y, z), + m_RT(std::make_shared()) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_OBJECT; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_OBJECT; } -biorbd::muscles::WrappingObject::WrappingObject( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::utils::Vector3d(x, y, z, name, parentName), - m_RT(std::make_shared()) +muscles::WrappingObject::WrappingObject( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::String &name, + const utils::String &parentName) : + utils::Vector3d(x, y, z, name, parentName), + m_RT(std::make_shared()) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_OBJECT; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_OBJECT; } -biorbd::muscles::WrappingObject::WrappingObject( - const biorbd::utils::Vector3d &other) : - biorbd::utils::Vector3d (other) +muscles::WrappingObject::WrappingObject( + const utils::Vector3d &other) : + utils::Vector3d (other) { try { - biorbd::muscles::WrappingObject& otherWrap( - const_cast( - dynamic_cast(other))); + muscles::WrappingObject& otherWrap( + const_cast( + dynamic_cast(other))); m_RT = otherWrap.m_RT; } catch(const std::bad_cast& e) { - m_RT = std::make_shared(); + m_RT = std::make_shared(); } } -biorbd::muscles::WrappingObject::WrappingObject( - const biorbd::utils::Vector3d &other, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::utils::Vector3d (other, name, parentName), - m_RT(std::make_shared()) +muscles::WrappingObject::WrappingObject( + const utils::Vector3d &other, + const utils::String &name, + const utils::String &parentName) : + utils::Vector3d (other, name, parentName), + m_RT(std::make_shared()) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_OBJECT; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_OBJECT; } -void biorbd::muscles::WrappingObject::DeepCopy( - const biorbd::muscles::WrappingObject &other) +void muscles::WrappingObject::DeepCopy( + const muscles::WrappingObject &other) { - biorbd::utils::Vector3d::DeepCopy(other); + utils::Vector3d::DeepCopy(other); *m_RT = *other.m_RT; } -const biorbd::utils::RotoTrans &biorbd::muscles::WrappingObject::RT() const +const utils::RotoTrans &muscles::WrappingObject::RT() const { return *m_RT; } diff --git a/src/Muscles/WrappingSphere.cpp b/src/Muscles/WrappingSphere.cpp index a83c48fd..50fbec64 100644 --- a/src/Muscles/WrappingSphere.cpp +++ b/src/Muscles/WrappingSphere.cpp @@ -4,77 +4,79 @@ #include "Utils/String.h" #include "Utils/RotoTrans.h" -biorbd::muscles::WrappingSphere::WrappingSphere() : - biorbd::muscles::WrappingObject (), - m_dia(std::make_shared(0)) +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::WrappingSphere::WrappingSphere() : + muscles::WrappingObject (), + m_dia(std::make_shared(0)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_SPHERE; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_SPHERE; } -biorbd::muscles::WrappingSphere::WrappingSphere( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::Scalar& diameter) : - biorbd::muscles::WrappingObject (x, y, z), - m_dia(std::make_shared(diameter)) +muscles::WrappingSphere::WrappingSphere( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::Scalar& diameter) : + muscles::WrappingObject (x, y, z), + m_dia(std::make_shared(diameter)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_SPHERE; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_SPHERE; } -biorbd::muscles::WrappingSphere::WrappingSphere( - const biorbd::utils::Scalar& x, - const biorbd::utils::Scalar& y, - const biorbd::utils::Scalar& z, - const biorbd::utils::Scalar& diameter, - const biorbd::utils::String &name, - const biorbd::utils::String &parentName) : - biorbd::muscles::WrappingObject (x, y, z, name, parentName), - m_dia(std::make_shared(diameter)) +muscles::WrappingSphere::WrappingSphere( + const utils::Scalar& x, + const utils::Scalar& y, + const utils::Scalar& z, + const utils::Scalar& diameter, + const utils::String &name, + const utils::String &parentName) : + muscles::WrappingObject (x, y, z, name, parentName), + m_dia(std::make_shared(diameter)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_SPHERE; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_SPHERE; } -biorbd::muscles::WrappingSphere::WrappingSphere( - const biorbd::utils::Vector3d &v, - const biorbd::utils::Scalar& diameter) : - biorbd::muscles::WrappingObject(v), - m_dia(std::make_shared(diameter)) +muscles::WrappingSphere::WrappingSphere( + const utils::Vector3d &v, + const utils::Scalar& diameter) : + muscles::WrappingObject(v), + m_dia(std::make_shared(diameter)) { - *m_typeOfNode = biorbd::utils::NODE_TYPE::WRAPPING_SPHERE; + *m_typeOfNode = utils::NODE_TYPE::WRAPPING_SPHERE; } -biorbd::muscles::WrappingSphere biorbd::muscles::WrappingSphere::DeepCopy() +muscles::WrappingSphere muscles::WrappingSphere::DeepCopy() const { - biorbd::muscles::WrappingSphere copy; + muscles::WrappingSphere copy; copy.DeepCopy(*this); return copy; } -void biorbd::muscles::WrappingSphere::DeepCopy(const - biorbd::muscles::WrappingSphere &other) +void muscles::WrappingSphere::DeepCopy(const + muscles::WrappingSphere &other) { - biorbd::muscles::WrappingObject::DeepCopy(other); + muscles::WrappingObject::DeepCopy(other); *m_dia = *other.m_dia; } -const biorbd::utils::RotoTrans& biorbd::muscles::WrappingSphere::RT( +const utils::RotoTrans& muscles::WrappingSphere::RT( biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, - const biorbd::rigidbody::GeneralizedCoordinates &, + const rigidbody::GeneralizedCoordinates &, bool) { return *m_RT; } -void biorbd::muscles::WrappingSphere::setDiameter( - const biorbd::utils::Scalar& val) +void muscles::WrappingSphere::setDiameter( + const utils::Scalar& val) { *m_dia = val; } -const biorbd::utils::Scalar& biorbd::muscles::WrappingSphere::diameter() const +const utils::Scalar& muscles::WrappingSphere::diameter() const { return *m_dia; } From ffb1f2630cde9da681783249f0626f4e288d3dd7 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 12:17:16 -0400 Subject: [PATCH 06/14] All modules in namespace --- binding/python3/biorbd_python.i.in | 5 +- include/ModelWriter.h | 7 +- include/Muscles/StaticOptimization.h | 73 +++++------ include/Muscles/StaticOptimizationIpopt.h | 35 +++--- .../StaticOptimizationIpoptLinearized.h | 19 +-- include/RigidBody/KalmanRecons.h | 53 ++++---- include/RigidBody/KalmanReconsIMU.h | 35 +++--- include/RigidBody/KalmanReconsMarkers.h | 43 ++++--- include/Utils/Rotation.h | 12 +- include/Utils/RotoTrans.h | 4 +- src/ModelWriter.cpp | 24 ++-- src/Muscles/StaticOptimization.cpp | 118 +++++++++--------- src/Muscles/StaticOptimizationIpopt.cpp | 78 ++++++------ .../StaticOptimizationIpoptLinearized.cpp | 44 +++---- src/RigidBody/KalmanRecons.cpp | 101 +++++++-------- src/RigidBody/KalmanReconsIMU.cpp | 86 ++++++------- src/RigidBody/KalmanReconsMarkers.cpp | 94 +++++++------- 17 files changed, 429 insertions(+), 402 deletions(-) diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 08aa3454..8007af78 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -76,7 +76,7 @@ // Interpret the string $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::String(PyUnicode_AsUTF8($input)); } else { - PyErr_SetString(PyExc_ValueError, "String must be a biorbd::BIORBD_MATH_NAMESPACE::utils::String or string"); + PyErr_SetString(PyExc_ValueError, "String must be a utils::String or string"); SWIG_fail; } }; @@ -125,8 +125,7 @@ $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints * >(argp1); } else { PyErr_SetString(PyExc_ValueError, - "biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints must be " - "a biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints"); + "rigidbody::Joints must be a rigidbody::Joints"); SWIG_fail; } } diff --git a/include/ModelWriter.h b/include/ModelWriter.h index cd4812ca..4da1061f 100644 --- a/include/ModelWriter.h +++ b/include/ModelWriter.h @@ -4,6 +4,8 @@ #include "biorbdConfig.h" namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ class Model; namespace utils @@ -24,11 +26,12 @@ class BIORBD_API Writer /// \param pathToWrite The path to write /// static void writeModel( - biorbd::Model &model, - const biorbd::utils::Path& pathToWrite); + Model &model, + const utils::Path& pathToWrite); #endif }; +} } #endif // BIORBD_UTILS_WRITER_H diff --git a/include/Muscles/StaticOptimization.h b/include/Muscles/StaticOptimization.h index 6bc713bd..22f96c08 100644 --- a/include/Muscles/StaticOptimization.h +++ b/include/Muscles/StaticOptimization.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ class Model; namespace utils @@ -44,10 +46,10 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, double initialActivationGuess = 0.01, unsigned int pNormFactor = 2, bool useResidualTorque = true, @@ -65,11 +67,11 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, - const biorbd::utils::Vector& initialActivationGuess, + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, + const utils::Vector& initialActivationGuess, unsigned int pNormFactor = 2, bool useResidualTorque = true, int verbose = 0); @@ -86,11 +88,11 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, - const std::vector& initialActivationGuess, + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, + const std::vector& initialActivationGuess, unsigned int pNormFactor = 2, bool useResidualTorque = true, int verbose = 0); @@ -107,10 +109,10 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, + Model& model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, double initialActivationGuess = 0.01, unsigned int pNormFactor = 2, bool useResidualTorque = true, @@ -128,11 +130,11 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, - const biorbd::utils::Vector& initialActivationGuess, + Model& model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, + const utils::Vector& initialActivationGuess, unsigned int pNormFactor = 2, bool useResidualTorque = true, int verbose = 0); @@ -149,11 +151,11 @@ class BIORBD_API StaticOptimization /// \param verbose Level of IPOPT verbose you want /// StaticOptimization( - biorbd::Model& model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, - const std::vector& initialActivationGuess, + Model& model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, + const std::vector& initialActivationGuess, unsigned int pNormFactor = 2, bool useResidualTorque = true, int verbose = 0); @@ -168,24 +170,24 @@ class BIORBD_API StaticOptimization /// \brief Return the final solution /// \return The final solution /// - std::vector finalSolution(); + std::vector finalSolution(); /// /// \brief Return the final solution at a specific index /// \return The final solution at a specific index /// - biorbd::utils::Vector finalSolution(unsigned int index); + utils::Vector finalSolution(unsigned int index); protected: - biorbd::Model& m_model; ///< A reference to the model + Model& m_model; ///< A reference to the model bool m_useResidualTorque; ///< To use residual torque - std::vector + std::vector m_allQ; ///< All the generalized coordinates - std::vector + std::vector m_allQdot; ///< All the generalized velocities - std::vector + std::vector m_allTorqueTarget; ///< All the torque targets - std::shared_ptr + std::shared_ptr m_initialActivationGuess; ///< Initial activation guess unsigned int m_pNormFactor; ///< The p-norm factor int m_verbose; /// m_nbQ; ///< The number of generalized coordinates std::shared_ptr m_nbQdot; ///< The number of generalized velocities @@ -234,22 +236,22 @@ class BIORBD_API StaticOptimizationIpopt : public Ipopt::TNLP std::shared_ptr m_nbTorqueResidual; ///< The number of torque residual std::shared_ptr m_eps; ///< Precision of the finite differentiate - std::shared_ptr m_activations; ///< The activations - std::shared_ptr + std::shared_ptr m_activations; ///< The activations + std::shared_ptr m_Q; ///< The generalized coordinates - std::shared_ptr + std::shared_ptr m_Qdot; ///< The generalized velocities - std::shared_ptr + std::shared_ptr m_torqueTarget; ///< The torque to match - std::shared_ptr + std::shared_ptr m_torqueResidual; ///< The torque residual std::shared_ptr m_torquePonderation; ///< The torque ponderation - std::shared_ptr>> + std::shared_ptr>> m_states; ///< The muscle states std::shared_ptr m_pNormFactor; ///< The p-norm factor std::shared_ptr m_verbose; ///< Verbose level of IPOPT - std::shared_ptr m_finalSolution; ///< The final solution - std::shared_ptr m_finalResidual; ///< The final residual + std::shared_ptr m_finalSolution; ///< The final solution + std::shared_ptr m_finalResidual; ///< The final residual /// /// \brief To dispatch the variables into biorbd format @@ -259,6 +261,7 @@ class BIORBD_API StaticOptimizationIpopt : public Ipopt::TNLP }; +} } } diff --git a/include/Muscles/StaticOptimizationIpoptLinearized.h b/include/Muscles/StaticOptimizationIpoptLinearized.h index e1de3161..0f177afc 100644 --- a/include/Muscles/StaticOptimizationIpoptLinearized.h +++ b/include/Muscles/StaticOptimizationIpoptLinearized.h @@ -6,9 +6,12 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class Matrix; +class Vector; } namespace muscles @@ -19,8 +22,7 @@ namespace muscles /// This algo should be much faster than tradition but less precise (Michaud, 2020) /// /// -class BIORBD_API StaticOptimizationIpoptLinearized : public - biorbd::muscles::StaticOptimizationIpopt +class BIORBD_API StaticOptimizationIpoptLinearized : public StaticOptimizationIpopt { public: /// @@ -36,11 +38,11 @@ class BIORBD_API StaticOptimizationIpoptLinearized : public /// \param eps The precision to perform the finite diffentiation /// StaticOptimizationIpoptLinearized( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, - const biorbd::utils::Vector& activationInit, + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, + const utils::Vector& activationInit, bool useResidual = true, unsigned int pNormFactor = 2, int verbose = 0, @@ -91,11 +93,12 @@ class BIORBD_API StaticOptimizationIpoptLinearized : public Ipopt::Number* values); protected: - std::shared_ptr m_jacobian; ///< The constraints jacobian + std::shared_ptr m_jacobian; ///< The constraints jacobian void prepareJacobian(); ///< Setup the constant constraints jacobian }; +} } } diff --git a/include/RigidBody/KalmanRecons.h b/include/RigidBody/KalmanRecons.h index 447610aa..be267314 100644 --- a/include/RigidBody/KalmanRecons.h +++ b/include/RigidBody/KalmanRecons.h @@ -8,6 +8,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ class Model; namespace utils @@ -16,17 +18,11 @@ class Matrix; class Vector; } -namespace BIORBD_MATH_NAMESPACE { -namespace rigidbody -{ -class GeneralizedAcceleration; -} -} - namespace rigidbody { class GeneralizedCoordinates; class GeneralizedVelocity; +class GeneralizedAcceleration; /// /// \brief Parameters of the reconstruction @@ -87,7 +83,7 @@ class BIORBD_API KalmanRecons /// \param params The Kalman filter parameters /// KalmanRecons( - biorbd::Model& model, + Model& model, unsigned int nbMeasure, KalmanParam params = KalmanParam()); @@ -100,7 +96,7 @@ class BIORBD_API KalmanRecons /// \brief Deep copy of Kalman reconstruction /// \param other The Kalman reconstruction to copy /// - void DeepCopy(const biorbd::rigidbody::KalmanRecons& other); + void DeepCopy(const KalmanRecons& other); /// @@ -110,8 +106,8 @@ class BIORBD_API KalmanRecons /// \param Qddot The generalized accelerations /// void getState( - biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, - biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, + GeneralizedCoordinates *Q = nullptr, + GeneralizedVelocity *Qdot = nullptr, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); /// @@ -121,8 +117,8 @@ class BIORBD_API KalmanRecons /// \param Qddot The generalized accelerations /// void setInitState( - const biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, - const biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, + const GeneralizedCoordinates *Q = nullptr, + const GeneralizedVelocity *Qdot = nullptr, const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); /// @@ -143,7 +139,7 @@ class BIORBD_API KalmanRecons /// \param Te Is equal to \f$\frac{1}{\text{Acquisition frequency}}\f$ /// \return The evolution matrix assuming constant frame rate /// - biorbd::utils::Matrix evolutionMatrix( + utils::Matrix evolutionMatrix( const unsigned int m, unsigned int n, double Te); @@ -154,7 +150,7 @@ class BIORBD_API KalmanRecons /// \param Te Is equal to \f$\frac{1}{\text{Acquisition frequency}}\f$ /// \return The noise matrix /// - biorbd::utils::Matrix processNoiseMatrix( + utils::Matrix processNoiseMatrix( const unsigned int nbQ, double Te); @@ -164,7 +160,7 @@ class BIORBD_API KalmanRecons /// \param val The noise level /// \return The matrix of the noise on the measurements /// - biorbd::utils::Matrix measurementNoiseMatrix( + utils::Matrix measurementNoiseMatrix( const unsigned int nbT, double val); @@ -174,7 +170,7 @@ class BIORBD_API KalmanRecons /// \param val The initial value to fill the matrix with /// \return The initial covariance matrix /// - biorbd::utils::Matrix initCovariance( + utils::Matrix initCovariance( const unsigned int nbQ, double val); @@ -183,7 +179,7 @@ class BIORBD_API KalmanRecons /// \param nbQ The number of degrees-of-freedom /// \return The initialized states /// - biorbd::rigidbody::GeneralizedCoordinates initState( + GeneralizedCoordinates initState( const unsigned int nbQ); /// @@ -194,9 +190,9 @@ class BIORBD_API KalmanRecons /// \param occlusion The vector where occlusionsoccurs /// void iteration( - biorbd::utils::Vector measure, - const biorbd::utils::Vector &projectedMeasure, - const biorbd::utils::Matrix &Hessian, + utils::Vector measure, + const utils::Vector &projectedMeasure, + const utils::Matrix &Hessian, const std::vector &occlusion = std::vector()); /// @@ -206,8 +202,8 @@ class BIORBD_API KalmanRecons /// \param occlusion The vector where occlusions occurs /// virtual void manageOcclusionDuringIteration( - biorbd::utils::Matrix &InvTp, - biorbd::utils::Vector &measure, + utils::Matrix &InvTp, + utils::Vector &measure, const std::vector &occlusion); // Variables attributes @@ -217,15 +213,16 @@ class BIORBD_API KalmanRecons std::shared_ptr m_nMeasure; ///< Number of measurements // Kalman filter attributes - std::shared_ptr m_xp; ///< State vector - std::shared_ptr m_A; ///< Evolution matrix - std::shared_ptr m_Q; ///< Noise matrix - std::shared_ptr + std::shared_ptr m_xp; ///< State vector + std::shared_ptr m_A; ///< Evolution matrix + std::shared_ptr m_Q; ///< Noise matrix + std::shared_ptr m_R; ///< Matrix of the noise on the measurements - std::shared_ptr m_Pp; ///< Covariance matrix + std::shared_ptr m_Pp; ///< Covariance matrix }; +} } } diff --git a/include/RigidBody/KalmanReconsIMU.h b/include/RigidBody/KalmanReconsIMU.h index 33d3c74b..187ded10 100644 --- a/include/RigidBody/KalmanReconsIMU.h +++ b/include/RigidBody/KalmanReconsIMU.h @@ -6,6 +6,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace utils { class RotoTrans; @@ -18,7 +20,7 @@ class IMU; /// /// \brief Class Kinematic reconstruction algorithm using an Extended Kalman Filter for IMU /// -class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons +class BIORBD_API KalmanReconsIMU : public KalmanRecons { public: @@ -34,21 +36,21 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons /// \param params The Kalman filter parameters /// KalmanReconsIMU( - biorbd::Model& model, - biorbd::rigidbody::KalmanParam params = biorbd::rigidbody::KalmanParam(100, + Model& model, + KalmanParam params = KalmanParam(100, 0.005, 1e-10)); /// /// \brief Deep copy of the Kalman reconstruction from inertial measurement units (IMU) data /// \return Copy of the Kalman reconstruction from IMU data /// - biorbd::rigidbody::KalmanReconsIMU DeepCopy() const; + KalmanReconsIMU DeepCopy() const; /// /// \brief Deep copy of a Kalman reconstruction from inertial measurement units (IMU) data /// \param other The Kalman reconstruction to copy /// - void DeepCopy(const biorbd::rigidbody::KalmanReconsIMU& other); + void DeepCopy(const KalmanReconsIMU& other); // Reconstruction of a frame @@ -61,10 +63,10 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons /// \param Qddot The generalized accelerations /// virtual void reconstructFrame( - biorbd::Model &model, - const std::vector &IMUobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, + Model &model, + const std::vector &IMUobs, + GeneralizedCoordinates *Q, + GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); /// @@ -76,10 +78,10 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons /// \param Qddot The generalized accelerations /// virtual void reconstructFrame( - biorbd::Model &model, - const biorbd::utils::Vector &IMUobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, + Model &model, + const utils::Vector &IMUobs, + GeneralizedCoordinates *Q, + GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); /// @@ -106,15 +108,16 @@ class BIORBD_API KalmanReconsIMU : public biorbd::rigidbody::KalmanRecons /// \param occlusion The vector where occlusions occurs /// virtual void manageOcclusionDuringIteration( - biorbd::utils::Matrix &InvTp, - biorbd::utils::Vector &measure, + utils::Matrix &InvTp, + utils::Vector &measure, const std::vector &occlusion); - std::shared_ptr + std::shared_ptr m_PpInitial; ///< Initial covariance matrix std::shared_ptr m_firstIteration; ///< If first iteration was done }; +} } } diff --git a/include/RigidBody/KalmanReconsMarkers.h b/include/RigidBody/KalmanReconsMarkers.h index 3a5e7d37..c4be11e3 100644 --- a/include/RigidBody/KalmanReconsMarkers.h +++ b/include/RigidBody/KalmanReconsMarkers.h @@ -7,6 +7,8 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { class Markers; @@ -15,7 +17,7 @@ class NodeSegment; /// /// \brief Class Kinematic reconstruction algorithm using an Extended Kalman Filter using skin markers /// -class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons +class BIORBD_API KalmanReconsMarkers : public KalmanRecons { public: @@ -32,20 +34,20 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons /// \param params The Kalman filter parameters /// KalmanReconsMarkers( - biorbd::Model& model, - biorbd::rigidbody::KalmanParam params = biorbd::rigidbody::KalmanParam()); + Model& model, + KalmanParam params = KalmanParam()); /// /// \brief Deep copy of the Kalman reconstruction /// \return Copy of the Kalman reconstruction /// - biorbd::rigidbody::KalmanReconsMarkers DeepCopy() const; + KalmanReconsMarkers DeepCopy() const; /// /// \brief Deep copy of the Kalman reconstruction /// \param other The Kalman reconstruction to copy /// - void DeepCopy(const biorbd::rigidbody::KalmanReconsMarkers& other); + void DeepCopy(const KalmanReconsMarkers& other); /// /// \brief Reconstruct the kinematics from markers data @@ -57,10 +59,10 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons /// \param removeAxes If the algo should ignore or not the removeAxis defined in the bioMod file /// virtual void reconstructFrame( - biorbd::Model &model, - const biorbd::rigidbody::Markers &Tobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, + Model &model, + const Markers &Tobs, + GeneralizedCoordinates *Q, + GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes=true); @@ -74,10 +76,10 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons /// \param removeAxes If the algo should ignore or not the removeAxis defined in the bioMod file /// virtual void reconstructFrame( - biorbd::Model &model, - const std::vector &Tobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, + Model &model, + const std::vector &Tobs, + GeneralizedCoordinates *Q, + GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes=true); @@ -91,10 +93,10 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons /// \param removeAxes If the algo should ignore or not the removeAxis defined in the bioMod file /// virtual void reconstructFrame( - biorbd::Model &model, - const biorbd::utils::Vector &Tobs, - biorbd::rigidbody::GeneralizedCoordinates *Q = nullptr, - biorbd::rigidbody::GeneralizedVelocity *Qdot = nullptr, + Model &model, + const utils::Vector &Tobs, + GeneralizedCoordinates *Q = nullptr, + GeneralizedVelocity *Qdot = nullptr, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, bool removeAxes=true); @@ -122,15 +124,16 @@ class BIORBD_API KalmanReconsMarkers : public biorbd::rigidbody::KalmanRecons /// \param occlusion The vector where occlusions occurs /// virtual void manageOcclusionDuringIteration( - biorbd::utils::Matrix& InvTp, - biorbd::utils::Vector &measure, + utils::Matrix& InvTp, + utils::Vector &measure, const std::vector &occlusion); - std::shared_ptr + std::shared_ptr m_PpInitial; ///< Initial covariance matrix std::shared_ptr m_firstIteration; ///< If first iteration was done }; +} } } diff --git a/include/Utils/Rotation.h b/include/Utils/Rotation.h index 2a0cee10..929f54be 100644 --- a/include/Utils/Rotation.h +++ b/include/Utils/Rotation.h @@ -145,11 +145,9 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param axisToRecalculate The axis to recalculate to ensure orthonormal system of axes /// \return The system of axes /// - static Matrix fromMarkersNonNormalized( - const std::pair& - axis1markers, - const std::pair& - axis2markers, + static utils::Matrix fromMarkersNonNormalized( + const std::pair& axis1markers, + const std::pair& axis2markers, const std::pair &axesNames, const String& axisToRecalculate); @@ -187,8 +185,8 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param mToMean The Rotation matrices to mean /// \return The mean Rotation matrix /// - static Rotation mean( - const std::vector& mToMean); + static utils::Rotation mean( + const std::vector& mToMean); #endif #ifndef SWIG diff --git a/include/Utils/RotoTrans.h b/include/Utils/RotoTrans.h index 0da7a0e5..8f64a433 100644 --- a/include/Utils/RotoTrans.h +++ b/include/Utils/RotoTrans.h @@ -221,8 +221,8 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param rt The RotoTrans matrices to mean /// \return The mean RotoTrans /// - static RotoTrans mean( - const std::vector&rt); + static utils::RotoTrans mean( + const std::vector&rt); #endif #ifndef SWIG diff --git a/src/ModelWriter.cpp b/src/ModelWriter.cpp index f795b98d..2b4d50c7 100644 --- a/src/ModelWriter.cpp +++ b/src/ModelWriter.cpp @@ -13,12 +13,14 @@ #include "RigidBody/Mesh.h" #include "RigidBody/SegmentCharacteristics.h" +using namespace biorbd::BIORBD_MATH_NAMESPACE; + #ifndef BIORBD_USE_CASADI_MATH -void biorbd::Writer::writeModel(biorbd::Model & model, - const biorbd::utils::Path& pathToWrite) +void Writer::writeModel(Model & model, + const utils::Path& pathToWrite) { - biorbd::utils::String sep("\t"); // separator in the file - biorbd::utils::String com("//"); // commentaire + utils::String sep("\t"); // separator in the file + utils::String com("//"); // commentaire // Manage the case where the destination folder does not exist if(!pathToWrite.isFolderExist()) { @@ -39,7 +41,7 @@ void biorbd::Writer::writeModel(biorbd::Model & model, biorbdModelFile << std::endl; // Information on the segments - std::vector localJCS = model.localJCS(); + std::vector localJCS = model.localJCS(); for (unsigned int i = 0; i markers (model.marker(model.segment( + std::vector markers (model.marker(model.segment( i).name())); if (markers.size() > 0) { biorbdModelFile << sep << com << " Markers" << std::endl; for (size_t j = 0; j< markers.size(); ++j) { biorbdModelFile << sep << "marker" << sep << - markers[j].biorbd::utils::Node::name() << std::endl; + markers[j].utils::Node::name() << std::endl; biorbdModelFile << sep << sep << "parent" << sep << markers[j].parent() << std::endl; biorbdModelFile << sep << sep << "position" << sep << markers[j].transpose() << @@ -104,11 +106,11 @@ void biorbd::Writer::writeModel(biorbd::Model & model, biorbdModelFile << std::endl; // Write the inertial units - std::vector imus(model.IMU(model.segment(i).name())); + std::vector imus(model.IMU(model.segment(i).name())); if (imus.size() > 0) { biorbdModelFile << sep << com << " Inertial Magnetic Unit" << std::endl; for (size_t j = 0; j< imus.size(); ++j) { - biorbdModelFile << sep << "imu" << sep << imus[j].biorbd::utils::Node::name() << + biorbdModelFile << sep << "imu" << sep << imus[j].utils::Node::name() << std::endl; biorbdModelFile << sep << sep << "parent" << sep << imus[j].parent() << std::endl; @@ -127,13 +129,13 @@ void biorbd::Writer::writeModel(biorbd::Model & model, // Write the custom RT - std::vector rts(model.RTs(model.segment( + std::vector rts(model.RTs(model.segment( i).name())); if (rts.size() > 0) { biorbdModelFile << sep << com << " Custom RT" << std::endl; for (size_t j = 0; j< rts.size(); ++j) { biorbdModelFile << sep << "customRT" << sep << - rts[j].biorbd::utils::Node::name() << std::endl; + rts[j].utils::Node::name() << std::endl; biorbdModelFile << sep << sep << "parent" << sep << rts[j].parent() << std::endl; biorbdModelFile << sep << sep << "RTinMatrix" << sep << true << std::endl; diff --git a/src/Muscles/StaticOptimization.cpp b/src/Muscles/StaticOptimization.cpp index cc10e498..9b760452 100644 --- a/src/Muscles/StaticOptimization.cpp +++ b/src/Muscles/StaticOptimization.cpp @@ -11,18 +11,20 @@ #include "Muscles/StateDynamics.h" #include "Muscles/StaticOptimizationIpoptLinearized.h" -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StaticOptimization::StaticOptimization( + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, double initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, int verbose) : m_model(model), m_useResidualTorque(useResidualTorque), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), @@ -37,18 +39,18 @@ biorbd::muscles::StaticOptimization::StaticOptimization( } } -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, - const biorbd::utils::Vector &initialActivationGuess, +muscles::StaticOptimization::StaticOptimization( + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, + const utils::Vector &initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, int verbose) : m_model(model), m_useResidualTorque(useResidualTorque), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), @@ -60,26 +62,26 @@ biorbd::muscles::StaticOptimization::StaticOptimization( m_allTorqueTarget.push_back(torqueTarget); if (initialActivationGuess.size() != m_model.nbMuscles()) { - biorbd::utils::Error::raise( + utils::Error::raise( "Initial guess must either be a single value or a vector " "of dimension nbMuscles"); } *m_initialActivationGuess = initialActivationGuess; } -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model& model, - const biorbd::rigidbody::GeneralizedCoordinates& Q, - const biorbd::rigidbody::GeneralizedVelocity& Qdot, - const biorbd::rigidbody::GeneralizedTorque& torqueTarget, - const std::vector &initialActivationGuess, +muscles::StaticOptimization::StaticOptimization( + Model& model, + const rigidbody::GeneralizedCoordinates& Q, + const rigidbody::GeneralizedVelocity& Qdot, + const rigidbody::GeneralizedTorque& torqueTarget, + const std::vector &initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, int verbose ) : m_model(model), m_useResidualTorque(useResidualTorque), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), @@ -90,7 +92,7 @@ biorbd::muscles::StaticOptimization::StaticOptimization( m_allTorqueTarget.push_back(torqueTarget); if (initialActivationGuess.size() != m_model.nbMuscles()) { - biorbd::utils::Error::raise( + utils::Error::raise( "Initial guess must either be a single value or a vector " "of dimension nbMuscles"); } @@ -102,11 +104,11 @@ biorbd::muscles::StaticOptimization::StaticOptimization( } } -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model& model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, +muscles::StaticOptimization::StaticOptimization( + Model& model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, double initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, @@ -116,7 +118,7 @@ biorbd::muscles::StaticOptimization::StaticOptimization( m_allQ(allQ), m_allQdot(allQdot), m_allTorqueTarget(allTorqueTarget), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), @@ -127,12 +129,12 @@ biorbd::muscles::StaticOptimization::StaticOptimization( } } -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model &model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, - const biorbd::utils::Vector &initialActivationGuess, +muscles::StaticOptimization::StaticOptimization( + Model &model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, + const utils::Vector &initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, int verbose) : @@ -141,26 +143,26 @@ biorbd::muscles::StaticOptimization::StaticOptimization( m_allQ(allQ), m_allQdot(allQdot), m_allTorqueTarget(allTorqueTarget), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), m_alreadyRun(false) { if (initialActivationGuess.size() != m_model.nbMuscles()) { - biorbd::utils::Error::raise( + utils::Error::raise( "Initial guess must either be a single value or a vector " "of dimension nbMuscles"); } *m_initialActivationGuess = initialActivationGuess; } -biorbd::muscles::StaticOptimization::StaticOptimization( - biorbd::Model& model, - const std::vector& allQ, - const std::vector& allQdot, - const std::vector& allTorqueTarget, - const std::vector& initialActivationGuess, +muscles::StaticOptimization::StaticOptimization( + Model& model, + const std::vector& allQ, + const std::vector& allQdot, + const std::vector& allTorqueTarget, + const std::vector& initialActivationGuess, unsigned int pNormFactor, bool useResidualTorque, int verbose): @@ -169,14 +171,14 @@ biorbd::muscles::StaticOptimization::StaticOptimization( m_allQ(allQ), m_allQdot(allQdot), m_allTorqueTarget(allTorqueTarget), - m_initialActivationGuess(std::make_shared + m_initialActivationGuess(std::make_shared (m_model.nbMuscles())), m_pNormFactor(pNormFactor), m_verbose(verbose), m_alreadyRun(false) { if (initialActivationGuess.size() != m_model.nbMuscles()) { - biorbd::utils::Error::raise( + utils::Error::raise( "Initial guess must either be a single value or a vector " "of dimension nbMuscles"); } @@ -188,7 +190,7 @@ biorbd::muscles::StaticOptimization::StaticOptimization( } } -void biorbd::muscles::StaticOptimization::run( +void muscles::StaticOptimization::run( bool useLinearizedState) { // Setup the Ipopt problem @@ -203,13 +205,13 @@ void biorbd::muscles::StaticOptimization::run( Ipopt::ApplicationReturnStatus status; status = app->Initialize(); - biorbd::utils::Error::check(status == Ipopt::Solve_Succeeded, + utils::Error::check(status == Ipopt::Solve_Succeeded, "Ipopt initialization failed"); for (unsigned int i=0; i( + static_cast( Ipopt::GetRawPtr(m_staticOptimProblem[i]))->finalSolution(); } m_alreadyRun = true; } -std::vector -biorbd::muscles::StaticOptimization::finalSolution() +std::vector +muscles::StaticOptimization::finalSolution() { - std::vector res; + std::vector res; if (!m_alreadyRun) { - biorbd::utils::Error::raise( + utils::Error::raise( "Problem has not been ran through the optimization process " "yet, you should optimize it first to get " "the optimized solution"); } else { for (unsigned int i=0; i( + res.push_back(static_cast( Ipopt::GetRawPtr(m_staticOptimProblem[i]))->finalSolution()); } } @@ -253,17 +255,17 @@ biorbd::muscles::StaticOptimization::finalSolution() return res; } -biorbd::utils::Vector biorbd::muscles::StaticOptimization::finalSolution( +utils::Vector muscles::StaticOptimization::finalSolution( unsigned int index) { - biorbd::utils::Vector res; + utils::Vector res; if (!m_alreadyRun) { - biorbd::utils::Error::raise( + utils::Error::raise( "Problem has not been ran through the optimization process " "yet, you should optimize it first to get " "the optimized solution"); } else { - res = static_cast( + res = static_cast( Ipopt::GetRawPtr(m_staticOptimProblem[index]))->finalSolution(); } return res; diff --git a/src/Muscles/StaticOptimizationIpopt.cpp b/src/Muscles/StaticOptimizationIpopt.cpp index 5f5977f6..fb6e9be2 100644 --- a/src/Muscles/StaticOptimizationIpopt.cpp +++ b/src/Muscles/StaticOptimizationIpopt.cpp @@ -12,12 +12,14 @@ #include "RigidBody/GeneralizedTorque.h" #include "Muscles/StateDynamics.h" -biorbd::muscles::StaticOptimizationIpopt::StaticOptimizationIpopt( - biorbd::Model &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedTorque &torqueTarget, - const biorbd::utils::Vector &activationInit, +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StaticOptimizationIpopt::StaticOptimizationIpopt( + Model &model, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedTorque &torqueTarget, + const utils::Vector &activationInit, bool useResidual, unsigned int pNormFactor, int verbose, @@ -30,29 +32,29 @@ biorbd::muscles::StaticOptimizationIpopt::StaticOptimizationIpopt( m_nbTorque(std::make_shared(model.nbGeneralizedTorque())), m_nbTorqueResidual(std::make_shared(*m_nbQ)), m_eps(std::make_shared(eps)), - m_activations(std::make_shared(activationInit)), - m_Q(std::make_shared(Q)), - m_Qdot(std::make_shared(Qdot)), - m_torqueTarget(std::make_shared + m_activations(std::make_shared(activationInit)), + m_Q(std::make_shared(Q)), + m_Qdot(std::make_shared(Qdot)), + m_torqueTarget(std::make_shared (torqueTarget)), - m_torqueResidual(std::make_shared - (biorbd::utils::Vector::Zero(*m_nbTorque))), + m_torqueResidual(std::make_shared + (utils::Vector::Zero(*m_nbTorque))), m_torquePonderation(std::make_shared(1000)), - m_states(std::make_shared>> + m_states(std::make_shared>> (*m_nbMus)), m_pNormFactor(std::make_shared(pNormFactor)), m_verbose(std::make_shared(verbose)), - m_finalSolution(std::make_shared(biorbd::utils::Vector( + m_finalSolution(std::make_shared(utils::Vector( *m_nbMus))), - m_finalResidual(std::make_shared(biorbd::utils::Vector( + m_finalResidual(std::make_shared(utils::Vector( *m_nbQ))) { if (*m_eps < 1e-12) { - biorbd::utils::Error::raise("epsilon for partial derivates approximation is too small ! \nLimit for epsilon is 1e-12"); + utils::Error::raise("epsilon for partial derivates approximation is too small ! \nLimit for epsilon is 1e-12"); } for (auto& s : *m_states) { - s = std::make_shared(); + s = std::make_shared(); } m_model.updateMuscles(*m_Q, *m_Qdot, true); @@ -63,12 +65,12 @@ biorbd::muscles::StaticOptimizationIpopt::StaticOptimizationIpopt( } } -biorbd::muscles::StaticOptimizationIpopt::~StaticOptimizationIpopt() +muscles::StaticOptimizationIpopt::~StaticOptimizationIpopt() { } -bool biorbd::muscles::StaticOptimizationIpopt::get_nlp_info( +bool muscles::StaticOptimizationIpopt::get_nlp_info( Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, @@ -96,7 +98,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::get_nlp_info( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::get_bounds_info( +bool muscles::StaticOptimizationIpopt::get_bounds_info( Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, @@ -126,7 +128,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::get_bounds_info( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::get_starting_point( +bool muscles::StaticOptimizationIpopt::get_starting_point( Ipopt::Index, bool init_x, Ipopt::Number* x, @@ -160,7 +162,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::get_starting_point( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::eval_f( +bool muscles::StaticOptimizationIpopt::eval_f( Ipopt::Index n, const Ipopt::Number *x, bool new_x, @@ -179,7 +181,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_f( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::eval_grad_f( +bool muscles::StaticOptimizationIpopt::eval_grad_f( Ipopt::Index n, const Ipopt::Number *x, bool new_x, @@ -191,9 +193,9 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_grad_f( dispatch(x); } - biorbd::utils::Vector grad_activ(m_activations->normGradient(*m_pNormFactor, + utils::Vector grad_activ(m_activations->normGradient(*m_pNormFactor, true)); - biorbd::utils::Vector grad_residual(m_torqueResidual->normGradient(2, true)); + utils::Vector grad_residual(m_torqueResidual->normGradient(2, true)); for( unsigned i = 0; i < *m_nbMus; i++ ) { grad_f[i] = grad_activ[i]; @@ -204,7 +206,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_grad_f( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::eval_g( +bool muscles::StaticOptimizationIpopt::eval_g( Ipopt::Index n, const Ipopt::Number *x, bool new_x, @@ -217,7 +219,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_g( dispatch(x); } - const biorbd::rigidbody::GeneralizedTorque& GeneralizedTorqueMusc( + const rigidbody::GeneralizedTorque& GeneralizedTorqueMusc( m_model.muscularJointTorque(*m_states)); for( unsigned int i = 0; i < static_cast(m); i++ ) { @@ -233,7 +235,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_g( return true; } -bool biorbd::muscles::StaticOptimizationIpopt::eval_jac_g( +bool muscles::StaticOptimizationIpopt::eval_jac_g( Ipopt::Index n, const Ipopt::Number *x, bool new_x, @@ -261,21 +263,21 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_jac_g( if (new_x) { dispatch(x); } - const biorbd::rigidbody::GeneralizedTorque& GeneralizedTorqueMusc( + const rigidbody::GeneralizedTorque& GeneralizedTorqueMusc( m_model.muscularJointTorque(*m_states)); unsigned int k(0); for( unsigned int j = 0; j < *m_nbMus; ++j ) { - std::vector> stateEpsilon; + std::vector> stateEpsilon; for (unsigned int i = 0; i < *m_nbMus; ++i) { unsigned int delta(0); if (i == j) { delta = 1; } stateEpsilon.push_back( - std::make_shared( - biorbd::muscles::State(0, (*m_activations)[i]+delta* *m_eps))); + std::make_shared( + muscles::State(0, (*m_activations)[i]+delta* *m_eps))); } - const biorbd::rigidbody::GeneralizedTorque& GeneralizedTorqueCalculEpsilon( + const rigidbody::GeneralizedTorque& GeneralizedTorqueCalculEpsilon( m_model.muscularJointTorque(stateEpsilon)); for( unsigned int i = 0; i < static_cast(m); i++ ) { values[k++] = (GeneralizedTorqueCalculEpsilon[i]-GeneralizedTorqueMusc[i])/ @@ -297,7 +299,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_jac_g( if (*m_verbose >= 2) { k = 0; - biorbd::utils::Matrix jacobian(biorbd::utils::Matrix::Zero(*m_nbTorque, + utils::Matrix jacobian(utils::Matrix::Zero(*m_nbTorque, static_cast(n))); for( unsigned int j = 0; j < *m_nbMus; j++ ) for( unsigned int i = 0; i < static_cast(m); i++ ) { @@ -314,7 +316,7 @@ bool biorbd::muscles::StaticOptimizationIpopt::eval_jac_g( } -void biorbd::muscles::StaticOptimizationIpopt::finalize_solution( +void muscles::StaticOptimizationIpopt::finalize_solution( Ipopt::SolverReturn, Ipopt::Index, const Ipopt::Number *x, @@ -351,19 +353,19 @@ void biorbd::muscles::StaticOptimizationIpopt::finalize_solution( } } -biorbd::utils::Vector biorbd::muscles::StaticOptimizationIpopt::finalSolution() +utils::Vector muscles::StaticOptimizationIpopt::finalSolution() const { return *m_finalSolution; } -biorbd::utils::Vector biorbd::muscles::StaticOptimizationIpopt::finalResidual() +utils::Vector muscles::StaticOptimizationIpopt::finalResidual() const { return *m_finalResidual; } -void biorbd::muscles::StaticOptimizationIpopt::dispatch(const Ipopt::Number *x) +void muscles::StaticOptimizationIpopt::dispatch(const Ipopt::Number *x) { for(unsigned int i = 0; i < *m_nbMus; i++ ) { (*m_activations)[i] = x[i]; diff --git a/src/Muscles/StaticOptimizationIpoptLinearized.cpp b/src/Muscles/StaticOptimizationIpoptLinearized.cpp index 44f166e4..32eca568 100644 --- a/src/Muscles/StaticOptimizationIpoptLinearized.cpp +++ b/src/Muscles/StaticOptimizationIpoptLinearized.cpp @@ -6,39 +6,41 @@ #include "RigidBody/GeneralizedTorque.h" #include "Muscles/State.h" -biorbd::muscles::StaticOptimizationIpoptLinearized::StaticOptimizationIpoptLinearized( - biorbd::Model &model, - const biorbd::rigidbody::GeneralizedCoordinates &Q, - const biorbd::rigidbody::GeneralizedVelocity &Qdot, - const biorbd::rigidbody::GeneralizedTorque &torqueTarget, - const biorbd::utils::Vector &activationInit, +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +muscles::StaticOptimizationIpoptLinearized::StaticOptimizationIpoptLinearized( + Model &model, + const rigidbody::GeneralizedCoordinates &Q, + const rigidbody::GeneralizedVelocity &Qdot, + const rigidbody::GeneralizedTorque &torqueTarget, + const utils::Vector &activationInit, bool useResidual, unsigned int pNormFactor, int verbose, double eps ) : - biorbd::muscles::StaticOptimizationIpopt( + muscles::StaticOptimizationIpopt( model, Q, Qdot, torqueTarget, activationInit, useResidual, pNormFactor, verbose, eps), - m_jacobian(std::make_shared(*m_nbDof, *m_nbMus)) + m_jacobian(std::make_shared(*m_nbDof, *m_nbMus)) { prepareJacobian(); } -void biorbd::muscles::StaticOptimizationIpoptLinearized::prepareJacobian() +void muscles::StaticOptimizationIpoptLinearized::prepareJacobian() { m_model.updateMuscles(*m_Q, *m_Qdot, true); - std::vector> state_zero; + std::vector> state_zero; for (unsigned int i = 0; i<*m_nbMus; ++i) { state_zero.push_back( - std::make_shared( - biorbd::muscles::State(0, 0))); + std::make_shared( + muscles::State(0, 0))); } - const biorbd::rigidbody::GeneralizedTorque& GeneralizedTorque_zero( + const rigidbody::GeneralizedTorque& GeneralizedTorque_zero( m_model.muscularJointTorque(state_zero)); for (unsigned int i = 0; i<*m_nbMus; ++i) { - std::vector> state; + std::vector> state; for (unsigned int j = 0; j<*m_nbMus; ++j) { unsigned int delta; if (j == i) { @@ -47,10 +49,10 @@ void biorbd::muscles::StaticOptimizationIpoptLinearized::prepareJacobian() delta = 0; } state.push_back( - std::make_shared - (biorbd::muscles::State(0, delta*1))); + std::make_shared + (muscles::State(0, delta*1))); } - const biorbd::rigidbody::GeneralizedTorque& GeneralizedTorque( + const rigidbody::GeneralizedTorque& GeneralizedTorque( m_model.muscularJointTorque( state, *m_Q.get(), *m_Qdot.get())); for (unsigned int j = 0; j<*m_nbTorque; ++j) { @@ -60,12 +62,12 @@ void biorbd::muscles::StaticOptimizationIpoptLinearized::prepareJacobian() } } -biorbd::muscles::StaticOptimizationIpoptLinearized::~StaticOptimizationIpoptLinearized() +muscles::StaticOptimizationIpoptLinearized::~StaticOptimizationIpoptLinearized() { } -bool biorbd::muscles::StaticOptimizationIpoptLinearized::eval_g( +bool muscles::StaticOptimizationIpoptLinearized::eval_g( Ipopt::Index n, const Ipopt::Number *x, bool new_x, @@ -78,7 +80,7 @@ bool biorbd::muscles::StaticOptimizationIpoptLinearized::eval_g( dispatch(x); } - biorbd::utils::Vector res(static_cast(m)); + utils::Vector res(static_cast(m)); res.setZero(); // TODO Optimization using Eigen? for( unsigned int i = 0; i < static_cast(m); i++ ) { @@ -96,7 +98,7 @@ bool biorbd::muscles::StaticOptimizationIpoptLinearized::eval_g( return true; } -bool biorbd::muscles::StaticOptimizationIpoptLinearized::eval_jac_g( +bool muscles::StaticOptimizationIpoptLinearized::eval_jac_g( Ipopt::Index, const Ipopt::Number *x, bool new_x, diff --git a/src/RigidBody/KalmanRecons.cpp b/src/RigidBody/KalmanRecons.cpp index 73c7b6f6..96440769 100644 --- a/src/RigidBody/KalmanRecons.cpp +++ b/src/RigidBody/KalmanRecons.cpp @@ -8,43 +8,46 @@ #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedAcceleration.h" -biorbd::rigidbody::KalmanRecons::KalmanRecons() : +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::KalmanRecons::KalmanRecons() : m_params(std::make_shared()), m_Te(std::make_shared(1.0/(m_params->acquisitionFrequency()))), m_nbDof(std::make_shared()), m_nMeasure(std::make_shared()), - m_xp(std::make_shared()), - m_A(std::make_shared()), - m_Q(std::make_shared()), - m_R(std::make_shared()), - m_Pp(std::make_shared()) + m_xp(std::make_shared()), + m_A(std::make_shared()), + m_Q(std::make_shared()), + m_R(std::make_shared()), + m_Pp(std::make_shared()) { } -biorbd::rigidbody::KalmanRecons::KalmanRecons(biorbd::Model &model, +rigidbody::KalmanRecons::KalmanRecons( + Model &model, unsigned int nbMeasure, KalmanParam params) : m_params(std::make_shared(params)), m_Te(std::make_shared(1.0/(m_params->acquisitionFrequency()))), m_nbDof(std::make_shared(model.dof_count)), m_nMeasure(std::make_shared(nbMeasure)), - m_xp(std::make_shared()), - m_A(std::make_shared()), - m_Q(std::make_shared()), - m_R(std::make_shared()), - m_Pp(std::make_shared()) + m_xp(std::make_shared()), + m_A(std::make_shared()), + m_Q(std::make_shared()), + m_R(std::make_shared()), + m_Pp(std::make_shared()) { } -biorbd::rigidbody::KalmanRecons::~KalmanRecons() +rigidbody::KalmanRecons::~KalmanRecons() { } -void biorbd::rigidbody::KalmanRecons::DeepCopy(const - biorbd::rigidbody::KalmanRecons &other) +void rigidbody::KalmanRecons::DeepCopy(const + rigidbody::KalmanRecons &other) { *m_params = *other.m_params; *m_Te = *other.m_Te; @@ -57,32 +60,32 @@ void biorbd::rigidbody::KalmanRecons::DeepCopy(const *m_Pp = *other.m_Pp; } -void biorbd::rigidbody::KalmanRecons::iteration( - biorbd::utils::Vector measure, - const biorbd::utils::Vector &projectedMeasure, - const biorbd::utils::Matrix &Hessian, +void rigidbody::KalmanRecons::iteration( + utils::Vector measure, + const utils::Vector &projectedMeasure, + const utils::Matrix &Hessian, const std::vector &occlusion) { // Prediction - const biorbd::utils::Vector& xkm(*m_A * *m_xp); - const biorbd::utils::Matrix& Pkm(*m_A * *m_Pp * m_A->transpose() + *m_Q); + const utils::Vector& xkm(*m_A * *m_xp); + const utils::Matrix& Pkm(*m_A * *m_Pp * m_A->transpose() + *m_Q); // Correction - biorbd::utils::Matrix InvTp( (Hessian * Pkm * Hessian.transpose() + + utils::Matrix InvTp( (Hessian * Pkm * Hessian.transpose() + *m_R).inverse() ); manageOcclusionDuringIteration(InvTp, measure, occlusion); - const biorbd::utils::Matrix& K(Pkm*Hessian.transpose() * InvTp); // Gain + const utils::Matrix& K(Pkm*Hessian.transpose() * InvTp); // Gain *m_xp = xkm+K*(measure-projectedMeasure); // New estimated state - const RigidBodyDynamics::Math::MatrixNd& temp(biorbd::utils::Matrix::Identity( + const RigidBodyDynamics::Math::MatrixNd& temp(utils::Matrix::Identity( 3* *m_nbDof, 3* *m_nbDof) - K*Hessian); *m_Pp = temp * Pkm * temp.transpose() + K* *m_R*K.transpose(); } -void biorbd::rigidbody::KalmanRecons::manageOcclusionDuringIteration( - biorbd::utils::Matrix &InvTp, - biorbd::utils::Vector &measure, +void rigidbody::KalmanRecons::manageOcclusionDuringIteration( + utils::Matrix &InvTp, + utils::Vector &measure, const std::vector &occlusion) { for (unsigned int i = 0; i < occlusion.size(); ++i) @@ -93,9 +96,9 @@ void biorbd::rigidbody::KalmanRecons::manageOcclusionDuringIteration( } } -void biorbd::rigidbody::KalmanRecons::getState( - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, +void rigidbody::KalmanRecons::getState( + rigidbody::GeneralizedCoordinates *Q, + rigidbody::GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { @@ -113,7 +116,7 @@ void biorbd::rigidbody::KalmanRecons::getState( } -biorbd::utils::Matrix biorbd::rigidbody::KalmanRecons::evolutionMatrix( +utils::Matrix rigidbody::KalmanRecons::evolutionMatrix( const unsigned int nQ, unsigned int n, double Te) @@ -123,7 +126,7 @@ biorbd::utils::Matrix biorbd::rigidbody::KalmanRecons::evolutionMatrix( // Te : 1 / (acquisition frequency) n += 1; - biorbd::utils::Matrix A(biorbd::utils::Matrix::Identity(nQ*n, nQ*n)); + utils::Matrix A(utils::Matrix::Identity(nQ*n, nQ*n)); double c = 1; for (unsigned int i=2; i()), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::KalmanReconsIMU::KalmanReconsIMU() : + rigidbody::KalmanRecons(), + m_PpInitial(std::make_shared()), m_firstIteration(std::make_shared(true)) { } -biorbd::rigidbody::KalmanReconsIMU::KalmanReconsIMU( - biorbd::Model &model, - biorbd::rigidbody::KalmanParam params) : - biorbd::rigidbody::KalmanRecons(model, model.nbTechIMUs()*9, params), - m_PpInitial(std::make_shared()), +rigidbody::KalmanReconsIMU::KalmanReconsIMU( + Model &model, + rigidbody::KalmanParam params) : + rigidbody::KalmanRecons(model, model.nbTechIMUs()*9, params), + m_PpInitial(std::make_shared()), m_firstIteration(std::make_shared(true)) { // Initialize the filter initialize(); } -biorbd::rigidbody::KalmanReconsIMU -biorbd::rigidbody::KalmanReconsIMU::DeepCopy() const +rigidbody::KalmanReconsIMU +rigidbody::KalmanReconsIMU::DeepCopy() const { - biorbd::rigidbody::KalmanReconsIMU copy; + rigidbody::KalmanReconsIMU copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::KalmanReconsIMU::DeepCopy(const - biorbd::rigidbody::KalmanReconsIMU &other) +void rigidbody::KalmanReconsIMU::DeepCopy(const + rigidbody::KalmanReconsIMU &other) { - biorbd::rigidbody::KalmanRecons::DeepCopy(other); + rigidbody::KalmanRecons::DeepCopy(other); *m_PpInitial = *other.m_PpInitial; *m_firstIteration = *other.m_firstIteration; } -void biorbd::rigidbody::KalmanReconsIMU::initialize() +void rigidbody::KalmanReconsIMU::initialize() { - biorbd::rigidbody::KalmanRecons::initialize(); + rigidbody::KalmanRecons::initialize(); // Keep in mind the m_Pp from the start *m_PpInitial = *m_Pp; } -void biorbd::rigidbody::KalmanReconsIMU::manageOcclusionDuringIteration( - biorbd::utils::Matrix &InvTp, - biorbd::utils::Vector &measure, +void rigidbody::KalmanReconsIMU::manageOcclusionDuringIteration( + utils::Matrix &InvTp, + utils::Vector &measure, const std::vector &occlusion) { for (unsigned int i = 0; i < occlusion.size(); ++i) @@ -68,20 +70,20 @@ void biorbd::rigidbody::KalmanReconsIMU::manageOcclusionDuringIteration( } } -bool biorbd::rigidbody::KalmanReconsIMU::first() +bool rigidbody::KalmanReconsIMU::first() { return *m_firstIteration; } -void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame( - biorbd::Model &m, - const std::vector &IMUobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, +void rigidbody::KalmanReconsIMU::reconstructFrame( + Model &m, + const std::vector &IMUobs, + rigidbody::GeneralizedCoordinates *Q, + rigidbody::GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) { // Separate the IMUobs in a big vector - biorbd::utils::Vector T(static_cast + utils::Vector T(static_cast (3*3*IMUobs.size())); // Matrix 3*3 * nbIMU for (unsigned int i=0; iblock(*m_nbDof, 0, *m_nbDof*2, - 1) = biorbd::utils::Vector::Zero(*m_nbDof*2); + 1) = utils::Vector::Zero(*m_nbDof*2); } } // Projected state - const biorbd::utils::Vector& xkm(*m_A * *m_xp); - biorbd::rigidbody::GeneralizedCoordinates Q_tp(xkm.topRows(*m_nbDof)); + const utils::Vector& xkm(*m_A * *m_xp); + rigidbody::GeneralizedCoordinates Q_tp(xkm.topRows(*m_nbDof)); model.UpdateKinematicsCustom (&Q_tp, nullptr, nullptr); // Projected markers - const std::vector& zest_tp = model.technicalIMU(Q_tp, + const std::vector& zest_tp = model.technicalIMU(Q_tp, false); // Jacobian - const std::vector& J_tp = model.TechnicalIMUJacobian( + const std::vector& J_tp = model.TechnicalIMUJacobian( Q_tp, false); // Create only one matrix for zest and Jacobian - biorbd::utils::Matrix H(biorbd::utils::Matrix::Zero(*m_nMeasure, + utils::Matrix H(utils::Matrix::Zero(*m_nMeasure, *m_nbDof*3)); // 3*nCentrales => X,Y,Z ; 3*nbDof => Q, Qdot, Qddot - biorbd::utils::Vector zest(biorbd::utils::Vector::Zero(*m_nMeasure)); + utils::Vector zest(utils::Vector::Zero(*m_nMeasure)); std::vector occlusionIdx; for (unsigned int i=0; i<*m_nMeasure/9; ++i) { - biorbd::utils::Scalar sum = 0; + utils::Scalar sum = 0; for (unsigned int j = 0; j < 9; ++j) { // Calculate the norm for the 9 components sum += IMUobs(i*9+j)*IMUobs(i*9+j); @@ -142,7 +144,7 @@ void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame( if (sum != 0.0 && !std::isnan(sum)) { // If there is an IMU (no zero or NaN) #endif H.block(i*9,0,9,*m_nbDof) = J_tp[i]; - const biorbd::utils::Rotation& rot = zest_tp[i].rot(); + const utils::Rotation& rot = zest_tp[i].rot(); for (unsigned int j = 0; j < 3; ++j) { zest.block(i*9+j*3, 0, 3, 1) = rot.block(0, j, 3, 1); } @@ -157,7 +159,7 @@ void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame( getState(Q, Qdot, Qddot); } -void biorbd::rigidbody::KalmanReconsIMU::reconstructFrame() +void rigidbody::KalmanReconsIMU::reconstructFrame() { - biorbd::utils::Error::raise("Reconstructing kinematics for IMU needs measurements"); + utils::Error::raise("Reconstructing kinematics for IMU needs measurements"); } diff --git a/src/RigidBody/KalmanReconsMarkers.cpp b/src/RigidBody/KalmanReconsMarkers.cpp index ebb4f5dc..09c6cc25 100644 --- a/src/RigidBody/KalmanReconsMarkers.cpp +++ b/src/RigidBody/KalmanReconsMarkers.cpp @@ -13,19 +13,21 @@ #include -biorbd::rigidbody::KalmanReconsMarkers::KalmanReconsMarkers() : - biorbd::rigidbody::KalmanRecons(), - m_PpInitial(std::make_shared()), +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +rigidbody::KalmanReconsMarkers::KalmanReconsMarkers() : + rigidbody::KalmanRecons(), + m_PpInitial(std::make_shared()), m_firstIteration(std::make_shared(true)) { } -biorbd::rigidbody::KalmanReconsMarkers::KalmanReconsMarkers( - biorbd::Model &model, - biorbd::rigidbody::KalmanParam params) : - biorbd::rigidbody::KalmanRecons(model, model.nbTechnicalMarkers()*3, params), - m_PpInitial(std::make_shared()), +rigidbody::KalmanReconsMarkers::KalmanReconsMarkers( + Model &model, + rigidbody::KalmanParam params) : + rigidbody::KalmanRecons(model, model.nbTechnicalMarkers()*3, params), + m_PpInitial(std::make_shared()), m_firstIteration(std::make_shared(true)) { @@ -34,32 +36,32 @@ biorbd::rigidbody::KalmanReconsMarkers::KalmanReconsMarkers( } -biorbd::rigidbody::KalmanReconsMarkers -biorbd::rigidbody::KalmanReconsMarkers::DeepCopy() const +rigidbody::KalmanReconsMarkers +rigidbody::KalmanReconsMarkers::DeepCopy() const { - biorbd::rigidbody::KalmanReconsMarkers copy; + rigidbody::KalmanReconsMarkers copy; copy.DeepCopy(*this); return copy; } -void biorbd::rigidbody::KalmanReconsMarkers::DeepCopy(const - biorbd::rigidbody::KalmanReconsMarkers &other) +void rigidbody::KalmanReconsMarkers::DeepCopy(const + rigidbody::KalmanReconsMarkers &other) { - biorbd::rigidbody::KalmanRecons::DeepCopy(other); + rigidbody::KalmanRecons::DeepCopy(other); *m_PpInitial = *other.m_PpInitial; *m_firstIteration = *other.m_firstIteration; } -void biorbd::rigidbody::KalmanReconsMarkers::initialize() +void rigidbody::KalmanReconsMarkers::initialize() { - biorbd::rigidbody::KalmanRecons::initialize(); + rigidbody::KalmanRecons::initialize(); // Keep in mind the initial m_Pp m_PpInitial = m_Pp; } -void biorbd::rigidbody::KalmanReconsMarkers::manageOcclusionDuringIteration( - biorbd::utils::Matrix &InvTp, +void rigidbody::KalmanReconsMarkers::manageOcclusionDuringIteration( + utils::Matrix &InvTp, utils::Vector &measure, const std::vector &occlusion) { @@ -71,21 +73,21 @@ void biorbd::rigidbody::KalmanReconsMarkers::manageOcclusionDuringIteration( } } -bool biorbd::rigidbody::KalmanReconsMarkers::first() +bool rigidbody::KalmanReconsMarkers::first() { return *m_firstIteration; } -void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame( - biorbd::Model &model, - const biorbd::rigidbody::Markers &Tobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, +void rigidbody::KalmanReconsMarkers::reconstructFrame( + Model &model, + const rigidbody::Markers &Tobs, + rigidbody::GeneralizedCoordinates *Q, + rigidbody::GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector - biorbd::utils::Vector T(3*Tobs.nbMarkers()); + utils::Vector T(3*Tobs.nbMarkers()); for (unsigned int i=0; i &Tobs, - biorbd::rigidbody::GeneralizedCoordinates *Q, - biorbd::rigidbody::GeneralizedVelocity *Qdot, +void rigidbody::KalmanReconsMarkers::reconstructFrame( + Model &model, + const std::vector &Tobs, + rigidbody::GeneralizedCoordinates *Q, + rigidbody::GeneralizedVelocity *Qdot, biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector - biorbd::utils::Vector T(static_cast(3*Tobs.size())); + utils::Vector T(static_cast(3*Tobs.size())); for (unsigned int i=0; iblock(*m_nbDof, 0, *m_nbDof*2, - 1) = biorbd::utils::Vector::Zero( + 1) = utils::Vector::Zero( *m_nbDof*2); // Set velocity and acceleration to zero } } } // Projected state - const biorbd::utils::Vector& xkm(*m_A * *m_xp); - const biorbd::rigidbody::GeneralizedCoordinates& Q_tp(xkm.topRows(*m_nbDof)); + const utils::Vector& xkm(*m_A * *m_xp); + const rigidbody::GeneralizedCoordinates& Q_tp(xkm.topRows(*m_nbDof)); model.UpdateKinematicsCustom (&Q_tp, nullptr, nullptr); // Projected markers - const std::vector& zest_tp( + const std::vector& zest_tp( model.technicalMarkers(Q_tp, removeAxes, false)); // Jacobian - const std::vector& J_tp(model.technicalMarkersJacobian( + const std::vector& J_tp(model.technicalMarkersJacobian( Q_tp, removeAxes, false)); // Create only one matrix for zest and Jacobian - biorbd::utils::Matrix H(biorbd::utils::Matrix::Zero(*m_nMeasure, + utils::Matrix H(utils::Matrix::Zero(*m_nMeasure, *m_nbDof*3)); // 3*nMarkers => X,Y,Z ; 3*nbDof => Q, Qdot, Qddot - biorbd::utils::Vector zest(biorbd::utils::Vector::Zero(*m_nMeasure)); + utils::Vector zest(utils::Vector::Zero(*m_nMeasure)); std::vector occlusionIdx; for (unsigned int i=0; i<*m_nMeasure/3; ++i) // Divided by 3 because we are integrate once xyz @@ -188,7 +190,7 @@ void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame( getState(Q, Qdot, Qddot); } -void biorbd::rigidbody::KalmanReconsMarkers::reconstructFrame() +void rigidbody::KalmanReconsMarkers::reconstructFrame() { - biorbd::utils::Error::raise("Implémentation impossible"); + utils::Error::raise("Implémentation impossible"); } From 17727de7adf0238ebc55d2fc9831ca61055182e7 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 12:35:01 -0400 Subject: [PATCH 07/14] Adapted examples to namespace --- examples/WrappingObjectsExample.cpp | 13 +++++++----- examples/forwardDynamicsExample.cpp | 11 ++++++---- .../forwardDynamicsFromMusclesExample.cpp | 11 ++++++---- examples/forwardKinematicsExample.cpp | 7 +++++-- examples/inverseDynamicsExample.cpp | 11 ++++++---- examples/inverseKinematicsKalmanExample.cpp | 21 +++++++++++-------- examples/staticOptimizationExample.cpp | 13 +++++++----- 7 files changed, 54 insertions(+), 33 deletions(-) diff --git a/examples/WrappingObjectsExample.cpp b/examples/WrappingObjectsExample.cpp index 2d5ae2f9..f0118e14 100644 --- a/examples/WrappingObjectsExample.cpp +++ b/examples/WrappingObjectsExample.cpp @@ -12,21 +12,24 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("WrappingObjectExample.bioMod"); + Model model("WrappingObjectExample.bioMod"); // Choose a position/velocity to compute dynamics from - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); Q = Q.setOnes()/10; Qdot.setZero(); // Set all muscles to half of their maximal activation - std::vector> states; + std::vector> states; for (unsigned int i=0; i(0, 0.5)); + states.push_back(std::make_shared(0, 0.5)); } // Proceed with the computation of joint torque from the muscles diff --git a/examples/forwardDynamicsExample.cpp b/examples/forwardDynamicsExample.cpp index f9513ef7..846e9ee6 100644 --- a/examples/forwardDynamicsExample.cpp +++ b/examples/forwardDynamicsExample.cpp @@ -12,15 +12,18 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Choose a position/velocity/torque to compute dynamics from - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); Q.setZero(); Qdot.setZero(); Tau.setZero(); diff --git a/examples/forwardDynamicsFromMusclesExample.cpp b/examples/forwardDynamicsFromMusclesExample.cpp index dc305f81..503a0949 100644 --- a/examples/forwardDynamicsFromMusclesExample.cpp +++ b/examples/forwardDynamicsFromMusclesExample.cpp @@ -13,19 +13,22 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("arm26.bioMod"); + Model model("arm26.bioMod"); // Choose a position/velocity to compute dynamics from - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); Q.setZero(); Qdot.setZero(); // Set all muscles to half of their maximal activation - std::vector> states = model.stateSet(); + std::vector> states = model.stateSet(); for (unsigned int i=0; isetExcitation(0); states[i]->setActivation(0.5); diff --git a/examples/forwardKinematicsExample.cpp b/examples/forwardKinematicsExample.cpp index 7bdb18be..3a89c97c 100644 --- a/examples/forwardKinematicsExample.cpp +++ b/examples/forwardKinematicsExample.cpp @@ -12,13 +12,16 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Choose a position to get the markers from - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); Q.setZero(); // Proceed with the forward kinematics diff --git a/examples/inverseDynamicsExample.cpp b/examples/inverseDynamicsExample.cpp index acc0d861..0b644f8d 100644 --- a/examples/inverseDynamicsExample.cpp +++ b/examples/inverseDynamicsExample.cpp @@ -12,15 +12,18 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Choose a position/velocity/acceleration to compute dynamics from - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedAcceleration Qddot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedAcceleration Qddot(model); Q.setZero(); Qdot.setZero(); Qddot.setZero(); diff --git a/examples/inverseKinematicsKalmanExample.cpp b/examples/inverseKinematicsKalmanExample.cpp index 33426bfb..ad68f1aa 100644 --- a/examples/inverseKinematicsKalmanExample.cpp +++ b/examples/inverseKinematicsKalmanExample.cpp @@ -14,31 +14,34 @@ /// Please note that this example will work only with the Eigen backend. /// Please also note that kalman will be VERY slow if compiled in debug /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Generate random data (3 frames) - biorbd::rigidbody::GeneralizedCoordinates targetQ(model); + rigidbody::GeneralizedCoordinates targetQ(model); targetQ.setZero(); std::cout << "Target Q = " << targetQ.transpose() << std::endl; - std::vector targetMarkers = model.markers( + std::vector targetMarkers = model.markers( targetQ); - std::vector< std::vector > markersOverFrames; + std::vector< std::vector > markersOverFrames; markersOverFrames.push_back(targetMarkers); markersOverFrames.push_back(targetMarkers); markersOverFrames.push_back(targetMarkers); // Create a Kalman filter double freq = 100; // 100 Hz - biorbd::rigidbody::KalmanParam params(freq); - biorbd::rigidbody::KalmanReconsMarkers kalman(model, params); + rigidbody::KalmanParam params(freq); + rigidbody::KalmanReconsMarkers kalman(model, params); // Perform the kalman filter for each frame (the first frame is much longer than the next) - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedAcceleration Qddot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedAcceleration Qddot(model); for (auto targetMarkers : markersOverFrames) { kalman.reconstructFrame(model, targetMarkers, &Q, &Qdot, &Qddot); diff --git a/examples/staticOptimizationExample.cpp b/examples/staticOptimizationExample.cpp index 91ff6af7..01e52580 100644 --- a/examples/staticOptimizationExample.cpp +++ b/examples/staticOptimizationExample.cpp @@ -14,23 +14,26 @@ /// /// Please note that this example will work only with the Eigen backend /// + +using namespace biorbd::BIORBD_MATH_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("arm26.bioMod"); + Model model("arm26.bioMod"); // Choose a position/velocity/torque to compute muscle activations from // If multiple frames, one can use std::vector of corresponding Generalized // to run them all. - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); Q.setZero(); Qdot.setZero(); Tau.setZero(); // Proceed with the static optimization - auto optim = biorbd::muscles::StaticOptimization(model, Q, Qdot, Tau); + auto optim = muscles::StaticOptimization(model, Q, Qdot, Tau); optim.run(); auto muscleActivationsPerFrame = optim.finalSolution(); From 8770da7222b0f70781de13798a260dc3c7638106 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 12:40:38 -0400 Subject: [PATCH 08/14] Adapted the tests --- include/biorbdConfig.h.in | 4 +- test/test_actuators.cpp | 34 +- test/test_biorbd.cpp | 28 +- test/test_muscles.cpp | 1084 +++++++++++++++++++------------------ test/test_rigidbody.cpp | 355 ++++++------ test/test_utils.cpp | 416 +++++++------- 6 files changed, 965 insertions(+), 956 deletions(-) diff --git a/include/biorbdConfig.h.in b/include/biorbdConfig.h.in index 6e5bedd8..2660ccde 100644 --- a/include/biorbdConfig.h.in +++ b/include/biorbdConfig.h.in @@ -89,7 +89,7 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname##_sym(casadi::MX::sym(#varname, model.nbGeneralizedTorque(), 1)); #define DECLARE_VECTOR(varname, nbElements) \ casadi::DM varname(nbElements, 1);\ - biorbd::utils::Vector varname##_sym(casadi::MX::sym(#varname, nbElements, 1)); + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector varname##_sym(casadi::MX::sym(#varname, nbElements, 1)); #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \ casadi::Function func_##funcname(#funcname, {arg1##_sym}, {model.funcname(arg1##_sym)}, {#arg1}, {#funcname});\ @@ -123,7 +123,7 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ #define DECLARE_GENERALIZED_TORQUE(varname, model) \ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname(model); #define DECLARE_VECTOR(varname, nbElements) \ - biorbd::utils::Vector varname(nbElements); + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector varname(nbElements); #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \ auto varname = model.funcname(arg1); diff --git a/test/test_actuators.cpp b/test/test_actuators.cpp index 7ddba243..eaf6c12e 100644 --- a/test/test_actuators.cpp +++ b/test/test_actuators.cpp @@ -15,6 +15,8 @@ #include "Actuators/ActuatorLinear.h" #include "Actuators/ActuatorSigmoidGauss3p.h" +using namespace biorbd::BIORBD_MATH_NAMESPACE; + static std::string modelPathForGeneralTesting("models/pyomecaman_withActuators.bioMod"); static std::string @@ -28,16 +30,16 @@ static double requiredPrecision(1e-10); TEST(FileIO, openModelWithActuators) { - EXPECT_NO_THROW(biorbd::Model model(modelPathForGeneralTesting)); - EXPECT_NO_THROW(biorbd::Model model(modelPathWithoutActuator)); - EXPECT_THROW(biorbd::Model model(modelPathWithMissingActuator), + EXPECT_NO_THROW(Model model(modelPathForGeneralTesting)); + EXPECT_NO_THROW(Model model(modelPathWithoutActuator)); + EXPECT_THROW(Model model(modelPathWithMissingActuator), std::runtime_error); - EXPECT_NO_THROW(biorbd::Model model(modelPathWithAllActuators)); + EXPECT_NO_THROW(Model model(modelPathWithAllActuators)); } TEST(ActuatorConstant, torqueMax) { - biorbd::actuator::ActuatorConstant const_torque_act(1, 150, 0); + actuator::ActuatorConstant const_torque_act(1, 150, 0); SCALAR_TO_DOUBLE(torqueMaxVal, const_torque_act.torqueMax()) EXPECT_NEAR(torqueMaxVal, 150, requiredPrecision); } @@ -45,11 +47,11 @@ TEST(ActuatorConstant, torqueMax) TEST(ActuatorGauss3p, torqueMax) { // A model is loaded so Q can be > 0 in size, it is not used otherwise - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); - biorbd::actuator::ActuatorGauss3p gauss3p_torque_act(1, 150, 25, 800, 324, 0.5, + actuator::ActuatorGauss3p gauss3p_torque_act(1, 150, 25, 800, 324, 0.5, 28, 90, 29, 133, 0); std::vector Q_val = {1.1, 1.1, 1.1, 1.1, 1.1}; FILL_VECTOR(Q, Q_val); @@ -84,11 +86,11 @@ TEST(ActuatorGauss3p, torqueMax) TEST(ActuatorGauss6p, torqueMax) { // A model is loaded so Q can be > 0 in size, it is not used otherwise - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); - biorbd::actuator::ActuatorGauss6p gauss6p_torque_act(1, 150, 25, 800, 324, 0.5, + actuator::ActuatorGauss6p gauss6p_torque_act(1, 150, 25, 800, 324, 0.5, 28, 90, 29, 133, 4, 73, 73, 0); std::vector Q_val = {1.1, 1.1, 1.1, 1.1, 1.1}; FILL_VECTOR(Q, Q_val); @@ -123,13 +125,13 @@ TEST(ActuatorGauss6p, torqueMax) TEST(ActuatorLinear, torqueMax) { // A model is loaded so Q can be > 0 in size, it is not used otherwise - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); std::vector val = {1.1}; FILL_VECTOR(Q, val); double torqueMaxExpected(88.025357464390567); - biorbd::actuator::ActuatorLinear linear_torque_act(1, 25, 1, 0); + actuator::ActuatorLinear linear_torque_act(1, 25, 1, 0); CALL_BIORBD_FUNCTION_1ARG(torqueMaxVal, linear_torque_act, torqueMax, Q); #ifdef BIORBD_USE_CASADI_MATH EXPECT_NEAR(static_cast(torqueMaxVal(0, 0)), torqueMaxExpected, @@ -142,12 +144,12 @@ TEST(ActuatorLinear, torqueMax) TEST(Actuators, NbActuators) { { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); size_t val(model.nbActuators()); EXPECT_NEAR(val, 13, requiredPrecision); } { - biorbd::Model model(modelPathWithoutActuator); + Model model(modelPathWithoutActuator); size_t val(model.nbActuators()); EXPECT_NEAR(val, 0, requiredPrecision); } @@ -155,7 +157,7 @@ TEST(Actuators, NbActuators) TEST(Actuators, jointTorqueFromAllTypesOfActuators) { - biorbd::Model model(modelPathWithAllActuators); + Model model(modelPathWithAllActuators); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); DECLARE_VECTOR(actuatorActivations, model.nbGeneralizedTorque()); @@ -311,11 +313,11 @@ TEST(Actuators, jointTorqueFromAllTypesOfActuators) TEST(ActuatorSigmoidGauss3p, torqueMax) { // A model is loaded so Q can be > 0 in size, it is not used otherwise - biorbd::Model model(modelPathWithAllActuators); + Model model(modelPathWithAllActuators); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); - biorbd::actuator::ActuatorSigmoidGauss3p sigmoid_gauss3p_torque_act(1, + actuator::ActuatorSigmoidGauss3p sigmoid_gauss3p_torque_act(1, 312.0780851217, 0.0100157340, 3.2702903919, 56.4021127893, -25.6939435543, 0); std::vector Q_val(model.nbQ()); diff --git a/test/test_biorbd.cpp b/test/test_biorbd.cpp index b0b02d4a..eb46e804 100644 --- a/test/test_biorbd.cpp +++ b/test/test_biorbd.cpp @@ -16,6 +16,8 @@ #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedTorque.h" +using namespace biorbd::BIORBD_MATH_NAMESPACE; + static double requiredPrecision(1e-10); static std::string modelPathWithMeshFile("models/simpleWithMeshFile.bioMod"); @@ -34,19 +36,19 @@ static std::string modelPathWithObj("models/violin.bioMod"); TEST(FileIO, OpenModel) { - EXPECT_NO_THROW(biorbd::Model model(modelPathForGeneralTesting)); + EXPECT_NO_THROW(Model model(modelPathForGeneralTesting)); } #ifndef BIORBD_USE_CASADI_MATH TEST(FileIO, WriteModel) { - biorbd::Model model("models/two_segments.bioMod"); - biorbd::utils::String savePath("temporary.bioMod"); - biorbd::Writer::writeModel(model, savePath); - biorbd::Model modelCopy(savePath); + Model model("models/two_segments.bioMod"); + utils::String savePath("temporary.bioMod"); + Writer::writeModel(model, savePath); + Model modelCopy(savePath); // Test if the model is properly written - biorbd::rigidbody::GeneralizedCoordinates Q(modelCopy); + rigidbody::GeneralizedCoordinates Q(modelCopy); Q.setOnes(); for (unsigned int k=0; k(idealizedActuator.position()).setOrigin( + const_cast(idealizedActuator.position()).setOrigin( newPosition); - const_cast + const_cast (idealizedActuator.position()).setInsertionInLocal(newPosition); - const biorbd::utils::Vector3d& origin = + const utils::Vector3d& origin = idealizedActuator.position().originInLocal(); - const biorbd::utils::Vector3d& insertion = + const 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()); + EXPECT_STREQ(origin.utils::Node::name().c_str(), "TRImed_origin"); + EXPECT_STREQ(insertion.utils::Node::name().c_str(), oldName.c_str()); } { - const_cast(idealizedActuator.position()).setOrigin( + const_cast(idealizedActuator.position()).setOrigin( newNode); - const_cast + const_cast (idealizedActuator.position()).setInsertionInLocal(newNode); - const biorbd::utils::Vector3d& origin = + const utils::Vector3d& origin = idealizedActuator.position().originInLocal(); - const biorbd::utils::Vector3d& insertion = + const 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()); + EXPECT_STREQ(origin.utils::Node::name().c_str(), newName.c_str()); + EXPECT_STREQ(insertion.utils::Node::name().c_str(), newName.c_str()); } } } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::IdealizedActuator idealizedActuator( + Model model(modelPathForMuscleForce); + muscles::IdealizedActuator idealizedActuator( model.muscleGroup(muscleGroupForIdealizedActuator).muscle( muscleForIdealizedActuator)); - biorbd::muscles::IdealizedActuator shallowCopy(idealizedActuator); - biorbd::muscles::IdealizedActuator deepCopyNow(idealizedActuator.DeepCopy()); - biorbd::muscles::IdealizedActuator deepCopyLater; + muscles::IdealizedActuator shallowCopy(idealizedActuator); + muscles::IdealizedActuator deepCopyNow(idealizedActuator.DeepCopy()); + muscles::IdealizedActuator deepCopyLater; deepCopyLater.DeepCopy(idealizedActuator); { @@ -382,7 +384,7 @@ TEST(IdealizedActuator, copy) EXPECT_NEAR(deepCopyLaterExcitation, 0., requiredPrecision); } - idealizedActuator.state().setExcitation(biorbd::utils::Scalar(5.)); + idealizedActuator.state().setExcitation(utils::Scalar(5.)); { SCALAR_TO_DOUBLE(excitation, idealizedActuator.state().excitation()); @@ -403,8 +405,8 @@ static unsigned int muscleForHillType(1); TEST(hillType, unitTest) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); hillType.setName("newName"); @@ -412,17 +414,17 @@ TEST(hillType, unitTest) } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; hillType.updateOrientations(model, Q, qDot); static double activationEmgForHillTypeTest(1); - biorbd::muscles::StateDynamics emg(0, activationEmgForHillTypeTest); + muscles::StateDynamics emg(0, activationEmgForHillTypeTest); SCALAR_TO_DOUBLE(flce, hillType.FlCE(emg)); SCALAR_TO_DOUBLE(flpe, hillType.FlPE()); @@ -436,61 +438,61 @@ TEST(hillType, unitTest) EXPECT_NEAR(force, 424.95358302550062, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType originalHillType( + Model model(modelPathForMuscleForce); + muscles::HillType originalHillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::muscles::HillType newHillType( + muscles::HillType newHillType( "newName", originalHillType.position(), originalHillType.characteristics()); EXPECT_STREQ(newHillType.name().c_str(), "newName"); - EXPECT_EQ(newHillType.type(), biorbd::muscles::MUSCLE_TYPE::HILL); + EXPECT_EQ(newHillType.type(), muscles::MUSCLE_TYPE::HILL); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType originalHillType( + Model model(modelPathForMuscleForce); + muscles::HillType originalHillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::muscles::HillType newHillType( + muscles::HillType newHillType( "newName", originalHillType.position(), originalHillType.characteristics(), originalHillType.state()); EXPECT_STREQ(newHillType.name().c_str(), "newName"); - EXPECT_EQ(newHillType.type(), biorbd::muscles::MUSCLE_TYPE::HILL); + EXPECT_EQ(newHillType.type(), muscles::MUSCLE_TYPE::HILL); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType originalHillType( + Model model(modelPathForMuscleForce); + muscles::HillType originalHillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::muscles::HillType newHillType( + muscles::HillType newHillType( "newName", originalHillType.position(), originalHillType.characteristics(), originalHillType.pathModifier()); EXPECT_STREQ(newHillType.name().c_str(), "newName"); - EXPECT_EQ(newHillType.type(), biorbd::muscles::MUSCLE_TYPE::HILL); + EXPECT_EQ(newHillType.type(), muscles::MUSCLE_TYPE::HILL); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; static double activationEmgForHillTypeTest(1); - biorbd::muscles::StateDynamics emg(0, activationEmgForHillTypeTest); + muscles::StateDynamics emg(0, activationEmgForHillTypeTest); SCALAR_TO_DOUBLE(force, hillType.force(model, Q, qDot, emg, 2)); EXPECT_NEAR(force, 424.95358302550062, requiredPrecision); @@ -500,24 +502,24 @@ TEST(hillType, unitTest) TEST(hillType, copy) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); Q = Q.setOnes() / 10; - biorbd::muscles::HillType shallowCopy(hillType); - biorbd::muscles::HillType deepCopyNow(hillType.DeepCopy()); - biorbd::muscles::HillType deepCopyLater; + muscles::HillType shallowCopy(hillType); + muscles::HillType deepCopyNow(hillType.DeepCopy()); + muscles::HillType deepCopyLater; deepCopyLater.DeepCopy(hillType); - biorbd::utils::String originalName(hillType.name()); + utils::String originalName(hillType.name()); EXPECT_STREQ(shallowCopy.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyNow.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); - biorbd::utils::String newName("MyNewMuscleName"); + utils::String newName("MyNewMuscleName"); hillType.setName(newName); EXPECT_STREQ(hillType.name().c_str(), newName.c_str()); EXPECT_STREQ(shallowCopy.name().c_str(), newName.c_str()); @@ -525,15 +527,15 @@ TEST(hillType, copy) EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); //copies of the Hill Type muscle - biorbd::muscles::HillType shallowCopy(hillType); - biorbd::muscles::HillType deepCopyNow(hillType.DeepCopy()); - biorbd::muscles::HillType deepCopyLater; + muscles::HillType shallowCopy(hillType); + muscles::HillType deepCopyNow(hillType.DeepCopy()); + muscles::HillType deepCopyLater; deepCopyLater.DeepCopy(hillType); { @@ -544,7 +546,7 @@ TEST(hillType, copy) EXPECT_NEAR(shallowCopyPennationAngle, 0.15707963, requiredPrecision); } - biorbd::muscles::Characteristics charac(hillType.characteristics()); + muscles::Characteristics charac(hillType.characteristics()); charac.setPennationAngle(0.523599); { SCALAR_TO_DOUBLE(pennationAngle, hillType.characteristics().pennationAngle()); @@ -562,19 +564,19 @@ TEST(hillType, copy) } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; hillType.updateOrientations(model, Q); - biorbd::muscles::HillType shallowCopy(hillType); - biorbd::muscles::HillType deepCopyNow(hillType.DeepCopy()); - biorbd::muscles::HillType deepCopyLater; + muscles::HillType shallowCopy(hillType); + muscles::HillType deepCopyNow(hillType.DeepCopy()); + muscles::HillType deepCopyLater; deepCopyLater.DeepCopy(hillType); { @@ -589,12 +591,12 @@ TEST(hillType, copy) } // Change the position of the insertion and pennation angle and compare again (length and insertion in Local) - biorbd::muscles::Characteristics charac(hillType.characteristics()); + muscles::Characteristics charac(hillType.characteristics()); charac.setPennationAngle(0.523599); - biorbd::utils::Vector3d insertion(hillType.position().insertionInLocal()); + utils::Vector3d insertion(hillType.position().insertionInLocal()); insertion.set(0.5, 0.6, 0.7); - biorbd::utils::String oldName(insertion.biorbd::utils::Node::name()); - biorbd::utils::String newName("MyNewName"); + utils::String oldName(insertion.utils::Node::name()); + utils::String newName("MyNewName"); insertion.setName(newName); hillType.updateOrientations(model, Q, qDot, 2); @@ -608,26 +610,26 @@ TEST(hillType, copy) EXPECT_NEAR(shallowCopyLength, 0.07570761027741163, requiredPrecision); EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision); EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision); - EXPECT_EQ(hillType.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(hillType.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(shallowCopy.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(deepCopyNow.position().insertionInLocal().utils::Node::name(), oldName); EXPECT_EQ( - deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), + deepCopyLater.position().insertionInLocal().utils::Node::name(), oldName); } } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillType hillType( + Model model(modelPathForMuscleForce); + muscles::HillType hillType( model.muscleGroup(muscleGroupForHillType).muscle( muscleForHillType)); - biorbd::muscles::HillType shallowCopy(hillType); - biorbd::muscles::HillType deepCopyNow(hillType.DeepCopy()); - biorbd::muscles::HillType deepCopyLater; + muscles::HillType shallowCopy(hillType); + muscles::HillType deepCopyNow(hillType.DeepCopy()); + muscles::HillType deepCopyLater; deepCopyLater.DeepCopy(hillType); { @@ -641,7 +643,7 @@ TEST(hillType, copy) EXPECT_NEAR(deepCopyLaterExcitation, 0., requiredPrecision); } - hillType.state().setExcitation(biorbd::utils::Scalar(5.)); + hillType.state().setExcitation(utils::Scalar(5.)); { SCALAR_TO_DOUBLE(excitation, hillType.state().excitation()); @@ -662,26 +664,26 @@ static unsigned int muscleForHillThelenType(1); TEST(hillThelenType, unitTest) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); hillThelenType.setName("newName"); EXPECT_STREQ(hillThelenType.name().c_str(), "newName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; model.updateMuscles(Q, 2); hillThelenType.updateOrientations(model, Q, qDot); static double activationEmgForHillTypeTest(1.0); - biorbd::muscles::StateDynamics emg(0, activationEmgForHillTypeTest); + muscles::StateDynamics emg(0, activationEmgForHillTypeTest); SCALAR_TO_DOUBLE(flce, hillThelenType.FlCE(emg)); SCALAR_TO_DOUBLE(flpe, hillThelenType.FlPE()); @@ -695,72 +697,72 @@ TEST(hillThelenType, unitTest) EXPECT_NEAR(force, 424.95358302550062, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenType hillThelenTypeNew( + muscles::HillThelenType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics()); EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); - EXPECT_EQ(hillThelenTypeNew.type(), biorbd::muscles::MUSCLE_TYPE::HILL_THELEN); + EXPECT_EQ(hillThelenTypeNew.type(), muscles::MUSCLE_TYPE::HILL_THELEN); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenType hillThelenTypeNew( + muscles::HillThelenType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics(), hillThelenType.state()); EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); - EXPECT_EQ(hillThelenTypeNew.type(), biorbd::muscles::MUSCLE_TYPE::HILL_THELEN); + EXPECT_EQ(hillThelenTypeNew.type(), muscles::MUSCLE_TYPE::HILL_THELEN); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenType hillThelenTypeNew( + muscles::HillThelenType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics(), hillThelenType.pathModifier()); EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); - EXPECT_EQ(hillThelenTypeNew.type(), biorbd::muscles::MUSCLE_TYPE::HILL_THELEN); + EXPECT_EQ(hillThelenTypeNew.type(), muscles::MUSCLE_TYPE::HILL_THELEN); } } TEST(hillThelenType, copy) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); Q = Q.setOnes() / 10; - biorbd::muscles::HillThelenType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenType deepCopyLater; + muscles::HillThelenType shallowCopy(hillThelenType); + muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); + muscles::HillThelenType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); - biorbd::utils::String originalName(hillThelenType.name()); + utils::String originalName(hillThelenType.name()); EXPECT_STREQ(shallowCopy.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyNow.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); - biorbd::utils::String newName("MyNewMuscleName"); + utils::String newName("MyNewMuscleName"); hillThelenType.setName(newName); EXPECT_STREQ(hillThelenType.name().c_str(), newName.c_str()); EXPECT_STREQ(shallowCopy.name().c_str(), newName.c_str()); @@ -768,14 +770,14 @@ TEST(hillThelenType, copy) EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenType deepCopyLater; + muscles::HillThelenType shallowCopy(hillThelenType); + muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); + muscles::HillThelenType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -787,7 +789,7 @@ TEST(hillThelenType, copy) EXPECT_NEAR(shallowCopyPennationAngle, 0.15707963, requiredPrecision); } - biorbd::muscles::Characteristics charac(hillThelenType.characteristics()); + muscles::Characteristics charac(hillThelenType.characteristics()); charac.setPennationAngle(0.523599); { @@ -807,19 +809,19 @@ TEST(hillThelenType, copy) } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; hillThelenType.updateOrientations(model, Q); - biorbd::muscles::HillThelenType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenType deepCopyLater; + muscles::HillThelenType shallowCopy(hillThelenType); + muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); + muscles::HillThelenType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -834,12 +836,12 @@ TEST(hillThelenType, copy) } // Change the position of the insertion and pennation angle and compare again (length and insertion in Local) - biorbd::muscles::Characteristics charac(hillThelenType.characteristics()); + muscles::Characteristics charac(hillThelenType.characteristics()); charac.setPennationAngle(0.523599); - biorbd::utils::Vector3d insertion(hillThelenType.position().insertionInLocal()); + utils::Vector3d insertion(hillThelenType.position().insertionInLocal()); insertion.set(0.5, 0.6, 0.7); - biorbd::utils::String oldName(insertion.biorbd::utils::Node::name()); - biorbd::utils::String newName("MyNewName"); + utils::String oldName(insertion.utils::Node::name()); + utils::String newName("MyNewName"); insertion.setName(newName); hillThelenType.updateOrientations(model, Q, qDot, 2); @@ -853,26 +855,26 @@ TEST(hillThelenType, copy) EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision); EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision); EXPECT_EQ( - hillThelenType.position().insertionInLocal().biorbd::utils::Node::name(), + hillThelenType.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(shallowCopy.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(deepCopyNow.position().insertionInLocal().utils::Node::name(), oldName); EXPECT_EQ( - deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), + deepCopyLater.position().insertionInLocal().utils::Node::name(), oldName); } } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenType deepCopyLater; + muscles::HillThelenType shallowCopy(hillThelenType); + muscles::HillThelenType deepCopyNow(hillThelenType.DeepCopy()); + muscles::HillThelenType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -886,7 +888,7 @@ TEST(hillThelenType, copy) EXPECT_NEAR(deepCopyLaterExcitation, 0., requiredPrecision); } - hillThelenType.state().setExcitation(biorbd::utils::Scalar(5.)); + hillThelenType.state().setExcitation(utils::Scalar(5.)); { SCALAR_TO_DOUBLE(excitation, hillThelenType.state().excitation()); @@ -904,26 +906,26 @@ TEST(hillThelenType, copy) TEST(hillThelenTypeActive, unitTest) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); hillThelenType.setName("newName"); EXPECT_STREQ(hillThelenType.name().c_str(), "newName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; model.updateMuscles(Q, 2); hillThelenType.updateOrientations(model, Q, qDot); static double activationEmgForHillTypeTest(1.0); - biorbd::muscles::StateDynamics emg(0, activationEmgForHillTypeTest); + muscles::StateDynamics emg(0, activationEmgForHillTypeTest); SCALAR_TO_DOUBLE(flce, hillThelenType.FlCE(emg)); SCALAR_TO_DOUBLE(flpe, hillThelenType.FlPE()); @@ -937,27 +939,27 @@ TEST(hillThelenTypeActive, unitTest) EXPECT_NEAR(force, 424.83162852148627, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenActiveOnlyType hillThelenTypeNew( + muscles::HillThelenActiveOnlyType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics()); EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); + muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenActiveOnlyType hillThelenTypeNew( + muscles::HillThelenActiveOnlyType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics(), @@ -965,15 +967,15 @@ TEST(hillThelenTypeActive, unitTest) EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); + muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenActiveOnlyType hillThelenTypeNew( + muscles::HillThelenActiveOnlyType hillThelenTypeNew( "newName", hillThelenType.position(), hillThelenType.characteristics(), @@ -981,32 +983,32 @@ TEST(hillThelenTypeActive, unitTest) EXPECT_STREQ(hillThelenTypeNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); + muscles::MUSCLE_TYPE::HILL_THELEN_ACTIVE); } } TEST(hillThelenActiveType, copy) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); Q = Q.setOnes() / 10; - biorbd::muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenActiveOnlyType deepCopyNow( + muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); + muscles::HillThelenActiveOnlyType deepCopyNow( hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenActiveOnlyType deepCopyLater; + muscles::HillThelenActiveOnlyType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); - biorbd::utils::String originalName(hillThelenType.name()); + utils::String originalName(hillThelenType.name()); EXPECT_STREQ(shallowCopy.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyNow.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); - biorbd::utils::String newName("MyNewMuscleName"); + utils::String newName("MyNewMuscleName"); hillThelenType.setName(newName); EXPECT_STREQ(hillThelenType.name().c_str(), newName.c_str()); EXPECT_STREQ(shallowCopy.name().c_str(), newName.c_str()); @@ -1014,15 +1016,15 @@ TEST(hillThelenActiveType, copy) EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenActiveOnlyType deepCopyNow( + muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); + muscles::HillThelenActiveOnlyType deepCopyNow( hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenActiveOnlyType deepCopyLater; + muscles::HillThelenActiveOnlyType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -1034,7 +1036,7 @@ TEST(hillThelenActiveType, copy) EXPECT_NEAR(shallowCopyPennationAngle, 0.15707963, requiredPrecision); } - biorbd::muscles::Characteristics charac(hillThelenType.characteristics()); + muscles::Characteristics charac(hillThelenType.characteristics()); charac.setPennationAngle(0.523599); { @@ -1054,20 +1056,20 @@ TEST(hillThelenActiveType, copy) } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity qDot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity qDot(model); Q = Q.setOnes() / 10; qDot = qDot.setOnes() / 10; hillThelenType.updateOrientations(model, Q); - biorbd::muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenActiveOnlyType deepCopyNow( + muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); + muscles::HillThelenActiveOnlyType deepCopyNow( hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenActiveOnlyType deepCopyLater; + muscles::HillThelenActiveOnlyType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -1082,12 +1084,12 @@ TEST(hillThelenActiveType, copy) } // Change the position of the insertion and pennation angle and compare again (length and insertion in Local) - biorbd::muscles::Characteristics charac(hillThelenType.characteristics()); + muscles::Characteristics charac(hillThelenType.characteristics()); charac.setPennationAngle(0.523599); - biorbd::utils::Vector3d insertion(hillThelenType.position().insertionInLocal()); + utils::Vector3d insertion(hillThelenType.position().insertionInLocal()); insertion.set(0.5, 0.6, 0.7); - biorbd::utils::String oldName(insertion.biorbd::utils::Node::name()); - biorbd::utils::String newName("MyNewName"); + utils::String oldName(insertion.utils::Node::name()); + utils::String newName("MyNewName"); insertion.setName(newName); hillThelenType.updateOrientations(model, Q, qDot, 2); @@ -1101,27 +1103,27 @@ TEST(hillThelenActiveType, copy) EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision); EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision); EXPECT_EQ( - hillThelenType.position().insertionInLocal().biorbd::utils::Node::name(), + hillThelenType.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(shallowCopy.position().insertionInLocal().utils::Node::name(), newName); - EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), + EXPECT_EQ(deepCopyNow.position().insertionInLocal().utils::Node::name(), oldName); EXPECT_EQ( - deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), + deepCopyLater.position().insertionInLocal().utils::Node::name(), oldName); } } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenActiveOnlyType hillThelenType( + Model model(modelPathForMuscleForce); + muscles::HillThelenActiveOnlyType hillThelenType( model.muscleGroup(muscleGroupForHillThelenType).muscle( muscleForHillThelenType)); - biorbd::muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); - biorbd::muscles::HillThelenActiveOnlyType deepCopyNow( + muscles::HillThelenActiveOnlyType shallowCopy(hillThelenType); + muscles::HillThelenActiveOnlyType deepCopyNow( hillThelenType.DeepCopy()); - biorbd::muscles::HillThelenActiveOnlyType deepCopyLater; + muscles::HillThelenActiveOnlyType deepCopyLater; deepCopyLater.DeepCopy(hillThelenType); { @@ -1135,7 +1137,7 @@ TEST(hillThelenActiveType, copy) EXPECT_NEAR(deepCopyLaterExcitation, 0., requiredPrecision); } - hillThelenType.state().setExcitation(biorbd::utils::Scalar(5.)); + hillThelenType.state().setExcitation(utils::Scalar(5.)); { SCALAR_TO_DOUBLE(excitation, hillThelenType.state().excitation()); @@ -1153,18 +1155,18 @@ TEST(hillThelenActiveType, copy) TEST(DynamicState, Normal) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::StateDynamics state(0.8, 0.5); + Model model(modelPathForMuscleForce); + muscles::StateDynamics state(0.8, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, 24.0, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::StateDynamics state(0.3, 0.5); + Model model(modelPathForMuscleForce); + muscles::StateDynamics state(0.3, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, -6.25, requiredPrecision); } @@ -1173,40 +1175,40 @@ TEST(DynamicState, Normal) TEST(DynamicState, Buchanan) { { - biorbd::Model model(modelPathForBuchananDynamics); - static_cast( + Model model(modelPathForBuchananDynamics); + static_cast( model.muscles()[2]->state()).shapeFactor(5.5); // Test shape factor changes - biorbd::utils::Scalar shapeFactor0 = - static_cast( + utils::Scalar shapeFactor0 = + static_cast( model.muscles()[0]->state()).shapeFactor(); SCALAR_TO_DOUBLE(shapeFactor0_double, shapeFactor0); EXPECT_NEAR(shapeFactor0_double, -3, requiredPrecision); - biorbd::utils::Scalar shapeFactor1 = - static_cast( + utils::Scalar shapeFactor1 = + static_cast( model.muscles()[1]->state()).shapeFactor(); SCALAR_TO_DOUBLE(shapeFactor1_double, shapeFactor1); EXPECT_NEAR(shapeFactor1_double, 10, requiredPrecision); - biorbd::utils::Scalar shapeFactor2 = - static_cast( + utils::Scalar shapeFactor2 = + static_cast( model.muscles()[2]->state()).shapeFactor(); SCALAR_TO_DOUBLE(shapeFactor2_double, shapeFactor2); EXPECT_NEAR(shapeFactor2_double, 5.5, requiredPrecision); - biorbd::muscles::StateDynamics state(0.8, 0.5); + muscles::StateDynamics state(0.8, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, -7.592740648890816, requiredPrecision); } { - biorbd::Model model(modelPathForBuchananDynamics); - biorbd::muscles::StateDynamics state(0.3, 0.5); + Model model(modelPathForBuchananDynamics); + muscles::StateDynamics state(0.3, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, -11.656766195499843, requiredPrecision); } @@ -1215,18 +1217,18 @@ TEST(DynamicState, Buchanan) TEST(DynamicState, DeGroote) { { - biorbd::Model model(modelPathForDeGrooteDynamics); - biorbd::muscles::StateDynamics state(0.8, 0.5); + Model model(modelPathForDeGrooteDynamics); + muscles::StateDynamics state(0.8, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, 16.906809211183873, requiredPrecision); } { - biorbd::Model model(modelPathForDeGrooteDynamics); - biorbd::muscles::StateDynamics state(0.3, 0.5); + Model model(modelPathForDeGrooteDynamics); + muscles::StateDynamics state(0.3, 0.5); - const biorbd::muscles::Muscle& m(model.muscle(0)); + const muscles::Muscle& m(model.muscle(0)); SCALAR_TO_DOUBLE(actDot, m.activationDot(state)); EXPECT_NEAR(actDot, -11.027512997920336, requiredPrecision); } @@ -1238,34 +1240,34 @@ static unsigned int muscleForHillThelenTypeFatigable(1); TEST(hillThelenTypeFatigable, unitTest) { { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; hillThelenTypeFatigable.setName("newName"); EXPECT_STREQ(hillThelenTypeFatigable.name().c_str(), "newName"); } { - biorbd::Model model(modelPathForMuscleForce); - EXPECT_THROW(biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable( + Model model(modelPathForMuscleForce); + EXPECT_THROW(muscles::HillThelenTypeFatigable hillThelenTypeFatigable( model.muscleGroup(muscleGroupForHillThelenTypeFatigable).muscle( muscleForHillThelenTypeFatigable)), std::bad_cast); } { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( + muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( "newName", hillThelenTypeFatigable.position(), hillThelenTypeFatigable.characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); EXPECT_STREQ(hillThelenTypeFatigableNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeFatigableNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); } { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( + muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( "newName", hillThelenTypeFatigable.position(), hillThelenTypeFatigable.characteristics(), @@ -1273,70 +1275,70 @@ TEST(hillThelenTypeFatigable, unitTest) EXPECT_STREQ(hillThelenTypeFatigableNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeFatigableNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); } { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( + muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( "newName", hillThelenTypeFatigable.position(), hillThelenTypeFatigable.characteristics(), hillThelenTypeFatigable.pathModifier(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); EXPECT_STREQ(hillThelenTypeFatigableNew.name().c_str(), "newName"); EXPECT_EQ(hillThelenTypeFatigableNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); } { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( + muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( "nameMuscle", hillThelenTypeFatigable.position(), hillThelenTypeFatigable.characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); EXPECT_EQ(hillThelenTypeFatigableNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); } { - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( + muscles::HillThelenTypeFatigable hillThelenTypeFatigableNew( "nameMuscle", hillThelenTypeFatigable.position(), hillThelenTypeFatigable.characteristics(), hillThelenTypeFatigable.pathModifier(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); EXPECT_EQ(hillThelenTypeFatigableNew.type(), - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE); } } TEST(hillThelenTypeFatigable, copy) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + Model model(modelPathForMuscleForce); + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); Q = Q.setOnes() / 10; - biorbd::muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); - biorbd::muscles::HillThelenTypeFatigable deepCopyNow( + muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); + muscles::HillThelenTypeFatigable deepCopyNow( hillThelenTypeFatigable.DeepCopy()); - biorbd::muscles::HillThelenTypeFatigable deepCopyLater; + muscles::HillThelenTypeFatigable deepCopyLater; deepCopyLater.DeepCopy(hillThelenTypeFatigable); - biorbd::utils::String originalName(hillThelenTypeFatigable.name()); + utils::String originalName(hillThelenTypeFatigable.name()); EXPECT_STREQ(shallowCopy.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyNow.name().c_str(), originalName.c_str()); EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); - biorbd::utils::String newName("MyNewMuscleName"); + utils::String newName("MyNewMuscleName"); hillThelenTypeFatigable.setName(newName); EXPECT_STREQ(hillThelenTypeFatigable.name().c_str(), newName.c_str()); EXPECT_STREQ(shallowCopy.name().c_str(), newName.c_str()); @@ -1344,13 +1346,13 @@ TEST(hillThelenTypeFatigable, copy) EXPECT_STREQ(deepCopyLater.name().c_str(), originalName.c_str()); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + Model model(modelPathForMuscleForce); + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); - biorbd::muscles::HillThelenTypeFatigable deepCopyNow( + muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); + muscles::HillThelenTypeFatigable deepCopyNow( hillThelenTypeFatigable.DeepCopy()); - biorbd::muscles::HillThelenTypeFatigable deepCopyLater; + muscles::HillThelenTypeFatigable deepCopyLater; deepCopyLater.DeepCopy(hillThelenTypeFatigable); { @@ -1361,7 +1363,7 @@ TEST(hillThelenTypeFatigable, copy) EXPECT_NEAR(pennationAngle, 0., requiredPrecision); EXPECT_NEAR(shallowCopyPennationAngle, 0., requiredPrecision); } - biorbd::muscles::Characteristics charac( + muscles::Characteristics charac( hillThelenTypeFatigable.characteristics()); charac.setPennationAngle(0.523599); { @@ -1381,13 +1383,13 @@ TEST(hillThelenTypeFatigable, copy) } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::HillThelenTypeFatigable hillThelenTypeFatigable; + Model model(modelPathForMuscleForce); + muscles::HillThelenTypeFatigable hillThelenTypeFatigable; - biorbd::muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); - biorbd::muscles::HillThelenTypeFatigable deepCopyNow( + muscles::HillThelenTypeFatigable shallowCopy(hillThelenTypeFatigable); + muscles::HillThelenTypeFatigable deepCopyNow( hillThelenTypeFatigable.DeepCopy()); - biorbd::muscles::HillThelenTypeFatigable deepCopyLater; + muscles::HillThelenTypeFatigable deepCopyLater; deepCopyLater.DeepCopy(hillThelenTypeFatigable); { @@ -1401,7 +1403,7 @@ TEST(hillThelenTypeFatigable, copy) EXPECT_NEAR(deepCopyLaterExcitation, 0., requiredPrecision); } - hillThelenTypeFatigable.state().setExcitation(biorbd::utils::Scalar(5.)); + hillThelenTypeFatigable.state().setExcitation(utils::Scalar(5.)); { SCALAR_TO_DOUBLE(excitation, hillThelenTypeFatigable.state().excitation()); @@ -1419,7 +1421,7 @@ TEST(hillThelenTypeFatigable, copy) #ifndef BIORBD_USE_CASADI_MATH TEST(FatigueState, unitTest) { - biorbd::muscles::FatigueState fatigueState; + muscles::FatigueState fatigueState; fatigueState.setState(0.0, 1.0, 0.0); SCALAR_TO_DOUBLE(activeFibers, fatigueState.activeFibers()); @@ -1433,11 +1435,11 @@ TEST(FatigueState, unitTest) TEST(FatigueState, copy) { - biorbd::muscles::FatigueState fatigueState; + muscles::FatigueState fatigueState; - biorbd::muscles::FatigueState shallowCopy(fatigueState); - biorbd::muscles::FatigueState deepCopyNow(fatigueState.DeepCopy()); - biorbd::muscles::FatigueState deepCopyLater; + muscles::FatigueState shallowCopy(fatigueState); + muscles::FatigueState deepCopyNow(fatigueState.DeepCopy()); + muscles::FatigueState deepCopyLater; deepCopyLater.DeepCopy(fatigueState); @@ -1487,21 +1489,21 @@ static unsigned int muscleForXiaDerivativeTest(0); TEST(FatigueDynamiqueState, DeepCopy) { // Prepare the model - biorbd::Model model(modelPathForXiaDerivativeTest); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity QDot(model); + Model model(modelPathForXiaDerivativeTest); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity QDot(model); Q.setZero(); QDot.setZero(); model.updateMuscles(Q, QDot, true); { - biorbd::muscles::HillThelenTypeFatigable muscle(model.muscleGroup( + muscles::HillThelenTypeFatigable muscle(model.muscleGroup( muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); - biorbd::muscles::FatigueDynamicStateXia fatigueDynamicStateXia; + muscles::FatigueDynamicStateXia fatigueDynamicStateXia; // Sanity check for the fatigue model EXPECT_EQ(fatigueDynamicStateXia.getType(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); // Initial value sanity check { @@ -1511,7 +1513,7 @@ TEST(FatigueDynamiqueState, DeepCopy) #ifndef BIORBD_USE_CASADI_MATH // Apply the derivative - biorbd::muscles::StateDynamics emg(0, + muscles::StateDynamics emg(0, activationEmgForXiaDerivativeTest); // Set target fatigueDynamicStateXia.setState(currentActiveFibersForXiaDerivativeTest, currentFatiguedFibersForXiaDerivativeTest, @@ -1525,10 +1527,10 @@ TEST(FatigueDynamiqueState, DeepCopy) expectedActivationDotForXiaDerivativeTest, requiredPrecision); // Make copies - biorbd::muscles::FatigueDynamicStateXia shallowCopy(fatigueDynamicStateXia); - biorbd::muscles::FatigueDynamicStateXia deepCopyNow( + muscles::FatigueDynamicStateXia shallowCopy(fatigueDynamicStateXia); + muscles::FatigueDynamicStateXia deepCopyNow( fatigueDynamicStateXia.DeepCopy()); - biorbd::muscles::FatigueDynamicStateXia deepCopyLater; + muscles::FatigueDynamicStateXia deepCopyLater; deepCopyLater.DeepCopy(fatigueDynamicStateXia); // Check the values @@ -1560,7 +1562,7 @@ TEST(FatigueDynamiqueState, DeepCopy) TEST(FatigueParameters, unitTest) { - biorbd::muscles::FatigueParameters fatigueParameters; + muscles::FatigueParameters fatigueParameters; fatigueParameters.setFatigueRate(25.0); SCALAR_TO_DOUBLE(fatigueRate, fatigueParameters.fatigueRate()); EXPECT_EQ(fatigueRate, 25.0); @@ -1580,10 +1582,10 @@ TEST(FatigueParameters, unitTest) TEST(FatigueParemeters, copy) { - biorbd::muscles::FatigueParameters fatigueParameters(1.0, 2.0, 3.0, 4.0); - biorbd::muscles::FatigueParameters shallowCopy(fatigueParameters); - biorbd::muscles::FatigueParameters deepCopyNow(fatigueParameters.DeepCopy()); - biorbd::muscles::FatigueParameters deepCopyLater; + muscles::FatigueParameters fatigueParameters(1.0, 2.0, 3.0, 4.0); + muscles::FatigueParameters shallowCopy(fatigueParameters); + muscles::FatigueParameters deepCopyNow(fatigueParameters.DeepCopy()); + muscles::FatigueParameters deepCopyLater; deepCopyLater.DeepCopy(fatigueParameters); { @@ -1614,36 +1616,36 @@ TEST(FatigueParemeters, copy) TEST(MuscleGroup, unitTest) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); muscleGroup.setName("newName"); EXPECT_STREQ(muscleGroup.name().c_str(), "newName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); muscleGroup.setOrigin("newOriginName"); EXPECT_STREQ(muscleGroup.origin().c_str(), "newOriginName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); muscleGroup.setInsertion("newInsertionName"); EXPECT_STREQ(muscleGroup.insertion().c_str(), "newInsertionName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); muscleGroup.addMuscle("newMuscleName", - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); // Check the id of the last muscle added EXPECT_NEAR(muscleGroup.nbMuscles(), 4, requiredPrecision); @@ -1658,80 +1660,80 @@ TEST(MuscleGroup, unitTest) TEST(MuscleGroup, AddMuscle) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); - biorbd::muscles::IdealizedActuator muscleToAdd; + muscles::IdealizedActuator muscleToAdd; muscleGroup.addMuscle(muscleToAdd); //Check number of muscle EXPECT_NEAR(muscleGroup.nbMuscles(), 4, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); //Check number of muscle EXPECT_NEAR(muscleGroup.nbMuscles(), 3, requiredPrecision); // Add muscle to muscle group muscleGroup.addMuscle("newMuscleName", - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_TYPE::SIMPLE_STATE, - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + muscles::STATE_TYPE::SIMPLE_STATE, + muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); // Check the number of muscles again EXPECT_NEAR(muscleGroup.nbMuscles(), 4, requiredPrecision); // Add HILL muscle to muscle group muscleGroup.addMuscle("newHillMuscle", - biorbd::muscles::MUSCLE_TYPE::HILL, + muscles::MUSCLE_TYPE::HILL, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_TYPE::DYNAMIC, - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + muscles::STATE_TYPE::DYNAMIC, + muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); // Check the number of muscles again EXPECT_NEAR(muscleGroup.nbMuscles(), 5, requiredPrecision); // Add HILL THELEN muscle to muscle group muscleGroup.addMuscle("newHillThelenMuscle", - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN, + muscles::MUSCLE_TYPE::HILL_THELEN, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_TYPE::BUCHANAN, - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + muscles::STATE_TYPE::BUCHANAN, + muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); // Check the number of muscles again EXPECT_NEAR(muscleGroup.nbMuscles(), 6, requiredPrecision); // Add muscle to muscle group muscleGroup.addMuscle("newHillThelenFatigable", - biorbd::muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE, + muscles::MUSCLE_TYPE::HILL_THELEN_FATIGABLE, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_TYPE::SIMPLE_STATE, - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_TYPE::SIMPLE_STATE, + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); // Check the number of muscles again EXPECT_NEAR(muscleGroup.nbMuscles(), 7, requiredPrecision); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); //Check number of muscle EXPECT_NEAR(muscleGroup.nbMuscles(), 3, requiredPrecision); // Add muscle to muscle group muscleGroup.addMuscle("newMuscleName", - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), model.muscleGroup(0).muscle(0).pathModifier(), - biorbd::muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); + muscles::STATE_FATIGUE_TYPE::NO_FATIGUE_STATE_TYPE); // Check the number of muscles again EXPECT_NEAR(muscleGroup.nbMuscles(), 4, requiredPrecision); @@ -1741,12 +1743,12 @@ TEST(MuscleGroup, AddMuscle) TEST(MuscleGroup, DeepCopy) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); - biorbd::muscles::MuscleGroup shallowCopy(muscleGroup); - biorbd::muscles::MuscleGroup deepCopyNow(muscleGroup.DeepCopy()); - biorbd::muscles::MuscleGroup deepCopyLater; + muscles::MuscleGroup shallowCopy(muscleGroup); + muscles::MuscleGroup deepCopyNow(muscleGroup.DeepCopy()); + muscles::MuscleGroup deepCopyLater; deepCopyLater.DeepCopy(muscleGroup); EXPECT_STREQ(muscleGroup.name().c_str(), "base_to_r_ulna_radius_hand"); @@ -1763,24 +1765,24 @@ TEST(MuscleGroup, DeepCopy) EXPECT_STREQ(deepCopyLater.name().c_str(), "base_to_r_ulna_radius_hand"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); muscleGroup.addMuscle("newIdealizedActuator", - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); muscleGroup.addMuscle("newHillActuator", - biorbd::muscles::MUSCLE_TYPE::HILL, + muscles::MUSCLE_TYPE::HILL, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); - biorbd::muscles::MuscleGroup shallowCopy(muscleGroup); - biorbd::muscles::MuscleGroup deepCopyNow(muscleGroup.DeepCopy()); - biorbd::muscles::MuscleGroup deepCopyLater; + muscles::MuscleGroup shallowCopy(muscleGroup); + muscles::MuscleGroup deepCopyNow(muscleGroup.DeepCopy()); + muscles::MuscleGroup deepCopyLater; deepCopyLater.DeepCopy(muscleGroup); EXPECT_STREQ(muscleGroup.muscle(4).name().c_str(), "newHillActuator"); @@ -1793,47 +1795,47 @@ TEST(MuscleGroup, DeepCopy) TEST(MuscleGroup, errors) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); EXPECT_THROW(muscleGroup.addMuscle("noTypeMuscle", - biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, + muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); EXPECT_THROW(muscleGroup.muscle(3), std::runtime_error); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); EXPECT_THROW(muscleGroup.addMuscle("noTypeMuscle", - biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, + muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_TYPE::SIMPLE_STATE, - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); + muscles::STATE_TYPE::SIMPLE_STATE, + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); + Model model(modelPathForMuscleForce); + muscles::MuscleGroup muscleGroup(model.muscleGroup(0)); EXPECT_THROW(muscleGroup.addMuscle("noTypeMuscle", - biorbd::muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, + muscles::MUSCLE_TYPE::NO_MUSCLE_TYPE, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), model.muscleGroup(0).muscle(0).pathModifier(), - biorbd::muscles::STATE_TYPE::SIMPLE_STATE, - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); + muscles::STATE_TYPE::SIMPLE_STATE, + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE), std::runtime_error); } } TEST(Muscles, unitTest) { { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::Muscles muscles; + Model model(modelPathForMuscleForce); + muscles::Muscles muscles; muscles.addMuscleGroup("muscleGroupName", "originName", "insertionName"); EXPECT_STREQ(muscles.muscleGroup(0).name().c_str(), "muscleGroupName"); @@ -1842,14 +1844,14 @@ TEST(Muscles, unitTest) "originName"); } { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::Muscles muscles; + Model model(modelPathForMuscleForce); + muscles::Muscles muscles; muscles.addMuscleGroup("muscleGroupName", "originName", "insertionName"); muscles.muscleGroup(0).addMuscle("newIdealizedActuator", - biorbd::muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, + muscles::MUSCLE_TYPE::IDEALIZED_ACTUATOR, model.muscleGroup(0).muscle(0).position(), model.muscleGroup(0).muscle(0).characteristics(), - biorbd::muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); + muscles::STATE_FATIGUE_TYPE::SIMPLE_STATE_FATIGUE); EXPECT_NEAR(muscles.muscleNames().size(), 1., requiredPrecision); } @@ -1857,8 +1859,8 @@ TEST(Muscles, unitTest) TEST(Muscles, errors) { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::Muscles muscles; + Model model(modelPathForMuscleForce); + muscles::Muscles muscles; muscles.addMuscleGroup("muscleGroupName", "originName", "insertionName"); EXPECT_THROW(muscles.muscleGroup(1), std::runtime_error); @@ -1867,13 +1869,13 @@ TEST(Muscles, errors) TEST(Muscles, deepCopy) { - biorbd::Model model(modelPathForMuscleForce); - biorbd::muscles::Muscles muscles; + Model model(modelPathForMuscleForce); + muscles::Muscles muscles; muscles.addMuscleGroup("muscleGroupName", "originName", "insertionName"); - biorbd::muscles::Muscles shallowCopy(muscles); - biorbd::muscles::Muscles deepCopyNow(muscles.DeepCopy()); - biorbd::muscles::Muscles deepCopyLater; + muscles::Muscles shallowCopy(muscles); + muscles::Muscles deepCopyNow(muscles.DeepCopy()); + muscles::Muscles deepCopyLater; deepCopyLater.DeepCopy(muscles); EXPECT_STREQ(muscles.muscleGroup(0).name().c_str(), "muscleGroupName"); @@ -1894,11 +1896,11 @@ TEST(Muscles, deepCopy) TEST(WrappingHalfCylinder, unitTest) { { - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1., 1., 1.), biorbd::utils::Vector3d(1., 1., 1.), + utils::RotoTrans rt( + utils::Vector3d(1., 1., 1.), utils::Vector3d(1., 1., 1.), "xyz"); - biorbd::muscles::WrappingHalfCylinder wrappingHalfCylinder; + muscles::WrappingHalfCylinder wrappingHalfCylinder; wrappingHalfCylinder.setRadius(0.75); SCALAR_TO_DOUBLE(diameter, wrappingHalfCylinder.diameter()); @@ -1907,42 +1909,42 @@ TEST(WrappingHalfCylinder, unitTest) EXPECT_NEAR(radius, 0.75, requiredPrecision); wrappingHalfCylinder.setName("wrappingHalfCylinderName"); - EXPECT_STREQ(wrappingHalfCylinder.biorbd::utils::Node::name().c_str(), + EXPECT_STREQ(wrappingHalfCylinder.utils::Node::name().c_str(), "wrappingHalfCylinderName"); } { - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1., 1., 1.), biorbd::utils::Vector3d(1., 1., 1.), + utils::RotoTrans rt( + utils::Vector3d(1., 1., 1.), utils::Vector3d(1., 1., 1.), "xyz"); - biorbd::muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.25, 1.); + muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.25, 1.); SCALAR_TO_DOUBLE(diameter, wrappingHalfCylinder.diameter()); EXPECT_NEAR(diameter, 0.5, requiredPrecision); SCALAR_TO_DOUBLE(length, wrappingHalfCylinder.length()); EXPECT_NEAR(length, 1., requiredPrecision); } { - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1., 1., 1.), biorbd::utils::Vector3d(1., 1., 1.), + utils::RotoTrans rt( + utils::Vector3d(1., 1., 1.), utils::Vector3d(1., 1., 1.), "xyz"); - biorbd::muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.5, 1., "name", + muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.5, 1., "name", "parentName"); EXPECT_STREQ(wrappingHalfCylinder.parent().c_str(), "parentName"); } { - biorbd::muscles::WrappingHalfCylinder wrappingHalfCylinder; - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1., 1., 1.), biorbd::utils::Vector3d(1., 1., 1.), + muscles::WrappingHalfCylinder wrappingHalfCylinder; + utils::RotoTrans rt( + utils::Vector3d(1., 1., 1.), utils::Vector3d(1., 1., 1.), "xyz"); - biorbd::utils::Vector3d p1(1., 1., 1.); - biorbd::utils::Vector3d p2(2., 2., 2.); + utils::Vector3d p1(1., 1., 1.); + utils::Vector3d p2(2., 2., 2.); wrappingHalfCylinder.wrapPoints( rt, - biorbd::utils::Vector3d(0.5, 1., 1.5), - biorbd::utils::Vector3d(4., 5., 6.), + utils::Vector3d(0.5, 1., 1.5), + utils::Vector3d(4., 5., 6.), p1, p2); SCALAR_TO_DOUBLE(p10, p1[0]); @@ -1962,16 +1964,16 @@ TEST(WrappingHalfCylinder, unitTest) TEST(WrappingHalfCylinder, deepCopy) { - biorbd::Model model(modelPathForMuscleForce); - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1, 1, 1), biorbd::utils::Vector3d(1, 1, 1), "xyz"); + Model model(modelPathForMuscleForce); + utils::RotoTrans rt( + utils::Vector3d(1, 1, 1), utils::Vector3d(1, 1, 1), "xyz"); - biorbd::muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.25, 1.); + muscles::WrappingHalfCylinder wrappingHalfCylinder(rt, 0.25, 1.); - biorbd::muscles::WrappingHalfCylinder shallowCopy(wrappingHalfCylinder); - biorbd::muscles::WrappingHalfCylinder deepCopyNow( + muscles::WrappingHalfCylinder shallowCopy(wrappingHalfCylinder); + muscles::WrappingHalfCylinder deepCopyNow( wrappingHalfCylinder.DeepCopy()); - biorbd::muscles::WrappingHalfCylinder deepCopyLater; + muscles::WrappingHalfCylinder deepCopyLater; deepCopyLater.DeepCopy(wrappingHalfCylinder); { @@ -2029,18 +2031,18 @@ TEST(MuscleForce, position) TEST(MuscleForce, force) { - biorbd::Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity QDot(model); + Model model(modelPathForMuscleForce); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity QDot(model); Q = Q.setOnes()/10; QDot = QDot.setOnes()/10; - std::vector> states; + std::vector> states; for (unsigned int i=0; i(0, 0.2)); + states.push_back(std::make_shared(0, 0.2)); } model.updateMuscles(Q, QDot, true); - const biorbd::utils::Vector& F = model.muscleForces(states); + const utils::Vector& F = model.muscleForces(states); std::vector ExpectedForce({ 164.3110575502927, 106.89637709077938, 84.340201458493794, @@ -2054,18 +2056,18 @@ TEST(MuscleForce, force) TEST(MuscleForce, torqueFromMuscles) { - biorbd::Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity QDot(model); - biorbd::rigidbody::GeneralizedAcceleration QDDot(model); + Model model(modelPathForMuscleForce); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity QDot(model); + rigidbody::GeneralizedAcceleration QDDot(model); Q.setOnes()/10; QDot.setOnes()/10; - std::vector> states; + std::vector> states; for (unsigned int i=0; i(0, 0.2)); + states.push_back(std::make_shared(0, 0.2)); } - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedTorque Tau(model); std::vector TauExpected({-18.271389285751727, -7.820566757538376}); Tau = model.muscularJointTorque(states, Q, QDot); for (unsigned int i=0; i + muscles::FatigueDynamicState& fatigueModel( + dynamic_cast (muscle.fatigueState())); // Sanity check for the fatigue model EXPECT_EQ(fatigueModel.getType(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); // Initial value sanity check { @@ -2351,7 +2353,7 @@ TEST(MuscleFatigue, FatigueXiaDerivativeViaPointers) } // Apply the derivative - biorbd::muscles::StateDynamics emg(0, + muscles::StateDynamics emg(0, activationEmgForXiaDerivativeTest); // Set target fatigueModel.setState(currentActiveFibersForXiaDerivativeTest, currentFatiguedFibersForXiaDerivativeTest, @@ -2371,11 +2373,11 @@ TEST(MuscleFatigue, FatigueXiaDerivativeViaPointers) // Values should be changed in the model itself { - biorbd::muscles::HillThelenTypeFatigable muscle(model.muscleGroup( + muscles::HillThelenTypeFatigable muscle(model.muscleGroup( muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); - biorbd::muscles::FatigueDynamicState& fatigueModel( - dynamic_cast + muscles::FatigueDynamicState& fatigueModel( + dynamic_cast (muscle.fatigueState())); // Check the values @@ -2393,20 +2395,20 @@ TEST(MuscleFatigue, FatigueXiaDerivativeViaPointers) TEST(MuscleFatigue, FatigueXiaDerivativeViaInterface) { // Prepare the model - biorbd::Model model(modelPathForXiaDerivativeTest); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity QDot(model); + Model model(modelPathForXiaDerivativeTest); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity QDot(model); Q.setZero(); QDot.setZero(); model.updateMuscles(Q, QDot, true); { - biorbd::muscles::HillThelenTypeFatigable muscle(model.muscleGroup( + muscles::HillThelenTypeFatigable muscle(model.muscleGroup( muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); // Apply the derivative - biorbd::muscles::StateDynamics emg(0, + muscles::StateDynamics emg(0, activationEmgForXiaDerivativeTest); // Set target muscle.setFatigueState(currentActiveFibersForXiaDerivativeTest, currentFatiguedFibersForXiaDerivativeTest, @@ -2417,9 +2419,9 @@ TEST(MuscleFatigue, FatigueXiaDerivativeViaInterface) // Values should be changed in the model itself { - biorbd::muscles::HillThelenTypeFatigable muscle(model.muscleGroup(0).muscle(0)); - biorbd::muscles::FatigueDynamicState& fatigueModel( - dynamic_cast + muscles::HillThelenTypeFatigable muscle(model.muscleGroup(0).muscle(0)); + muscles::FatigueDynamicState& fatigueModel( + dynamic_cast (muscle.fatigueState())); // Check the values @@ -2437,23 +2439,23 @@ TEST(MuscleFatigue, FatigueXiaDerivativeViaInterface) TEST(MuscleFatigue, FatigueXiaDerivativeShallowViaCopy) { // Prepare the model - biorbd::Model model(modelPathForXiaDerivativeTest); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity QDot(model); + Model model(modelPathForXiaDerivativeTest); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity QDot(model); Q.setZero(); QDot.setZero(); model.updateMuscles(Q, QDot, true); { - biorbd::muscles::HillThelenTypeFatigable muscle( + muscles::HillThelenTypeFatigable muscle( model.muscleGroup(muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); - biorbd::muscles::FatigueDynamicStateXia fatigueModel( - dynamic_cast + muscles::FatigueDynamicStateXia fatigueModel( + dynamic_cast (muscle.fatigueState())); // Sanity check for the fatigue model EXPECT_EQ(fatigueModel.getType(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); // Initial value sanity check { @@ -2466,7 +2468,7 @@ TEST(MuscleFatigue, FatigueXiaDerivativeShallowViaCopy) } // Apply the derivative - biorbd::muscles::StateDynamics emg(0, + muscles::StateDynamics emg(0, activationEmgForXiaDerivativeTest); // Set target fatigueModel.setState(currentActiveFibersForXiaDerivativeTest, currentFatiguedFibersForXiaDerivativeTest, @@ -2486,11 +2488,11 @@ TEST(MuscleFatigue, FatigueXiaDerivativeShallowViaCopy) // Values should be changed in the model itself since everything is shallowcopied { - biorbd::muscles::HillThelenTypeFatigable muscle(model.muscleGroup( + muscles::HillThelenTypeFatigable muscle(model.muscleGroup( muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); - biorbd::muscles::FatigueDynamicState& fatigueModel( - dynamic_cast + muscles::FatigueDynamicState& fatigueModel( + dynamic_cast (muscle.fatigueState())); // Check the values @@ -2508,16 +2510,16 @@ TEST(MuscleFatigue, FatigueXiaDerivativeShallowViaCopy) TEST(MuscleFatigue, FatigueXiaSetStateLimitsTest) { // Prepare the model - biorbd::Model model(modelPathForXiaDerivativeTest); - biorbd::muscles::HillThelenTypeFatigable muscle( + Model model(modelPathForXiaDerivativeTest); + muscles::HillThelenTypeFatigable muscle( model.muscleGroup(muscleGroupForXiaDerivativeTest).muscle( muscleForXiaDerivativeTest)); - biorbd::muscles::FatigueDynamicStateXia fatigueModel( - dynamic_cast + muscles::FatigueDynamicStateXia fatigueModel( + dynamic_cast (muscle.fatigueState())); // Sanity check for the fatigue model EXPECT_EQ(fatigueModel.getType(), - biorbd::muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); + muscles::STATE_FATIGUE_TYPE::DYNAMIC_XIA); // Teast limit active quantity { @@ -2598,11 +2600,11 @@ TEST(StaticOptim, OneFrameNoActivations) std::cout << "StaticOptim is not tested for CasADi backend" << std::endl; #else - biorbd::Model model(modelPathForMuscleForce); + Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; @@ -2610,7 +2612,7 @@ TEST(StaticOptim, OneFrameNoActivations) } // Proceed with the static optimization - auto optim = biorbd::muscles::StaticOptimization(model, Q, Qdot, Tau); + auto optim = muscles::StaticOptimization(model, Q, Qdot, Tau); optim.run(); auto muscleActivations = optim.finalSolution()[0]; @@ -2631,11 +2633,11 @@ TEST(StaticOptim, OneFrameOneActivationDouble) std::cout << "StaticOptim is not tested for CasADi backend" << std::endl; #else - biorbd::Model model(modelPathForMuscleForce); + Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; @@ -2644,7 +2646,7 @@ TEST(StaticOptim, OneFrameOneActivationDouble) // Proceed with the static optimization double initialActivationGuess = 0.5; - auto optim = biorbd::muscles::StaticOptimization(model, Q, Qdot, Tau, + auto optim = muscles::StaticOptimization(model, Q, Qdot, Tau, initialActivationGuess); optim.run(); auto muscleActivations = optim.finalSolution()[0]; @@ -2666,11 +2668,11 @@ TEST(StaticOptim, OneFrameOneActivationVector) std::cout << "StaticOptim is not tested for CasADi backend" << std::endl; #else - biorbd::Model model(modelPathForMuscleForce); + Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; @@ -2678,11 +2680,11 @@ TEST(StaticOptim, OneFrameOneActivationVector) } // Proceed with the static optimization - biorbd::utils::Vector initialActivationGuess(model.nbMuscles()); + utils::Vector initialActivationGuess(model.nbMuscles()); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; Tau[i] = static_cast(i) * 1.1; } - std::vector allQ; - std::vector allQdot; - std::vector allTau; + std::vector allQ; + std::vector allQdot; + std::vector allTau; allQ.push_back(Q); allQ.push_back(Q); allQ.push_back(Q); @@ -2728,7 +2730,7 @@ TEST(StaticOptim, MultiFrameNoActivation) allTau.push_back(Tau); // Proceed with the static optimization - auto optim = biorbd::muscles::StaticOptimization(model, Q, Qdot, Tau); + auto optim = muscles::StaticOptimization(model, Q, Qdot, Tau); optim.run(); auto allMuscleActivations = optim.finalSolution(); @@ -2751,19 +2753,19 @@ TEST(StaticOptim, MultiFrameActivationDouble) std::cout << "StaticOptim is not tested for CasADi backend" << std::endl; #else - biorbd::Model model(modelPathForMuscleForce); + Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; Tau[i] = static_cast(i) * 1.1; } - std::vector allQ; - std::vector allQdot; - std::vector allTau; + std::vector allQ; + std::vector allQdot; + std::vector allTau; allQ.push_back(Q); allQ.push_back(Q); allQ.push_back(Q); @@ -2776,7 +2778,7 @@ TEST(StaticOptim, MultiFrameActivationDouble) // Proceed with the static optimization double initialActivationGuess = 0.5; - auto optim = biorbd::muscles::StaticOptimization(model, Q, Qdot, Tau, + auto optim = muscles::StaticOptimization(model, Q, Qdot, Tau, initialActivationGuess); optim.run(); auto allMuscleActivations = optim.finalSolution(); @@ -2800,19 +2802,19 @@ TEST(StaticOptim, MultiFrameNoActivationVector) std::cout << "StaticOptim is not tested for CasADi backend" << std::endl; #else - biorbd::Model model(modelPathForMuscleForce); + Model model(modelPathForMuscleForce); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); for (unsigned int i=0; i(i) * 1.1; Qdot[i] = static_cast(i) * 1.1; Tau[i] = static_cast(i) * 1.1; } - std::vector allQ; - std::vector allQdot; - std::vector allTau; + std::vector allQ; + std::vector allQdot; + std::vector allTau; allQ.push_back(Q); allQ.push_back(Q); allQ.push_back(Q); @@ -2824,11 +2826,11 @@ TEST(StaticOptim, MultiFrameNoActivationVector) allTau.push_back(Tau); // Proceed with the static optimization - biorbd::utils::Vector initialActivationGuess(model.nbMuscles()); + utils::Vector initialActivationGuess(model.nbMuscles()); for (unsigned int i=0; i QDDot_expected(13); @@ -73,8 +74,8 @@ TEST(Gravity, change) TEST(Contacts, unitTest) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Contacts contacts(model); + Model model(modelPathForGeneralTesting); + rigidbody::Contacts contacts(model); EXPECT_NEAR(contacts.nbContacts(), 6., requiredPrecision); EXPECT_STREQ(contacts.contactName(1).c_str(), "PiedG_1_Z"); @@ -84,25 +85,25 @@ TEST(Contacts, unitTest) EXPECT_NEAR(fy, 0., requiredPrecision); } { - biorbd::rigidbody::Contacts contacts; + rigidbody::Contacts contacts; EXPECT_EQ(contacts.hasContacts(), false); } { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Contacts contacts(model); + Model model(modelPathForGeneralTesting); + rigidbody::Contacts contacts(model); EXPECT_THROW(contacts.contactName(7), std::runtime_error); } { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Contacts contacts(model); + Model model(modelPathForGeneralTesting); + rigidbody::Contacts contacts(model); EXPECT_NEAR(contacts.nbContacts(), 6., requiredPrecision); contacts.AddConstraint( 7, - biorbd::utils::Vector3d(0, 0, 0), - biorbd::utils::Vector3d(1, 1, 1), + utils::Vector3d(0, 0, 0), + utils::Vector3d(1, 1, 1), "constraintName", 2.0); @@ -113,12 +114,12 @@ TEST(Contacts, unitTest) TEST(Contacts, DeepCopy) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Contacts contacts(model); + Model model(modelPathForGeneralTesting); + rigidbody::Contacts contacts(model); - biorbd::rigidbody::Contacts shallowCopy(contacts); - biorbd::rigidbody::Contacts deepCopyNow(contacts.DeepCopy()); - biorbd::rigidbody::Contacts deepCopyLater; + rigidbody::Contacts shallowCopy(contacts); + rigidbody::Contacts deepCopyNow(contacts.DeepCopy()); + rigidbody::Contacts deepCopyLater; deepCopyLater.DeepCopy(contacts); EXPECT_NEAR(contacts.nbContacts(), 6., requiredPrecision); @@ -128,8 +129,8 @@ TEST(Contacts, DeepCopy) contacts.AddConstraint( 7, - biorbd::utils::Vector3d(0, 0, 0), - biorbd::utils::Vector3d(1, 1, 1), + utils::Vector3d(0, 0, 0), + utils::Vector3d(1, 1, 1), "constraintName", 2.0); @@ -147,13 +148,13 @@ static std::vector Qtest = { 0.1, 0.1, 0.1, 0.3, 0.3, 0.3, TEST(GeneralizedCoordinates, unitTest) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::GeneralizedCoordinates Q(model); + Model model(modelPathForGeneralTesting); + rigidbody::GeneralizedCoordinates Q(model); for (unsigned int i = 0; i < model.nbQ(); ++i) { Q[i] = Qtest[i]; } - biorbd::rigidbody::GeneralizedCoordinates newQ(Q); + rigidbody::GeneralizedCoordinates newQ(Q); std::vector Q_expected = { 0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.4, 0.3 @@ -169,8 +170,8 @@ TEST(GeneralizedCoordinates, unitTest) TEST(GeneralizedVelocity, unitTest) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); + Model model(modelPathForGeneralTesting); + rigidbody::GeneralizedVelocity Qdot(model); for (unsigned int i = 0; i < model.nbQdot(); ++i) { Qdot[i] = Qtest[i] * 10; } @@ -179,7 +180,7 @@ TEST(GeneralizedVelocity, unitTest) 3., 3., 3., 3., 3., 4., 3. }; - biorbd::rigidbody::GeneralizedVelocity newQdot(Qdot); + rigidbody::GeneralizedVelocity newQdot(Qdot); for (unsigned int i = 0; i < model.nbQdot(); ++i) { SCALAR_TO_DOUBLE(qdot, newQdot[i]); @@ -187,11 +188,11 @@ TEST(GeneralizedVelocity, unitTest) } } { - biorbd::rigidbody::GeneralizedVelocity Qdot; + rigidbody::GeneralizedVelocity Qdot; SCALAR_TO_DOUBLE(qdotNorm, Qdot.norm()); EXPECT_NEAR(qdotNorm, 0., requiredPrecision); - biorbd::rigidbody::GeneralizedVelocity newQdot(Qdot); + rigidbody::GeneralizedVelocity newQdot(Qdot); SCALAR_TO_DOUBLE(newQdotNorm, newQdot.norm()); EXPECT_NEAR(newQdotNorm, 0., requiredPrecision); } @@ -200,13 +201,13 @@ TEST(GeneralizedVelocity, unitTest) TEST(GeneralizedAcceleration, unitTest) { { - biorbd::rigidbody::GeneralizedAcceleration Qddot; + rigidbody::GeneralizedAcceleration Qddot; SCALAR_TO_DOUBLE(QddotNorm, Qddot.norm()); EXPECT_NEAR(QddotNorm, 0., requiredPrecision); } { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::GeneralizedAcceleration Qddot(model); + Model model(modelPathForGeneralTesting); + rigidbody::GeneralizedAcceleration Qddot(model); for (unsigned int i = 0; i < model.nbQ(); ++i) { Qddot[i] = Qtest[i] * 100; } @@ -215,7 +216,7 @@ TEST(GeneralizedAcceleration, unitTest) 30., 30., 30., 30., 30., 40., 30. }; - biorbd::rigidbody::GeneralizedAcceleration newQddot(Qddot); + rigidbody::GeneralizedAcceleration newQddot(Qddot); for (unsigned int i = 0; i < model.nbQ(); ++i) { SCALAR_TO_DOUBLE(qddot, newQddot[i]); @@ -227,8 +228,8 @@ TEST(GeneralizedAcceleration, unitTest) TEST(GeneralizedTorque, unitTest) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::GeneralizedTorque Tau(model); + Model model(modelPathForGeneralTesting); + rigidbody::GeneralizedTorque Tau(model); #ifdef BIORBD_USE_EIGEN3_MATH Tau << 0.1, 0.1, 0.1, 0.3, 0.3, 0.3, @@ -258,13 +259,13 @@ TEST(GeneralizedTorque, unitTest) EXPECT_NEAR(tau, Tau_expected[i], requiredPrecision); } - biorbd::rigidbody::GeneralizedTorque newTau(Tau); + rigidbody::GeneralizedTorque newTau(Tau); for (unsigned int i = 0; i < 12; ++i) { SCALAR_TO_DOUBLE(tau, newTau[i]); EXPECT_NEAR(tau, Tau_expected[i], requiredPrecision); } - biorbd::rigidbody::GeneralizedTorque tauWithNoArgument; + rigidbody::GeneralizedTorque tauWithNoArgument; SCALAR_TO_DOUBLE(tauWithNoArgumentNorm, tauWithNoArgument.norm()); EXPECT_NEAR(tauWithNoArgumentNorm, 0., requiredPrecision); } @@ -272,18 +273,18 @@ TEST(GeneralizedTorque, unitTest) TEST(IMU, unitTest) { - biorbd::rigidbody::IMU imu(true, false); + rigidbody::IMU imu(true, false); EXPECT_EQ(imu.isTechnical(), true); EXPECT_EQ(imu.isAnatomical(), false); } TEST(IMU, DeepCopy) { - biorbd::rigidbody::IMU imu(false, true); + rigidbody::IMU imu(false, true); - biorbd::rigidbody::IMU shallowCopy(imu); - biorbd::rigidbody::IMU deepCopyNow(imu.DeepCopy()); - biorbd::rigidbody::IMU deepCopyLater; + rigidbody::IMU shallowCopy(imu); + rigidbody::IMU deepCopyNow(imu.DeepCopy()); + rigidbody::IMU deepCopyLater; deepCopyLater.DeepCopy(imu); EXPECT_EQ(shallowCopy.isTechnical(), false); @@ -296,24 +297,24 @@ modelPathForPyomecaman_withIMUs("models/IMUandCustomRT/pyomecaman_withIMUs.bioMo TEST(IMUs, unitTest) { { - biorbd::rigidbody::IMUs imus; + rigidbody::IMUs imus; imus.addIMU(true, true); EXPECT_EQ(imus.nbIMUs(), 1); EXPECT_EQ(imus.IMU(0).isTechnical(), true); } { - biorbd::rigidbody::IMUs imus; - biorbd::rigidbody::IMU imu(true, false); + rigidbody::IMUs imus; + rigidbody::IMU imu(true, false); imu.setName("imuName"); imus.addIMU(imu); - EXPECT_STREQ(imus.technicalIMU()[0].biorbd::utils::Node::name().c_str(), + EXPECT_STREQ(imus.technicalIMU()[0].utils::Node::name().c_str(), "imuName"); } { - biorbd::Model model(modelPathForPyomecaman_withIMUs); - biorbd::rigidbody::IMUs imus(model); + Model model(modelPathForPyomecaman_withIMUs); + rigidbody::IMUs imus(model); EXPECT_EQ(imus.anatomicalIMU().size(), 2); EXPECT_EQ(imus.technicalIMU().size(), 4); @@ -322,12 +323,12 @@ TEST(IMUs, unitTest) TEST(IMUs, deepCopy) { - biorbd::Model model(modelPathForPyomecaman_withIMUs); - biorbd::rigidbody::IMUs imus(model); + Model model(modelPathForPyomecaman_withIMUs); + rigidbody::IMUs imus(model); - biorbd::rigidbody::IMUs shallowCopy(imus); - biorbd::rigidbody::IMUs deepCopyNow(imus.DeepCopy()); - biorbd::rigidbody::IMUs deepCopyLater; + rigidbody::IMUs shallowCopy(imus); + rigidbody::IMUs deepCopyNow(imus.DeepCopy()); + rigidbody::IMUs deepCopyLater; deepCopyLater.DeepCopy(imus); EXPECT_EQ(shallowCopy.nbIMUs(), 4); @@ -344,12 +345,12 @@ TEST(IMUs, deepCopy) TEST(Joints, copy) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Joints joints(model); + Model model(modelPathForGeneralTesting); + rigidbody::Joints joints(model); - biorbd::rigidbody::Joints shallowCopy(joints); - biorbd::rigidbody::Joints deepCopyNow(joints.DeepCopy()); - biorbd::rigidbody::Joints deepCopyLater; + rigidbody::Joints shallowCopy(joints); + rigidbody::Joints deepCopyNow(joints.DeepCopy()); + rigidbody::Joints deepCopyLater; deepCopyLater.DeepCopy(joints); { SCALAR_TO_DOUBLE(shallowCopyMass, shallowCopy.mass()); @@ -360,10 +361,10 @@ TEST(Joints, copy) EXPECT_NEAR(deepCopyLaterMass, 52.412120000000002, requiredPrecision); } - biorbd::rigidbody::SegmentCharacteristics characteristics( - 10, biorbd::utils::Vector3d(0.5, 0.5, 0.5), + rigidbody::SegmentCharacteristics characteristics( + 10, utils::Vector3d(0.5, 0.5, 0.5), RigidBodyDynamics::Math::Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1)); - std::vector ranges(6); + std::vector ranges(6); joints.AddSegment("segmentName", "parentName", "zyx", "yzx", ranges, ranges, ranges, @@ -385,11 +386,11 @@ TEST(Joints, copy) TEST(Joints, unitTest) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Joints joints(model); - std::vector names(joints.nameDof()); + Model model(modelPathForGeneralTesting); + rigidbody::Joints joints(model); + std::vector names(joints.nameDof()); - std::vector expectedNames(joints.nbDof()); + std::vector expectedNames(joints.nbDof()); expectedNames = { "Pelvis_TransY", "Pelvis_TransZ", "Pelvis_RotX", "BrasD_RotZ", "BrasD_RotX", "BrasG_RotZ", "BrasG_RotX", "CuisseD_RotX", "JambeD_RotX", "PiedD_RotX", "CuisseG_RotX", @@ -402,23 +403,23 @@ TEST(Joints, unitTest) } } { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Joints joints(model); - biorbd::rigidbody::Segment segmentToTest(joints.segment("Tronc")); + Model model(modelPathForGeneralTesting); + rigidbody::Joints joints(model); + rigidbody::Segment segmentToTest(joints.segment("Tronc")); EXPECT_EQ(segmentToTest.id(), 2147483647); //TODO: Verify ID value } { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Joints joints(model); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); + Model model(modelPathForGeneralTesting); + rigidbody::Joints joints(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); for (size_t i=0; i(i) * 0.2; Qdot[i] = static_cast(i) * 1.2; } - biorbd::utils::Vector3d angularMomentum(joints.angularMomentum(Q, Qdot)); + utils::Vector3d angularMomentum(joints.angularMomentum(Q, Qdot)); std::vector expectedAngularMomentum = {15.957205552043206, -2.399856350425782, 2.0751269909741334}; for (int i = 0; i < 3; ++i) { @@ -431,12 +432,12 @@ TEST(Joints, unitTest) TEST(Markers, copy) { { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::Markers markers(model); + Model model(modelPathForGeneralTesting); + rigidbody::Markers markers(model); - biorbd::rigidbody::Markers shallowCopy(markers); - biorbd::rigidbody::Markers deepCopyNow(markers.DeepCopy()); - biorbd::rigidbody::Markers deepCopyLater; + rigidbody::Markers shallowCopy(markers); + rigidbody::Markers deepCopyNow(markers.DeepCopy()); + rigidbody::Markers deepCopyLater; deepCopyLater.DeepCopy(markers); EXPECT_EQ(markers.nbMarkers(), 97); @@ -445,7 +446,7 @@ TEST(Markers, copy) EXPECT_EQ(deepCopyLater.nbMarkers(), 97); - biorbd::rigidbody::NodeSegment nodeSegment; + rigidbody::NodeSegment nodeSegment; markers.addMarker(nodeSegment, "markerName", "parentName", true, true, "x", 98); @@ -458,7 +459,7 @@ TEST(Markers, copy) TEST(SegmentCharacteristics, length) { - biorbd::rigidbody::SegmentCharacteristics segmentCharac; + rigidbody::SegmentCharacteristics segmentCharac; SCALAR_TO_DOUBLE(length1, segmentCharac.length()); EXPECT_NEAR(length1, 0., requiredPrecision); segmentCharac.setLength(2.); @@ -468,7 +469,7 @@ TEST(SegmentCharacteristics, length) TEST(Segment, nameDof) { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); EXPECT_THROW(model.segment(128), std::runtime_error); } @@ -488,34 +489,34 @@ modelPathForRTwrong6("models/IMUandCustomRT/RT_wrong6.bioMod"); TEST(RotoTransNode, Read) { { - biorbd::Model model(modelPathForRTsane); - biorbd::rigidbody::RotoTransNodes rt(model); + Model model(modelPathForRTsane); + rigidbody::RotoTransNodes rt(model); EXPECT_EQ(rt.size(), 3); } { - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong1), std::runtime_error); - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong2), std::runtime_error); - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong3), std::runtime_error); - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong4), std::runtime_error); - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong5), std::runtime_error); - EXPECT_THROW(biorbd::Model model(modelPathForRTwrong6), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong1), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong2), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong3), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong4), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong5), std::runtime_error); + EXPECT_THROW(Model model(modelPathForRTwrong6), std::runtime_error); } } TEST(RotoTransNode, copy) { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::RotoTransNodes rtNode(model); - biorbd::utils::RotoTransNode rt( - biorbd::utils::RotoTrans(biorbd::utils::Vector3d(2, 3, 4), - biorbd::utils::Vector3d(), "xyz"), "", "" ); + Model model(modelPathForGeneralTesting); + rigidbody::RotoTransNodes rtNode(model); + utils::RotoTransNode rt( + utils::RotoTrans(utils::Vector3d(2, 3, 4), + utils::Vector3d(), "xyz"), "", "" ); rtNode.addRT(rt); - biorbd::rigidbody::RotoTransNodes shallowCopy(rtNode); - biorbd::rigidbody::RotoTransNodes deepCopyNow(rtNode.DeepCopy()); - biorbd::rigidbody::RotoTransNodes deepCopyLater; + rigidbody::RotoTransNodes shallowCopy(rtNode); + rigidbody::RotoTransNodes deepCopyNow(rtNode.DeepCopy()); + rigidbody::RotoTransNodes deepCopyLater; deepCopyLater.DeepCopy(rtNode); EXPECT_EQ(shallowCopy.RTs().size(), 1); @@ -532,10 +533,10 @@ TEST(RotoTransNode, copy) TEST(RotoTransNode, unitTest) { { - biorbd::rigidbody::RotoTransNodes rtNode; - biorbd::utils::RotoTransNode rt( - biorbd::utils::RotoTrans(biorbd::utils::Vector3d(2, 3, 4), - biorbd::utils::Vector3d(), "xyz"), "", "" ); + rigidbody::RotoTransNodes rtNode; + utils::RotoTransNode rt( + utils::RotoTrans(utils::Vector3d(2, 3, 4), + utils::Vector3d(), "xyz"), "", "" ); rtNode.addRT(rt); auto rt_vector(rtNode.RTs()); #ifndef BIORBD_USE_CASADI_MATH @@ -544,23 +545,23 @@ TEST(RotoTransNode, unitTest) #endif } { - biorbd::rigidbody::RotoTransNodes rtNode; - biorbd::utils::RotoTransNode rt( - biorbd::utils::RotoTrans(biorbd::utils::Vector3d(2, 3, 4), - biorbd::utils::Vector3d(), "xyz"), "", "" ); + rigidbody::RotoTransNodes rtNode; + utils::RotoTransNode rt( + utils::RotoTrans(utils::Vector3d(2, 3, 4), + utils::Vector3d(), "xyz"), "", "" ); rtNode.addRT(rt); - std::vector rt_vector(rtNode.RTs()); + std::vector rt_vector(rtNode.RTs()); rt_vector[0].setParent("parentName"); rt_vector[0].setName("nameSet"); - std::vector expectedNames = { "nameSet" }; + std::vector expectedNames = { "nameSet" }; for (unsigned int i = 0; i < rt_vector.size(); ++i) { - EXPECT_STREQ(rtNode.RTs("parentName")[i].biorbd::utils::Node::name().c_str(), + EXPECT_STREQ(rtNode.RTs("parentName")[i].utils::Node::name().c_str(), expectedNames[i].c_str()); EXPECT_STREQ(rtNode.RTsNames()[i].c_str(), expectedNames[i].c_str()); } } { - biorbd::rigidbody::RotoTransNodes rtNode; + rigidbody::RotoTransNodes rtNode; rtNode.addRT(); unsigned int numberOfRTs(rtNode.nbRTs()); EXPECT_EQ(numberOfRTs, 1); @@ -571,7 +572,7 @@ TEST(RotoTransNode, unitTest) TEST(NodeSegment, unitTests) { { - biorbd::rigidbody::NodeSegment nodeSegment(1.1, 2.2, 3.3); + rigidbody::NodeSegment nodeSegment(1.1, 2.2, 3.3); SCALAR_TO_DOUBLE(x, nodeSegment.x()); SCALAR_TO_DOUBLE(y, nodeSegment.y()); SCALAR_TO_DOUBLE(z, nodeSegment.z()); @@ -580,36 +581,36 @@ TEST(NodeSegment, unitTests) EXPECT_NEAR(z, 3.3, requiredPrecision); } { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); EXPECT_STREQ(nodeSegment.parent().c_str(), "parentName"); } { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); EXPECT_EQ(nodeSegment.isAxisKept(2), false); EXPECT_EQ(nodeSegment.isAxisRemoved(2), true); } { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); - std::vector vector = { "x", "y" }; + std::vector vector = { "x", "y" }; nodeSegment.addAxesToRemove(vector); EXPECT_STREQ(nodeSegment.axesToRemove().c_str(), "xyz"); } { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); std::vector vector = {0, 1}; nodeSegment.addAxesToRemove(vector); EXPECT_STREQ(nodeSegment.axesToRemove().c_str(), "xyz"); } { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); EXPECT_THROW(nodeSegment.addAxesToRemove(4), std::runtime_error); - biorbd::utils::String string("m"); + utils::String string("m"); EXPECT_THROW(nodeSegment.addAxesToRemove(string), std::runtime_error); } } @@ -617,11 +618,11 @@ TEST(NodeSegment, unitTests) TEST(NodeSegment, copy) { - biorbd::rigidbody::NodeSegment nodeSegment(biorbd::utils::Vector3d(2, 3, 4), + rigidbody::NodeSegment nodeSegment(utils::Vector3d(2, 3, 4), "nodeSegmentName", "parentName", true, true, "z", 8); - biorbd::rigidbody::NodeSegment deepCopyNow(nodeSegment.DeepCopy()); - biorbd::rigidbody::NodeSegment deepCopyLater; + rigidbody::NodeSegment deepCopyNow(nodeSegment.DeepCopy()); + rigidbody::NodeSegment deepCopyLater; deepCopyLater.DeepCopy(nodeSegment); EXPECT_EQ(nodeSegment.nbAxesToRemove(), 1); @@ -632,7 +633,7 @@ TEST(NodeSegment, copy) TEST(DegreesOfFreedom, count) { { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); EXPECT_EQ(model.nbQ(), 13); EXPECT_EQ(model.nbQdot(), 13); EXPECT_EQ(model.nbQddot(), 13); @@ -640,7 +641,7 @@ TEST(DegreesOfFreedom, count) EXPECT_EQ(model.nbRoot(), 3); } { - biorbd::Model model(modelNoRoot); + Model model(modelNoRoot); EXPECT_EQ(model.nbQ(), 13); EXPECT_EQ(model.nbQdot(), 13); EXPECT_EQ(model.nbQddot(), 13); @@ -651,12 +652,12 @@ TEST(DegreesOfFreedom, count) TEST(DegressOfFreedom, ranges) { - biorbd::Model model(modelPathForGeneralTesting); - std::vector QRanges; - std::vector QDotRanges; - std::vector QDDotRanges; + Model model(modelPathForGeneralTesting); + std::vector QRanges; + std::vector QDotRanges; + std::vector QDDotRanges; - auto a = model.meshPoints(biorbd::rigidbody::GeneralizedCoordinates(model)); + auto a = model.meshPoints(rigidbody::GeneralizedCoordinates(model)); // Pelvis QRanges = model.segment(0).QRanges(); @@ -747,7 +748,7 @@ static std::vector QtestEqualsMarker = {0.1, 0.1, 0.1, 0.3, 0.3, 0.3}; TEST(CoM, kinematics) { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(Qdot, model); DECLARE_GENERALIZED_ACCELERATION(Qddot, model); @@ -777,18 +778,18 @@ TEST(CoM, kinematics) TEST(Segment, copy) { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::SegmentCharacteristics characteristics( - 10, biorbd::utils::Vector3d(0.5, 0.5, 0.5), + Model model(modelPathForGeneralTesting); + rigidbody::SegmentCharacteristics characteristics( + 10, utils::Vector3d(0.5, 0.5, 0.5), RigidBodyDynamics::Math::Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1)); - std::vector ranges(6); - biorbd::rigidbody::Segment MasterSegment( + std::vector ranges(6); + rigidbody::Segment MasterSegment( model, "MasterSegment", "NoParent", "zyx", "yzx", ranges, ranges, ranges, characteristics, RigidBodyDynamics::Math::SpatialTransform()); - biorbd::rigidbody::Segment ShallowCopy(MasterSegment); - biorbd::rigidbody::Segment ShallowCopyEqual = MasterSegment; - biorbd::rigidbody::Segment DeepCopyNow(MasterSegment.DeepCopy()); - biorbd::rigidbody::Segment DeepCopyLater; + rigidbody::Segment ShallowCopy(MasterSegment); + rigidbody::Segment ShallowCopyEqual = MasterSegment; + rigidbody::Segment DeepCopyNow(MasterSegment.DeepCopy()); + rigidbody::Segment DeepCopyLater; DeepCopyLater.DeepCopy(MasterSegment); EXPECT_STREQ(MasterSegment.parent().c_str(), "NoParent"); @@ -806,11 +807,11 @@ TEST(Segment, copy) TEST(Mesh, copy) { - biorbd::rigidbody::Mesh MasterMesh; + rigidbody::Mesh MasterMesh; MasterMesh.setPath("./MyFile.bioMesh"); - biorbd::rigidbody::Mesh ShallowCopy(MasterMesh); - biorbd::rigidbody::Mesh DeepCopyNow(MasterMesh.DeepCopy()); - biorbd::rigidbody::Mesh DeepCopyLater; + rigidbody::Mesh ShallowCopy(MasterMesh); + rigidbody::Mesh DeepCopyNow(MasterMesh.DeepCopy()); + rigidbody::Mesh DeepCopyLater; DeepCopyLater.DeepCopy(MasterMesh); EXPECT_STREQ(MasterMesh.path().relativePath().c_str(), "./MyFile.bioMesh"); @@ -832,17 +833,17 @@ static std::vector> expectedMarkers = { }; TEST(Markers, allPositions) { - biorbd::Model model(modelPathMeshEqualsMarker); + Model model(modelPathMeshEqualsMarker); EXPECT_EQ(model.nbQ(), 6); EXPECT_EQ(model.nbMarkers(), 4); - biorbd::rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedCoordinates Q(model); for (unsigned int i=0; i markers(model.markers(Q, true, + std::vector markers(model.markers(Q, true, true)); for (unsigned int i=0; i> mesh(model.meshPoints(Q)); - std::vector markers(model.markers(Q)); + std::vector> mesh(model.meshPoints(Q)); + std::vector markers(model.markers(Q)); for (unsigned int idx=0; idx> mesh(model.meshPoints(Q)); - std::vector markers(model.markers(Q)); + std::vector> mesh(model.meshPoints(Q)); + std::vector markers(model.markers(Q)); for (unsigned int idx=0; idx f_ext; + std::vector f_ext; for (size_t i=0; i<2; ++i) { double di = static_cast(i); - f_ext.push_back(biorbd::utils::SpatialVector( + f_ext.push_back(utils::SpatialVector( (di+1)*11.1, (di+1)*22.2, (di+1)*33.3, (di+1)*44.4, (di+1)*55.5, (di+1)*66.6)); } @@ -1035,7 +1036,7 @@ TEST(Dynamics, ForwardDynAndExternalForces) TEST(QDot, ComputeConstraintImpulsesDirect) { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); @@ -1065,7 +1066,7 @@ TEST(QDot, ComputeConstraintImpulsesDirect) TEST(Dynamics, ForwardLoopConstraint) { - biorbd::Model model(modelPathForLoopConstraintTesting); + Model model(modelPathForLoopConstraintTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); DECLARE_GENERALIZED_TORQUE(Tau, model); @@ -1094,7 +1095,7 @@ TEST(Dynamics, ForwardLoopConstraint) TEST(Dynamics, ForwardAccelerationConstraint) { - biorbd::Model model(modelPathForGeneralTesting); + Model model(modelPathForGeneralTesting); DECLARE_GENERALIZED_COORDINATES(Q, model); DECLARE_GENERALIZED_VELOCITY(QDot, model); DECLARE_GENERALIZED_ACCELERATION(QDDot_constrained, model); @@ -1117,7 +1118,7 @@ TEST(Dynamics, ForwardAccelerationConstraint) -30.485214214095965, 112.82341345760311 }; - biorbd::rigidbody::Contacts cs(model.getConstraints()); + rigidbody::Contacts cs(model.getConstraints()); CALL_BIORBD_FUNCTION_3ARGS1PARAM(QDDot, model, ForwardDynamicsConstraintsDirect, Q, QDot, Tau, cs); for (unsigned int i = 0; i({1, 2, 3})); { @@ -1182,8 +1183,8 @@ TEST(Kinematics, computeQdot) #ifndef SKIP_LONG_TESTS TEST(Kalman, markers) { - biorbd::Model model(modelPathForGeneralTesting); - biorbd::rigidbody::KalmanReconsMarkers kalman(model); + Model model(modelPathForGeneralTesting); + rigidbody::KalmanReconsMarkers kalman(model); #ifdef BIORBD_USE_CASADI_MATH // Because the evaluation functions are not sym, it takes a LOOOONG time unsigned int nQToTest(1); @@ -1192,15 +1193,15 @@ TEST(Kalman, markers) #endif // Compute reference - biorbd::rigidbody::GeneralizedCoordinates Qref(model); + rigidbody::GeneralizedCoordinates Qref(model); for (unsigned int i=0; i targetMarkers(model.markers(Qref)); + std::vector targetMarkers(model.markers(Qref)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedAcceleration Qddot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedAcceleration Qddot(model); kalman.reconstructFrame(model, targetMarkers, &Q, &Qdot, &Qddot); // Compare results (since the initialization of the filter is done 50X, it is expected to have converged) @@ -1252,8 +1253,8 @@ TEST(Kalman, markers) #ifndef SKIP_LONG_TESTS TEST(Kalman, imu) { - biorbd::Model model(modelPathForPyomecaman_withIMUs); - biorbd::rigidbody::KalmanReconsIMU kalman(model); + Model model(modelPathForPyomecaman_withIMUs); + rigidbody::KalmanReconsIMU kalman(model); #ifdef BIORBD_USE_CASADI_MATH // Because the evaluation functions are not sym, it takes a LOOOONG time unsigned int nQToTest(3); @@ -1265,15 +1266,15 @@ TEST(Kalman, imu) #endif // Compute reference - biorbd::rigidbody::GeneralizedCoordinates Qref(model); + rigidbody::GeneralizedCoordinates Qref(model); for (unsigned int i=0; i targetImus(model.IMU(Qref)); + std::vector targetImus(model.IMU(Qref)); - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedAcceleration Qddot(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedAcceleration Qddot(model); kalman.reconstructFrame(model, targetImus, &Q, &Qdot, &Qddot); // Compare results (since the initialization of the filter is done 50X, it is expected to have converged) diff --git a/test/test_utils.cpp b/test/test_utils.cpp index d6aa9f6f..6dbf2eef 100644 --- a/test/test_utils.cpp +++ b/test/test_utils.cpp @@ -16,34 +16,36 @@ #include "RigidBody/NodeSegment.h" static double requiredPrecision(1e-10); +using namespace biorbd::BIORBD_MATH_NAMESPACE; + TEST(ShallowCopy, DeepCopy) { // DeepCopying a shallow copy, should also change the reference // Warning that may be surprising because one may be tend to DeepCopy // itself afterward, this doesn't release the shallowcopy referencing - biorbd::utils::Vector3d MainNode(0, 0, 0, "NoName", "NoParent"); - biorbd::utils::Vector3d ShallowToDeep(MainNode); - biorbd::utils::Vector3d NewNode(0, 0, 0, "MyName", "MyParent"); - EXPECT_STREQ(MainNode.biorbd::utils::Node::name().c_str(), "NoName"); - EXPECT_STREQ(ShallowToDeep.biorbd::utils::Node::name().c_str(), "NoName"); - EXPECT_STREQ(NewNode.biorbd::utils::Node::name().c_str(), "MyName"); + utils::Vector3d MainNode(0, 0, 0, "NoName", "NoParent"); + utils::Vector3d ShallowToDeep(MainNode); + utils::Vector3d NewNode(0, 0, 0, "MyName", "MyParent"); + EXPECT_STREQ(MainNode.utils::Node::name().c_str(), "NoName"); + EXPECT_STREQ(ShallowToDeep.utils::Node::name().c_str(), "NoName"); + EXPECT_STREQ(NewNode.utils::Node::name().c_str(), "MyName"); ShallowToDeep.setName("StillNoName"); - EXPECT_STREQ(MainNode.biorbd::utils::Node::name().c_str(), "StillNoName"); - EXPECT_STREQ(ShallowToDeep.biorbd::utils::Node::name().c_str(), "StillNoName"); - EXPECT_STREQ(NewNode.biorbd::utils::Node::name().c_str(), "MyName"); + EXPECT_STREQ(MainNode.utils::Node::name().c_str(), "StillNoName"); + EXPECT_STREQ(ShallowToDeep.utils::Node::name().c_str(), "StillNoName"); + EXPECT_STREQ(NewNode.utils::Node::name().c_str(), "MyName"); ShallowToDeep.DeepCopy(NewNode); - EXPECT_STREQ(MainNode.biorbd::utils::Node::name().c_str(), "MyName"); - EXPECT_STREQ(ShallowToDeep.biorbd::utils::Node::name().c_str(), "MyName"); - EXPECT_STREQ(NewNode.biorbd::utils::Node::name().c_str(), "MyName"); + EXPECT_STREQ(MainNode.utils::Node::name().c_str(), "MyName"); + EXPECT_STREQ(ShallowToDeep.utils::Node::name().c_str(), "MyName"); + EXPECT_STREQ(NewNode.utils::Node::name().c_str(), "MyName"); ShallowToDeep.setName("BackToNoName"); - EXPECT_STREQ(MainNode.biorbd::utils::Node::name().c_str(), "BackToNoName"); - EXPECT_STREQ(ShallowToDeep.biorbd::utils::Node::name().c_str(), "BackToNoName"); - EXPECT_STREQ(NewNode.biorbd::utils::Node::name().c_str(), "MyName"); + EXPECT_STREQ(MainNode.utils::Node::name().c_str(), "BackToNoName"); + EXPECT_STREQ(ShallowToDeep.utils::Node::name().c_str(), "BackToNoName"); + EXPECT_STREQ(NewNode.utils::Node::name().c_str(), "MyName"); } TEST(String, conversions) { - biorbd::utils::String str(biorbd::utils::String::to_string(M_PI)); + utils::String str(utils::String::to_string(M_PI)); double pi = std::stod(str); EXPECT_EQ(pi, M_PI); } @@ -51,11 +53,11 @@ TEST(String, conversions) TEST(Path, Create) { { - biorbd::utils::Path emptyPath; + utils::Path emptyPath; EXPECT_STREQ(emptyPath.absolutePath().c_str(), - biorbd::utils::Path::currentDir().c_str()); + utils::Path::currentDir().c_str()); EXPECT_STREQ(emptyPath.absoluteFolder().c_str(), - biorbd::utils::Path::currentDir().c_str()); + utils::Path::currentDir().c_str()); EXPECT_STREQ(emptyPath.filename().c_str(), ""); EXPECT_STREQ(emptyPath.extension().c_str(), ""); EXPECT_STREQ(emptyPath.originalPath().c_str(), ""); @@ -63,22 +65,22 @@ TEST(Path, Create) { EXPECT_STREQ( - biorbd::utils::Path::toUnixFormat( + utils::Path::toUnixFormat( "MyUgly\\\\WindowsPath\\\\ToMyFile.biorbd").c_str(), - biorbd::utils::String ( + utils::String ( "MyUgly/WindowsPath/ToMyFile.biorbd").c_str()); EXPECT_STREQ( - biorbd::utils::Path::toWindowsFormat( + utils::Path::toWindowsFormat( "MyCute/UnixPath/ToMyFile.biorbd").c_str(), - biorbd::utils::String ( + utils::String ( "MyCute\\\\UnixPath\\\\ToMyFile.biorbd").c_str()); } { - biorbd::utils::String myPathInUglyWindowsStyleString( + utils::String myPathInUglyWindowsStyleString( "MyUgly\\\\WindowsPath\\\\ToMyFile.biorbd"); - biorbd::utils::Path myPathInUglyWindowsStyle( + utils::Path myPathInUglyWindowsStyle( myPathInUglyWindowsStyleString); EXPECT_STREQ(myPathInUglyWindowsStyle.relativePath().c_str(), "./MyUgly/WindowsPath/ToMyFile.biorbd"); @@ -88,20 +90,20 @@ TEST(Path, Create) { #ifdef _WIN32 - biorbd::utils::String path( + utils::String path( "C:\\MyLovely\\AbsolutePath\\ToMyLovelyFile.biorbd"); - biorbd::utils::String unixPath( + utils::String unixPath( "C:/MyLovely/AbsolutePath/ToMyLovelyFile.biorbd"); - biorbd::utils::String absoluteUnixFolder("C:/MyLovely/AbsolutePath/"); + utils::String absoluteUnixFolder("C:/MyLovely/AbsolutePath/"); #else - biorbd::utils::String path( + utils::String path( "/MyLovely/AbsolutePath/ToMyLovelyFile.biorbd"); - biorbd::utils::String unixPath( + utils::String unixPath( "/MyLovely/AbsolutePath/ToMyLovelyFile.biorbd"); - biorbd::utils::String absoluteUnixFolder("/MyLovely/AbsolutePath/"); + utils::String absoluteUnixFolder("/MyLovely/AbsolutePath/"); #endif - biorbd::utils::Path absolutePath(path); + utils::Path absolutePath(path); EXPECT_STREQ(absolutePath.absolutePath().c_str(), unixPath.c_str()); EXPECT_STREQ(absolutePath.absoluteFolder().c_str(), absoluteUnixFolder.c_str()); EXPECT_STREQ(absolutePath.filename().c_str(), "ToMyLovelyFile"); @@ -110,12 +112,12 @@ TEST(Path, Create) } { - biorbd::utils::Path relativePath("MyLovely/RelativePath/ToMyLovelyFile.biorbd"); + utils::Path relativePath("MyLovely/RelativePath/ToMyLovelyFile.biorbd"); EXPECT_STREQ(relativePath.absolutePath().c_str(), - (biorbd::utils::Path::currentDir() + + (utils::Path::currentDir() + "MyLovely/RelativePath/ToMyLovelyFile.biorbd").c_str()); EXPECT_STREQ(relativePath.absoluteFolder().c_str(), - (biorbd::utils::Path::currentDir() + "MyLovely/RelativePath/").c_str()); + (utils::Path::currentDir() + "MyLovely/RelativePath/").c_str()); EXPECT_STREQ(relativePath.filename().c_str(), "ToMyLovelyFile"); EXPECT_STREQ(relativePath.extension().c_str(), "biorbd"); EXPECT_STREQ(relativePath.relativePath().c_str(), @@ -125,13 +127,13 @@ TEST(Path, Create) } { - biorbd::utils::Path + utils::Path weirdRelativePath("./MyLovely/RelativePath/ToMyLovelyFile.biorbd"); EXPECT_STREQ(weirdRelativePath.absolutePath().c_str(), - (biorbd::utils::Path::currentDir() + + (utils::Path::currentDir() + "MyLovely/RelativePath/ToMyLovelyFile.biorbd").c_str()); EXPECT_STREQ(weirdRelativePath.absoluteFolder().c_str(), - (biorbd::utils::Path::currentDir() + "MyLovely/RelativePath/").c_str()); + (utils::Path::currentDir() + "MyLovely/RelativePath/").c_str()); EXPECT_STREQ(weirdRelativePath.filename().c_str(), "ToMyLovelyFile"); EXPECT_STREQ(weirdRelativePath.extension().c_str(), "biorbd"); EXPECT_STREQ(weirdRelativePath.relativePath().c_str(), @@ -141,13 +143,13 @@ TEST(Path, Create) } { - biorbd::utils::Path + utils::Path parentRelativePath("../MyLovely/ParentPath/ToMyLovelyFile.biorbd"); EXPECT_STREQ(parentRelativePath.absolutePath().c_str(), - (biorbd::utils::Path::currentDir() + + (utils::Path::currentDir() + "../MyLovely/ParentPath/ToMyLovelyFile.biorbd").c_str()); EXPECT_STREQ(parentRelativePath.absoluteFolder().c_str(), - (biorbd::utils::Path::currentDir() + "../MyLovely/ParentPath/").c_str()); + (utils::Path::currentDir() + "../MyLovely/ParentPath/").c_str()); EXPECT_STREQ(parentRelativePath.filename().c_str(), "ToMyLovelyFile"); EXPECT_STREQ(parentRelativePath.extension().c_str(), "biorbd"); EXPECT_STREQ(parentRelativePath.relativePath().c_str(), @@ -157,11 +159,11 @@ TEST(Path, Create) } { - biorbd::utils::Path noPath("MyLonelyFile.biorbd"); + utils::Path noPath("MyLonelyFile.biorbd"); EXPECT_STREQ(noPath.absolutePath().c_str(), - (biorbd::utils::Path::currentDir() + "MyLonelyFile.biorbd").c_str()); + (utils::Path::currentDir() + "MyLonelyFile.biorbd").c_str()); EXPECT_STREQ(noPath.absoluteFolder().c_str(), - biorbd::utils::Path::currentDir().c_str()); + utils::Path::currentDir().c_str()); EXPECT_STREQ(noPath.filename().c_str(), "MyLonelyFile"); EXPECT_STREQ(noPath.extension().c_str(), "biorbd"); EXPECT_STREQ(noPath.relativePath().c_str(), "./MyLonelyFile.biorbd"); @@ -169,11 +171,11 @@ TEST(Path, Create) } { - biorbd::utils::Path almostNoPath("./MyKinbDofLonelyFile.biorbd"); + utils::Path almostNoPath("./MyKinbDofLonelyFile.biorbd"); EXPECT_STREQ(almostNoPath.absolutePath().c_str(), - (biorbd::utils::Path::currentDir() + "MyKinbDofLonelyFile.biorbd").c_str()); + (utils::Path::currentDir() + "MyKinbDofLonelyFile.biorbd").c_str()); EXPECT_STREQ(almostNoPath.absoluteFolder().c_str(), - biorbd::utils::Path::currentDir().c_str()); + utils::Path::currentDir().c_str()); EXPECT_STREQ(almostNoPath.filename().c_str(), "MyKinbDofLonelyFile"); EXPECT_STREQ(almostNoPath.extension().c_str(), "biorbd"); EXPECT_STREQ(almostNoPath.relativePath().c_str(), @@ -185,10 +187,10 @@ TEST(Path, Create) TEST(Path, Copy) { - biorbd::utils::Path MainPath("MyLovelyPath.biorbd"); - biorbd::utils::Path ShallowCopy(MainPath); - biorbd::utils::Path DeepCopyNow(MainPath.DeepCopy()); - biorbd::utils::Path DeepCopyLater; + utils::Path MainPath("MyLovelyPath.biorbd"); + utils::Path ShallowCopy(MainPath); + utils::Path DeepCopyNow(MainPath.DeepCopy()); + utils::Path DeepCopyLater; DeepCopyLater.DeepCopy(MainPath); EXPECT_STREQ(MainPath.relativePath().c_str(), "./MyLovelyPath.biorbd"); @@ -216,13 +218,13 @@ TEST(Path, Copy) TEST(Vector3d, rotate) { - biorbd::utils::Vector3d node(2, 3, 4); - biorbd::utils::Vector3d nodeXrot(node.DeepCopy()); + utils::Vector3d node(2, 3, 4); + utils::Vector3d nodeXrot(node.DeepCopy()); SCALAR_TO_DOUBLE(x, node[0]); SCALAR_TO_DOUBLE(y, node[1]); SCALAR_TO_DOUBLE(z, node[2]); - nodeXrot.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(M_PI, 0, 0), - biorbd::utils::Vector3d(0, 0, 0), + nodeXrot.applyRT( utils::RotoTrans(utils::Vector3d(M_PI, 0, 0), + utils::Vector3d(0, 0, 0), "xyz") ); { SCALAR_TO_DOUBLE(nodeX, nodeXrot[0]); @@ -233,9 +235,9 @@ TEST(Vector3d, rotate) EXPECT_NEAR(nodeZ, -z, requiredPrecision); } - biorbd::utils::Vector3d nodeYrot(node.DeepCopy()); - nodeYrot.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(0, M_PI, 0), - biorbd::utils::Vector3d(0, 0, 0), + utils::Vector3d nodeYrot(node.DeepCopy()); + nodeYrot.applyRT( utils::RotoTrans(utils::Vector3d(0, M_PI, 0), + utils::Vector3d(0, 0, 0), "xyz") ); { SCALAR_TO_DOUBLE(nodeX, nodeYrot[0]); @@ -246,9 +248,9 @@ TEST(Vector3d, rotate) EXPECT_NEAR(nodeZ, -z, requiredPrecision); } - biorbd::utils::Vector3d nodeZrot(node.DeepCopy()); - nodeZrot.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(0, 0, M_PI), - biorbd::utils::Vector3d(0, 0, 0), + utils::Vector3d nodeZrot(node.DeepCopy()); + nodeZrot.applyRT( utils::RotoTrans(utils::Vector3d(0, 0, M_PI), + utils::Vector3d(0, 0, 0), "xyz") ); { SCALAR_TO_DOUBLE(nodeX, nodeZrot[0]); @@ -259,9 +261,9 @@ TEST(Vector3d, rotate) EXPECT_NEAR(nodeZ, z, requiredPrecision); } - biorbd::utils::Vector3d nodeZrot2(node.DeepCopy()); - nodeZrot2.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(M_PI, 0, 0), - biorbd::utils::Vector3d(0, 0, 0), + utils::Vector3d nodeZrot2(node.DeepCopy()); + nodeZrot2.applyRT( utils::RotoTrans(utils::Vector3d(M_PI, 0, 0), + utils::Vector3d(0, 0, 0), "zxy") ); { SCALAR_TO_DOUBLE(nodeX, nodeZrot2[0]); @@ -273,10 +275,10 @@ TEST(Vector3d, rotate) } double trans(2); - biorbd::utils::Vector3d nodeRot(node.DeepCopy()); - nodeRot.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(M_PI/6, + utils::Vector3d nodeRot(node.DeepCopy()); + nodeRot.applyRT( utils::RotoTrans(utils::Vector3d(M_PI/6, M_PI/6, M_PI/6), - biorbd::utils::Vector3d(trans, trans, trans), "xyz") ); + utils::Vector3d(trans, trans, trans), "xyz") ); { SCALAR_TO_DOUBLE(nodeX, nodeRot[0]); SCALAR_TO_DOUBLE(nodeY, nodeRot[1]); @@ -286,9 +288,9 @@ TEST(Vector3d, rotate) EXPECT_NEAR(nodeZ, 6.698557158514987, requiredPrecision); } - biorbd::utils::Vector3d nodeTrans(node.DeepCopy()); - nodeTrans.applyRT( biorbd::utils::RotoTrans(biorbd::utils::Vector3d(0, 0, 0), - biorbd::utils::Vector3d(2, 2, 2), "xyz") ); + utils::Vector3d nodeTrans(node.DeepCopy()); + nodeTrans.applyRT( utils::RotoTrans(utils::Vector3d(0, 0, 0), + utils::Vector3d(2, 2, 2), "xyz") ); { SCALAR_TO_DOUBLE(nodeX, nodeTrans[0]); SCALAR_TO_DOUBLE(nodeY, nodeTrans[1]); @@ -301,10 +303,10 @@ TEST(Vector3d, rotate) TEST(Vector3d, Copy) { - biorbd::utils::Vector3d MainNode(1, 2, 3, "MainNodeName", "NoParent"); - biorbd::utils::Vector3d ShallowCopy(MainNode); - biorbd::utils::Vector3d DeepCopyNow(MainNode.DeepCopy()); - biorbd::utils::Vector3d DeepCopyLater; + utils::Vector3d MainNode(1, 2, 3, "MainNodeName", "NoParent"); + utils::Vector3d ShallowCopy(MainNode); + utils::Vector3d DeepCopyNow(MainNode.DeepCopy()); + utils::Vector3d DeepCopyLater; DeepCopyLater.DeepCopy(MainNode); EXPECT_STREQ(MainNode.parent().c_str(), "NoParent"); @@ -383,13 +385,13 @@ TEST(Vector3d, Copy) TEST(Matrix, Copy) { - biorbd::utils::Matrix MainMatrix(4, 4); + utils::Matrix MainMatrix(4, 4); FILL_MATRIX(MainMatrix, std::vector({0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 })); - biorbd::utils::Matrix DeepCopy(MainMatrix); + utils::Matrix DeepCopy(MainMatrix); // Test for the values { @@ -431,20 +433,20 @@ TEST(Matrix, Copy) TEST(Matrix, unitTest) { - biorbd::utils::Matrix mat1(3, 4); + utils::Matrix mat1(3, 4); FILL_MATRIX(mat1, std::vector({4.1, 5.1, 6.1, 7.1, 8.1, 9.1, 10.1, 11.1, 12.1, 13.1, 14.1, 15.1 })); { - biorbd::utils::Matrix mat2(3, 4); + utils::Matrix mat2(3, 4); FILL_MATRIX(mat2, std::vector({4.1, 5.1, 6.1, 7.1, 8.1, 9.1, 10.1, 11.1, 12.1, 13.1, 14.1, 15.1 })); - biorbd::utils::Matrix sum(mat1 + mat2); + utils::Matrix sum(mat1 + mat2); std::vector expectedSum = { 4.1*2, 5.1*2, 6.1*2, 7.1*2, 8.1*2, 9.1*2, 10.1*2, 11.1*2, @@ -463,7 +465,7 @@ TEST(Matrix, unitTest) } { - biorbd::utils::Matrix mat2(4, 2); + utils::Matrix mat2(4, 2); FILL_MATRIX(mat2, std::vector({ 4.1, 5.1, 6.1, 7.1, @@ -471,7 +473,7 @@ TEST(Matrix, unitTest) 10.1, 11.1 })); - biorbd::utils::Matrix mult(mat1 * mat2); + utils::Matrix mult(mat1 * mat2); std::vector expectedMult = { 169.04, 191.44, 282.64, 321.04, @@ -489,9 +491,9 @@ TEST(Matrix, unitTest) } { - biorbd::utils::Vector vec(4); + utils::Vector vec(4); FILL_VECTOR(vec, std::vector({4.1, 5.1, 6.1, 7.1})); - biorbd::utils::Vector mult(mat1 * vec); + utils::Vector mult(mat1 * vec); std::vector expectedMult({130.44, 220.04, 309.64}); @@ -507,13 +509,13 @@ TEST(Matrix, unitTest) TEST(Rotation, unitTest) { - biorbd::utils::Rotation rot1( - biorbd::utils::Vector3d(M_PI/3, M_PI/3, -M_PI/3), "xyz"); - biorbd::utils::Rotation rot2( - biorbd::utils::Vector3d(M_PI/3, M_PI/3, M_PI/3), "xyz"); + utils::Rotation rot1( + utils::Vector3d(M_PI/3, M_PI/3, -M_PI/3), "xyz"); + utils::Rotation rot2( + utils::Vector3d(M_PI/3, M_PI/3, M_PI/3), "xyz"); { - biorbd::utils::Rotation mult(rot1 * rot2); + utils::Rotation mult(rot1 * rot2); std::vector expectedMult({ 0.87439881604791125, 0.4185095264191645, 0.24551270189221938, 0.48131011839520887, -0.68413452641916439, -0.54799682452694543, @@ -532,8 +534,8 @@ TEST(Rotation, unitTest) TEST(RotoTrans, unitTest) { { - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(1, 1, 1), biorbd::utils::Vector3d(1, 1, 1), "xyz"); + utils::RotoTrans rt( + utils::Vector3d(1, 1, 1), utils::Vector3d(1, 1, 1), "xyz"); std::vector rtExpected({ 0.29192658172642888, -0.45464871341284091, 0.84147098480789650, 1, 0.83722241402998721, -0.30389665486452672, -0.45464871341284091, 1, @@ -548,7 +550,7 @@ TEST(RotoTrans, unitTest) } } - biorbd::utils::RotoTrans rt_t(rt.transpose()); + utils::RotoTrans rt_t(rt.transpose()); std::vector rtTransposedExpected({ 0.29192658172642888, 0.83722241402998721, 0.46242567005663016, -1.5915746658130461, -0.45464871341284091, -0.30389665486452672, 0.83722241402998732, -0.07867704575261969, @@ -564,7 +566,7 @@ TEST(RotoTrans, unitTest) } } - biorbd::utils::Vector3d marker(1, 1, 1); + utils::Vector3d marker(1, 1, 1); marker.applyRT(rt_t); for (unsigned int i=0; i<3; ++i) { SCALAR_TO_DOUBLE(val, marker(i, 0)); @@ -573,15 +575,15 @@ TEST(RotoTrans, unitTest) } { - biorbd::rigidbody::NodeSegment origin(1, 2, 3); - biorbd::rigidbody::NodeSegment axis1(4, 2, 5); - biorbd::rigidbody::NodeSegment axis2(3, -2, 1); + rigidbody::NodeSegment origin(1, 2, 3); + rigidbody::NodeSegment axis1(4, 2, 5); + rigidbody::NodeSegment axis2(3, -2, 1); { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"x", "y"}, "x")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"y", "x"}, "x")); std::vector rtExpected({ @@ -602,11 +604,11 @@ TEST(RotoTrans, unitTest) } { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"x", "y"}, "y")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"y", "x"}, "y")); std::vector rtExpected({ 0.8320502943378437, 0.31606977062050695, 0.4558423058385518, 1, @@ -626,11 +628,11 @@ TEST(RotoTrans, unitTest) } { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"x", "z"}, "x")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"z", "x"}, "x")); std::vector rtExpected({ 0.7909115788387002, -0.4558423058385518, 0.4082482904638631, 1, @@ -650,11 +652,11 @@ TEST(RotoTrans, unitTest) } { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"x", "z"}, "z")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"z", "x"}, "z")); std::vector rtExpected({ 0.8320502943378437, -0.4558423058385518, 0.31606977062050695, 1, @@ -674,11 +676,11 @@ TEST(RotoTrans, unitTest) } { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"y", "z"}, "y")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"z", "y"}, "y")); std::vector rtExpected({ 0.4558423058385518, 0.7909115788387002, 0.4082482904638631, 1, @@ -698,11 +700,11 @@ TEST(RotoTrans, unitTest) } { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt( + utils::RotoTrans::fromMarkers( origin, {origin, axis1}, {origin, axis2}, {"y", "z"}, "z")); - biorbd::utils::RotoTrans rt2( - biorbd::utils::RotoTrans::fromMarkers( + utils::RotoTrans rt2( + utils::RotoTrans::fromMarkers( origin, {origin, axis2}, {origin, axis1}, {"z", "y"}, "z")); std::vector rtExpected({ 0.4558423058385518, 0.8320502943378437, 0.31606977062050695, 1, @@ -725,14 +727,14 @@ TEST(RotoTrans, unitTest) TEST(RotoTransNode, Copy) { - biorbd::utils::RotoTrans tp( - biorbd::utils::Vector3d(1, 2, 3), - biorbd::utils::Vector3d(1, 2, 3), "xyz"); - - biorbd::utils::RotoTransNode MainRotoTransNode(tp, "NoName", "NoParent"); - biorbd::utils::RotoTransNode ShallowCopy(MainRotoTransNode); - biorbd::utils::RotoTransNode DeepCopyNow(MainRotoTransNode.DeepCopy()); - biorbd::utils::RotoTransNode DeepCopyLater; + utils::RotoTrans tp( + utils::Vector3d(1, 2, 3), + utils::Vector3d(1, 2, 3), "xyz"); + + utils::RotoTransNode MainRotoTransNode(tp, "NoName", "NoParent"); + utils::RotoTransNode ShallowCopy(MainRotoTransNode); + utils::RotoTransNode DeepCopyNow(MainRotoTransNode.DeepCopy()); + utils::RotoTransNode DeepCopyLater; DeepCopyLater.DeepCopy(MainRotoTransNode); EXPECT_STREQ(MainRotoTransNode.parent().c_str(), "NoParent"); @@ -783,10 +785,10 @@ TEST(ModelReading, equations) // The equation model was built so the x coordinates of the meshes should // be evaluated to the y coordinates. - biorbd::Model m("models/equations.bioMod"); - biorbd::rigidbody::GeneralizedCoordinates Q(m); + Model m("models/equations.bioMod"); + rigidbody::GeneralizedCoordinates Q(m); Q.setZero(); - std::vector mesh(m.meshPoints(Q, 0, true)); + std::vector mesh(m.meshPoints(Q, 0, true)); for (auto node : mesh) { SCALAR_TO_DOUBLE(nodeX, node.x()); SCALAR_TO_DOUBLE(nodeY, node.y()); @@ -797,7 +799,7 @@ TEST(ModelReading, equations) TEST(Quaternion, creation) { { - biorbd::utils::Quaternion quat; + utils::Quaternion quat; SCALAR_TO_DOUBLE(quatW, quat.w()); SCALAR_TO_DOUBLE(quatX, quat.x()); SCALAR_TO_DOUBLE(quatY, quat.y()); @@ -809,7 +811,7 @@ TEST(Quaternion, creation) EXPECT_NEAR(quat.kStab(), 1, requiredPrecision); } { - biorbd::utils::Quaternion quat(1,2,3,4); + utils::Quaternion quat(1,2,3,4); SCALAR_TO_DOUBLE(quatW, quat.w()); SCALAR_TO_DOUBLE(quatX, quat.x()); SCALAR_TO_DOUBLE(quatY, quat.y()); @@ -830,7 +832,7 @@ TEST(Quaternion, creation) } { #ifdef BIORBD_USE_EIGEN3_MATH - biorbd::utils::Quaternion quat(Eigen::Vector4d(1,2,3,4)); + utils::Quaternion quat(Eigen::Vector4d(1,2,3,4)); EXPECT_NEAR(quat.w(), 1, requiredPrecision); EXPECT_NEAR(quat.x(), 2, requiredPrecision); EXPECT_NEAR(quat.y(), 3, requiredPrecision); @@ -838,7 +840,7 @@ TEST(Quaternion, creation) #endif } { - biorbd::utils::Quaternion quat(1, biorbd::utils::Vector3d(2,3,4)); + utils::Quaternion quat(1, utils::Vector3d(2,3,4)); SCALAR_TO_DOUBLE(quatW, quat.w()); SCALAR_TO_DOUBLE(quatX, quat.x()); SCALAR_TO_DOUBLE(quatY, quat.y()); @@ -849,9 +851,9 @@ TEST(Quaternion, creation) EXPECT_NEAR(quatZ, 4, requiredPrecision); } { - biorbd::utils::Quaternion quat1(1,2,3,4,5); - biorbd::utils::Quaternion quat2(quat1); - biorbd::utils::Quaternion quat3 = quat1; + utils::Quaternion quat1(1,2,3,4,5); + utils::Quaternion quat2(quat1); + utils::Quaternion quat3 = quat1; SCALAR_TO_DOUBLE(quat2W, quat2.w()); SCALAR_TO_DOUBLE(quat2X, quat2.x()); @@ -876,8 +878,8 @@ TEST(Quaternion, creation) { #ifdef BIORBD_USE_EIGEN3_MATH Eigen::Vector4d quat1(1,2,3,4); - biorbd::utils::Quaternion quat2(quat1); - biorbd::utils::Quaternion quat3; + utils::Quaternion quat2(quat1); + utils::Quaternion quat3; quat3 = quat1; EXPECT_NEAR(quat2.w(), 1, requiredPrecision); @@ -897,9 +899,9 @@ TEST(Quaternion, creation) TEST(Quaternion, addition) { - biorbd::utils::Quaternion q1(1,2,3,4,2); - biorbd::utils::Quaternion q2(2,3,4,5,3); - biorbd::utils::Quaternion q12 = q1+q2; + utils::Quaternion q1(1,2,3,4,2); + utils::Quaternion q2(2,3,4,5,3); + utils::Quaternion q12 = q1+q2; SCALAR_TO_DOUBLE(q12W, q12.w()); SCALAR_TO_DOUBLE(q12X, q12.x()); @@ -914,13 +916,13 @@ TEST(Quaternion, addition) TEST(Quaternion, multiplication) { - biorbd::utils::Quaternion q1(1,2,3,4,2); - biorbd::utils::Quaternion q2(2,3,4,5,3); + utils::Quaternion q1(1,2,3,4,2); + utils::Quaternion q2(2,3,4,5,3); double d(5); float f(5); { - biorbd::utils::Quaternion q12 = q1*q2; + utils::Quaternion q12 = q1*q2; SCALAR_TO_DOUBLE(q12W, q12.w()); SCALAR_TO_DOUBLE(q12X, q12.x()); SCALAR_TO_DOUBLE(q12Y, q12.y()); @@ -932,7 +934,7 @@ TEST(Quaternion, multiplication) EXPECT_NEAR(q12.kStab(), 2.5, requiredPrecision); } { - biorbd::utils::Quaternion q1d = q1*d; + utils::Quaternion q1d = q1*d; SCALAR_TO_DOUBLE(q1dW, q1d.w()); SCALAR_TO_DOUBLE(q1dX, q1d.x()); SCALAR_TO_DOUBLE(q1dY, q1d.y()); @@ -945,7 +947,7 @@ TEST(Quaternion, multiplication) } { - biorbd::utils::Quaternion q1f = q1*f; + utils::Quaternion q1f = q1*f; SCALAR_TO_DOUBLE(q1fW, q1f.w()); SCALAR_TO_DOUBLE(q1fX, q1f.x()); SCALAR_TO_DOUBLE(q1fY, q1f.y()); @@ -961,8 +963,8 @@ TEST(Quaternion, multiplication) TEST(Quaternion, conversion) { { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromGLRotate(1, 2, 3, 4, 5)); + utils::Quaternion q( + utils::Quaternion::fromGLRotate(1, 2, 3, 4, 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -974,9 +976,9 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromAxisAngle( - 1, biorbd::utils::Vector3d(2, 3, 4), 5)); + utils::Quaternion q( + utils::Quaternion::fromAxisAngle( + 1, utils::Vector3d(2, 3, 4), 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -987,11 +989,11 @@ TEST(Quaternion, conversion) EXPECT_NEAR(qZ, 0.35610835008729086, requiredPrecision); EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } - biorbd::utils::RotoTrans rt( - biorbd::utils::Vector3d(2, 3, 4), biorbd::utils::Vector3d(), "xyz"); + utils::RotoTrans rt( + utils::Vector3d(2, 3, 4), utils::Vector3d(), "xyz"); { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromMatrix(rt, 5)); + utils::Quaternion q( + utils::Quaternion::fromMatrix(rt, 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -1003,8 +1005,8 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromMatrix(rt.rot(), 5)); + utils::Quaternion q( + utils::Quaternion::fromMatrix(rt.rot(), 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -1016,9 +1018,9 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromZYXAngles( - biorbd::utils::Vector3d(2, 3, 4), 5)); + utils::Quaternion q( + utils::Quaternion::fromZYXAngles( + utils::Vector3d(2, 3, 4), 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -1030,9 +1032,9 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromYXZAngles( - biorbd::utils::Vector3d(2, 3, 4), 5)); + utils::Quaternion q( + utils::Quaternion::fromYXZAngles( + utils::Vector3d(2, 3, 4), 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -1044,9 +1046,9 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromXYZAngles( - biorbd::utils::Vector3d(2, 3, 4), 5)); + utils::Quaternion q( + utils::Quaternion::fromXYZAngles( + utils::Vector3d(2, 3, 4), 5)); SCALAR_TO_DOUBLE(qW, q.w()); SCALAR_TO_DOUBLE(qX, q.x()); SCALAR_TO_DOUBLE(qY, q.y()); @@ -1058,11 +1060,11 @@ TEST(Quaternion, conversion) EXPECT_NEAR(q.kStab(), 5, requiredPrecision); } { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); + utils::Quaternion q(2, 3, 4, 5, 6); #ifndef BIORBD_USE_CASADI_MATH EXPECT_THROW(q.toMatrix(), std::runtime_error); #endif - biorbd::utils::Rotation mat(q.toMatrix(true)); + utils::Rotation mat(q.toMatrix(true)); SCALAR_TO_DOUBLE(mat00, mat(0, 0)); SCALAR_TO_DOUBLE(mat01, mat(0, 1)); @@ -1084,14 +1086,14 @@ TEST(Quaternion, conversion) EXPECT_NEAR(mat22, -49, requiredPrecision); } { - biorbd::utils::Vector3d rot (0.2, 0.3, 0.4); - biorbd::utils::RotoTrans rt_from_euler; - rt_from_euler = biorbd::utils::RotoTrans::fromEulerAngles(rot, - biorbd::utils::Vector3d(), "xyz"); + utils::Vector3d rot (0.2, 0.3, 0.4); + utils::RotoTrans rt_from_euler; + rt_from_euler = utils::RotoTrans::fromEulerAngles(rot, + utils::Vector3d(), "xyz"); - biorbd::utils::Quaternion q( - biorbd::utils::Quaternion::fromXYZAngles(rot, 5)); - biorbd::utils::RotoTrans rt_from_quat(q.toMatrix()); + utils::Quaternion q( + utils::Quaternion::fromXYZAngles(rot, 5)); + utils::RotoTrans rt_from_quat(q.toMatrix()); for (unsigned int i=0; i<4; ++i) { for (unsigned int j=0; j<4; ++j) { @@ -1101,7 +1103,7 @@ TEST(Quaternion, conversion) } } - biorbd::utils::Quaternion qFromRt(biorbd::utils::Quaternion::fromMatrix( + utils::Quaternion qFromRt(utils::Quaternion::fromMatrix( rt_from_quat)); for (unsigned int i=0; i<4; ++i) { SCALAR_TO_DOUBLE(valQ, q(i)); @@ -1115,9 +1117,9 @@ TEST(Quaternion, otherOperations) { #ifndef BIORBD_USE_CASADI_MATH { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); - biorbd::utils::Quaternion q2(3, 4, 5, 6, 8); - biorbd::utils::Quaternion qSlerp(q.slerp (7, q2)); + utils::Quaternion q(2, 3, 4, 5, 6); + utils::Quaternion q2(3, 4, 5, 6, 8); + utils::Quaternion qSlerp(q.slerp (7, q2)); SCALAR_TO_DOUBLE(qSlerpW, qSlerp.w()); SCALAR_TO_DOUBLE(qSlerpX, qSlerp.x()); @@ -1131,8 +1133,8 @@ TEST(Quaternion, otherOperations) } #endif { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); - biorbd::utils::Quaternion qConj(q.conjugate()); + utils::Quaternion q(2, 3, 4, 5, 6); + utils::Quaternion qConj(q.conjugate()); SCALAR_TO_DOUBLE(qConjW, qConj.w()); SCALAR_TO_DOUBLE(qConjX, qConj.x()); @@ -1144,9 +1146,9 @@ TEST(Quaternion, otherOperations) EXPECT_NEAR(qConjZ, -5, requiredPrecision); EXPECT_NEAR(qConj.kStab(), 6, requiredPrecision); } { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); - biorbd::utils::Quaternion qTime( - q.timeStep(biorbd::utils::Vector3d(7, 8, 9), 0.1)); + utils::Quaternion q(2, 3, 4, 5, 6); + utils::Quaternion qTime( + q.timeStep(utils::Vector3d(7, 8, 9), 0.1)); SCALAR_TO_DOUBLE(qTimeW, qTime.w()); SCALAR_TO_DOUBLE(qTimeX, qTime.x()); @@ -1158,8 +1160,8 @@ TEST(Quaternion, otherOperations) EXPECT_NEAR(qTimeZ, 4.8489346122578709, requiredPrecision); EXPECT_NEAR(qTime.kStab(), 6, requiredPrecision); } { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); - biorbd::utils::Vector3d vec(q.rotate(biorbd::utils::Vector3d(7, 8, 9))); + utils::Quaternion q(2, 3, 4, 5, 6); + utils::Vector3d vec(q.rotate(utils::Vector3d(7, 8, 9))); SCALAR_TO_DOUBLE(vecX, vec.x()); SCALAR_TO_DOUBLE(vecY, vec.y()); @@ -1168,8 +1170,8 @@ TEST(Quaternion, otherOperations) EXPECT_NEAR(vecY, 384, requiredPrecision); EXPECT_NEAR(vecZ, 582, requiredPrecision); } { - biorbd::utils::Quaternion q(2, 3, 4, 5, 6); - biorbd::utils::Quaternion qdot(q.omegaToQDot(biorbd::utils::Vector3d(7, 8, 9))); + utils::Quaternion q(2, 3, 4, 5, 6); + utils::Quaternion qdot(q.omegaToQDot(utils::Vector3d(7, 8, 9))); SCALAR_TO_DOUBLE(qdotW, qdot.w()); SCALAR_TO_DOUBLE(qdotX, qdot.x()); @@ -1181,8 +1183,8 @@ TEST(Quaternion, otherOperations) EXPECT_NEAR(qdotZ, 7, requiredPrecision); EXPECT_NEAR(qdot.kStab(), 6, requiredPrecision); } { - biorbd::utils::Quaternion q1(1, 0, 0, 0, 5); - biorbd::utils::Vector v(3); + utils::Quaternion q1(1, 0, 0, 0, 5); + utils::Vector v(3); FILL_VECTOR(v, std::vector({1,2,3})); q1.derivate(v); { @@ -1200,7 +1202,7 @@ TEST(Quaternion, otherOperations) double x(0.7035975447302919); double y(0.7035975447302919); double z(0.07035975447302918); - biorbd::utils::Quaternion q2(w,x,y,z, 5); + utils::Quaternion q2(w,x,y,z, 5); q2.derivate(v); { SCALAR_TO_DOUBLE(q2W, q2.w()); @@ -1219,10 +1221,10 @@ TEST(Quaternion, otherOperations) TEST(Quaternion, velocities) { { - biorbd::utils::Vector3d e(0.1,0.2,0.3); - biorbd::utils::Vector3d eR(0.4,0.5,0.6); - biorbd::utils::Quaternion q; - biorbd::utils::Vector3d w(q.eulerDotToOmega(eR,e,"xyz")); + utils::Vector3d e(0.1,0.2,0.3); + utils::Vector3d eR(0.4,0.5,0.6); + utils::Quaternion q; + utils::Vector3d w(q.eulerDotToOmega(eR,e,"xyz")); SCALAR_TO_DOUBLE(w0, w[0]); SCALAR_TO_DOUBLE(w1, w[1]); @@ -1237,7 +1239,7 @@ TEST(Quaternion, velocities) TEST(Quaternion, normalization) { { - biorbd::utils::Quaternion q(1,1,1,1); + utils::Quaternion q(1,1,1,1); q.normalize(); SCALAR_TO_DOUBLE(q0, q[0]); SCALAR_TO_DOUBLE(q1, q[1]); @@ -1256,15 +1258,15 @@ TEST(Vector, operation) //TODO: Addition Scalar + Vector is undefined //{ - // biorbd::utils::Vector v(4); + // utils::Vector v(4); // v[0] = 1.1; // v[1] = 1.2; // v[2] = 1.3; // v[3] = 1.4; - // biorbd::utils::Scalar s(5); - // biorbd::utils::Vector multVectAndScalar(v + s); + // utils::Scalar s(5); + // utils::Vector multVectAndScalar(v + s); - // biorbd::utils::Vector expectedMultVectAndScalar(4); + // utils::Vector expectedMultVectAndScalar(4); // expectedMultVectAndScalar << 6.1, 6.2, 6.3, 6.4; // for (unsigned int i = 0; i < 4; ++i) { @@ -1273,13 +1275,13 @@ TEST(Vector, operation) //} { - biorbd::utils::Vector v(4); + utils::Vector v(4); v[0] = 1.1; v[1] = 1.2; v[2] = 1.3; v[3] = 1.4; - biorbd::utils::Scalar s(5); - biorbd::utils::Vector multVectAndScalar(s * v); + utils::Scalar s(5); + utils::Vector multVectAndScalar(s * v); std::vector expectedMultVectAndScalar({5.5, 6.0, 6.5, 7.0}); @@ -1293,7 +1295,7 @@ TEST(Vector, operation) TEST(Vector, norm) { { - biorbd::utils::Vector v(4); + utils::Vector v(4); FILL_VECTOR(v, std::vector({1.1, 1.2, 1.3, 1.4})); SCALAR_TO_DOUBLE(val2false, v.norm(2, false)); @@ -1306,10 +1308,10 @@ TEST(Vector, norm) TEST(Vector, normGradient) { { - biorbd::utils::Vector v(4); + utils::Vector v(4); FILL_VECTOR(v, std::vector({1.1, 1.2, 1.3, 1.4})); - biorbd::utils::Vector nG(v.normGradient(2, false)); + utils::Vector nG(v.normGradient(2, false)); std::vector expectednG({ 0.43825049008927769, 0.47809144373375745, 0.51793239737823726, 0.55777335102271697 }); From b2e618e28401b238402e6a3616f4090fc1f49de6 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 12:47:33 -0400 Subject: [PATCH 09/14] Adapted c --- binding/c/biorbd_c.cpp | 163 +++++++++++++++++++++-------------------- binding/c/biorbd_c.h | 79 ++++++++++---------- 2 files changed, 123 insertions(+), 119 deletions(-) diff --git a/binding/c/biorbd_c.cpp b/binding/c/biorbd_c.cpp index 1daacb83..a5afe73f 100644 --- a/binding/c/biorbd_c.cpp +++ b/binding/c/biorbd_c.cpp @@ -19,85 +19,86 @@ #include "RigidBody/KalmanReconsIMU.h" #endif -biorbd::Model* c_biorbdModel( +using namespace biorbd::BIORBD_MATH_NAMESPACE; + +Model* c_biorbdModel( const char* pathToModel) { - return new biorbd::Model(biorbd::Reader::readModelFile(biorbd::utils::String( - pathToModel))); + return new Model(Reader::readModelFile(utils::String(pathToModel))); } void c_deleteBiorbdModel( - biorbd::Model* model) + Model* model) { delete model; } void c_writeBiorbdModel( - biorbd::Model* model, + Model* model, const char * path) { - biorbd::Writer::writeModel(*model, biorbd::utils::Path(path)); + Writer::writeModel(*model, utils::Path(path)); } // Joints functions void c_boneRotationSequence( - biorbd::Model* m, + Model* m, const char* segName, char* seq) { // Memory for seq must be already allocated - biorbd::utils::String sequence(m->segment(segName).seqR()); + utils::String sequence(m->segment(segName).seqR()); snprintf(seq, sequence.length() + 1, "%s", sequence.c_str()); } void c_localJCS( - biorbd::Model* m, + Model* m, int i, double* rt_out) { - biorbd::utils::RotoTrans RT(m->segment(static_cast + utils::RotoTrans RT(m->segment(static_cast (i)).localJCS()); dispatchRToutput(RT, rt_out); } void c_globalJCS( - biorbd::Model* m, + Model* m, const double* Q, double* jcs) { // Dispatch des données d'entrée - biorbd::rigidbody::GeneralizedCoordinates eQ(dispatchQinput(m, Q)); + rigidbody::GeneralizedCoordinates eQ(dispatchQinput(m, Q)); // Récupérer JCS - std::vector pre_jcs = m->allGlobalJCS(eQ); + std::vector pre_jcs = m->allGlobalJCS(eQ); // Dispatch de l'output dispatchRToutput(pre_jcs, jcs); } void c_inverseDynamics( - biorbd::Model *model, + Model *model, const double *q, const double *qdot, const double *qddot, double *tau) { - biorbd::rigidbody::GeneralizedCoordinates Q( + rigidbody::GeneralizedCoordinates Q( dispatchQinput(model, q)); - biorbd::rigidbody::GeneralizedVelocity Qdot( + rigidbody::GeneralizedVelocity Qdot( dispatchQinput(model, qdot)); - biorbd::rigidbody::GeneralizedAcceleration Qddot( + rigidbody::GeneralizedAcceleration Qddot( dispatchQinput(model, qddot)); - biorbd::rigidbody::GeneralizedTorque Tau(*model); + rigidbody::GeneralizedTorque Tau(*model); RigidBodyDynamics::InverseDynamics(*model, Q, Qdot, Qddot, Tau); dispatchTauOutput(Tau, tau); } void c_massMatrix( - biorbd::Model* model, + Model* model, const double* q, double* massMatrix) { unsigned int nQ(model->nbQ()); - biorbd::rigidbody::GeneralizedCoordinates Q( + rigidbody::GeneralizedCoordinates Q( dispatchQinput(model, q)); RigidBodyDynamics::Math::MatrixNd Mass(nQ, nQ); @@ -110,13 +111,13 @@ void c_massMatrix( } } void c_CoM( - biorbd::Model* model, + Model* model, const double* q, double *com) { - biorbd::rigidbody::GeneralizedCoordinates Q(dispatchQinput(model, q)); + rigidbody::GeneralizedCoordinates Q(dispatchQinput(model, q)); - biorbd::utils::Vector3d CoM(model->CoM(Q)); + utils::Vector3d CoM(model->CoM(Q)); dispatchVectorOutput(CoM, com); } @@ -124,22 +125,22 @@ void c_CoM( // dof functions int c_nQ( - biorbd::Model* model) + Model* model) { return static_cast(model->nbQ()); } int c_nQDot( - biorbd::Model* model) + Model* model) { return static_cast(model->nbQdot()); } int c_nQDDot( - biorbd::Model* model) + Model* model) { return static_cast(model->nbQddot()); } int c_nGeneralizedTorque( - biorbd::Model* model) + Model* model) { return static_cast(model->nbGeneralizedTorque()); } @@ -147,36 +148,36 @@ int c_nGeneralizedTorque( // Markers functions int c_nMarkers( - biorbd::Model* model) + Model* model) { return static_cast(model->nbMarkers()); } void c_markersInLocal( - biorbd::Model* model, + Model* model, double* markPos) { // Prepare output dispatchMarkersOutput(model->markers(), markPos); } void c_markers( - biorbd::Model* model, + Model* model, const double* Q, double* markPos, bool removeAxis, bool updateKin) { // Prepare parameters - biorbd::rigidbody::GeneralizedCoordinates eQ(dispatchQinput(model, Q)); + rigidbody::GeneralizedCoordinates eQ(dispatchQinput(model, Q)); // Call the main function - std::vector pos(model->markers(eQ, removeAxis, + std::vector pos(model->markers(eQ, removeAxis, updateKin)); // Prepare output dispatchMarkersOutput(pos, markPos); } void c_addMarker( - biorbd::Model *model, + Model *model, const double *markPos, const char* name, const char* parentName, @@ -185,7 +186,7 @@ void c_addMarker( const char* axesToRemove) { int parent_int = static_cast(model->GetBodyId(parentName)); - biorbd::utils::Vector3d pos(dispatchMarkersInput( + utils::Vector3d pos(dispatchMarkersInput( markPos)); // Position du marker dans le repère local model->addMarker(pos, name, parentName, technical, anatomical, axesToRemove, parent_int); @@ -194,39 +195,39 @@ void c_addMarker( // IMUs functions int c_nIMUs( - biorbd::Model* model) + Model* model) { return static_cast(model->nbIMUs()); } void c_addIMU( - biorbd::Model *model, + Model *model, const double *imuRT, const char *name, const char *parentName, bool technical, bool anatomical) { - biorbd::utils::RotoTransNode pos(dispatchRTinput(imuRT), name, parentName); + utils::RotoTransNode pos(dispatchRTinput(imuRT), name, parentName); model->addIMU(pos, technical, anatomical); } // Kalman IMU #ifndef SKIP_KALMAN -biorbd::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( - biorbd::Model* model, +rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( + Model* model, double* QinitialGuess, double freq, double noiseF, double errorF) { // Créer un pointeur sur un filtre de kalman - biorbd::rigidbody::KalmanReconsIMU* kalman = new - biorbd::rigidbody::KalmanReconsIMU( - *model, biorbd::rigidbody::KalmanParam(freq, noiseF, errorF)); + rigidbody::KalmanReconsIMU* kalman = new + rigidbody::KalmanReconsIMU( + *model, rigidbody::KalmanParam(freq, noiseF, errorF)); // Mettre le initial guess - biorbd::rigidbody::GeneralizedCoordinates e_QinitialGuess(*model); + rigidbody::GeneralizedCoordinates e_QinitialGuess(*model); if (QinitialGuess != nullptr) { for (size_t i = 0; inbQ(); ++i) { e_QinitialGuess[static_cast(i)] = QinitialGuess[i]; @@ -236,28 +237,28 @@ biorbd::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( return kalman; } -void c_deleteBiorbdKalmanReconsIMU(biorbd::rigidbody::KalmanReconsIMU* model) +void c_deleteBiorbdKalmanReconsIMU(rigidbody::KalmanReconsIMU* model) { delete model; } void c_BiorbdKalmanReconsIMUstep( - biorbd::Model* model, - biorbd::rigidbody::KalmanReconsIMU* kalman, + Model* model, + rigidbody::KalmanReconsIMU* kalman, double* imu, double* Q, double* QDot, double* QDDot) { // Copier les valeurs des matrices de rotation des IMUs dans un stl Vector - biorbd::utils::Vector T(3*3*model->nbIMUs()); // Matrice 3*3 * nIMU + utils::Vector T(3*3*model->nbIMUs()); // Matrice 3*3 * nIMU for (unsigned int i=0; inbIMUs(); ++i) for (unsigned int j=0; j<9; ++j) { // matrice 3*3 T[9*i+j] = imu[9*i+j]; } // Se faire des entrés sur Q, QDot et QDDot - biorbd::rigidbody::GeneralizedCoordinates e_Q(*model); - biorbd::rigidbody::GeneralizedVelocity e_QDot(*model); - biorbd::rigidbody::GeneralizedAcceleration e_QDDot(*model); + rigidbody::GeneralizedCoordinates e_Q(*model); + rigidbody::GeneralizedVelocity e_QDot(*model); + rigidbody::GeneralizedAcceleration e_QDDot(*model); // Faire le filtre kalman->reconstructFrame(*model, T, &e_Q, &e_QDot, &e_QDDot); @@ -276,8 +277,8 @@ void c_matrixMultiplication( double* Mout) { // Recueillir les données d'entrée - biorbd::utils::RotoTrans mM1(dispatchRTinput(M1)); - biorbd::utils::RotoTrans mM2(dispatchRTinput(M2)); + utils::RotoTrans mM1(dispatchRTinput(M1)); + utils::RotoTrans mM2(dispatchRTinput(M2)); // Projeter et préparer les données de sortie dispatchRToutput(mM1.operator*(mM2), Mout); @@ -287,7 +288,7 @@ void c_meanRT( unsigned int nFrame, double* imuRT_mean) { - std::vector m; + std::vector m; // Dispatch des données d'entrée for (unsigned int i=0; i &allMarkers, + const std::vector &allMarkers, double* markers) { // Warning markers must already be allocated! @@ -363,17 +364,17 @@ void dispatchMarkersOutput( } } -biorbd::rigidbody::GeneralizedCoordinates dispatchQinput( - biorbd::Model* model, +rigidbody::GeneralizedCoordinates dispatchQinput( + Model* model, const double* Q) { - biorbd::rigidbody::GeneralizedCoordinates eQ(*model); + rigidbody::GeneralizedCoordinates eQ(*model); for (int i = 0; i < static_cast(model->nbQ()); ++i) { eQ[i] = Q[i]; } return eQ; } -void dispatchQoutput(const biorbd::rigidbody::GeneralizedCoordinates &eQ, +void dispatchQoutput(const rigidbody::GeneralizedCoordinates &eQ, double*Q) { // Warnging Q must already be allocated @@ -382,7 +383,7 @@ void dispatchQoutput(const biorbd::rigidbody::GeneralizedCoordinates &eQ, } } void dispatchTauOutput( - const biorbd::rigidbody::GeneralizedTorque &eTau, + const rigidbody::GeneralizedTorque &eTau, double* Tau) { // Warnging Q must already be allocated @@ -391,7 +392,7 @@ void dispatchTauOutput( } } void dispatchDoubleOutput( - const biorbd::utils::Vector& e, + const utils::Vector& e, double* d) { // Warning output must already be allocated @@ -399,10 +400,10 @@ void dispatchDoubleOutput( d[i] = e[i]; } } -biorbd::utils::RotoTrans dispatchRTinput( +utils::RotoTrans dispatchRTinput( const double* rt) { - biorbd::utils::RotoTrans pos; + utils::RotoTrans pos; pos << rt[0], rt[4], rt[8], rt[12], rt[1], rt[5], rt[9], rt[13], rt[2], rt[6], rt[10], rt[14], @@ -410,7 +411,7 @@ biorbd::utils::RotoTrans dispatchRTinput( return pos; } void dispatchRToutput( - const biorbd::utils::RotoTrans& rt_in, + const utils::RotoTrans& rt_in, double* rt_out) { // Attention la mémoire doit déjà être allouée pour rt_out @@ -419,7 +420,7 @@ void dispatchRToutput( } } void dispatchRToutput( - const std::vector& rt_in, + const std::vector& rt_in, double* rt_out) { // Attention la mémoire doit déjà être allouée pour rt_out @@ -430,12 +431,12 @@ void dispatchRToutput( } } -biorbd::utils::Matrix dispatchMatrixInput( +utils::Matrix dispatchMatrixInput( const double* matXd, int nRows, int nCols) { - biorbd::utils::Matrix res( + utils::Matrix res( static_cast(nRows), static_cast(nCols)); for (int i = 0; i < nCols; ++i) { @@ -446,18 +447,18 @@ biorbd::utils::Matrix dispatchMatrixInput( return res; } -biorbd::utils::Vector dispatchVectorInput( +utils::Vector dispatchVectorInput( const double* vecXd, int nElements) { - biorbd::utils::Vector res(static_cast(nElements)); + utils::Vector res(static_cast(nElements)); for (int i = 0; i < nElements; ++i) { res(i) = vecXd[i]; } return res; } void dispatchVectorOutput( - const biorbd::utils::Vector& vect, + const utils::Vector& vect, double* vect_out) { // Warnging vect_out must already be allocated diff --git a/binding/c/biorbd_c.h b/binding/c/biorbd_c.h index 7226b69a..b5773c1b 100644 --- a/binding/c/biorbd_c.h +++ b/binding/c/biorbd_c.h @@ -12,80 +12,83 @@ namespace biorbd { +namespace BIORBD_MATH_NAMESPACE +{ namespace rigidbody { #ifdef MODULE_KALMAN class KalmanReconsIMU; #endif } - } +} + extern "C" { // Create a pointer on a model - BIORBD_API_C biorbd::Model* c_biorbdModel( + BIORBD_API_C biorbd::BIORBD_MATH_NAMESPACE::Model* c_biorbdModel( const char* pathToModel); BIORBD_API_C void c_deleteBiorbdModel( - biorbd::Model*); + biorbd::BIORBD_MATH_NAMESPACE::Model*); BIORBD_API_C void c_writeBiorbdModel( - biorbd::Model*, const char * path); + biorbd::BIORBD_MATH_NAMESPACE::Model*, const char * path); // Joints functions BIORBD_API_C void c_boneRotationSequence( // Return the angle sequence of a bone named segName - biorbd::Model* m, + biorbd::BIORBD_MATH_NAMESPACE::Model* m, const char* segName, char* seq); BIORBD_API_C void c_localJCS( // Return the LCS for segment of index i in parent coordinate system - biorbd::Model* m, + biorbd::BIORBD_MATH_NAMESPACE::Model* m, int i, double* RtOut); BIORBD_API_C void c_globalJCS( - biorbd::Model*, + biorbd::BIORBD_MATH_NAMESPACE::Model*, const double* Q, double* jcs); BIORBD_API_C void c_inverseDynamics( - biorbd::Model* model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double* q, const double* qdot, const double* qddot, double* tau); BIORBD_API_C void c_massMatrix( - biorbd::Model* model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double* q, double* massMatrix); BIORBD_API_C void c_CoM( - biorbd::Model* model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double* q, double *com); // dof functions BIORBD_API_C int c_nQ( - biorbd::Model* model); + biorbd::BIORBD_MATH_NAMESPACE::Model* model); BIORBD_API_C int c_nQDot( - biorbd::Model* model); + biorbd::BIORBD_MATH_NAMESPACE::Model* model); BIORBD_API_C int c_nQDDot( - biorbd::Model* model); + biorbd::BIORBD_MATH_NAMESPACE::Model* model); BIORBD_API_C int c_nGeneralizedTorque( - biorbd::Model* model); + biorbd::BIORBD_MATH_NAMESPACE::Model* model); // Markers functions BIORBD_API_C int c_nMarkers( - biorbd::Model* model); + biorbd::BIORBD_MATH_NAMESPACE::Model* model); BIORBD_API_C void c_markersInLocal( - biorbd::Model* model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, double* markPos); BIORBD_API_C void c_markers( - biorbd::Model* model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double* Q, double* markPos, bool removeAxis = true, bool updateKin = true); BIORBD_API_C void c_addMarker( - biorbd::Model *model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double *markPos, const char* name = "", const char* parentName = "", @@ -95,9 +98,9 @@ extern "C" { // IMUs functions BIORBD_API_C int c_nIMUs( - biorbd::Model*); + biorbd::BIORBD_MATH_NAMESPACE::Model*); BIORBD_API_C void c_addIMU( - biorbd::Model *model, + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double *imuRT, const char* name = "", const char* parentName = "", @@ -106,17 +109,17 @@ extern "C" { // Kalman IMU #ifdef MODULE_KALMAN - BIORBD_API_C biorbd::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( - biorbd::Model*, + BIORBD_API_C biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( + biorbd::BIORBD_MATH_NAMESPACE::Model*, double* QinitialGuess = nullptr, double freq = 100, double noiseF = 5e-3, double errorF = 1e-10); BIORBD_API_C void c_deleteBiorbdKalmanReconsIMU( - biorbd::rigidbody::KalmanReconsIMU*); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU*); BIORBD_API_C void c_BiorbdKalmanReconsIMUstep( - biorbd::Model*, - biorbd::rigidbody::KalmanReconsIMU*, + biorbd::BIORBD_MATH_NAMESPACE::Model*, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU*, double* imu, double* Q = nullptr, double* QDot = nullptr, @@ -149,40 +152,40 @@ extern "C" { } // Fonctions de dispatch pour les données d'entré et de sortie -biorbd::utils::Vector3d dispatchMarkersInput( +biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d dispatchMarkersInput( const double * pos); void dispatchMarkersOutput( - const std::vector &allMarkers, + const std::vector &allMarkers, double* markers); -biorbd::rigidbody::GeneralizedCoordinates dispatchQinput( - biorbd::Model* model, +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates dispatchQinput( + biorbd::BIORBD_MATH_NAMESPACE::Model* model, const double* Q); void dispatchQoutput( - const biorbd::rigidbody::GeneralizedCoordinates &eQ, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates &eQ, double* Q); void dispatchTauOutput( - const biorbd::rigidbody::GeneralizedTorque &eTau, + const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque &eTau, double* Tau); void dispatchDoubleOutput( - const biorbd::utils::Vector&, + const biorbd::BIORBD_MATH_NAMESPACE::utils::Vector&, double*); -biorbd::utils::RotoTrans dispatchRTinput( +biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans dispatchRTinput( const double* rt); void dispatchRToutput( - const biorbd::utils::RotoTrans& rt_in, + const biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans& rt_in, double* rt_out); void dispatchRToutput( - const std::vector& rt_in, + const std::vector& rt_in, double* rt_out); -biorbd::utils::Matrix dispatchMatrixInput( +biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix dispatchMatrixInput( const double* matXd, int nRows, int nCols); -biorbd::utils::Vector dispatchVectorInput( +biorbd::BIORBD_MATH_NAMESPACE::utils::Vector dispatchVectorInput( const double* vecXd, int nElement); void dispatchVectorOutput( - const biorbd::utils::Vector& vect, + const biorbd::BIORBD_MATH_NAMESPACE::utils::Vector& vect, double* vect_out); //#ifdef UNITY From 61b313f1d3e32ac460a986d71c2c69023937320e Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 13:06:18 -0400 Subject: [PATCH 10/14] Adapted Matlab --- binding/matlab/Matlab_ChangeShapeFactors.h | 4 +- binding/matlab/Matlab_CoM.h | 4 +- binding/matlab/Matlab_CoMJacobian.h | 4 +- binding/matlab/Matlab_CoMangularMomentum.h | 6 +- binding/matlab/Matlab_CoMddot.h | 8 +- binding/matlab/Matlab_CoMdot.h | 6 +- binding/matlab/Matlab_ContactGamma.h | 6 +- binding/matlab/Matlab_ContactJacobian.h | 4 +- binding/matlab/Matlab_ContactsPosition.h | 8 +- binding/matlab/Matlab_IMU.h | 20 ++-- binding/matlab/Matlab_IMUJacobian.h | 8 +- binding/matlab/Matlab_LocalMarkers.h | 14 +-- binding/matlab/Matlab_Markers.h | 20 ++-- binding/matlab/Matlab_MarkersJacobian.h | 8 +- binding/matlab/Matlab_Mesh.h | 8 +- binding/matlab/Matlab_MusclesActivationDot.h | 4 +- .../Matlab_MusclesExcitationDotBuchanan.h | 6 +- binding/matlab/Matlab_MusclesForce.h | 12 +- binding/matlab/Matlab_MusclesForceMax.h | 2 +- binding/matlab/Matlab_MusclesJacobian.h | 4 +- binding/matlab/Matlab_MusclesLength.h | 8 +- binding/matlab/Matlab_MusclesNames.h | 2 +- binding/matlab/Matlab_MusclesParentNames.h | 2 +- binding/matlab/Matlab_MusclesPoints.h | 24 ++-- binding/matlab/Matlab_NLeffects.h | 8 +- binding/matlab/Matlab_Patch.h | 12 +- binding/matlab/Matlab_ProjectCustomPoint.h | 12 +- binding/matlab/Matlab_ProjectPoint.h | 12 +- binding/matlab/Matlab_ProjectPointJacobian.h | 10 +- binding/matlab/Matlab_changeGravity.h | 2 +- binding/matlab/Matlab_computeQdot.h | 8 +- binding/matlab/Matlab_delete.h | 2 +- binding/matlab/Matlab_forwardDynamics.h | 12 +- binding/matlab/Matlab_globalJCS.h | 12 +- binding/matlab/Matlab_inverseDynamics.h | 12 +- binding/matlab/Matlab_inverseKinematics.h | 8 +- binding/matlab/Matlab_inverseKinematicsEKF.h | 44 +++---- .../matlab/Matlab_inverseKinematicsEKF_IMU.h | 44 +++---- binding/matlab/Matlab_localJCS.h | 6 +- binding/matlab/Matlab_massMatrix.h | 4 +- .../Matlab_muscleJointTorqueFromActivation.h | 12 +- .../Matlab_muscleJointTorqueFromExcitation.h | 14 +-- .../Matlab_muscleJointTorqueFromMuscleForce.h | 8 +- binding/matlab/Matlab_muscleLengthJacobian.h | 6 +- binding/matlab/Matlab_muscleUpdate.h | 12 +- binding/matlab/Matlab_muscleVelocity.h | 6 +- binding/matlab/Matlab_nBody.h | 2 +- binding/matlab/Matlab_nControl.h | 2 +- binding/matlab/Matlab_nIMU.h | 2 +- binding/matlab/Matlab_nMarkers.h | 2 +- binding/matlab/Matlab_nMuscles.h | 2 +- binding/matlab/Matlab_nQ.h | 2 +- binding/matlab/Matlab_nQddot.h | 2 +- binding/matlab/Matlab_nQdot.h | 2 +- binding/matlab/Matlab_nRoot.h | 2 +- binding/matlab/Matlab_nameBody.h | 2 +- binding/matlab/Matlab_nameDof.h | 2 +- binding/matlab/Matlab_nameIMU.h | 12 +- binding/matlab/Matlab_nameMarkers.h | 12 +- binding/matlab/Matlab_new.h | 6 +- binding/matlab/Matlab_parent.h | 2 +- binding/matlab/Matlab_rangeQ.h | 2 +- .../matlab/Matlab_segmentAngularMomentum.h | 8 +- binding/matlab/Matlab_segmentCoM.h | 8 +- binding/matlab/Matlab_segmentCoMddot.h | 12 +- binding/matlab/Matlab_segmentCoMdot.h | 10 +- binding/matlab/Matlab_segmentMarkers.h | 14 +-- binding/matlab/Matlab_segmentMass.h | 2 +- binding/matlab/Matlab_segmentsInertia.h | 6 +- binding/matlab/Matlab_segmentsVelocities.h | 15 ++- binding/matlab/Matlab_torqueActivation.h | 8 +- binding/matlab/Matlab_totalMass.h | 2 +- binding/matlab/biorbd.h | 12 +- binding/matlab/processArguments.h | 108 +++++++++--------- test/binding/c/test_binder_c.cpp | 80 ++++++------- 75 files changed, 400 insertions(+), 399 deletions(-) diff --git a/binding/matlab/Matlab_ChangeShapeFactors.h b/binding/matlab/Matlab_ChangeShapeFactors.h index 145faba1..dd83e1f0 100644 --- a/binding/matlab/Matlab_ChangeShapeFactors.h +++ b/binding/matlab/Matlab_ChangeShapeFactors.h @@ -16,7 +16,7 @@ void Matlab_ChangeShapeFactors( int, mxArray *[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model, 3rd is the shape factor for each muscle"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les shape factors std::vector shapeFactors = getDoubleArray(prhs, 2); @@ -32,7 +32,7 @@ void Matlab_ChangeShapeFactors( int, mxArray *[], for (unsigned int i=0; inbMuscleGroups(); ++i) for (unsigned int j=0; jmuscleGroup(i).nbMuscles(); ++j) { // Recueillir shape factor - dynamic_cast( model->muscleGroup( + dynamic_cast( model->muscleGroup( i).muscle(j).state() ).shapeFactor( shapeFactors[cmp]); ++cmp; diff --git a/binding/matlab/Matlab_CoM.h b/binding/matlab/Matlab_CoM.h index c65c7c89..d50a9748 100644 --- a/binding/matlab/Matlab_CoM.h +++ b/binding/matlab/Matlab_CoM.h @@ -14,11 +14,11 @@ void Matlab_CoM( int nlhs, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_CoMJacobian.h b/binding/matlab/Matlab_CoMJacobian.h index 933d78ba..54a7c4df 100644 --- a/binding/matlab/Matlab_CoMJacobian.h +++ b/binding/matlab/Matlab_CoMJacobian.h @@ -15,11 +15,11 @@ void Matlab_CoMJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la jacobienne du COM diff --git a/binding/matlab/Matlab_CoMangularMomentum.h b/binding/matlab/Matlab_CoMangularMomentum.h index b27a506c..a3921b7c 100644 --- a/binding/matlab/Matlab_CoMangularMomentum.h +++ b/binding/matlab/Matlab_CoMangularMomentum.h @@ -14,15 +14,15 @@ void Matlab_CoMangularMomentum( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q et Qdot font la même dimension diff --git a/binding/matlab/Matlab_CoMddot.h b/binding/matlab/Matlab_CoMddot.h index b940549b..7e27e07f 100644 --- a/binding/matlab/Matlab_CoMddot.h +++ b/binding/matlab/Matlab_CoMddot.h @@ -14,19 +14,19 @@ void Matlab_CoMddot( int nlhs, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector QDDot = + std::vector QDDot = getParameterQddot(prhs, 4, nQddot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension diff --git a/binding/matlab/Matlab_CoMdot.h b/binding/matlab/Matlab_CoMdot.h index be88a166..de5b6c47 100644 --- a/binding/matlab/Matlab_CoMdot.h +++ b/binding/matlab/Matlab_CoMdot.h @@ -15,15 +15,15 @@ void Matlab_CoMdot( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension diff --git a/binding/matlab/Matlab_ContactGamma.h b/binding/matlab/Matlab_ContactGamma.h index 8b73a8d3..79b78b1f 100644 --- a/binding/matlab/Matlab_ContactGamma.h +++ b/binding/matlab/Matlab_ContactGamma.h @@ -15,14 +15,14 @@ void Matlab_ContactGamma( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is the Qdot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); - biorbd::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); unsigned int nContacts = model->nbContacts(); diff --git a/binding/matlab/Matlab_ContactJacobian.h b/binding/matlab/Matlab_ContactJacobian.h index 4a672aef..e9812c2e 100644 --- a/binding/matlab/Matlab_ContactJacobian.h +++ b/binding/matlab/Matlab_ContactJacobian.h @@ -15,11 +15,11 @@ void Matlab_ContactJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la matrice jacobienne de tous les contacts diff --git a/binding/matlab/Matlab_ContactsPosition.h b/binding/matlab/Matlab_ContactsPosition.h index 8543bf49..981bff75 100644 --- a/binding/matlab/Matlab_ContactsPosition.h +++ b/binding/matlab/Matlab_ContactsPosition.h @@ -14,15 +14,15 @@ void Matlab_ContactsPosition( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver où sont les marqueurs - std::vector Contact_tp = model->constraintsInGlobal(Q, + std::vector Contact_tp = model->constraintsInGlobal(Q, true); @@ -31,7 +31,7 @@ void Matlab_ContactsPosition( int, mxArray *plhs[], double *contact = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=Contact_tp.begin(); + std::vector::iterator it=Contact_tp.begin(); for (unsigned int i=0; (it+i)!=Contact_tp.end(); ++i) { contact[i*3] = (*(it+i))(0); contact[i*3+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_IMU.h b/binding/matlab/Matlab_IMU.h index 8e45b7c9..85c12c2b 100644 --- a/binding/matlab/Matlab_IMU.h +++ b/binding/matlab/Matlab_IMU.h @@ -14,33 +14,33 @@ void Matlab_IMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 4, "3 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the Q and 4th is the wanted IMUType to be return (['all'], 'technical' or anatomical')"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Récupérer les IMU selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nIMUs(0); // Nombre de IMU - std::vector> IMU_tp; // récupérer les IMU + std::vector> IMU_tp; // récupérer les IMU if (nrhs == 4) { - biorbd::utils::String type(getString(prhs,3)); + biorbd::BIORBD_MATH_NAMESPACE::utils::String type(getString(prhs,3)); if (!type.tolower().compare("all")) { nIMUs = model->nbIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->IMU(*Q_it)); } } else if (!type.tolower().compare("anatomical")) { nIMUs = model->nbAnatIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->anatomicalIMU(*Q_it)); } } else if (!type.tolower().compare("technical")) { nIMUs = model->nbTechIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->technicalIMU(*Q_it)); } @@ -52,7 +52,7 @@ void Matlab_IMU( int, mxArray *plhs[], } else { nIMUs = model->nbIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->IMU(*Q_it)); } @@ -70,10 +70,10 @@ void Matlab_IMU( int, mxArray *plhs[], // Remplir l'output unsigned int cmpIMU = 0; - for (std::vector>::iterator AllIMU_it = + for (std::vector>::iterator AllIMU_it = IMU_tp.begin(); AllIMU_it != IMU_tp.end(); ++AllIMU_it) - for (std::vector::iterator IMU_it=(*AllIMU_it).begin(); + for (std::vector::iterator IMU_it=(*AllIMU_it).begin(); IMU_it!=(*AllIMU_it).end(); ++IMU_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_IMUJacobian.h b/binding/matlab/Matlab_IMUJacobian.h index e44d17e3..fc762439 100644 --- a/binding/matlab/Matlab_IMUJacobian.h +++ b/binding/matlab/Matlab_IMUJacobian.h @@ -14,17 +14,17 @@ void Matlab_IMUJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp = model->IMUJacobian(Q); - std::vector::iterator it=Jac_tp.begin(); + std::vector Jac_tp = model->IMUJacobian(Q); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument unsigned int nIMUs = model->nbIMUs(); diff --git a/binding/matlab/Matlab_LocalMarkers.h b/binding/matlab/Matlab_LocalMarkers.h index c6ebf16d..d13061c2 100644 --- a/binding/matlab/Matlab_LocalMarkers.h +++ b/binding/matlab/Matlab_LocalMarkers.h @@ -14,10 +14,10 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 4, "2 arguments are required [+2 optional] where the 2nd is the handler on the model, 3rd is the wanted markerType to be return ('all' [default], 'technical' or anatomical') and 4th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Gestion du type - biorbd::utils::String type("all"); + biorbd::BIORBD_MATH_NAMESPACE::utils::String type("all"); if (nrhs >= 3) { type = getString(prhs,2); } @@ -28,17 +28,17 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nMarkers(0); // Nombre de marqueurs - std::vector + std::vector markers_tp; // récupérer les marqueurs if (!type.tolower().compare("all")) { nMarkers = model->nbMarkers(); - markers_tp = model->biorbd::rigidbody::Markers::markers(removeAxes); + markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::markers(removeAxes); } else if (!type.tolower().compare("anatomical")) { nMarkers = model->nbAnatomicalMarkers(); - markers_tp = model->biorbd::rigidbody::Markers::anatomicalMarkers(removeAxes); + markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::anatomicalMarkers(removeAxes); } else if (!type.tolower().compare("technical")) { nMarkers = model->nbTechnicalMarkers(); - markers_tp = model->biorbd::rigidbody::Markers::technicalMarkers(removeAxes); + markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::technicalMarkers(removeAxes); } else { std::ostringstream msg; msg << "Wrong type for markers!"; @@ -55,7 +55,7 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], // Remplir le output unsigned int cmp(0); - std::vector::iterator it=markers_tp.begin(); + std::vector::iterator it=markers_tp.begin(); for (unsigned int i=0; (it+i)!=markers_tp.end(); ++i) { markers[cmp+0] = (*(it+i))(0); markers[cmp+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_Markers.h b/binding/matlab/Matlab_Markers.h index e8093eda..894e6da9 100644 --- a/binding/matlab/Matlab_Markers.h +++ b/binding/matlab/Matlab_Markers.h @@ -14,11 +14,11 @@ void Matlab_Markers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required [+2 optional] where the 2nd is the handler on the model, 3rd is the Q and 4th is the wanted markerType to be return ('all', 'technical' or anatomical') and 5th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); bool removeAxes(true); @@ -28,25 +28,25 @@ void Matlab_Markers( int, mxArray *plhs[], // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nMarkers(0); // Nombre de marqueurs - std::vector> + std::vector> markers_tp; // récupérer les marqueurs if (nrhs >= 4) { - biorbd::utils::String type(getString(prhs,3)); + biorbd::BIORBD_MATH_NAMESPACE::utils::String type(getString(prhs,3)); if (!type.tolower().compare("all")) { nMarkers = model->nbMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->markers(*Q_it, removeAxes)); } } else if (!type.tolower().compare("anatomical")) { nMarkers = model->nbAnatomicalMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->anatomicalMarkers(*Q_it, removeAxes)); } } else if (!type.tolower().compare("technical")) { nMarkers = model->nbTechnicalMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->technicalMarkers(*Q_it, removeAxes)); } @@ -58,7 +58,7 @@ void Matlab_Markers( int, mxArray *plhs[], } else { nMarkers = model->nbMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->markers(*Q_it, removeAxes)); } @@ -75,10 +75,10 @@ void Matlab_Markers( int, mxArray *plhs[], // Remplir le output unsigned int cmp(0); - for (std::vector>::iterator + for (std::vector>::iterator markers_it = markers_tp.begin(); markers_it!=markers_tp.end(); ++markers_it) { - std::vector::iterator it=(*markers_it).begin(); + std::vector::iterator it=(*markers_it).begin(); for (unsigned int i=0; (it+i)!=(*markers_it).end(); ++i) { markers[cmp+0] = (*(it+i))(0); markers[cmp+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_MarkersJacobian.h b/binding/matlab/Matlab_MarkersJacobian.h index 05c9309c..15ea9b63 100644 --- a/binding/matlab/Matlab_MarkersJacobian.h +++ b/binding/matlab/Matlab_MarkersJacobian.h @@ -14,11 +14,11 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], "3 arguments are required [+1 optional] where the 2nd is the handler on the model and 3rd is the Q, an optional 4th if you only want technical marker [default = false] and 5th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); bool technicalMarkersOnly(false); @@ -33,7 +33,7 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp; + std::vector Jac_tp; unsigned int nMarkers; if (technicalMarkersOnly) { Jac_tp = model->technicalMarkersJacobian(Q, @@ -44,7 +44,7 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], removeAxes); // Retourne la jacobienne des markers nMarkers = model->nbMarkers(); } - std::vector::iterator it=Jac_tp.begin(); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3*nMarkers, nQ, mxREAL); diff --git a/binding/matlab/Matlab_Mesh.h b/binding/matlab/Matlab_Mesh.h index e49bfb02..e23d9784 100644 --- a/binding/matlab/Matlab_Mesh.h +++ b/binding/matlab/Matlab_Mesh.h @@ -14,11 +14,11 @@ void Matlab_Mesh( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 4, "3 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q and 4th optional is a specific segment index"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir l'index (si envoye) @@ -30,7 +30,7 @@ void Matlab_Mesh( int, mxArray *plhs[], // Output if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMesh(model->meshPoints(Q)); + std::vector> allMesh(model->meshPoints(Q)); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMesh.size(), 1); @@ -50,7 +50,7 @@ void Matlab_Mesh( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector Mesh_tp(model->meshPoints(Q, + std::vector Mesh_tp(model->meshPoints(Q, static_cast(idx))); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_MusclesActivationDot.h b/binding/matlab/Matlab_MusclesActivationDot.h index 065aea86..1e8b4b9d 100644 --- a/binding/matlab/Matlab_MusclesActivationDot.h +++ b/binding/matlab/Matlab_MusclesActivationDot.h @@ -18,10 +18,10 @@ void Matlab_MusclesActivationDot( int, mxArray *plhs[], "3rd is the excitation, 4th is the activation and 5th is a bool to express if " "excitation is already normalized"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les états musculaires - std::vector>> state + std::vector>> state = getParameterMuscleState(prhs, 2, 3, model->nbMuscleTotal()); // Already normalized diff --git a/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h b/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h index 476310cd..4ddacdec 100644 --- a/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h +++ b/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h @@ -16,10 +16,10 @@ void Matlab_MusclesExcitationDotBuchanan( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 5, 5, "4 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the excitation, 4th is the activation and 5th is a bool to express if excitation is already normalized"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les états musculaires - std::vector> state = + std::vector> state = getParameterMuscleStateBuchanan(prhs, 2, 3, model->nbMuscleTotal()); @@ -45,7 +45,7 @@ void Matlab_MusclesExcitationDotBuchanan( int, mxArray *plhs[], for (unsigned int j=0; jmuscleGroup(i).nbMuscles(); ++j) { // Recueillir dérivées d'excitations double shapeFactor = - dynamic_cast( model->muscleGroup( + dynamic_cast( model->muscleGroup( i).muscle( j).state() ).shapeFactor(); (*((*(state.begin()+iTime)).begin() + cmpState)).shapeFactor(shapeFactor); diff --git a/binding/matlab/Matlab_MusclesForce.h b/binding/matlab/Matlab_MusclesForce.h index e951b993..7ca06c51 100644 --- a/binding/matlab/Matlab_MusclesForce.h +++ b/binding/matlab/Matlab_MusclesForce.h @@ -16,14 +16,14 @@ void Matlab_MusclesForce( int, mxArray *plhs[], "WARNING: if the function is called without Q and Qdot, the user MUST update by himself " "before calling this function (using updateMuscle)."); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF unsigned int nMuscleTotal = model->nbMuscles(); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateActivation(prhs,2, nMuscleTotal); unsigned int nFrame(static_cast(s.size())); @@ -44,8 +44,8 @@ void Matlab_MusclesForce( int, mxArray *plhs[], } // Recueillir la cinématique - std::vector Q; - std::vector QDot; + std::vector Q; + std::vector QDot; if (updateKin) { // Si on update pas la cinématique Q et Qdot ne sont pas nécessaire // Recevoir Q Q = getParameterQ(prhs, 3, nQ); @@ -71,8 +71,8 @@ void Matlab_MusclesForce( int, mxArray *plhs[], double * Mus = mxGetPr(plhs[0]); // Remplir le output - biorbd::rigidbody::GeneralizedTorque muscleTorque; - biorbd::utils::Vector muscleForces; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; imuscleForces(s[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_MusclesForceMax.h b/binding/matlab/Matlab_MusclesForceMax.h index 77328578..cd6fdee6 100644 --- a/binding/matlab/Matlab_MusclesForceMax.h +++ b/binding/matlab/Matlab_MusclesForceMax.h @@ -18,7 +18,7 @@ void Matlab_MusclesForceMax( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model. If 2 arguments were sent, the function returns values for each muscles, if a 3rd is sent, the max forces are replace by the sent values."); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); double *muscleForceMax = nullptr; // Pointeur pour la sortie Eigen::VectorXd Forces; diff --git a/binding/matlab/Matlab_MusclesJacobian.h b/binding/matlab/Matlab_MusclesJacobian.h index a88ad628..f87e2371 100644 --- a/binding/matlab/Matlab_MusclesJacobian.h +++ b/binding/matlab/Matlab_MusclesJacobian.h @@ -18,11 +18,11 @@ void Matlab_MusclesJacobian( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie diff --git a/binding/matlab/Matlab_MusclesLength.h b/binding/matlab/Matlab_MusclesLength.h index 61c5be28..b737a70e 100644 --- a/binding/matlab/Matlab_MusclesLength.h +++ b/binding/matlab/Matlab_MusclesLength.h @@ -16,11 +16,11 @@ void Matlab_MusclesLength( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie @@ -54,11 +54,11 @@ void Matlab_MusclesTendonLength( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie diff --git a/binding/matlab/Matlab_MusclesNames.h b/binding/matlab/Matlab_MusclesNames.h index a5a49afd..6238cbeb 100644 --- a/binding/matlab/Matlab_MusclesNames.h +++ b/binding/matlab/Matlab_MusclesNames.h @@ -17,7 +17,7 @@ void Matlab_MusclesNames( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateCellMatrix(model->nbMuscleGroups(), diff --git a/binding/matlab/Matlab_MusclesParentNames.h b/binding/matlab/Matlab_MusclesParentNames.h index 5ddc0a32..293cab44 100644 --- a/binding/matlab/Matlab_MusclesParentNames.h +++ b/binding/matlab/Matlab_MusclesParentNames.h @@ -18,7 +18,7 @@ void Matlab_MusclesParentNames( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateCellMatrix(model->nbMuscleGroups(), diff --git a/binding/matlab/Matlab_MusclesPoints.h b/binding/matlab/Matlab_MusclesPoints.h index f3e03c6b..9cc95c87 100644 --- a/binding/matlab/Matlab_MusclesPoints.h +++ b/binding/matlab/Matlab_MusclesPoints.h @@ -20,18 +20,18 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie plhs[0] = mxCreateCellMatrix(model->nbMuscleTotal(), Q.size()); // Utilisable que si nlhs >= 2 - std::vector wrap_type; // forme du wrapping - std::vector wrap_RT; // orientation du wrapping + std::vector wrap_type; // forme du wrapping + std::vector wrap_RT; // orientation du wrapping std::vector wrap_dim1; // dimension du wrapping std::vector wrap_dim2; // dimension deux du wrapping @@ -44,26 +44,26 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], model->UpdateKinematicsCustom(&Q[iQ]); // Recueillir tout le chemin musculaire - std::vector via(model->muscleGroup(i).muscle( + std::vector via(model->muscleGroup(i).muscle( j).musclesPointsInGlobal()); // Si le nombre de wrap est > 0, c'est qu'il n'y a pas de viapoints et il n'y a qu'UN wrap if (model->muscleGroup(i).muscle(j).pathModifier().nbWraps() > 0) { - const biorbd::muscles::WrappingObject& wrappingObject( - dynamic_cast( + const biorbd::BIORBD_MATH_NAMESPACE::muscles::WrappingObject& wrappingObject( + dynamic_cast( model->muscleGroup(0).muscle(0).pathModifier().object(0))); // De quel type - biorbd::utils::NODE_TYPE type(wrappingObject.typeOfNode()); + biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE type(wrappingObject.typeOfNode()); wrap_type.push_back(type); // Dans quelle orientation wrap_RT.push_back(wrappingObject.RT()); // Quel est sa dimension - if (type == biorbd::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { - const biorbd::muscles::WrappingHalfCylinder& cylinder( - dynamic_cast + if (type == biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { + const biorbd::BIORBD_MATH_NAMESPACE::muscles::WrappingHalfCylinder& cylinder( + dynamic_cast (wrappingObject)); wrap_dim1.push_back(cylinder.radius()); wrap_dim2.push_back(cylinder.length()); @@ -96,7 +96,7 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], for (unsigned int i=0; i(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -39,7 +39,7 @@ void Matlab_NLeffects( int, mxArray *plhs[], unsigned int cmp(0); // Trouver les effets non-linéaires pour chaque configuration - biorbd::rigidbody::GeneralizedTorque Tau(nTau); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); for (unsigned int j=0; j(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir l'index (si envoye) int idx(-1); @@ -26,7 +26,7 @@ void Matlab_Patch( int, mxArray *plhs[], // Output if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMesh( + std::vector> allMesh( model->meshFaces()); // Create a matrix for the return argument @@ -38,7 +38,7 @@ void Matlab_Patch( int, mxArray *plhs[], // Remplir le output for (unsigned int i=0; allMesh[i_bone].size(); ++i) { Mesh[i*3] = allMesh[i_bone][i](0) - +1; // +1 Car l'indice dans biorbd::rigidbody::s est par rapport à 0 + +1; // +1 Car l'indice dans biorbd::BIORBD_MATH_NAMESPACE::rigidbody::s est par rapport à 0 Mesh[i*3+1] = allMesh[i_bone][i](1)+1; Mesh[i*3+2] = allMesh[i_bone][i](2)+1; } @@ -47,7 +47,7 @@ void Matlab_Patch( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector Mesh_tp(model->meshFaces( + std::vector Mesh_tp(model->meshFaces( static_cast(idx))); // Create a matrix for the return argument @@ -55,10 +55,10 @@ void Matlab_Patch( int, mxArray *plhs[], double *Mesh = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=Mesh_tp.begin(); + std::vector::iterator it=Mesh_tp.begin(); for (unsigned int i=0; (it+i)!=Mesh_tp.end(); ++i) { Mesh[i*3] = (*(it+i))(0) - +1; // +1 Car l'indice dans biorbd::rigidbody::s est par rapport à 0 + +1; // +1 Car l'indice dans biorbd::BIORBD_MATH_NAMESPACE::rigidbody::s est par rapport à 0 Mesh[i*3+1] = (*(it+i))(1)+1; Mesh[i*3+2] = (*(it+i))(2)+1; } diff --git a/binding/matlab/Matlab_ProjectCustomPoint.h b/binding/matlab/Matlab_ProjectCustomPoint.h index 22c16ef0..ae428331 100644 --- a/binding/matlab/Matlab_ProjectCustomPoint.h +++ b/binding/matlab/Matlab_ProjectCustomPoint.h @@ -14,22 +14,22 @@ void Matlab_ProjectCustomPoint( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 6, 6, "6 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th are the 3xNxT points in global reference frame, 5th is the body idx to project on and 6th are axes to remove on this body axes"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Qall = getParameterQ( + std::vector Qall = getParameterQ( prhs, 2, nQ); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3); // Body index int bodyIdx(getInteger(prhs,4)-1); // Nom des axes à retirer - biorbd::utils::String axesToRemove(getString(prhs,5)); + biorbd::BIORBD_MATH_NAMESPACE::utils::String axesToRemove(getString(prhs,5)); unsigned int nFrames(static_cast(markersOverTime.size())); if (Qall.size()!=nFrames) { @@ -49,9 +49,9 @@ void Matlab_ProjectCustomPoint( int, mxArray *plhs[], // Projeter les points unsigned int cmp(0); for (unsigned int i=0; i(nMarker); ++j) { - biorbd::rigidbody::NodeSegment tp; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment tp; if (j==1) { tp = model->projectPoint(Q, markersOverTime[i][j], bodyIdx, axesToRemove, true); } else { diff --git a/binding/matlab/Matlab_ProjectPoint.h b/binding/matlab/Matlab_ProjectPoint.h index 82cb56a3..b6b41a7c 100644 --- a/binding/matlab/Matlab_ProjectPoint.h +++ b/binding/matlab/Matlab_ProjectPoint.h @@ -14,15 +14,15 @@ void Matlab_ProjectPoint( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 4, 4, "6 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th are the 3xNxT points in global reference frame where N = nMarkers of the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Qall = getParameterQ( + std::vector Qall = getParameterQ( prhs, 2, nQ); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3); unsigned int nFrames(static_cast(markersOverTime.size())); @@ -43,11 +43,11 @@ void Matlab_ProjectPoint( int, mxArray *plhs[], // Projeter les points unsigned int cmp(0); for (unsigned int i=0; i projectedPoint(model->projectPoint( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q(*(Qall.begin()+i)); + std::vector projectedPoint(model->projectPoint( Q, *(markersOverTime.begin()+i), true)); for (unsigned int j=0; j(nMarker); ++j) { - biorbd::rigidbody::NodeSegment tp(*(projectedPoint.begin()+j)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment tp(*(projectedPoint.begin()+j)); Markers[cmp+0] = tp(0); Markers[cmp+1] = tp(1); Markers[cmp+2] = tp(2); diff --git a/binding/matlab/Matlab_ProjectPointJacobian.h b/binding/matlab/Matlab_ProjectPointJacobian.h index c689ab3f..5a3631cc 100644 --- a/binding/matlab/Matlab_ProjectPointJacobian.h +++ b/binding/matlab/Matlab_ProjectPointJacobian.h @@ -14,21 +14,21 @@ void Matlab_ProjectPointJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q and 4th are the 3xN markers where N=nMarkers of the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector markersOverTime = + std::vector markersOverTime = *getParameterAllMarkers(prhs,3).begin(); // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp = model->projectPointJacobian(Q, + std::vector Jac_tp = model->projectPointJacobian(Q, markersOverTime, true); - std::vector::iterator it=Jac_tp.begin(); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument unsigned int nMarkers = model->nbMarkers(); diff --git a/binding/matlab/Matlab_changeGravity.h b/binding/matlab/Matlab_changeGravity.h index f42a50ea..ba3546c1 100644 --- a/binding/matlab/Matlab_changeGravity.h +++ b/binding/matlab/Matlab_changeGravity.h @@ -13,7 +13,7 @@ void Matlab_changeGravity ( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the gravity field 3d vector"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_computeQdot.h b/binding/matlab/Matlab_computeQdot.h index 00d343b1..b9921b43 100644 --- a/binding/matlab/Matlab_computeQdot.h +++ b/binding/matlab/Matlab_computeQdot.h @@ -16,15 +16,15 @@ void Matlab_computeQdot( int, mxArray *plhs[], "4 are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); unsigned int nQdot = model->nbQdot(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q et Qdot sont de la bonne dimension @@ -44,7 +44,7 @@ void Matlab_computeQdot( int, mxArray *plhs[], model->UpdateKinematicsCustom(&Q[j], &QDot[j], nullptr); // Trouver la dynamique directe a cette configuration - biorbd::rigidbody::GeneralizedCoordinates QDotPost( + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QDotPost( model->computeQdot(Q[j], QDot[j])); // Remplir l'output diff --git a/binding/matlab/Matlab_delete.h b/binding/matlab/Matlab_delete.h index 2d2850d6..eacb3936 100644 --- a/binding/matlab/Matlab_delete.h +++ b/binding/matlab/Matlab_delete.h @@ -15,7 +15,7 @@ void Matlab_delete( int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the model"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); diff --git a/binding/matlab/Matlab_forwardDynamics.h b/binding/matlab/Matlab_forwardDynamics.h index abc80e35..3e4d6135 100644 --- a/binding/matlab/Matlab_forwardDynamics.h +++ b/binding/matlab/Matlab_forwardDynamics.h @@ -30,20 +30,20 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], } // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); unsigned int nQddot = model->nbQddot(); unsigned int nTau = model->nbGeneralizedTorque(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Tau - std::vector Tau = + std::vector Tau = getParameterGeneralizedTorque(prhs, 4, nTau); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -69,7 +69,7 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], force = mxGetPr(plhs[1]); // Force sur les points de contacts } - biorbd::rigidbody::GeneralizedCoordinates QDDot(nQddot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QDDot(nQddot); for (unsigned int j=0; jUpdateKinematicsCustom(&Q[j], &QDot[j], nullptr); @@ -82,7 +82,7 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], Tau[j], model->getConstraints(), QDDot);// Forward dynamics } else if (contact == -1) { // Si on a une impulsion - biorbd::rigidbody::GeneralizedCoordinates QdotPost(static_cast((* + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QdotPost(static_cast((* (Q.begin()+j)).size())); RigidBodyDynamics::ComputeConstraintImpulsesDirect(*model, Q[j], QDot[j], model->getConstraints(), QdotPost); diff --git a/binding/matlab/Matlab_globalJCS.h b/binding/matlab/Matlab_globalJCS.h index 46346083..bf610056 100644 --- a/binding/matlab/Matlab_globalJCS.h +++ b/binding/matlab/Matlab_globalJCS.h @@ -14,16 +14,16 @@ void Matlab_globalJCS( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Trouver les RT - std::vector> JSC_vec; - for (std::vector::iterator Q_it = + std::vector> JSC_vec; + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { JSC_vec.push_back(model->allGlobalJCS(*Q_it)); } @@ -35,10 +35,10 @@ void Matlab_globalJCS( int, mxArray *plhs[], // Remplir l'output unsigned int cmpJCS = 0; - for (std::vector>::iterator AllJCS_it = + for (std::vector>::iterator AllJCS_it = JSC_vec.begin(); AllJCS_it != JSC_vec.end(); ++AllJCS_it) - for (std::vector::iterator JSC_it= + for (std::vector::iterator JSC_it= (*AllJCS_it).begin(); JSC_it!=(*AllJCS_it).end(); ++JSC_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_inverseDynamics.h b/binding/matlab/Matlab_inverseDynamics.h index 8363b394..e4417b94 100644 --- a/binding/matlab/Matlab_inverseDynamics.h +++ b/binding/matlab/Matlab_inverseDynamics.h @@ -19,19 +19,19 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], } // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector QDDot = + std::vector QDDot = getParameterQddot(prhs, 4, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -45,7 +45,7 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], "QDDot must have the same number of frames than Q"); } - std::vector> f_tp; + std::vector> f_tp; if (externalForces) { f_tp = getForcePlate(prhs, 5); if (f_tp.size() != nFrame) { @@ -60,7 +60,7 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], unsigned int cmp(0); // Trouver la dynamique inverse a cette configuration - biorbd::rigidbody::GeneralizedTorque Tau(nTau); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); for (unsigned int j=0; j(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir la matrice des markers - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,2,static_cast(model->nbTechnicalMarkers())); // Recevoir Qinit - biorbd::rigidbody::GeneralizedCoordinates Qinit = *getParameterQ(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit = *getParameterQ(prhs, 3, nQ).begin(); bool removeAxes(true); @@ -36,7 +36,7 @@ void Matlab_inverseKinematics( int, mxArray *plhs[], // Faire la cinématique inverse a chaque instant for (unsigned int i=0; i(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // S'assurer que la personne recueille l'acces au filtre de Kalman if (nlhs != 1) { @@ -34,12 +34,12 @@ void Matlab_setEKF(int nlhs, mxArray *plhs[], if (nrhs >= 3) { freq = getDouble(prhs,2,"Acquisition Frequency"); } - biorbd::rigidbody::KalmanParam kParams(freq, noiseF, errorF); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); // Créer un filtre de Kalman try { - plhs[0] = convertPtr2Mat - (new biorbd::rigidbody::KalmanReconsMarkers(*model, + plhs[0] = convertPtr2Mat + (new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers(*model, kParams)); } catch (std::string m) { mexErrMsgTxt(m.c_str()); @@ -55,7 +55,7 @@ void Matlab_delEKF(int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the kalman filter"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); @@ -72,14 +72,14 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], "4 arguments are required (+2 optional) where the 2nd is the handler on the model, 3rd is the handler on kalman filter info, 4th is the 3xN marker positions matrix, the optional 5th is the initial guess for Q (ignored after first iteration) and 6th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir le kalman - biorbd::rigidbody::KalmanReconsMarkers * kalman = - convertMat2Ptr(prhs[2]); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers * kalman = + convertMat2Ptr(prhs[2]); bool removeAxes(true); if (nrhs >= 6) { @@ -88,14 +88,14 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], // Recevoir la matrice des markers (Ne traite que le premier frame) - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3,static_cast(model->nbTechnicalMarkers())); - std::vector markers = markersOverTime[0]; + std::vector markers = markersOverTime[0]; // Si c'est le premier frame recevoir Qinit if (kalman->first() && nrhs >= 5) { - biorbd::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, nQ).begin()); kalman->setInitState(&Qinit); } @@ -109,9 +109,9 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], double *qddot = mxGetPr(plhs[2]); // Faire la cinématique inverse a chaque instant - biorbd::rigidbody::GeneralizedCoordinates Q(nQ); - biorbd::rigidbody::GeneralizedVelocity QDot(nQdot); - biorbd::rigidbody::GeneralizedAcceleration QDDot(nQddot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); // Faire la cinématique inverse kalman->reconstructFrame(*model, markers, &Q, &QDot, &QDDot, removeAxes); @@ -148,7 +148,7 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], "7th is error factor [default 1e-5] and 7th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF @@ -166,8 +166,8 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], } // Créer un filtre de Kalman - biorbd::rigidbody::KalmanReconsMarkers kalman(*model, - biorbd::rigidbody::KalmanParam(freq, noiseF, errorF)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers kalman(*model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); bool removeAxes(true); if (nrhs >= 8) { @@ -176,13 +176,13 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], // Recevoir la matrice des markers - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,2,static_cast(model->nbTechnicalMarkers())); unsigned int nFrames(static_cast(markersOverTime.size())); // Recevoir Qinit if (kalman.first() && nrhs >= 4) { - biorbd::rigidbody::GeneralizedCoordinates Qinit(getParameterQ(prhs, 3, nQ)[0]); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(getParameterQ(prhs, 3, nQ)[0]); kalman.setInitState(&Qinit); } @@ -197,9 +197,9 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], unsigned int cmp(0); for (unsigned int i=0; i(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // S'assurer que la personne recueille l'acces au filtre de Kalman if (nlhs != 1) { @@ -33,12 +33,12 @@ void Matlab_setEKF_IMU(int nlhs, mxArray *plhs[], if (nrhs >= 3) { freq = getDouble(prhs,2,"Acquisition Frequency"); } - biorbd::rigidbody::KalmanParam kParams(freq, noiseF, errorF); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); // Créer un filtre de Kalman try { - plhs[0] = convertPtr2Mat - (new biorbd::rigidbody::KalmanReconsIMU(*model, kParams)); + plhs[0] = convertPtr2Mat + (new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU(*model, kParams)); } catch (std::string m) { mexErrMsgTxt(m.c_str()); } @@ -53,7 +53,7 @@ void Matlab_delEKF_IMU(int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the kalman filter"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); @@ -70,24 +70,24 @@ void Matlab_inverseKinematicsEKF_IMUstep( int , mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the handler on kalman filter info, 4th is the 3x3xNxT or 4x4xNxT IMU hypermatrix, the optional 5th is the initial guess for Q [default 0], 5th is acquisition frequency [default 100Hz]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir le kalman - biorbd::rigidbody::KalmanReconsIMU * kalman = - convertMat2Ptr(prhs[2]); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU * kalman = + convertMat2Ptr(prhs[2]); // Recevoir la matrice des markers (Ne traite que le premier frame) - std::vector> imusOverTime = + std::vector> imusOverTime = getParameterAllIMUs(prhs,3); - std::vector imus = imusOverTime[0]; + std::vector imus = imusOverTime[0]; // Si c'est le premier frame recevoir Qinit if (kalman->first() && nrhs >= 5) { - biorbd::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, nQ).begin()); kalman->setInitState(&Qinit); } @@ -101,9 +101,9 @@ void Matlab_inverseKinematicsEKF_IMUstep( int , mxArray *plhs[], double *qddot = mxGetPr(plhs[2]); // Faire la cinématique inverse a chaque instant - biorbd::rigidbody::GeneralizedCoordinates Q(nQ); - biorbd::rigidbody::GeneralizedVelocity QDot(nQdot); - biorbd::rigidbody::GeneralizedAcceleration QDDot(nQddot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); // Faire la cinématique inverse kalman->reconstructFrame(*model, imus, &Q, &QDot, &QDDot); @@ -133,7 +133,7 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], "4 arguments are required (+5 optional) where the 2nd is the handler on the model, 3th is the 3x3xNxT or 4x4xNxT IMU hypermatrix, the optional 4th is the initial guess for Q [default 0], 5th is acquisition frequency [default 100Hz], 6th is noise factor [default 5e-3] and 7th is error factor [default 1e-10]"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF @@ -151,17 +151,17 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], } // Créer un filtre de Kalman - biorbd::rigidbody::KalmanReconsIMU kalman(*model, - biorbd::rigidbody::KalmanParam(freq, noiseF, errorF)); + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU kalman(*model, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); // Recevoir la matrice des imus - std::vector> imusOverTime = + std::vector> imusOverTime = getParameterAllIMUs(prhs,2); unsigned int nFrames(static_cast(imusOverTime.size())); // Recevoir Qinit if (kalman.first() && nrhs >= 4) { - biorbd::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 3, nQ).begin()); kalman.setInitState(&Qinit); } @@ -177,9 +177,9 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], unsigned int cmp(0); for (unsigned int i=0; i(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver les RT - std::vector JSC_vec(model->localJCS()); + std::vector JSC_vec(model->localJCS()); // Create a matrix for the return argument const mwSize dims[3]= {4,4,mwSize(model->nbSegment())}; @@ -26,7 +26,7 @@ void Matlab_localJCS( int, mxArray *plhs[], // Remplir l'output unsigned int cmpJCS = 0; - for (std::vector::iterator JSC_it=JSC_vec.begin(); + for (std::vector::iterator JSC_it=JSC_vec.begin(); JSC_it!=JSC_vec.end(); ++JSC_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_massMatrix.h b/binding/matlab/Matlab_massMatrix.h index ae45ee7c..25c2fa9f 100644 --- a/binding/matlab/Matlab_massMatrix.h +++ b/binding/matlab/Matlab_massMatrix.h @@ -17,11 +17,11 @@ void Matlab_massMatrix( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_muscleJointTorqueFromActivation.h b/binding/matlab/Matlab_muscleJointTorqueFromActivation.h index 40c1b8bc..b7c01e85 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromActivation.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromActivation.h @@ -17,14 +17,14 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], "WARNING: if the function is called without Q and Qdot, the user MUST update by himself " "before calling this function (using updateMuscle)."); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF unsigned int nMuscleTotal = model->nbMuscles(); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateActivation(prhs,2, nMuscleTotal); unsigned int nFrame(static_cast(s.size())); @@ -45,8 +45,8 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], } // Recueillir la cinématique - std::vector Q; - std::vector QDot; + std::vector Q; + std::vector QDot; if (updateKin) { // Si on update pas la cinématique Q et Qdot ne sont pas nécessaire // Recevoir Q Q = getParameterQ(prhs, 3, nQ); @@ -82,8 +82,8 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], } // Remplir le output - biorbd::rigidbody::GeneralizedTorque muscleTorque; - biorbd::utils::Vector muscleForces; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; imuscleForces(s[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h b/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h index 926797b9..ce76892e 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h @@ -19,7 +19,7 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], "6th is if update [true] must be done. Note that if update is set to [false], " "the user MUST update it by himself before calling this function"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = @@ -27,13 +27,13 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], unsigned int nMuscleTotal = model->nbMuscleTotal(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateExcitation(prhs,4, model->nbMuscleTotal()); @@ -76,13 +76,13 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], } // Remplir le output - biorbd::rigidbody::GeneralizedTorque muscleTorque; - biorbd::utils::Vector muscleForces; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; inbMuscleGroups(); ++k) for (unsigned int j=0; jmuscleGroup(k).nbMuscles(); ++j) { - std::dynamic_pointer_cast(s[i][iMus]) + std::dynamic_pointer_cast(s[i][iMus]) ->timeDerivativeActivation(model->muscleGroup(k).muscle(j).characteristics(), true); ++iMus; diff --git a/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h b/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h index fc0c7f3d..92aab8be 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h @@ -17,7 +17,7 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], "6th is if update [true] must be done. Note that if update is set to [false], " "the user MUST update it by himself before calling this function"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = @@ -25,10 +25,10 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], unsigned int nMuscleTotal = model->nbMuscleTotal(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir muscleStates std::vector Fm = getParameterMuscleForceNorm(prhs,4, @@ -63,7 +63,7 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], double *GeneralizedTorque = mxGetPr(plhs[0]); // Remplir le output - biorbd::rigidbody::GeneralizedTorque muscleTorque; + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; for (unsigned int i=0; imuscularJointTorque(Fm[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_muscleLengthJacobian.h b/binding/matlab/Matlab_muscleLengthJacobian.h index aaffa640..0e020ec4 100644 --- a/binding/matlab/Matlab_muscleLengthJacobian.h +++ b/binding/matlab/Matlab_muscleLengthJacobian.h @@ -14,18 +14,18 @@ void Matlab_muscleLengthJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( model->nbMuscleTotal(), nQ, mxREAL); double *Jac = mxGetPr(plhs[0]); - biorbd::utils::Matrix jaco(model->musclesLengthJacobian(Q)); + biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix jaco(model->musclesLengthJacobian(Q)); int cmp(0); for (unsigned int j=0; j(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer qu'il n'y a qu'un seul frame @@ -57,7 +57,7 @@ void Matlab_muscleUpdate( int, mxArray *[], // Mettre le nombre nécessaire dans chaque case int cmpMus(0); for (unsigned int i = 0; inbMuscleGroups(); ++i) { - biorbd::muscles::MuscleGroup grMus(model->muscleGroup(i)); + biorbd::BIORBD_MATH_NAMESPACE::muscles::MuscleGroup grMus(model->muscleGroup(i)); for (unsigned int j = 0; j> musclePosition( + std::vector> musclePosition( getMusclePosition(prhs, 4, nPoints)); // Recueillir la matrice jacobienne - std::vector musclePointsJaco(getMusclePointsJaco(prhs, 5, + std::vector musclePointsJaco(getMusclePointsJaco(prhs, 5, nPoints, nQ)); // Appeler la fonction d'update diff --git a/binding/matlab/Matlab_muscleVelocity.h b/binding/matlab/Matlab_muscleVelocity.h index 92693811..adede61a 100644 --- a/binding/matlab/Matlab_muscleVelocity.h +++ b/binding/matlab/Matlab_muscleVelocity.h @@ -16,16 +16,16 @@ void Matlab_muscleVelocity( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 4, 4, "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is the Qdot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector Qdot = getParameterQdot( + std::vector Qdot = getParameterQdot( prhs, 3, nQdot); // S'assurer que le même nombre d'instants a été envoyé diff --git a/binding/matlab/Matlab_nBody.h b/binding/matlab/Matlab_nBody.h index aee7cf85..bee1e98d 100644 --- a/binding/matlab/Matlab_nBody.h +++ b/binding/matlab/Matlab_nBody.h @@ -13,7 +13,7 @@ void Matlab_nBody( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nControl.h b/binding/matlab/Matlab_nControl.h index af373dff..2bf3d6b2 100644 --- a/binding/matlab/Matlab_nControl.h +++ b/binding/matlab/Matlab_nControl.h @@ -13,7 +13,7 @@ void Matlab_nGeneralizedTorque(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nIMU.h b/binding/matlab/Matlab_nIMU.h index 66713bf8..202d6ecb 100644 --- a/binding/matlab/Matlab_nIMU.h +++ b/binding/matlab/Matlab_nIMU.h @@ -13,7 +13,7 @@ void Matlab_nIMU( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nMarkers.h b/binding/matlab/Matlab_nMarkers.h index ea4c8a08..e8042942 100644 --- a/binding/matlab/Matlab_nMarkers.h +++ b/binding/matlab/Matlab_nMarkers.h @@ -13,7 +13,7 @@ void Matlab_nMarkers( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nMuscles.h b/binding/matlab/Matlab_nMuscles.h index f7df5a21..07dc998f 100644 --- a/binding/matlab/Matlab_nMuscles.h +++ b/binding/matlab/Matlab_nMuscles.h @@ -14,7 +14,7 @@ void Matlab_nMuscles( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQ.h b/binding/matlab/Matlab_nQ.h index e0b835ab..7fbe027c 100644 --- a/binding/matlab/Matlab_nQ.h +++ b/binding/matlab/Matlab_nQ.h @@ -13,7 +13,7 @@ void Matlab_nQ(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQddot.h b/binding/matlab/Matlab_nQddot.h index 35a9cce2..2fd706c2 100644 --- a/binding/matlab/Matlab_nQddot.h +++ b/binding/matlab/Matlab_nQddot.h @@ -13,7 +13,7 @@ void Matlab_nQddot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQdot.h b/binding/matlab/Matlab_nQdot.h index 75084491..0b8b2600 100644 --- a/binding/matlab/Matlab_nQdot.h +++ b/binding/matlab/Matlab_nQdot.h @@ -13,7 +13,7 @@ void Matlab_nQdot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nRoot.h b/binding/matlab/Matlab_nRoot.h index 269d9bc4..d28a4d4b 100644 --- a/binding/matlab/Matlab_nRoot.h +++ b/binding/matlab/Matlab_nRoot.h @@ -13,7 +13,7 @@ void Matlab_nRoot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nameBody.h b/binding/matlab/Matlab_nameBody.h index 927e233e..77c5d919 100644 --- a/binding/matlab/Matlab_nameBody.h +++ b/binding/matlab/Matlab_nameBody.h @@ -14,7 +14,7 @@ void Matlab_nameBody( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int idx; if (nrhs>2) { diff --git a/binding/matlab/Matlab_nameDof.h b/binding/matlab/Matlab_nameDof.h index 8e4a40ca..3eec92c3 100644 --- a/binding/matlab/Matlab_nameDof.h +++ b/binding/matlab/Matlab_nameDof.h @@ -14,7 +14,7 @@ void Matlab_nameDof( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF diff --git a/binding/matlab/Matlab_nameIMU.h b/binding/matlab/Matlab_nameIMU.h index fe3065ba..ee5fce56 100644 --- a/binding/matlab/Matlab_nameIMU.h +++ b/binding/matlab/Matlab_nameIMU.h @@ -14,10 +14,10 @@ void Matlab_nameIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->IMUsNames()); + std::vector allIMU(model->IMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); @@ -37,10 +37,10 @@ void Matlab_nameTechnicalIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->technicalIMUsNames()); + std::vector allIMU(model->technicalIMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); @@ -60,10 +60,10 @@ void Matlab_nameAnatomicalIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->anatomicalIMUsNames()); + std::vector allIMU(model->anatomicalIMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); diff --git a/binding/matlab/Matlab_nameMarkers.h b/binding/matlab/Matlab_nameMarkers.h index f6cfbb3d..b210d7bd 100644 --- a/binding/matlab/Matlab_nameMarkers.h +++ b/binding/matlab/Matlab_nameMarkers.h @@ -14,10 +14,10 @@ void Matlab_nameMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->markerNames()); + std::vector allMarkers(model->markerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); @@ -38,10 +38,10 @@ void Matlab_nameTechnicalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->technicalMarkerNames()); + std::vector allMarkers(model->technicalMarkerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); @@ -62,10 +62,10 @@ void Matlab_nameAnatomicalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->anatomicalMarkerNames()); + std::vector allMarkers(model->anatomicalMarkerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); diff --git a/binding/matlab/Matlab_new.h b/binding/matlab/Matlab_new.h index 4a91d4c9..15fcfd57 100644 --- a/binding/matlab/Matlab_new.h +++ b/binding/matlab/Matlab_new.h @@ -26,13 +26,13 @@ void Matlab_new( int nlhs, mxArray *plhs[], "You must catch the pointer address!"); } char *buf = mxArrayToString(prhs[1]); - biorbd::utils::String filepath(buf); // Copier le cstring dans un std::string + biorbd::BIORBD_MATH_NAMESPACE::utils::String filepath(buf); // Copier le cstring dans un std::string // Loader le modèle musculosquelettique // Definition des variables globales du modele try { - plhs[0] = convertPtr2Mat( - new biorbd::Model(biorbd::Reader::readModelFile(filepath))); + plhs[0] = convertPtr2Mat( + new biorbd::BIORBD_MATH_NAMESPACE::Model(biorbd::BIORBD_MATH_NAMESPACE::Reader::readModelFile(filepath))); } catch (std::string m) { mexErrMsgTxt(m.c_str()); } diff --git a/binding/matlab/Matlab_parent.h b/binding/matlab/Matlab_parent.h index 70c76a11..2cce4b4d 100644 --- a/binding/matlab/Matlab_parent.h +++ b/binding/matlab/Matlab_parent.h @@ -13,7 +13,7 @@ void Matlab_parent( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and the 3rd is the name of the segment"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_rangeQ.h b/binding/matlab/Matlab_rangeQ.h index 3eaf52d0..02d90284 100644 --- a/binding/matlab/Matlab_rangeQ.h +++ b/binding/matlab/Matlab_rangeQ.h @@ -14,7 +14,7 @@ void Matlab_rangeQ(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nbSegments = model->nbSegment(); mwSize outSize[2] = {nbSegments, 1}; diff --git a/binding/matlab/Matlab_segmentAngularMomentum.h b/binding/matlab/Matlab_segmentAngularMomentum.h index c7d2d622..47e853d6 100644 --- a/binding/matlab/Matlab_segmentAngularMomentum.h +++ b/binding/matlab/Matlab_segmentAngularMomentum.h @@ -15,15 +15,15 @@ void Matlab_segmentAngularMomentum( int, mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot and 5th is the index of body segment"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir QDot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); unsigned int nFrame(static_cast(Q.size())); @@ -61,7 +61,7 @@ void Matlab_segmentAngularMomentum( int, mxArray *plhs[], // Préparer la sortie conditionnelle (tous si idx==-1, seulement celui demandé sinon) unsigned int cmp(0); for (unsigned int i=0; i am_all(model->CalcSegmentsAngularMomentum ( + std::vector am_all(model->CalcSegmentsAngularMomentum ( *model, *(Q.begin()+i), *(QDot.begin()+i), true)); if (idx==-1) { diff --git a/binding/matlab/Matlab_segmentCoM.h b/binding/matlab/Matlab_segmentCoM.h index 3bb25f00..2761dd64 100644 --- a/binding/matlab/Matlab_segmentCoM.h +++ b/binding/matlab/Matlab_segmentCoM.h @@ -14,11 +14,11 @@ void Matlab_segmentCOM( int, mxArray *plhs[], "3 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q and 4th is the index of body segment"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -29,7 +29,7 @@ void Matlab_segmentCOM( int, mxArray *plhs[], // Trouver la position du CoM if (i==-1) { - std::vector COM = model->CoMbySegment(Q,true); + std::vector COM = model->CoMbySegment(Q,true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); // Remplir l'output @@ -39,7 +39,7 @@ void Matlab_segmentCOM( int, mxArray *plhs[], tp[3*j+k] = COM[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::rigidbody::NodeSegment COM = model->CoMbySegment(Q, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment COM = model->CoMbySegment(Q, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentCoMddot.h b/binding/matlab/Matlab_segmentCoMddot.h index a8918d29..56ca83c8 100644 --- a/binding/matlab/Matlab_segmentCoMddot.h +++ b/binding/matlab/Matlab_segmentCoMddot.h @@ -14,19 +14,19 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], "5 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot, 5th is Qddot and 6th is the index of body segment"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Recevoir Qddot - biorbd::rigidbody::GeneralizedAcceleration QDDot = *getParameterQddot(prhs, 4, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot = *getParameterQddot(prhs, 4, nQddot).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -37,7 +37,7 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], // Trouver la vitesse du CoM if (i==-1) { - std::vector COMddot = model->CoMddotBySegment(Q,QDot, + std::vector COMddot = model->CoMddotBySegment(Q,QDot, QDDot,true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); @@ -48,7 +48,7 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], tp[3*j+k] = COMddot[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::utils::Vector3d COMddot = model->CoMddotBySegment(Q,QDot,QDDot, + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d COMddot = model->CoMddotBySegment(Q,QDot,QDDot, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentCoMdot.h b/binding/matlab/Matlab_segmentCoMdot.h index 18d68e5b..7195beeb 100644 --- a/binding/matlab/Matlab_segmentCoMdot.h +++ b/binding/matlab/Matlab_segmentCoMdot.h @@ -14,15 +14,15 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot and 5th is the index of body segment"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -33,7 +33,7 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], // Trouver la vitesse du CoM if (i==-1) { - std::vector COMdot = model->CoMdotBySegment(Q,QDot, + std::vector COMdot = model->CoMdotBySegment(Q,QDot, true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); @@ -44,7 +44,7 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], tp[3*j+k] = COMdot[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::utils::Vector3d COMdot = model->CoMdotBySegment(Q,QDot, + biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d COMdot = model->CoMdotBySegment(Q,QDot, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentMarkers.h b/binding/matlab/Matlab_segmentMarkers.h index 69cc08bb..dd7a5225 100644 --- a/binding/matlab/Matlab_segmentMarkers.h +++ b/binding/matlab/Matlab_segmentMarkers.h @@ -14,11 +14,11 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required (+2 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th if you want to remove axes as specified in the model file [default = true] and 5th optional is a specific segment index"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); bool removeAxes(true); @@ -35,9 +35,9 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMarkers; + std::vector> allMarkers; for (unsigned int i=0; inbSegment(); ++i) { - std::vector markers_tp = model->segmentMarkers( + std::vector markers_tp = model->segmentMarkers( Q, i, removeAxes); allMarkers.push_back(markers_tp); } @@ -49,7 +49,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], double *markers = mxGetPr(markers_out_tp); // Remplir le output - std::vector::iterator it=(* + std::vector::iterator it=(* (allMarkers.begin()+i_bone)).begin(); for (unsigned int i=0; (it+i)!=(*(allMarkers.begin()+i_bone)).end(); ++i) { markers[i*3] = (*(it+i))(0); @@ -61,7 +61,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector markers_tp = + std::vector markers_tp = model->segmentMarkers(Q, static_cast(idx), removeAxes); // Create a matrix for the return argument @@ -69,7 +69,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], double *markers = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=markers_tp.begin(); + std::vector::iterator it=markers_tp.begin(); for (unsigned int i=0; (it+i)!=markers_tp.end(); ++i) { markers[i*3] = (*(it+i))(0); markers[i*3+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_segmentMass.h b/binding/matlab/Matlab_segmentMass.h index 5b8b8665..eb210d88 100644 --- a/binding/matlab/Matlab_segmentMass.h +++ b/binding/matlab/Matlab_segmentMass.h @@ -15,7 +15,7 @@ void Matlab_segmentMass( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int idx; if (nrhs>2) { diff --git a/binding/matlab/Matlab_segmentsInertia.h b/binding/matlab/Matlab_segmentsInertia.h index 1c18bdb2..89df7de4 100644 --- a/binding/matlab/Matlab_segmentsInertia.h +++ b/binding/matlab/Matlab_segmentsInertia.h @@ -16,11 +16,11 @@ void Matlab_segmentsInertia( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Update sur la cinématique (placer les segments) @@ -53,7 +53,7 @@ void Matlab_segmentsInertiaLocal( int, mxArray *plhs[], "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nBones = model->nbSegment(); // Get the number of DoF diff --git a/binding/matlab/Matlab_segmentsVelocities.h b/binding/matlab/Matlab_segmentsVelocities.h index 23ac9ed1..688d335e 100644 --- a/binding/matlab/Matlab_segmentsVelocities.h +++ b/binding/matlab/Matlab_segmentsVelocities.h @@ -15,15 +15,15 @@ void Matlab_segmentsVelocities( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Update sur la cinématique @@ -35,11 +35,10 @@ void Matlab_segmentsVelocities( int, mxArray *plhs[], // Remplir l'output unsigned int cmp = 0; - for (std::vector::iterator v_it = - model->v.begin(); v_it != model->v.end(); - ++v_it) { - for (unsigned int i = 0; i<6; ++i) { - vel[i+6*cmp] = (*v_it)(i); + for (unsigned int i=0; iRigidBodyDynamics::Model::v.size(); ++i){ + auto& v = model->RigidBodyDynamics::Model::v[i]; + for (unsigned int j = 0; j<6; ++j) { + vel[i+6*cmp] = v(j); } ++cmp; } diff --git a/binding/matlab/Matlab_torqueActivation.h b/binding/matlab/Matlab_torqueActivation.h index 30b2e608..ddc19b59 100644 --- a/binding/matlab/Matlab_torqueActivation.h +++ b/binding/matlab/Matlab_torqueActivation.h @@ -14,20 +14,20 @@ void Matlab_torqueActivation( int, mxArray *plhs[], "5 or 5 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th is QDot and 5th is GeneralizedTorque"); // Recevoir le model - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector act = + std::vector act = getParameterGeneralizedTorque(prhs, 4, model->nbGeneralizedTorque()); diff --git a/binding/matlab/Matlab_totalMass.h b/binding/matlab/Matlab_totalMass.h index 2b515525..b88d31dd 100644 --- a/binding/matlab/Matlab_totalMass.h +++ b/binding/matlab/Matlab_totalMass.h @@ -14,7 +14,7 @@ void Matlab_totalMass( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::Model * model = convertMat2Ptr(prhs[1]); + biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/biorbd.h b/binding/matlab/biorbd.h index 13408fc9..6f5cf4b2 100644 --- a/binding/matlab/biorbd.h +++ b/binding/matlab/biorbd.h @@ -274,7 +274,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Nombre de Markers if (!toLower(cmd).compare("nmimu")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"nmimu\" est obsolete. Remplacer par \"nimu\". Elle sera retirée prochainement"); Matlab_nIMU(nlhs, plhs, nrhs, prhs); return; @@ -286,7 +286,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers if (!toLower(cmd).compare("namemimu")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"namemimu\" est obsolete. Remplacer par \"nameimu\". Elle sera retirée prochainement"); Matlab_nameIMU(nlhs, plhs, nrhs, prhs); return; @@ -298,7 +298,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers techniques if (!toLower(cmd).compare("nametechnicalmimu")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"nametechnicalmimu\" est obsolete. Remplacer par \"nametechnicalimu\". Elle sera retirée prochainement"); Matlab_nameTechnicalIMU(nlhs, plhs, nrhs, prhs); return; @@ -310,7 +310,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers anatomiques if (!toLower(cmd).compare("nameanatomicalmimu")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"nameanatomicalmimu\" est obsolete. Remplacer par \"nameanatomicalimu\". Elle sera retirée prochainement"); Matlab_nameAnatomicalIMU(nlhs, plhs, nrhs, prhs); return; @@ -322,7 +322,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Fonction de cinématique directe if(!toLower(cmd).compare("mimu")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"mimu\" est obsolete. Remplacer par \"imu\". Elle sera retirée prochainement"); Matlab_IMU(nlhs, plhs, nrhs, prhs); return; @@ -334,7 +334,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Fonction de cinématique directe if(!toLower(cmd).compare("mimujacobian")) { - biorbd::utils::Error::warning(0, + biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, "La fonction \"mimujacobian\" est obsolete. Remplacer par \"imujacobian\". Elle sera retirée prochainement"); Matlab_IMUJacobian(nlhs, plhs, nrhs, prhs); return; diff --git a/binding/matlab/processArguments.h b/binding/matlab/processArguments.h index 213127c2..f2c06741 100644 --- a/binding/matlab/processArguments.h +++ b/binding/matlab/processArguments.h @@ -27,7 +27,7 @@ void checkNombreInputParametres(int nrhs, int min, int max, } } -std::vector> getParameterAllMarkers( +std::vector> getParameterAllMarkers( const mxArray*prhs[], unsigned int idx, int nMark=-1) { @@ -80,15 +80,15 @@ std::vector> getParameterAllMarkers( // Créer la sortie - std::vector> markersOverTime; + std::vector> markersOverTime; // Stocker les valeurs dans le format de sortie unsigned int cmp(0); for (unsigned int i=0; i markers_tp; // Markers a un temps i + std::vector markers_tp; // Markers a un temps i for (int j=0; j> getParameterAllMarkers( // Retourner la matrice return markersOverTime; } -std::vector> getParameterAllIMUs( +std::vector> getParameterAllIMUs( const mxArray*prhs[], unsigned int idx) { // Check data type of input argument @@ -149,14 +149,14 @@ std::vector> getParameterAllIMUs( double *imus = mxGetPr(prhs[idx]); // Créer la sortie - std::vector> imuOverTime; + std::vector> imuOverTime; // Stocker les valeurs dans le format de sortie for (unsigned int i=0; i imus_tp; // IMUs a un temps i + std::vector imus_tp; // IMUs a un temps i for (unsigned int j=0; j> getParameterAllIMUs( imus[i*9*nIMUs+j*9+2], imus[i*9*nIMUs+j*9+5], imus[i*9*nIMUs+j*9+8]; trans.setZero(); } - imus_tp.push_back(biorbd::utils::RotoTrans(rot, trans)); + imus_tp.push_back(biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans(rot, trans)); } imuOverTime.push_back(imus_tp); } @@ -180,7 +180,7 @@ std::vector> getParameterAllIMUs( } -biorbd::rigidbody::GeneralizedCoordinates getVector(const mxArray*prhs[], +biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates getVector(const mxArray*prhs[], unsigned int idx, std::string type = "") { // Check data type of input argument @@ -208,7 +208,7 @@ biorbd::rigidbody::GeneralizedCoordinates getVector(const mxArray*prhs[], double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - biorbd::rigidbody::GeneralizedCoordinates vect(static_cast + biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates vect(static_cast (length)); for (unsigned int i=0; i getParameterQ( +std::vector getParameterQ( const mxArray*prhs[], unsigned int idx, unsigned int nDof, std::string type = "q") { @@ -263,9 +263,9 @@ std::vector getParameterQ( double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Q; + std::vector Q; for (unsigned int j=0; j getParameterQ( return Q; } -std::vector getParameterQddot( +std::vector getParameterQddot( const mxArray*prhs[], unsigned int idx, unsigned int nbQddot, std::string type = "qddot") { @@ -301,9 +301,9 @@ std::vector getParameterQddot( double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Qddot; + std::vector Qddot; for (unsigned int j=0; j getParameterQddot( return Qddot; } -std::vector getParameterQdot( +std::vector getParameterQdot( const mxArray*prhs[], unsigned int idx, unsigned int nbQdot, std::string type = "qdot") { @@ -339,9 +339,9 @@ std::vector getParameterQdot( double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Qdot; + std::vector Qdot; for (unsigned int j=0; j getParameterQdot( return Qdot; } -std::vector getParameterGeneralizedTorque( +std::vector getParameterGeneralizedTorque( const mxArray*prhs[], unsigned int idx, unsigned int nControl) { - std::vector AllGeneralizedTorque_tp = + std::vector AllGeneralizedTorque_tp = getParameterQ(prhs, idx, nControl, "GeneralizedTorque"); - std::vector AllGeneralizedTorque; + std::vector AllGeneralizedTorque; for (unsigned int j=0; j getParameterGeneralizedTorque( return AllGeneralizedTorque; } -std::vector> getForcePlate( +std::vector> getForcePlate( const mxArray*prhs[], unsigned int idx) { if (!(mxIsDouble(prhs[idx]))) { @@ -401,12 +401,12 @@ std::vector> getForcePlate( double *pf = mxGetPr(prhs[idx]); // Matrice des plateforme de force // stockage des plateformes - std::vector> PF; + std::vector> PF; unsigned int cmp(0); for (unsigned int j=0; j PF_tp; + std::vector PF_tp; for (unsigned int i=0; i getDoubleArray(const mxArray*prhs[], unsigned int idx) return out; } -biorbd::utils::String getString(const mxArray*prhs[], unsigned int idx) +biorbd::BIORBD_MATH_NAMESPACE::utils::String getString(const mxArray*prhs[], unsigned int idx) { // Check data type of input argument if (!(mxIsChar(prhs[idx]))) { @@ -525,7 +525,7 @@ bool isStateExist(const mxArray*prhs[], unsigned int nMus, int idx, return isThere; } -std::vector>> +std::vector>> getParameterMuscleState( const mxArray*prhs[], int idxExcitation, @@ -571,18 +571,18 @@ getParameterMuscleState( // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector>> States; + std::vector>> States; for (unsigned int j=0; j> States_tp; + std::vector> States_tp; for (unsigned int i=0; i + States_tp.push_back( std::make_shared (stateExcitation[j*nMus+i], stateActivation[j*nMus+i])); } else if (!isThereExcitation && isThereActivation) { - States_tp.push_back( std::make_shared(0, + States_tp.push_back( std::make_shared(0, stateActivation[j*nMus+i])); } else if (isThereExcitation && !isThereActivation) { - States_tp.push_back( std::make_shared + States_tp.push_back( std::make_shared (stateExcitation[j*nMus+i], 0)); } @@ -592,7 +592,7 @@ getParameterMuscleState( } -std::vector> +std::vector> getParameterMuscleStateBuchanan(const mxArray*prhs[], int idxExcitation, int idxActivation, unsigned int nMus) { @@ -635,18 +635,18 @@ std::vector> // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector> States; + std::vector> States; for (unsigned int j=0; j States_tp; + std::vector States_tp; for (unsigned int i=0; i> return States; } -std::vector>> +std::vector>> getParameterMuscleStateActivation( const mxArray*prhs[], int idxActivation, unsigned int nMus) { return getParameterMuscleState(prhs, -1, idxActivation, nMus); } -std::vector>> +std::vector>> getParameterMuscleStateExcitation( const mxArray*prhs[], int idxExcitation, unsigned int nMus) { return getParameterMuscleState(prhs, idxExcitation, -1, nMus); } -std::vector> +std::vector> getParameterMuscleStateActivationBuchanan( const mxArray*prhs[], int idxActivation, unsigned int nMus) { return getParameterMuscleStateBuchanan(prhs, -1, idxActivation, nMus); } -std::vector> +std::vector> getParameterMuscleStateExcitationBuchanan( const mxArray*prhs[], int idxExcitation, unsigned int nMus) { @@ -719,7 +719,7 @@ std::vector getParameterMuscleForceNorm(const mxArray*prhs[], } -std::vector> getMusclePosition( +std::vector> getMusclePosition( const mxArray*prhs[], unsigned int idx, Eigen::VectorXd nPointsByMuscles) { @@ -755,13 +755,13 @@ std::vector> getMusclePosition( double *via=mxGetPr(prhs[idx]); //matrice de position // Préparer la matrice de sortie - std::vector> out; + std::vector> out; unsigned int cmpMus(0); for (unsigned int i=0; i mus; + std::vector mus; for (unsigned int j=0; j> getMusclePosition( return out; } -std::vector getMusclePointsJaco(const mxArray*prhs[], +std::vector getMusclePointsJaco(const mxArray*prhs[], unsigned int idx, Eigen::VectorXd nPointsByMuscles, unsigned int nQ) { @@ -807,12 +807,12 @@ std::vector getMusclePointsJaco(const mxArray*prhs[], double *jaco=mxGetPr(prhs[idx]); //matrice de position // Préparer la matrice de sortie - std::vector jacoOut; + std::vector jacoOut; unsigned int cmpMus(0); for (unsigned int i=0; i(nPointsByMuscles(i))*3, nQ); + biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix mus(static_cast(nPointsByMuscles(i))*3, nQ); for (unsigned int j=0; j(nPointsByMuscles(i)); ++j) { // Stocker for (unsigned int k1 = 0; k1<3; ++k1) diff --git a/test/binding/c/test_binder_c.cpp b/test/binding/c/test_binder_c.cpp index 6145c407..a70742cc 100644 --- a/test/binding/c/test_binder_c.cpp +++ b/test/binding/c/test_binder_c.cpp @@ -16,6 +16,8 @@ #include "RigidBody/KalmanReconsIMU.h" #endif +using namespace biorbd::BIORBD_MATH_NAMESPACE; + static double requiredPrecision(1e-10); static std::string modelPathForGeneralTesting("models/pyomecaman.bioMod"); @@ -24,13 +26,13 @@ modelPathForIMUTesting("models/IMUandCustomRT/pyomecaman_withIMUs.bioMod"); TEST(BinderC, OpenCloseModel) { - biorbd::Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); + Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); c_deleteBiorbdModel(model); } TEST(BinderC, nDofs) { - biorbd::Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); + Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); EXPECT_EQ(c_nQ(model), model->nbQ()); EXPECT_EQ(c_nQDot(model), model->nbQdot()); EXPECT_EQ(c_nQDDot(model), model->nbQddot()); @@ -40,11 +42,11 @@ TEST(BinderC, nDofs) TEST(BinderC, markers) { - biorbd::Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); + Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); EXPECT_EQ(c_nMarkers(model), model->nbMarkers()); // Markers in global reference frame - biorbd::rigidbody::GeneralizedCoordinates Q(*model); + rigidbody::GeneralizedCoordinates Q(*model); double *dQ = new double[model->nbQ()]; for (unsigned int i=0; inbQ(); ++i) { Q[i] = 0.1; @@ -52,7 +54,7 @@ TEST(BinderC, markers) } double *dMarkersInGlobal = new double[model->nbMarkers()*3]; c_markers(model, dQ, dMarkersInGlobal); - std::vector markersInGlobal(model->markers(Q)); + std::vector markersInGlobal(model->markers(Q)); for (unsigned int i=0; inbMarkers(); ++i) for (unsigned int j=0; j<3; ++j) { EXPECT_NEAR(dMarkersInGlobal[i*3+j], markersInGlobal[i][j], requiredPrecision); @@ -65,7 +67,7 @@ TEST(BinderC, markers) c_addMarker(model, newMarkerPosition, "MyNewMarker", model->segment(1).name().c_str(), false, true, "x"); EXPECT_EQ(model->nbMarkers(), nMarkersBeforeAdding + 1); - biorbd::rigidbody::NodeSegment newMarker(model->marker(nMarkersBeforeAdding)); + rigidbody::NodeSegment newMarker(model->marker(nMarkersBeforeAdding)); EXPECT_STREQ(newMarker.name().c_str(), "MyNewMarker"); EXPECT_STREQ(newMarker.parent().c_str(), model->segment(1).name().c_str()); EXPECT_EQ(newMarker.isTechnical(), false); @@ -75,7 +77,7 @@ TEST(BinderC, markers) // Markers in local reference frame double *dMarkersInLocal = new double[model->nbMarkers()*3]; c_markersInLocal(model, dMarkersInLocal); - std::vector markersInLocal(model->markers()); + std::vector markersInLocal(model->markers()); for (unsigned int i=0; inbMarkers(); ++i) for (unsigned int j=0; j<3; ++j) { EXPECT_NEAR(dMarkersInLocal[i*3+j], markersInLocal[i][j], requiredPrecision); @@ -88,7 +90,7 @@ TEST(BinderC, markers) TEST(BinderC, jcs) { - biorbd::Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); + Model* model(c_biorbdModel(modelPathForGeneralTesting.c_str())); // Get angle sequence of some bodies for (unsigned int i=0; inbSegment(); ++i) { @@ -98,7 +100,7 @@ TEST(BinderC, jcs) } // JCS in global reference frame - biorbd::rigidbody::GeneralizedCoordinates Q(*model); + rigidbody::GeneralizedCoordinates Q(*model); double *dQ = new double[model->nbQ()]; for (unsigned int i=0; inbQ(); ++i) { Q[i] = 0.1; @@ -106,7 +108,7 @@ TEST(BinderC, jcs) } double *dRtInGlobal = new double[model->nbSegment()*4*4]; c_globalJCS(model, dQ, dRtInGlobal); - std::vector jcsInGlobal(model->allGlobalJCS(Q)); + std::vector jcsInGlobal(model->allGlobalJCS(Q)); for (unsigned int i=0; inbSegment(); ++i) for (unsigned int row=0; row<4; ++row) for (unsigned int col=0; col<4; ++col) { @@ -118,7 +120,7 @@ TEST(BinderC, jcs) // JCS in local reference frame double *dRtInLocal = new double[model->nbSegment()*4*4]; c_globalJCS(model, dQ, dRtInLocal); - std::vector jcsInLocal(model->allGlobalJCS()); + std::vector jcsInLocal(model->allGlobalJCS()); for (unsigned int i=0; inbSegment(); ++i) for (unsigned int row=0; row<4; ++row) for (unsigned int col=0; col<4; ++col) { @@ -133,11 +135,11 @@ TEST(BinderC, jcs) TEST(BinderC, imu) { - biorbd::Model* model(c_biorbdModel(modelPathForIMUTesting.c_str())); + Model* model(c_biorbdModel(modelPathForIMUTesting.c_str())); EXPECT_EQ(c_nIMUs(model), model->nbIMUs()); - biorbd::utils::RotoTrans RT( - biorbd::utils::RotoTrans::fromEulerAngles( + utils::RotoTrans RT( + utils::RotoTrans::fromEulerAngles( Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Vector3d(0.1, 0.1, 0.1), "xyz")); double rt[16]; @@ -150,7 +152,7 @@ TEST(BinderC, imu) unsigned int nImuBeforeAdding(model->nbIMUs()); c_addIMU(model, rt, "MyNewIMU", "Tete", false, true); EXPECT_EQ(model->nbIMUs(), nImuBeforeAdding+1); - biorbd::rigidbody::IMU newImu(model->IMU(nImuBeforeAdding)); + rigidbody::IMU newImu(model->IMU(nImuBeforeAdding)); EXPECT_STREQ(newImu.name().c_str(), "MyNewIMU"); EXPECT_STREQ(newImu.parent().c_str(), "Tete"); EXPECT_EQ(newImu.isTechnical(), false); @@ -163,22 +165,22 @@ TEST(BinderC, imu) #ifndef SKIP_KALMAN TEST(BinderC, kalmanImu) { - biorbd::Model* model(c_biorbdModel(modelPathForIMUTesting.c_str())); - biorbd::rigidbody::KalmanReconsIMU kalman(*model); - biorbd::rigidbody::KalmanReconsIMU* kalman_c(c_BiorbdKalmanReconsIMU(model)); + Model* model(c_biorbdModel(modelPathForIMUTesting.c_str())); + rigidbody::KalmanReconsIMU kalman(*model); + rigidbody::KalmanReconsIMU* kalman_c(c_BiorbdKalmanReconsIMU(model)); // Compute reference - std::vector targetImus; + std::vector targetImus; for (unsigned int i=0; inbIMUs(); ++i) { - biorbd::utils::RotoTrans rt( - biorbd::utils::RotoTrans::fromEulerAngles( + utils::RotoTrans rt( + utils::RotoTrans::fromEulerAngles( Eigen::Vector3d(0.1*i, 0.1*i, 0.1*i), Eigen::Vector3d(0.1*i, 0.1*i, 0.1*i), "xyz")); targetImus.push_back(rt); } - biorbd::rigidbody::GeneralizedCoordinates Q(*model); - biorbd::rigidbody::GeneralizedVelocity Qdot(*model); - biorbd::rigidbody::GeneralizedAcceleration Qddot(*model); + rigidbody::GeneralizedCoordinates Q(*model); + rigidbody::GeneralizedVelocity Qdot(*model); + rigidbody::GeneralizedAcceleration Qddot(*model); kalman.reconstructFrame(*model, targetImus, &Q, &Qdot, &Qddot); // Compute from C-interface @@ -213,10 +215,10 @@ TEST(BinderC, math) { // Simple matrix multiplaction (RT3 = RT1 * RT2) { - biorbd::utils::RotoTrans RT1, RT2, RT3; - RT1 = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), + utils::RotoTrans RT1, RT2, RT3; + RT1 = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Vector3d(0.1, 0.1, 0.1), "xyz"); - RT2 = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, 0.3), + RT2 = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, 0.3), Eigen::Vector3d(0.2, 0.2, 0.2), "xyz"); RT3 = RT1.operator*(RT2); @@ -238,18 +240,18 @@ TEST(BinderC, math) // Mean multiple matrices { - std::vector allRT(3); - biorbd::utils::RotoTrans meanRT; - allRT[0] = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, + std::vector allRT(3); + utils::RotoTrans meanRT; + allRT[0] = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Vector3d(0.1, 0.1, 0.1), "xyz"); - allRT[1] = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.2, 0.2, + allRT[1] = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.2, 0.2, 0.2), Eigen::Vector3d(0.2, 0.2, 0.2), "xyz"); - allRT[2] = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, + allRT[2] = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, 0.3), Eigen::Vector3d(0.3, 0.3, 0.3), "xyz"); - meanRT = biorbd::utils::RotoTrans::mean(allRT); + meanRT = utils::RotoTrans::mean(allRT); double* rt = new double[meanRT.size()*16]; double mean_rt[16]; @@ -272,10 +274,10 @@ TEST(BinderC, math) // Project jcs onto another (RT3 = RT1.tranpose() * RT2) { - biorbd::utils::RotoTrans RT1, RT2, RT3; - RT1 = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), + utils::RotoTrans RT1, RT2, RT3; + RT1 = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Vector3d(0.1, 0.1, 0.1), "xyz"); - RT2 = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, 0.3), + RT2 = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.3, 0.3, 0.3), Eigen::Vector3d(0.2, 0.2, 0.2), "xyz"); RT3 = RT1.transpose().operator*(RT2); @@ -297,10 +299,10 @@ TEST(BinderC, math) // Get the cardan angles from a matrix { - biorbd::utils::RotoTrans RT; - RT = biorbd::utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), + utils::RotoTrans RT; + RT = utils::RotoTrans::fromEulerAngles(Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Vector3d(0.1, 0.1, 0.1), "xyz"); - biorbd::utils::Vector realCardan(biorbd::utils::RotoTrans::toEulerAngles(RT, + utils::Vector realCardan(utils::RotoTrans::toEulerAngles(RT, "xyz")); double rt[16]; From 634b2c6596383443f24b7dc12ff4b1a4529027f9 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 13:53:16 -0400 Subject: [PATCH 11/14] Removed one layer for the namespace --- binding/biorbd_muscles.i.in | 64 +++--- binding/biorbd_rigidbody.i.in | 32 +-- binding/biorbd_utils.i.in | 44 ++-- binding/c/biorbd_c.cpp | 2 +- binding/c/biorbd_c.h | 79 ++++--- binding/matlab/Matlab_ChangeShapeFactors.h | 4 +- binding/matlab/Matlab_CoM.h | 4 +- binding/matlab/Matlab_CoMJacobian.h | 4 +- binding/matlab/Matlab_CoMangularMomentum.h | 6 +- binding/matlab/Matlab_CoMddot.h | 8 +- binding/matlab/Matlab_CoMdot.h | 6 +- binding/matlab/Matlab_ContactGamma.h | 6 +- binding/matlab/Matlab_ContactJacobian.h | 4 +- binding/matlab/Matlab_ContactsPosition.h | 8 +- binding/matlab/Matlab_IMU.h | 20 +- binding/matlab/Matlab_IMUJacobian.h | 8 +- binding/matlab/Matlab_LocalMarkers.h | 14 +- binding/matlab/Matlab_Markers.h | 20 +- binding/matlab/Matlab_MarkersJacobian.h | 8 +- binding/matlab/Matlab_Mesh.h | 8 +- binding/matlab/Matlab_MusclesActivationDot.h | 4 +- .../Matlab_MusclesExcitationDotBuchanan.h | 6 +- binding/matlab/Matlab_MusclesForce.h | 12 +- binding/matlab/Matlab_MusclesForceMax.h | 2 +- binding/matlab/Matlab_MusclesJacobian.h | 4 +- binding/matlab/Matlab_MusclesLength.h | 8 +- binding/matlab/Matlab_MusclesNames.h | 2 +- binding/matlab/Matlab_MusclesParentNames.h | 2 +- binding/matlab/Matlab_MusclesPoints.h | 24 +- binding/matlab/Matlab_NLeffects.h | 8 +- binding/matlab/Matlab_Patch.h | 12 +- binding/matlab/Matlab_ProjectCustomPoint.h | 12 +- binding/matlab/Matlab_ProjectPoint.h | 12 +- binding/matlab/Matlab_ProjectPointJacobian.h | 10 +- binding/matlab/Matlab_changeGravity.h | 2 +- binding/matlab/Matlab_computeQdot.h | 8 +- binding/matlab/Matlab_delete.h | 2 +- binding/matlab/Matlab_forwardDynamics.h | 12 +- binding/matlab/Matlab_globalJCS.h | 12 +- binding/matlab/Matlab_inverseDynamics.h | 12 +- binding/matlab/Matlab_inverseKinematics.h | 8 +- binding/matlab/Matlab_inverseKinematicsEKF.h | 44 ++-- .../matlab/Matlab_inverseKinematicsEKF_IMU.h | 44 ++-- binding/matlab/Matlab_localJCS.h | 6 +- binding/matlab/Matlab_massMatrix.h | 4 +- .../Matlab_muscleJointTorqueFromActivation.h | 12 +- .../Matlab_muscleJointTorqueFromExcitation.h | 14 +- .../Matlab_muscleJointTorqueFromMuscleForce.h | 8 +- binding/matlab/Matlab_muscleLengthJacobian.h | 6 +- binding/matlab/Matlab_muscleUpdate.h | 12 +- binding/matlab/Matlab_muscleVelocity.h | 6 +- binding/matlab/Matlab_nBody.h | 2 +- binding/matlab/Matlab_nControl.h | 2 +- binding/matlab/Matlab_nIMU.h | 2 +- binding/matlab/Matlab_nMarkers.h | 2 +- binding/matlab/Matlab_nMuscles.h | 2 +- binding/matlab/Matlab_nQ.h | 2 +- binding/matlab/Matlab_nQddot.h | 2 +- binding/matlab/Matlab_nQdot.h | 2 +- binding/matlab/Matlab_nRoot.h | 2 +- binding/matlab/Matlab_nameBody.h | 2 +- binding/matlab/Matlab_nameDof.h | 2 +- binding/matlab/Matlab_nameIMU.h | 12 +- binding/matlab/Matlab_nameMarkers.h | 12 +- binding/matlab/Matlab_new.h | 6 +- binding/matlab/Matlab_parent.h | 2 +- binding/matlab/Matlab_rangeQ.h | 2 +- .../matlab/Matlab_segmentAngularMomentum.h | 8 +- binding/matlab/Matlab_segmentCoM.h | 8 +- binding/matlab/Matlab_segmentCoMddot.h | 12 +- binding/matlab/Matlab_segmentCoMdot.h | 10 +- binding/matlab/Matlab_segmentMarkers.h | 14 +- binding/matlab/Matlab_segmentMass.h | 2 +- binding/matlab/Matlab_segmentsInertia.h | 6 +- binding/matlab/Matlab_segmentsVelocities.h | 6 +- binding/matlab/Matlab_torqueActivation.h | 8 +- binding/matlab/Matlab_totalMass.h | 2 +- binding/matlab/biorbd.h | 12 +- binding/matlab/processArguments.h | 108 ++++----- binding/python3/biorbd_python.i.in | 214 +++++++++--------- examples/WrappingObjectsExample.cpp | 2 +- examples/forwardDynamicsExample.cpp | 2 +- .../forwardDynamicsFromMusclesExample.cpp | 2 +- examples/forwardKinematicsExample.cpp | 2 +- examples/inverseDynamicsExample.cpp | 2 +- examples/inverseKinematicsKalmanExample.cpp | 2 +- examples/staticOptimizationExample.cpp | 2 +- include/Actuators/Actuator.h | 5 +- include/Actuators/ActuatorConstant.h | 5 +- include/Actuators/ActuatorEnums.h | 5 +- include/Actuators/ActuatorGauss3p.h | 5 +- include/Actuators/ActuatorGauss6p.h | 5 +- include/Actuators/ActuatorLinear.h | 5 +- include/Actuators/ActuatorSigmoidGauss3p.h | 5 +- include/Actuators/Actuators.h | 5 +- include/BiorbdModel.h | 7 +- include/ModelReader.h | 15 +- include/ModelWriter.h | 5 +- include/Muscles/Characteristics.h | 5 +- include/Muscles/Compound.h | 5 +- include/Muscles/FatigueDynamicState.h | 5 +- include/Muscles/FatigueDynamicStateXia.h | 5 +- include/Muscles/FatigueModel.h | 5 +- include/Muscles/FatigueParameters.h | 5 +- include/Muscles/FatigueState.h | 5 +- include/Muscles/Geometry.h | 19 +- include/Muscles/HillThelenActiveOnlyType.h | 5 +- include/Muscles/HillThelenType.h | 5 +- include/Muscles/HillThelenTypeFatigable.h | 5 +- include/Muscles/HillType.h | 9 +- include/Muscles/IdealizedActuator.h | 9 +- include/Muscles/Muscle.h | 17 +- include/Muscles/MuscleGroup.h | 5 +- include/Muscles/Muscles.h | 5 +- include/Muscles/MusclesEnums.h | 5 +- include/Muscles/PathModifiers.h | 5 +- include/Muscles/State.h | 5 +- include/Muscles/StateDynamics.h | 5 +- include/Muscles/StateDynamicsBuchanan.h | 5 +- include/Muscles/StateDynamicsDeGroote.h | 5 +- include/Muscles/StaticOptimization.h | 5 +- include/Muscles/StaticOptimizationIpopt.h | 5 +- .../StaticOptimizationIpoptLinearized.h | 5 +- include/Muscles/ViaPoint.h | 5 +- include/Muscles/WrappingHalfCylinder.h | 9 +- include/Muscles/WrappingObject.h | 9 +- include/Muscles/WrappingSphere.h | 9 +- include/RigidBody/Contacts.h | 5 +- include/RigidBody/GeneralizedAcceleration.h | 7 +- include/RigidBody/GeneralizedCoordinates.h | 5 +- include/RigidBody/GeneralizedTorque.h | 5 +- include/RigidBody/GeneralizedVelocity.h | 5 +- include/RigidBody/IMU.h | 5 +- include/RigidBody/IMUs.h | 5 +- include/RigidBody/Joints.h | 29 ++- include/RigidBody/KalmanRecons.h | 9 +- include/RigidBody/KalmanReconsIMU.h | 9 +- include/RigidBody/KalmanReconsMarkers.h | 11 +- include/RigidBody/Markers.h | 9 +- include/RigidBody/Mesh.h | 5 +- include/RigidBody/MeshFace.h | 5 +- include/RigidBody/NodeSegment.h | 5 +- include/RigidBody/RotoTransNodes.h | 6 +- include/RigidBody/Segment.h | 15 +- include/RigidBody/SegmentCharacteristics.h | 5 +- include/Utils/Benchmark.h | 5 +- include/Utils/Equation.h | 5 +- include/Utils/Error.h | 5 +- include/Utils/IfStream.h | 5 +- include/Utils/Matrix.h | 5 +- include/Utils/Node.h | 5 +- include/Utils/Path.h | 5 +- include/Utils/Quaternion.h | 5 +- include/Utils/Range.h | 5 +- include/Utils/Rotation.h | 7 +- include/Utils/RotoTrans.h | 14 +- include/Utils/RotoTransNode.h | 5 +- include/Utils/Scalar.h | 5 +- include/Utils/SpatialVector.h | 5 +- include/Utils/String.h | 7 +- include/Utils/Timer.h | 5 +- include/Utils/UtilsEnum.h | 5 +- include/Utils/Vector.h | 5 +- include/Utils/Vector3d.h | 5 +- include/biorbdConfig.h.in | 22 +- src/Actuators/Actuator.cpp | 2 +- src/Actuators/ActuatorConstant.cpp | 2 +- src/Actuators/ActuatorGauss3p.cpp | 2 +- src/Actuators/ActuatorGauss6p.cpp | 2 +- src/Actuators/ActuatorLinear.cpp | 2 +- src/Actuators/ActuatorSigmoidGauss3p.cpp | 2 +- src/Actuators/Actuators.cpp | 15 +- src/BiorbdModel.cpp | 2 +- src/ModelReader.cpp | 2 +- src/ModelWriter.cpp | 2 +- src/Muscles/Characteristics.cpp | 2 +- src/Muscles/Compound.cpp | 2 +- src/Muscles/FatigueDynamicState.cpp | 2 +- src/Muscles/FatigueDynamicStateXia.cpp | 2 +- src/Muscles/FatigueModel.cpp | 2 +- src/Muscles/FatigueParameters.cpp | 2 +- src/Muscles/FatigueState.cpp | 2 +- src/Muscles/Geometry.cpp | 16 +- src/Muscles/HillThelenActiveOnlyType.cpp | 2 +- src/Muscles/HillThelenType.cpp | 2 +- src/Muscles/HillThelenTypeFatigable.cpp | 2 +- src/Muscles/HillType.cpp | 6 +- src/Muscles/IdealizedActuator.cpp | 6 +- src/Muscles/Muscle.cpp | 14 +- src/Muscles/MuscleGroup.cpp | 2 +- src/Muscles/Muscles.cpp | 9 +- src/Muscles/PathModifiers.cpp | 2 +- src/Muscles/State.cpp | 2 +- src/Muscles/StateDynamics.cpp | 2 +- src/Muscles/StateDynamicsBuchanan.cpp | 2 +- src/Muscles/StateDynamicsDeGroote.cpp | 2 +- src/Muscles/StaticOptimization.cpp | 2 +- src/Muscles/StaticOptimizationIpopt.cpp | 2 +- .../StaticOptimizationIpoptLinearized.cpp | 2 +- src/Muscles/ViaPoint.cpp | 2 +- src/Muscles/WrappingHalfCylinder.cpp | 6 +- src/Muscles/WrappingObject.cpp | 2 +- src/Muscles/WrappingSphere.cpp | 4 +- src/RigidBody/Contacts.cpp | 2 +- src/RigidBody/GeneralizedAcceleration.cpp | 4 +- src/RigidBody/GeneralizedCoordinates.cpp | 4 +- src/RigidBody/GeneralizedTorque.cpp | 2 +- src/RigidBody/GeneralizedVelocity.cpp | 2 +- src/RigidBody/IMU.cpp | 2 +- src/RigidBody/IMUs.cpp | 9 +- src/RigidBody/Joints.cpp | 28 +-- src/RigidBody/KalmanRecons.cpp | 6 +- src/RigidBody/KalmanReconsIMU.cpp | 6 +- src/RigidBody/KalmanReconsMarkers.cpp | 8 +- src/RigidBody/Markers.cpp | 33 ++- src/RigidBody/Mesh.cpp | 2 +- src/RigidBody/MeshFace.cpp | 2 +- src/RigidBody/NodeSegment.cpp | 2 +- src/RigidBody/RotoTransNodes.cpp | 9 +- src/RigidBody/Segment.cpp | 12 +- src/RigidBody/SegmentCharacteristics.cpp | 2 +- src/Utils/Benchmark.cpp | 2 +- src/Utils/Equation.cpp | 2 +- src/Utils/Error.cpp | 2 +- src/Utils/IfStream.cpp | 2 +- src/Utils/Matrix.cpp | 2 +- src/Utils/Node.cpp | 2 +- src/Utils/Path.cpp | 2 +- src/Utils/Quaternion.cpp | 2 +- src/Utils/Range.cpp | 2 +- src/Utils/Rotation.cpp | 2 +- src/Utils/RotoTrans.cpp | 2 +- src/Utils/RotoTransNode.cpp | 2 +- src/Utils/Scalar.cpp | 2 +- src/Utils/SpatialVector.cpp | 2 +- src/Utils/String.cpp | 2 +- src/Utils/Timer.cpp | 2 +- src/Utils/Vector.cpp | 2 +- src/Utils/Vector3d.cpp | 2 +- test/binding/c/test_binder_c.cpp | 2 +- test/test_actuators.cpp | 2 +- test/test_biorbd.cpp | 2 +- test/test_muscles.cpp | 2 +- test/test_rigidbody.cpp | 2 +- test/test_utils.cpp | 2 +- 245 files changed, 883 insertions(+), 1128 deletions(-) diff --git a/binding/biorbd_muscles.i.in b/binding/biorbd_muscles.i.in index 19900fd8..9da96563 100644 --- a/binding/biorbd_muscles.i.in +++ b/binding/biorbd_muscles.i.in @@ -25,63 +25,63 @@ %} // Instantiate templates -%template(VecBiorbdMuscleGroup) std::vector; -%template(SharedBiorbdMuscle) std::shared_ptr; -%template(VecSharedBiorbdMuscle) std::vector>; -%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::State); -%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::StateDynamics); -%shared_ptr(biorbd::BIORBD_MATH_NAMESPACE::muscles::StateDynamicsBuchanan); -%template(VecBiorbdMuscleState) std::vector>; -%template(MatBiorbdMuscleState) std::vector>>; -%template(SharedBiorbdMuscleFatigueState) std::shared_ptr; +%template(VecBiorbdMuscleGroup) std::vector; +%template(SharedBiorbdMuscle) std::shared_ptr; +%template(VecSharedBiorbdMuscle) std::vector>; +%shared_ptr(BIORBD_NAMESPACE::muscles::State); +%shared_ptr(BIORBD_NAMESPACE::muscles::StateDynamics); +%shared_ptr(BIORBD_NAMESPACE::muscles::StateDynamicsBuchanan); +%template(VecBiorbdMuscleState) std::vector>; +%template(MatBiorbdMuscleState) std::vector>>; +%template(SharedBiorbdMuscleFatigueState) std::shared_ptr; // extension of muscle casting -%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator{ - static biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) +%extend BIORBD_NAMESPACE::muscles::IdealizedActuator{ + static BIORBD_NAMESPACE::muscles::IdealizedActuator DeepCopy(const BIORBD_NAMESPACE::muscles::Muscle& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::IdealizedActuator copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::IdealizedActuator copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType{ - static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) +%extend BIORBD_NAMESPACE::muscles::HillType{ + static BIORBD_NAMESPACE::muscles::HillType DeepCopy(const BIORBD_NAMESPACE::muscles::Muscle& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::HillType copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::HillType copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType{ - static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) +%extend BIORBD_NAMESPACE::muscles::HillThelenType{ + static BIORBD_NAMESPACE::muscles::HillThelenType DeepCopy(const BIORBD_NAMESPACE::muscles::Muscle& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenType copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::HillThelenType copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } -%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable{ - static biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::Muscle& other) +%extend BIORBD_NAMESPACE::muscles::HillThelenTypeFatigable{ + static BIORBD_NAMESPACE::muscles::HillThelenTypeFatigable DeepCopy(const BIORBD_NAMESPACE::muscles::Muscle& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::HillThelenTypeFatigable copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::HillThelenTypeFatigable copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } // extension of muscle fatigueState casting -%extend biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia{ - static biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueState& other) +%extend BIORBD_NAMESPACE::muscles::FatigueDynamicStateXia{ + static BIORBD_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const BIORBD_NAMESPACE::muscles::FatigueState& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::FatigueDynamicStateXia copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } - static biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicState& other) + static BIORBD_NAMESPACE::muscles::FatigueDynamicStateXia DeepCopy(const BIORBD_NAMESPACE::muscles::FatigueDynamicState& other) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::FatigueDynamicStateXia copy; - copy.DeepCopy(dynamic_cast(other)); + BIORBD_NAMESPACE::muscles::FatigueDynamicStateXia copy; + copy.DeepCopy(dynamic_cast(other)); return copy; } } diff --git a/binding/biorbd_rigidbody.i.in b/binding/biorbd_rigidbody.i.in index 1c4f8a87..90de1040 100644 --- a/binding/biorbd_rigidbody.i.in +++ b/binding/biorbd_rigidbody.i.in @@ -24,25 +24,25 @@ %} namespace std { -%template(VecBiorbdGeneralizedTorque) std::vector; -%template(MatBiorbdGeneralizedTorque) std::vector>; -%template(PairBiorbdGeneralizedTorque) std::pair; -%template(VecBiorbdGeneralizedCoordinates) std::vector; -%template(MatBiorbdGeneralizedCoordinates) std::vector>; -%template(VecBiorbdGeneralizedVelocity) std::vector; -%template(MatBiorbdGeneralizedVelocity) std::vector>; -%template(VecBiorbdGeneralizedAcceleration) std::vector; -%template(MatBiorbdGeneralizedAcceleration) std::vector>; +%template(VecBiorbdGeneralizedTorque) std::vector; +%template(MatBiorbdGeneralizedTorque) std::vector>; +%template(PairBiorbdGeneralizedTorque) std::pair; +%template(VecBiorbdGeneralizedCoordinates) std::vector; +%template(MatBiorbdGeneralizedCoordinates) std::vector>; +%template(VecBiorbdGeneralizedVelocity) std::vector; +%template(MatBiorbdGeneralizedVelocity) std::vector>; +%template(VecBiorbdGeneralizedAcceleration) std::vector; +%template(MatBiorbdGeneralizedAcceleration) std::vector>; -%template(VecBiorbdNodeSegment) std::vector; -%template(MatBiorbdNodeSegment) std::vector>; -%template(VecBiorbdMeshFace) std::vector; -%template(MatBiorbdMeshFace) std::vector>; +%template(VecBiorbdNodeSegment) std::vector; +%template(MatBiorbdNodeSegment) std::vector>; +%template(VecBiorbdMeshFace) std::vector; +%template(MatBiorbdMeshFace) std::vector>; -%template(PairBiorbdNodeSegment) std::pair; +%template(PairBiorbdNodeSegment) std::pair; -%template(VecBiorbdIMU) std::vector; -%template(MatBiorbdIMU) std::vector>; +%template(VecBiorbdIMU) std::vector; +%template(MatBiorbdIMU) std::vector>; } %include "@CMAKE_SOURCE_DIR@/include/RigidBody/RigidBodyEnums.h" diff --git a/binding/biorbd_utils.i.in b/binding/biorbd_utils.i.in index 0dbdcc39..1f071813 100644 --- a/binding/biorbd_utils.i.in +++ b/binding/biorbd_utils.i.in @@ -17,44 +17,44 @@ // Instantiate templates namespace std { -%template(VecBiorbdString) std::vector; -%template(PairBiorbdString) std::pair; +%template(VecBiorbdString) std::vector; +%template(PairBiorbdString) std::pair; -%template(VecBiorbdVector) std::vector; -%template(MatBiorbdVector) std::vector>; -%template(VecBiorbdSpatialVector) std::vector; -%template(MatBiorbdMatrix) std::vector; -%template(VecBiorbdRotoTrans) std::vector; -%template(MatBiorbdRotoTrans) std::vector>; -%template(VecBiorbdRototation) std::vector; -%template(MatBiorbdRototation) std::vector>; -%template(VecBiorbdNode) std::vector; -%template(MatBiorbdNode) std::vector>; -%template(VecBiorbdRange) std::vector; -%template(MatBiorbdRange) std::vector>; +%template(VecBiorbdVector) std::vector; +%template(MatBiorbdVector) std::vector>; +%template(VecBiorbdSpatialVector) std::vector; +%template(MatBiorbdMatrix) std::vector; +%template(VecBiorbdRotoTrans) std::vector; +%template(MatBiorbdRotoTrans) std::vector>; +%template(VecBiorbdRototation) std::vector; +%template(MatBiorbdRototation) std::vector>; +%template(VecBiorbdNode) std::vector; +%template(MatBiorbdNode) std::vector>; +%template(VecBiorbdRange) std::vector; +%template(MatBiorbdRange) std::vector>; } -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::String{ +%extend BIORBD_NAMESPACE::utils::String{ std::string to_string(){ return *$self; } } -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation{ - biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation transpose(){ +%extend BIORBD_NAMESPACE::utils::Rotation{ + BIORBD_NAMESPACE::utils::Rotation transpose(){ return $self->RigidBodyDynamics::Math::Matrix3d::transpose(); } - biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation operator*( - const biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation& other){ + BIORBD_NAMESPACE::utils::Rotation operator*( + const BIORBD_NAMESPACE::utils::Rotation& other){ return $self->RigidBodyDynamics::Math::Matrix3d::operator*(other); } } -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d{ - biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE typeOfNode() const{ - return $self->biorbd::BIORBD_MATH_NAMESPACE::utils::Node::typeOfNode(); +%extend BIORBD_NAMESPACE::utils::Vector3d{ + BIORBD_NAMESPACE::utils::NODE_TYPE typeOfNode() const{ + return $self->BIORBD_NAMESPACE::utils::Node::typeOfNode(); } } diff --git a/binding/c/biorbd_c.cpp b/binding/c/biorbd_c.cpp index a5afe73f..4548da02 100644 --- a/binding/c/biorbd_c.cpp +++ b/binding/c/biorbd_c.cpp @@ -19,7 +19,7 @@ #include "RigidBody/KalmanReconsIMU.h" #endif -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; Model* c_biorbdModel( const char* pathToModel) diff --git a/binding/c/biorbd_c.h b/binding/c/biorbd_c.h index b5773c1b..407bc9a3 100644 --- a/binding/c/biorbd_c.h +++ b/binding/c/biorbd_c.h @@ -10,9 +10,7 @@ #define BIORBD_API_C #endif -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -21,74 +19,73 @@ namespace rigidbody #endif } } -} extern "C" { // Create a pointer on a model - BIORBD_API_C biorbd::BIORBD_MATH_NAMESPACE::Model* c_biorbdModel( + BIORBD_API_C BIORBD_NAMESPACE::Model* c_biorbdModel( const char* pathToModel); BIORBD_API_C void c_deleteBiorbdModel( - biorbd::BIORBD_MATH_NAMESPACE::Model*); + BIORBD_NAMESPACE::Model*); BIORBD_API_C void c_writeBiorbdModel( - biorbd::BIORBD_MATH_NAMESPACE::Model*, const char * path); + BIORBD_NAMESPACE::Model*, const char * path); // Joints functions BIORBD_API_C void c_boneRotationSequence( // Return the angle sequence of a bone named segName - biorbd::BIORBD_MATH_NAMESPACE::Model* m, + BIORBD_NAMESPACE::Model* m, const char* segName, char* seq); BIORBD_API_C void c_localJCS( // Return the LCS for segment of index i in parent coordinate system - biorbd::BIORBD_MATH_NAMESPACE::Model* m, + BIORBD_NAMESPACE::Model* m, int i, double* RtOut); BIORBD_API_C void c_globalJCS( - biorbd::BIORBD_MATH_NAMESPACE::Model*, + BIORBD_NAMESPACE::Model*, const double* Q, double* jcs); BIORBD_API_C void c_inverseDynamics( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double* q, const double* qdot, const double* qddot, double* tau); BIORBD_API_C void c_massMatrix( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double* q, double* massMatrix); BIORBD_API_C void c_CoM( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double* q, double *com); // dof functions BIORBD_API_C int c_nQ( - biorbd::BIORBD_MATH_NAMESPACE::Model* model); + BIORBD_NAMESPACE::Model* model); BIORBD_API_C int c_nQDot( - biorbd::BIORBD_MATH_NAMESPACE::Model* model); + BIORBD_NAMESPACE::Model* model); BIORBD_API_C int c_nQDDot( - biorbd::BIORBD_MATH_NAMESPACE::Model* model); + BIORBD_NAMESPACE::Model* model); BIORBD_API_C int c_nGeneralizedTorque( - biorbd::BIORBD_MATH_NAMESPACE::Model* model); + BIORBD_NAMESPACE::Model* model); // Markers functions BIORBD_API_C int c_nMarkers( - biorbd::BIORBD_MATH_NAMESPACE::Model* model); + BIORBD_NAMESPACE::Model* model); BIORBD_API_C void c_markersInLocal( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, double* markPos); BIORBD_API_C void c_markers( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double* Q, double* markPos, bool removeAxis = true, bool updateKin = true); BIORBD_API_C void c_addMarker( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double *markPos, const char* name = "", const char* parentName = "", @@ -98,9 +95,9 @@ extern "C" { // IMUs functions BIORBD_API_C int c_nIMUs( - biorbd::BIORBD_MATH_NAMESPACE::Model*); + BIORBD_NAMESPACE::Model*); BIORBD_API_C void c_addIMU( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, + BIORBD_NAMESPACE::Model* model, const double *imuRT, const char* name = "", const char* parentName = "", @@ -109,17 +106,17 @@ extern "C" { // Kalman IMU #ifdef MODULE_KALMAN - BIORBD_API_C biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( - biorbd::BIORBD_MATH_NAMESPACE::Model*, + BIORBD_API_C BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU( + BIORBD_NAMESPACE::Model*, double* QinitialGuess = nullptr, double freq = 100, double noiseF = 5e-3, double errorF = 1e-10); BIORBD_API_C void c_deleteBiorbdKalmanReconsIMU( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU*); + BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU*); BIORBD_API_C void c_BiorbdKalmanReconsIMUstep( - biorbd::BIORBD_MATH_NAMESPACE::Model*, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU*, + BIORBD_NAMESPACE::Model*, + BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU*, double* imu, double* Q = nullptr, double* QDot = nullptr, @@ -152,40 +149,40 @@ extern "C" { } // Fonctions de dispatch pour les données d'entré et de sortie -biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d dispatchMarkersInput( +BIORBD_NAMESPACE::utils::Vector3d dispatchMarkersInput( const double * pos); void dispatchMarkersOutput( - const std::vector &allMarkers, + const std::vector &allMarkers, double* markers); -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates dispatchQinput( - biorbd::BIORBD_MATH_NAMESPACE::Model* model, +BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates dispatchQinput( + BIORBD_NAMESPACE::Model* model, const double* Q); void dispatchQoutput( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates &eQ, + const BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates &eQ, double* Q); void dispatchTauOutput( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque &eTau, + const BIORBD_NAMESPACE::rigidbody::GeneralizedTorque &eTau, double* Tau); void dispatchDoubleOutput( - const biorbd::BIORBD_MATH_NAMESPACE::utils::Vector&, + const BIORBD_NAMESPACE::utils::Vector&, double*); -biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans dispatchRTinput( +BIORBD_NAMESPACE::utils::RotoTrans dispatchRTinput( const double* rt); void dispatchRToutput( - const biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans& rt_in, + const BIORBD_NAMESPACE::utils::RotoTrans& rt_in, double* rt_out); void dispatchRToutput( - const std::vector& rt_in, + const std::vector& rt_in, double* rt_out); -biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix dispatchMatrixInput( +BIORBD_NAMESPACE::utils::Matrix dispatchMatrixInput( const double* matXd, int nRows, int nCols); -biorbd::BIORBD_MATH_NAMESPACE::utils::Vector dispatchVectorInput( +BIORBD_NAMESPACE::utils::Vector dispatchVectorInput( const double* vecXd, int nElement); void dispatchVectorOutput( - const biorbd::BIORBD_MATH_NAMESPACE::utils::Vector& vect, + const BIORBD_NAMESPACE::utils::Vector& vect, double* vect_out); //#ifdef UNITY diff --git a/binding/matlab/Matlab_ChangeShapeFactors.h b/binding/matlab/Matlab_ChangeShapeFactors.h index dd83e1f0..cd12179d 100644 --- a/binding/matlab/Matlab_ChangeShapeFactors.h +++ b/binding/matlab/Matlab_ChangeShapeFactors.h @@ -16,7 +16,7 @@ void Matlab_ChangeShapeFactors( int, mxArray *[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model, 3rd is the shape factor for each muscle"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les shape factors std::vector shapeFactors = getDoubleArray(prhs, 2); @@ -32,7 +32,7 @@ void Matlab_ChangeShapeFactors( int, mxArray *[], for (unsigned int i=0; inbMuscleGroups(); ++i) for (unsigned int j=0; jmuscleGroup(i).nbMuscles(); ++j) { // Recueillir shape factor - dynamic_cast( model->muscleGroup( + dynamic_cast( model->muscleGroup( i).muscle(j).state() ).shapeFactor( shapeFactors[cmp]); ++cmp; diff --git a/binding/matlab/Matlab_CoM.h b/binding/matlab/Matlab_CoM.h index d50a9748..21752af5 100644 --- a/binding/matlab/Matlab_CoM.h +++ b/binding/matlab/Matlab_CoM.h @@ -14,11 +14,11 @@ void Matlab_CoM( int nlhs, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_CoMJacobian.h b/binding/matlab/Matlab_CoMJacobian.h index 54a7c4df..af8b4c67 100644 --- a/binding/matlab/Matlab_CoMJacobian.h +++ b/binding/matlab/Matlab_CoMJacobian.h @@ -15,11 +15,11 @@ void Matlab_CoMJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la jacobienne du COM diff --git a/binding/matlab/Matlab_CoMangularMomentum.h b/binding/matlab/Matlab_CoMangularMomentum.h index a3921b7c..a4059c94 100644 --- a/binding/matlab/Matlab_CoMangularMomentum.h +++ b/binding/matlab/Matlab_CoMangularMomentum.h @@ -14,15 +14,15 @@ void Matlab_CoMangularMomentum( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q et Qdot font la même dimension diff --git a/binding/matlab/Matlab_CoMddot.h b/binding/matlab/Matlab_CoMddot.h index 7e27e07f..fbf612bc 100644 --- a/binding/matlab/Matlab_CoMddot.h +++ b/binding/matlab/Matlab_CoMddot.h @@ -14,19 +14,19 @@ void Matlab_CoMddot( int nlhs, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector QDDot = + std::vector QDDot = getParameterQddot(prhs, 4, nQddot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension diff --git a/binding/matlab/Matlab_CoMdot.h b/binding/matlab/Matlab_CoMdot.h index de5b6c47..30aa148a 100644 --- a/binding/matlab/Matlab_CoMdot.h +++ b/binding/matlab/Matlab_CoMdot.h @@ -15,15 +15,15 @@ void Matlab_CoMdot( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension diff --git a/binding/matlab/Matlab_ContactGamma.h b/binding/matlab/Matlab_ContactGamma.h index 79b78b1f..d7d8377c 100644 --- a/binding/matlab/Matlab_ContactGamma.h +++ b/binding/matlab/Matlab_ContactGamma.h @@ -15,14 +15,14 @@ void Matlab_ContactGamma( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is the Qdot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); unsigned int nContacts = model->nbContacts(); diff --git a/binding/matlab/Matlab_ContactJacobian.h b/binding/matlab/Matlab_ContactJacobian.h index e9812c2e..9afdc2b4 100644 --- a/binding/matlab/Matlab_ContactJacobian.h +++ b/binding/matlab/Matlab_ContactJacobian.h @@ -15,11 +15,11 @@ void Matlab_ContactJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la matrice jacobienne de tous les contacts diff --git a/binding/matlab/Matlab_ContactsPosition.h b/binding/matlab/Matlab_ContactsPosition.h index 981bff75..72ba2e16 100644 --- a/binding/matlab/Matlab_ContactsPosition.h +++ b/binding/matlab/Matlab_ContactsPosition.h @@ -14,15 +14,15 @@ void Matlab_ContactsPosition( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver où sont les marqueurs - std::vector Contact_tp = model->constraintsInGlobal(Q, + std::vector Contact_tp = model->constraintsInGlobal(Q, true); @@ -31,7 +31,7 @@ void Matlab_ContactsPosition( int, mxArray *plhs[], double *contact = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=Contact_tp.begin(); + std::vector::iterator it=Contact_tp.begin(); for (unsigned int i=0; (it+i)!=Contact_tp.end(); ++i) { contact[i*3] = (*(it+i))(0); contact[i*3+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_IMU.h b/binding/matlab/Matlab_IMU.h index 85c12c2b..00ec813e 100644 --- a/binding/matlab/Matlab_IMU.h +++ b/binding/matlab/Matlab_IMU.h @@ -14,33 +14,33 @@ void Matlab_IMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 4, "3 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the Q and 4th is the wanted IMUType to be return (['all'], 'technical' or anatomical')"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Récupérer les IMU selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nIMUs(0); // Nombre de IMU - std::vector> IMU_tp; // récupérer les IMU + std::vector> IMU_tp; // récupérer les IMU if (nrhs == 4) { - biorbd::BIORBD_MATH_NAMESPACE::utils::String type(getString(prhs,3)); + BIORBD_NAMESPACE::utils::String type(getString(prhs,3)); if (!type.tolower().compare("all")) { nIMUs = model->nbIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->IMU(*Q_it)); } } else if (!type.tolower().compare("anatomical")) { nIMUs = model->nbAnatIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->anatomicalIMU(*Q_it)); } } else if (!type.tolower().compare("technical")) { nIMUs = model->nbTechIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->technicalIMU(*Q_it)); } @@ -52,7 +52,7 @@ void Matlab_IMU( int, mxArray *plhs[], } else { nIMUs = model->nbIMUs(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { IMU_tp.push_back(model->IMU(*Q_it)); } @@ -70,10 +70,10 @@ void Matlab_IMU( int, mxArray *plhs[], // Remplir l'output unsigned int cmpIMU = 0; - for (std::vector>::iterator AllIMU_it = + for (std::vector>::iterator AllIMU_it = IMU_tp.begin(); AllIMU_it != IMU_tp.end(); ++AllIMU_it) - for (std::vector::iterator IMU_it=(*AllIMU_it).begin(); + for (std::vector::iterator IMU_it=(*AllIMU_it).begin(); IMU_it!=(*AllIMU_it).end(); ++IMU_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_IMUJacobian.h b/binding/matlab/Matlab_IMUJacobian.h index fc762439..8cd052a8 100644 --- a/binding/matlab/Matlab_IMUJacobian.h +++ b/binding/matlab/Matlab_IMUJacobian.h @@ -14,17 +14,17 @@ void Matlab_IMUJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp = model->IMUJacobian(Q); - std::vector::iterator it=Jac_tp.begin(); + std::vector Jac_tp = model->IMUJacobian(Q); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument unsigned int nIMUs = model->nbIMUs(); diff --git a/binding/matlab/Matlab_LocalMarkers.h b/binding/matlab/Matlab_LocalMarkers.h index d13061c2..b82dc4fe 100644 --- a/binding/matlab/Matlab_LocalMarkers.h +++ b/binding/matlab/Matlab_LocalMarkers.h @@ -14,10 +14,10 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 4, "2 arguments are required [+2 optional] where the 2nd is the handler on the model, 3rd is the wanted markerType to be return ('all' [default], 'technical' or anatomical') and 4th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Gestion du type - biorbd::BIORBD_MATH_NAMESPACE::utils::String type("all"); + BIORBD_NAMESPACE::utils::String type("all"); if (nrhs >= 3) { type = getString(prhs,2); } @@ -28,17 +28,17 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nMarkers(0); // Nombre de marqueurs - std::vector + std::vector markers_tp; // récupérer les marqueurs if (!type.tolower().compare("all")) { nMarkers = model->nbMarkers(); - markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::markers(removeAxes); + markers_tp = model->BIORBD_NAMESPACE::rigidbody::Markers::markers(removeAxes); } else if (!type.tolower().compare("anatomical")) { nMarkers = model->nbAnatomicalMarkers(); - markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::anatomicalMarkers(removeAxes); + markers_tp = model->BIORBD_NAMESPACE::rigidbody::Markers::anatomicalMarkers(removeAxes); } else if (!type.tolower().compare("technical")) { nMarkers = model->nbTechnicalMarkers(); - markers_tp = model->biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers::technicalMarkers(removeAxes); + markers_tp = model->BIORBD_NAMESPACE::rigidbody::Markers::technicalMarkers(removeAxes); } else { std::ostringstream msg; msg << "Wrong type for markers!"; @@ -55,7 +55,7 @@ void Matlab_LocalMarkers( int, mxArray *plhs[], // Remplir le output unsigned int cmp(0); - std::vector::iterator it=markers_tp.begin(); + std::vector::iterator it=markers_tp.begin(); for (unsigned int i=0; (it+i)!=markers_tp.end(); ++i) { markers[cmp+0] = (*(it+i))(0); markers[cmp+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_Markers.h b/binding/matlab/Matlab_Markers.h index 894e6da9..155603d1 100644 --- a/binding/matlab/Matlab_Markers.h +++ b/binding/matlab/Matlab_Markers.h @@ -14,11 +14,11 @@ void Matlab_Markers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required [+2 optional] where the 2nd is the handler on the model, 3rd is the Q and 4th is the wanted markerType to be return ('all', 'technical' or anatomical') and 5th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); bool removeAxes(true); @@ -28,25 +28,25 @@ void Matlab_Markers( int, mxArray *plhs[], // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques unsigned int nMarkers(0); // Nombre de marqueurs - std::vector> + std::vector> markers_tp; // récupérer les marqueurs if (nrhs >= 4) { - biorbd::BIORBD_MATH_NAMESPACE::utils::String type(getString(prhs,3)); + BIORBD_NAMESPACE::utils::String type(getString(prhs,3)); if (!type.tolower().compare("all")) { nMarkers = model->nbMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->markers(*Q_it, removeAxes)); } } else if (!type.tolower().compare("anatomical")) { nMarkers = model->nbAnatomicalMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->anatomicalMarkers(*Q_it, removeAxes)); } } else if (!type.tolower().compare("technical")) { nMarkers = model->nbTechnicalMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->technicalMarkers(*Q_it, removeAxes)); } @@ -58,7 +58,7 @@ void Matlab_Markers( int, mxArray *plhs[], } else { nMarkers = model->nbMarkers(); - for (std::vector::iterator Q_it = + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { markers_tp.push_back(model->markers(*Q_it, removeAxes)); } @@ -75,10 +75,10 @@ void Matlab_Markers( int, mxArray *plhs[], // Remplir le output unsigned int cmp(0); - for (std::vector>::iterator + for (std::vector>::iterator markers_it = markers_tp.begin(); markers_it!=markers_tp.end(); ++markers_it) { - std::vector::iterator it=(*markers_it).begin(); + std::vector::iterator it=(*markers_it).begin(); for (unsigned int i=0; (it+i)!=(*markers_it).end(); ++i) { markers[cmp+0] = (*(it+i))(0); markers[cmp+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_MarkersJacobian.h b/binding/matlab/Matlab_MarkersJacobian.h index 15ea9b63..2b3eabe6 100644 --- a/binding/matlab/Matlab_MarkersJacobian.h +++ b/binding/matlab/Matlab_MarkersJacobian.h @@ -14,11 +14,11 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], "3 arguments are required [+1 optional] where the 2nd is the handler on the model and 3rd is the Q, an optional 4th if you only want technical marker [default = false] and 5th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); bool technicalMarkersOnly(false); @@ -33,7 +33,7 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp; + std::vector Jac_tp; unsigned int nMarkers; if (technicalMarkersOnly) { Jac_tp = model->technicalMarkersJacobian(Q, @@ -44,7 +44,7 @@ void Matlab_MarkersJacobian( int, mxArray *plhs[], removeAxes); // Retourne la jacobienne des markers nMarkers = model->nbMarkers(); } - std::vector::iterator it=Jac_tp.begin(); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3*nMarkers, nQ, mxREAL); diff --git a/binding/matlab/Matlab_Mesh.h b/binding/matlab/Matlab_Mesh.h index e23d9784..7b9ccc9c 100644 --- a/binding/matlab/Matlab_Mesh.h +++ b/binding/matlab/Matlab_Mesh.h @@ -14,11 +14,11 @@ void Matlab_Mesh( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 4, "3 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q and 4th optional is a specific segment index"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir l'index (si envoye) @@ -30,7 +30,7 @@ void Matlab_Mesh( int, mxArray *plhs[], // Output if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMesh(model->meshPoints(Q)); + std::vector> allMesh(model->meshPoints(Q)); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMesh.size(), 1); @@ -50,7 +50,7 @@ void Matlab_Mesh( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector Mesh_tp(model->meshPoints(Q, + std::vector Mesh_tp(model->meshPoints(Q, static_cast(idx))); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_MusclesActivationDot.h b/binding/matlab/Matlab_MusclesActivationDot.h index 1e8b4b9d..9d4f5e3e 100644 --- a/binding/matlab/Matlab_MusclesActivationDot.h +++ b/binding/matlab/Matlab_MusclesActivationDot.h @@ -18,10 +18,10 @@ void Matlab_MusclesActivationDot( int, mxArray *plhs[], "3rd is the excitation, 4th is the activation and 5th is a bool to express if " "excitation is already normalized"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les états musculaires - std::vector>> state + std::vector>> state = getParameterMuscleState(prhs, 2, 3, model->nbMuscleTotal()); // Already normalized diff --git a/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h b/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h index 4ddacdec..97615509 100644 --- a/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h +++ b/binding/matlab/Matlab_MusclesExcitationDotBuchanan.h @@ -16,10 +16,10 @@ void Matlab_MusclesExcitationDotBuchanan( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 5, 5, "4 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the excitation, 4th is the activation and 5th is a bool to express if excitation is already normalized"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir les états musculaires - std::vector> state = + std::vector> state = getParameterMuscleStateBuchanan(prhs, 2, 3, model->nbMuscleTotal()); @@ -45,7 +45,7 @@ void Matlab_MusclesExcitationDotBuchanan( int, mxArray *plhs[], for (unsigned int j=0; jmuscleGroup(i).nbMuscles(); ++j) { // Recueillir dérivées d'excitations double shapeFactor = - dynamic_cast( model->muscleGroup( + dynamic_cast( model->muscleGroup( i).muscle( j).state() ).shapeFactor(); (*((*(state.begin()+iTime)).begin() + cmpState)).shapeFactor(shapeFactor); diff --git a/binding/matlab/Matlab_MusclesForce.h b/binding/matlab/Matlab_MusclesForce.h index 7ca06c51..8f52bc8f 100644 --- a/binding/matlab/Matlab_MusclesForce.h +++ b/binding/matlab/Matlab_MusclesForce.h @@ -16,14 +16,14 @@ void Matlab_MusclesForce( int, mxArray *plhs[], "WARNING: if the function is called without Q and Qdot, the user MUST update by himself " "before calling this function (using updateMuscle)."); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF unsigned int nMuscleTotal = model->nbMuscles(); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateActivation(prhs,2, nMuscleTotal); unsigned int nFrame(static_cast(s.size())); @@ -44,8 +44,8 @@ void Matlab_MusclesForce( int, mxArray *plhs[], } // Recueillir la cinématique - std::vector Q; - std::vector QDot; + std::vector Q; + std::vector QDot; if (updateKin) { // Si on update pas la cinématique Q et Qdot ne sont pas nécessaire // Recevoir Q Q = getParameterQ(prhs, 3, nQ); @@ -71,8 +71,8 @@ void Matlab_MusclesForce( int, mxArray *plhs[], double * Mus = mxGetPr(plhs[0]); // Remplir le output - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + BIORBD_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; imuscleForces(s[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_MusclesForceMax.h b/binding/matlab/Matlab_MusclesForceMax.h index cd6fdee6..edc78a64 100644 --- a/binding/matlab/Matlab_MusclesForceMax.h +++ b/binding/matlab/Matlab_MusclesForceMax.h @@ -18,7 +18,7 @@ void Matlab_MusclesForceMax( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model. If 2 arguments were sent, the function returns values for each muscles, if a 3rd is sent, the max forces are replace by the sent values."); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); double *muscleForceMax = nullptr; // Pointeur pour la sortie Eigen::VectorXd Forces; diff --git a/binding/matlab/Matlab_MusclesJacobian.h b/binding/matlab/Matlab_MusclesJacobian.h index f87e2371..458b7153 100644 --- a/binding/matlab/Matlab_MusclesJacobian.h +++ b/binding/matlab/Matlab_MusclesJacobian.h @@ -18,11 +18,11 @@ void Matlab_MusclesJacobian( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie diff --git a/binding/matlab/Matlab_MusclesLength.h b/binding/matlab/Matlab_MusclesLength.h index b737a70e..0db05caa 100644 --- a/binding/matlab/Matlab_MusclesLength.h +++ b/binding/matlab/Matlab_MusclesLength.h @@ -16,11 +16,11 @@ void Matlab_MusclesLength( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie @@ -54,11 +54,11 @@ void Matlab_MusclesTendonLength( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie diff --git a/binding/matlab/Matlab_MusclesNames.h b/binding/matlab/Matlab_MusclesNames.h index 6238cbeb..ee7a010f 100644 --- a/binding/matlab/Matlab_MusclesNames.h +++ b/binding/matlab/Matlab_MusclesNames.h @@ -17,7 +17,7 @@ void Matlab_MusclesNames( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateCellMatrix(model->nbMuscleGroups(), diff --git a/binding/matlab/Matlab_MusclesParentNames.h b/binding/matlab/Matlab_MusclesParentNames.h index 293cab44..098fdd9e 100644 --- a/binding/matlab/Matlab_MusclesParentNames.h +++ b/binding/matlab/Matlab_MusclesParentNames.h @@ -18,7 +18,7 @@ void Matlab_MusclesParentNames( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateCellMatrix(model->nbMuscleGroups(), diff --git a/binding/matlab/Matlab_MusclesPoints.h b/binding/matlab/Matlab_MusclesPoints.h index 9cc95c87..70779e66 100644 --- a/binding/matlab/Matlab_MusclesPoints.h +++ b/binding/matlab/Matlab_MusclesPoints.h @@ -20,18 +20,18 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Cellules de sortie plhs[0] = mxCreateCellMatrix(model->nbMuscleTotal(), Q.size()); // Utilisable que si nlhs >= 2 - std::vector wrap_type; // forme du wrapping - std::vector wrap_RT; // orientation du wrapping + std::vector wrap_type; // forme du wrapping + std::vector wrap_RT; // orientation du wrapping std::vector wrap_dim1; // dimension du wrapping std::vector wrap_dim2; // dimension deux du wrapping @@ -44,26 +44,26 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], model->UpdateKinematicsCustom(&Q[iQ]); // Recueillir tout le chemin musculaire - std::vector via(model->muscleGroup(i).muscle( + std::vector via(model->muscleGroup(i).muscle( j).musclesPointsInGlobal()); // Si le nombre de wrap est > 0, c'est qu'il n'y a pas de viapoints et il n'y a qu'UN wrap if (model->muscleGroup(i).muscle(j).pathModifier().nbWraps() > 0) { - const biorbd::BIORBD_MATH_NAMESPACE::muscles::WrappingObject& wrappingObject( - dynamic_cast( + const BIORBD_NAMESPACE::muscles::WrappingObject& wrappingObject( + dynamic_cast( model->muscleGroup(0).muscle(0).pathModifier().object(0))); // De quel type - biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE type(wrappingObject.typeOfNode()); + BIORBD_NAMESPACE::utils::NODE_TYPE type(wrappingObject.typeOfNode()); wrap_type.push_back(type); // Dans quelle orientation wrap_RT.push_back(wrappingObject.RT()); // Quel est sa dimension - if (type == biorbd::BIORBD_MATH_NAMESPACE::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { - const biorbd::BIORBD_MATH_NAMESPACE::muscles::WrappingHalfCylinder& cylinder( - dynamic_cast + if (type == BIORBD_NAMESPACE::utils::NODE_TYPE::WRAPPING_HALF_CYLINDER) { + const BIORBD_NAMESPACE::muscles::WrappingHalfCylinder& cylinder( + dynamic_cast (wrappingObject)); wrap_dim1.push_back(cylinder.radius()); wrap_dim2.push_back(cylinder.length()); @@ -96,7 +96,7 @@ void Matlab_MusclesPoints( int nlhs, mxArray *plhs[], for (unsigned int i=0; i(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -39,7 +39,7 @@ void Matlab_NLeffects( int, mxArray *plhs[], unsigned int cmp(0); // Trouver les effets non-linéaires pour chaque configuration - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); for (unsigned int j=0; j(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Recevoir l'index (si envoye) int idx(-1); @@ -26,7 +26,7 @@ void Matlab_Patch( int, mxArray *plhs[], // Output if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMesh( + std::vector> allMesh( model->meshFaces()); // Create a matrix for the return argument @@ -38,7 +38,7 @@ void Matlab_Patch( int, mxArray *plhs[], // Remplir le output for (unsigned int i=0; allMesh[i_bone].size(); ++i) { Mesh[i*3] = allMesh[i_bone][i](0) - +1; // +1 Car l'indice dans biorbd::BIORBD_MATH_NAMESPACE::rigidbody::s est par rapport à 0 + +1; // +1 Car l'indice dans BIORBD_NAMESPACE::rigidbody::s est par rapport à 0 Mesh[i*3+1] = allMesh[i_bone][i](1)+1; Mesh[i*3+2] = allMesh[i_bone][i](2)+1; } @@ -47,7 +47,7 @@ void Matlab_Patch( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector Mesh_tp(model->meshFaces( + std::vector Mesh_tp(model->meshFaces( static_cast(idx))); // Create a matrix for the return argument @@ -55,10 +55,10 @@ void Matlab_Patch( int, mxArray *plhs[], double *Mesh = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=Mesh_tp.begin(); + std::vector::iterator it=Mesh_tp.begin(); for (unsigned int i=0; (it+i)!=Mesh_tp.end(); ++i) { Mesh[i*3] = (*(it+i))(0) - +1; // +1 Car l'indice dans biorbd::BIORBD_MATH_NAMESPACE::rigidbody::s est par rapport à 0 + +1; // +1 Car l'indice dans BIORBD_NAMESPACE::rigidbody::s est par rapport à 0 Mesh[i*3+1] = (*(it+i))(1)+1; Mesh[i*3+2] = (*(it+i))(2)+1; } diff --git a/binding/matlab/Matlab_ProjectCustomPoint.h b/binding/matlab/Matlab_ProjectCustomPoint.h index ae428331..2b4fcbf3 100644 --- a/binding/matlab/Matlab_ProjectCustomPoint.h +++ b/binding/matlab/Matlab_ProjectCustomPoint.h @@ -14,22 +14,22 @@ void Matlab_ProjectCustomPoint( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 6, 6, "6 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th are the 3xNxT points in global reference frame, 5th is the body idx to project on and 6th are axes to remove on this body axes"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Qall = getParameterQ( + std::vector Qall = getParameterQ( prhs, 2, nQ); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3); // Body index int bodyIdx(getInteger(prhs,4)-1); // Nom des axes à retirer - biorbd::BIORBD_MATH_NAMESPACE::utils::String axesToRemove(getString(prhs,5)); + BIORBD_NAMESPACE::utils::String axesToRemove(getString(prhs,5)); unsigned int nFrames(static_cast(markersOverTime.size())); if (Qall.size()!=nFrames) { @@ -49,9 +49,9 @@ void Matlab_ProjectCustomPoint( int, mxArray *plhs[], // Projeter les points unsigned int cmp(0); for (unsigned int i=0; i(nMarker); ++j) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment tp; + BIORBD_NAMESPACE::rigidbody::NodeSegment tp; if (j==1) { tp = model->projectPoint(Q, markersOverTime[i][j], bodyIdx, axesToRemove, true); } else { diff --git a/binding/matlab/Matlab_ProjectPoint.h b/binding/matlab/Matlab_ProjectPoint.h index b6b41a7c..bbaedecc 100644 --- a/binding/matlab/Matlab_ProjectPoint.h +++ b/binding/matlab/Matlab_ProjectPoint.h @@ -14,15 +14,15 @@ void Matlab_ProjectPoint( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 4, 4, "6 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th are the 3xNxT points in global reference frame where N = nMarkers of the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Qall = getParameterQ( + std::vector Qall = getParameterQ( prhs, 2, nQ); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3); unsigned int nFrames(static_cast(markersOverTime.size())); @@ -43,11 +43,11 @@ void Matlab_ProjectPoint( int, mxArray *plhs[], // Projeter les points unsigned int cmp(0); for (unsigned int i=0; i projectedPoint(model->projectPoint( + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q(*(Qall.begin()+i)); + std::vector projectedPoint(model->projectPoint( Q, *(markersOverTime.begin()+i), true)); for (unsigned int j=0; j(nMarker); ++j) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment tp(*(projectedPoint.begin()+j)); + BIORBD_NAMESPACE::rigidbody::NodeSegment tp(*(projectedPoint.begin()+j)); Markers[cmp+0] = tp(0); Markers[cmp+1] = tp(1); Markers[cmp+2] = tp(2); diff --git a/binding/matlab/Matlab_ProjectPointJacobian.h b/binding/matlab/Matlab_ProjectPointJacobian.h index 5a3631cc..3c3421c9 100644 --- a/binding/matlab/Matlab_ProjectPointJacobian.h +++ b/binding/matlab/Matlab_ProjectPointJacobian.h @@ -14,21 +14,21 @@ void Matlab_ProjectPointJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q and 4th are the 3xN markers where N=nMarkers of the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Récupérer les marqueurs selon que l'on veut tous ou seulement anatomiques ou techniques - std::vector markersOverTime = + std::vector markersOverTime = *getParameterAllMarkers(prhs,3).begin(); // Trouver la matrice jacobienne de tous les marqueurs - std::vector Jac_tp = model->projectPointJacobian(Q, + std::vector Jac_tp = model->projectPointJacobian(Q, markersOverTime, true); - std::vector::iterator it=Jac_tp.begin(); + std::vector::iterator it=Jac_tp.begin(); // Create a matrix for the return argument unsigned int nMarkers = model->nbMarkers(); diff --git a/binding/matlab/Matlab_changeGravity.h b/binding/matlab/Matlab_changeGravity.h index ba3546c1..860b5598 100644 --- a/binding/matlab/Matlab_changeGravity.h +++ b/binding/matlab/Matlab_changeGravity.h @@ -13,7 +13,7 @@ void Matlab_changeGravity ( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and 3rd is the gravity field 3d vector"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_computeQdot.h b/binding/matlab/Matlab_computeQdot.h index b9921b43..b3b17454 100644 --- a/binding/matlab/Matlab_computeQdot.h +++ b/binding/matlab/Matlab_computeQdot.h @@ -16,15 +16,15 @@ void Matlab_computeQdot( int, mxArray *plhs[], "4 are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); unsigned int nQdot = model->nbQdot(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer que Q et Qdot sont de la bonne dimension @@ -44,7 +44,7 @@ void Matlab_computeQdot( int, mxArray *plhs[], model->UpdateKinematicsCustom(&Q[j], &QDot[j], nullptr); // Trouver la dynamique directe a cette configuration - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QDotPost( + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates QDotPost( model->computeQdot(Q[j], QDot[j])); // Remplir l'output diff --git a/binding/matlab/Matlab_delete.h b/binding/matlab/Matlab_delete.h index eacb3936..d98b6b7b 100644 --- a/binding/matlab/Matlab_delete.h +++ b/binding/matlab/Matlab_delete.h @@ -15,7 +15,7 @@ void Matlab_delete( int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the model"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); diff --git a/binding/matlab/Matlab_forwardDynamics.h b/binding/matlab/Matlab_forwardDynamics.h index 3e4d6135..a9cc4838 100644 --- a/binding/matlab/Matlab_forwardDynamics.h +++ b/binding/matlab/Matlab_forwardDynamics.h @@ -30,20 +30,20 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], } // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); unsigned int nQddot = model->nbQddot(); unsigned int nTau = model->nbGeneralizedTorque(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Tau - std::vector Tau = + std::vector Tau = getParameterGeneralizedTorque(prhs, 4, nTau); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -69,7 +69,7 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], force = mxGetPr(plhs[1]); // Force sur les points de contacts } - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QDDot(nQddot); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates QDDot(nQddot); for (unsigned int j=0; jUpdateKinematicsCustom(&Q[j], &QDot[j], nullptr); @@ -82,7 +82,7 @@ void Matlab_forwardDynamics( int, mxArray *plhs[], Tau[j], model->getConstraints(), QDDot);// Forward dynamics } else if (contact == -1) { // Si on a une impulsion - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates QdotPost(static_cast((* + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates QdotPost(static_cast((* (Q.begin()+j)).size())); RigidBodyDynamics::ComputeConstraintImpulsesDirect(*model, Q[j], QDot[j], model->getConstraints(), QdotPost); diff --git a/binding/matlab/Matlab_globalJCS.h b/binding/matlab/Matlab_globalJCS.h index bf610056..1dabe03a 100644 --- a/binding/matlab/Matlab_globalJCS.h +++ b/binding/matlab/Matlab_globalJCS.h @@ -14,16 +14,16 @@ void Matlab_globalJCS( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Trouver les RT - std::vector> JSC_vec; - for (std::vector::iterator Q_it = + std::vector> JSC_vec; + for (std::vector::iterator Q_it = Q.begin(); Q_it!=Q.end(); ++Q_it) { JSC_vec.push_back(model->allGlobalJCS(*Q_it)); } @@ -35,10 +35,10 @@ void Matlab_globalJCS( int, mxArray *plhs[], // Remplir l'output unsigned int cmpJCS = 0; - for (std::vector>::iterator AllJCS_it = + for (std::vector>::iterator AllJCS_it = JSC_vec.begin(); AllJCS_it != JSC_vec.end(); ++AllJCS_it) - for (std::vector::iterator JSC_it= + for (std::vector::iterator JSC_it= (*AllJCS_it).begin(); JSC_it!=(*AllJCS_it).end(); ++JSC_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_inverseDynamics.h b/binding/matlab/Matlab_inverseDynamics.h index e4417b94..840112d4 100644 --- a/binding/matlab/Matlab_inverseDynamics.h +++ b/binding/matlab/Matlab_inverseDynamics.h @@ -19,19 +19,19 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], } // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector QDDot = + std::vector QDDot = getParameterQddot(prhs, 4, nQdot); // S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension @@ -45,7 +45,7 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], "QDDot must have the same number of frames than Q"); } - std::vector> f_tp; + std::vector> f_tp; if (externalForces) { f_tp = getForcePlate(prhs, 5); if (f_tp.size() != nFrame) { @@ -60,7 +60,7 @@ void Matlab_inverseDynamics( int, mxArray *plhs[], unsigned int cmp(0); // Trouver la dynamique inverse a cette configuration - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque Tau(nTau); for (unsigned int j=0; j(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir la matrice des markers - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,2,static_cast(model->nbTechnicalMarkers())); // Recevoir Qinit - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit = *getParameterQ(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit = *getParameterQ(prhs, 3, nQ).begin(); bool removeAxes(true); @@ -36,7 +36,7 @@ void Matlab_inverseKinematics( int, mxArray *plhs[], // Faire la cinématique inverse a chaque instant for (unsigned int i=0; i(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // S'assurer que la personne recueille l'acces au filtre de Kalman if (nlhs != 1) { @@ -34,12 +34,12 @@ void Matlab_setEKF(int nlhs, mxArray *plhs[], if (nrhs >= 3) { freq = getDouble(prhs,2,"Acquisition Frequency"); } - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); + BIORBD_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); // Créer un filtre de Kalman try { - plhs[0] = convertPtr2Mat - (new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers(*model, + plhs[0] = convertPtr2Mat + (new BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers(*model, kParams)); } catch (std::string m) { mexErrMsgTxt(m.c_str()); @@ -55,7 +55,7 @@ void Matlab_delEKF(int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the kalman filter"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); @@ -72,14 +72,14 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], "4 arguments are required (+2 optional) where the 2nd is the handler on the model, 3rd is the handler on kalman filter info, 4th is the 3xN marker positions matrix, the optional 5th is the initial guess for Q (ignored after first iteration) and 6th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir le kalman - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers * kalman = - convertMat2Ptr(prhs[2]); + BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers * kalman = + convertMat2Ptr(prhs[2]); bool removeAxes(true); if (nrhs >= 6) { @@ -88,14 +88,14 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], // Recevoir la matrice des markers (Ne traite que le premier frame) - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,3,static_cast(model->nbTechnicalMarkers())); - std::vector markers = markersOverTime[0]; + std::vector markers = markersOverTime[0]; // Si c'est le premier frame recevoir Qinit if (kalman->first() && nrhs >= 5) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, nQ).begin()); kalman->setInitState(&Qinit); } @@ -109,9 +109,9 @@ void Matlab_inverseKinematicsEKFstep( int, mxArray *plhs[], double *qddot = mxGetPr(plhs[2]); // Faire la cinématique inverse a chaque instant - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); + BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); // Faire la cinématique inverse kalman->reconstructFrame(*model, markers, &Q, &QDot, &QDDot, removeAxes); @@ -148,7 +148,7 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], "7th is error factor [default 1e-5] and 7th if you want to remove axes as specified in the model file [default = true]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF @@ -166,8 +166,8 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], } // Créer un filtre de Kalman - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsMarkers kalman(*model, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); + BIORBD_NAMESPACE::rigidbody::KalmanReconsMarkers kalman(*model, + BIORBD_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); bool removeAxes(true); if (nrhs >= 8) { @@ -176,13 +176,13 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], // Recevoir la matrice des markers - std::vector> markersOverTime = + std::vector> markersOverTime = getParameterAllMarkers(prhs,2,static_cast(model->nbTechnicalMarkers())); unsigned int nFrames(static_cast(markersOverTime.size())); // Recevoir Qinit if (kalman.first() && nrhs >= 4) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(getParameterQ(prhs, 3, nQ)[0]); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(getParameterQ(prhs, 3, nQ)[0]); kalman.setInitState(&Qinit); } @@ -197,9 +197,9 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[], unsigned int cmp(0); for (unsigned int i=0; i(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // S'assurer que la personne recueille l'acces au filtre de Kalman if (nlhs != 1) { @@ -33,12 +33,12 @@ void Matlab_setEKF_IMU(int nlhs, mxArray *plhs[], if (nrhs >= 3) { freq = getDouble(prhs,2,"Acquisition Frequency"); } - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); + BIORBD_NAMESPACE::rigidbody::KalmanParam kParams(freq, noiseF, errorF); // Créer un filtre de Kalman try { - plhs[0] = convertPtr2Mat - (new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU(*model, kParams)); + plhs[0] = convertPtr2Mat + (new BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU(*model, kParams)); } catch (std::string m) { mexErrMsgTxt(m.c_str()); } @@ -53,7 +53,7 @@ void Matlab_delEKF_IMU(int nlhs, mxArray *[], "2 arguments are required where the 2nd is the handler on the kalman filter"); // Destroy the C++ object - destroyObject(prhs[1]); + destroyObject(prhs[1]); // Warn if other commands were ignored if (nlhs != 0 || nrhs != 2) { mexWarnMsgTxt("Delete: Unexpected output arguments ignored."); @@ -70,24 +70,24 @@ void Matlab_inverseKinematicsEKF_IMUstep( int , mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the handler on kalman filter info, 4th is the 3x3xNxT or 4x4xNxT IMU hypermatrix, the optional 5th is the initial guess for Q [default 0], 5th is acquisition frequency [default 100Hz]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir le kalman - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU * kalman = - convertMat2Ptr(prhs[2]); + BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU * kalman = + convertMat2Ptr(prhs[2]); // Recevoir la matrice des markers (Ne traite que le premier frame) - std::vector> imusOverTime = + std::vector> imusOverTime = getParameterAllIMUs(prhs,3); - std::vector imus = imusOverTime[0]; + std::vector imus = imusOverTime[0]; // Si c'est le premier frame recevoir Qinit if (kalman->first() && nrhs >= 5) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 4, nQ).begin()); kalman->setInitState(&Qinit); } @@ -101,9 +101,9 @@ void Matlab_inverseKinematicsEKF_IMUstep( int , mxArray *plhs[], double *qddot = mxGetPr(plhs[2]); // Faire la cinématique inverse a chaque instant - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q(nQ); + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot(nQdot); + BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(nQddot); // Faire la cinématique inverse kalman->reconstructFrame(*model, imus, &Q, &QDot, &QDDot); @@ -133,7 +133,7 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], "4 arguments are required (+5 optional) where the 2nd is the handler on the model, 3th is the 3x3xNxT or 4x4xNxT IMU hypermatrix, the optional 4th is the initial guess for Q [default 0], 5th is acquisition frequency [default 100Hz], 6th is noise factor [default 5e-3] and 7th is error factor [default 1e-10]"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF @@ -151,17 +151,17 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], } // Créer un filtre de Kalman - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanReconsIMU kalman(*model, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); + BIORBD_NAMESPACE::rigidbody::KalmanReconsIMU kalman(*model, + BIORBD_NAMESPACE::rigidbody::KalmanParam(freq, noiseF, errorF)); // Recevoir la matrice des imus - std::vector> imusOverTime = + std::vector> imusOverTime = getParameterAllIMUs(prhs,2); unsigned int nFrames(static_cast(imusOverTime.size())); // Recevoir Qinit if (kalman.first() && nrhs >= 4) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 3, nQ).begin()); kalman.setInitState(&Qinit); } @@ -177,9 +177,9 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[], unsigned int cmp(0); for (unsigned int i=0; i(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver les RT - std::vector JSC_vec(model->localJCS()); + std::vector JSC_vec(model->localJCS()); // Create a matrix for the return argument const mwSize dims[3]= {4,4,mwSize(model->nbSegment())}; @@ -26,7 +26,7 @@ void Matlab_localJCS( int, mxArray *plhs[], // Remplir l'output unsigned int cmpJCS = 0; - for (std::vector::iterator JSC_it=JSC_vec.begin(); + for (std::vector::iterator JSC_it=JSC_vec.begin(); JSC_it!=JSC_vec.end(); ++JSC_it) for (unsigned int i=0; i<4; ++i) for (unsigned int j=0; j<4; ++j) { diff --git a/binding/matlab/Matlab_massMatrix.h b/binding/matlab/Matlab_massMatrix.h index 25c2fa9f..5dc542ed 100644 --- a/binding/matlab/Matlab_massMatrix.h +++ b/binding/matlab/Matlab_massMatrix.h @@ -17,11 +17,11 @@ void Matlab_massMatrix( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_muscleJointTorqueFromActivation.h b/binding/matlab/Matlab_muscleJointTorqueFromActivation.h index b7c01e85..fe99b3ff 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromActivation.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromActivation.h @@ -17,14 +17,14 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], "WARNING: if the function is called without Q and Qdot, the user MUST update by himself " "before calling this function (using updateMuscle)."); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF unsigned int nMuscleTotal = model->nbMuscles(); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateActivation(prhs,2, nMuscleTotal); unsigned int nFrame(static_cast(s.size())); @@ -45,8 +45,8 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], } // Recueillir la cinématique - std::vector Q; - std::vector QDot; + std::vector Q; + std::vector QDot; if (updateKin) { // Si on update pas la cinématique Q et Qdot ne sont pas nécessaire // Recevoir Q Q = getParameterQ(prhs, 3, nQ); @@ -82,8 +82,8 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[], } // Remplir le output - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + BIORBD_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; imuscleForces(s[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h b/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h index ce76892e..0941db39 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromExcitation.h @@ -19,7 +19,7 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], "6th is if update [true] must be done. Note that if update is set to [false], " "the user MUST update it by himself before calling this function"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = @@ -27,13 +27,13 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], unsigned int nMuscleTotal = model->nbMuscleTotal(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir muscleStates - std::vector>> s = + std::vector>> s = getParameterMuscleStateExcitation(prhs,4, model->nbMuscleTotal()); @@ -76,13 +76,13 @@ void Matlab_muscleJointTorqueFromExcitation( int nlhs, mxArray *plhs[], } // Remplir le output - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector muscleForces; + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + BIORBD_NAMESPACE::utils::Vector muscleForces; for (unsigned int i=0; inbMuscleGroups(); ++k) for (unsigned int j=0; jmuscleGroup(k).nbMuscles(); ++j) { - std::dynamic_pointer_cast(s[i][iMus]) + std::dynamic_pointer_cast(s[i][iMus]) ->timeDerivativeActivation(model->muscleGroup(k).muscle(j).characteristics(), true); ++iMus; diff --git a/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h b/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h index 92aab8be..d4e482ab 100644 --- a/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h +++ b/binding/matlab/Matlab_muscleJointTorqueFromMuscleForce.h @@ -17,7 +17,7 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], "6th is if update [true] must be done. Note that if update is set to [false], " "the user MUST update it by himself before calling this function"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = @@ -25,10 +25,10 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], unsigned int nMuscleTotal = model->nbMuscleTotal(); // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir muscleStates std::vector Fm = getParameterMuscleForceNorm(prhs,4, @@ -63,7 +63,7 @@ void Matlab_muscleJointTorqueFromMuscleForce( int, mxArray *plhs[], double *GeneralizedTorque = mxGetPr(plhs[0]); // Remplir le output - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque muscleTorque; for (unsigned int i=0; imuscularJointTorque(Fm[i], Q[i], QDot[i]); diff --git a/binding/matlab/Matlab_muscleLengthJacobian.h b/binding/matlab/Matlab_muscleLengthJacobian.h index 0e020ec4..11cd1250 100644 --- a/binding/matlab/Matlab_muscleLengthJacobian.h +++ b/binding/matlab/Matlab_muscleLengthJacobian.h @@ -14,18 +14,18 @@ void Matlab_muscleLengthJacobian( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( model->nbMuscleTotal(), nQ, mxREAL); double *Jac = mxGetPr(plhs[0]); - biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix jaco(model->musclesLengthJacobian(Q)); + BIORBD_NAMESPACE::utils::Matrix jaco(model->musclesLengthJacobian(Q)); int cmp(0); for (unsigned int j=0; j(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // S'assurer qu'il n'y a qu'un seul frame @@ -57,7 +57,7 @@ void Matlab_muscleUpdate( int, mxArray *[], // Mettre le nombre nécessaire dans chaque case int cmpMus(0); for (unsigned int i = 0; inbMuscleGroups(); ++i) { - biorbd::BIORBD_MATH_NAMESPACE::muscles::MuscleGroup grMus(model->muscleGroup(i)); + BIORBD_NAMESPACE::muscles::MuscleGroup grMus(model->muscleGroup(i)); for (unsigned int j = 0; j> musclePosition( + std::vector> musclePosition( getMusclePosition(prhs, 4, nPoints)); // Recueillir la matrice jacobienne - std::vector musclePointsJaco(getMusclePointsJaco(prhs, 5, + std::vector musclePointsJaco(getMusclePointsJaco(prhs, 5, nPoints, nQ)); // Appeler la fonction d'update diff --git a/binding/matlab/Matlab_muscleVelocity.h b/binding/matlab/Matlab_muscleVelocity.h index adede61a..8a72354c 100644 --- a/binding/matlab/Matlab_muscleVelocity.h +++ b/binding/matlab/Matlab_muscleVelocity.h @@ -16,16 +16,16 @@ void Matlab_muscleVelocity( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 4, 4, "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is the Qdot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector Qdot = getParameterQdot( + std::vector Qdot = getParameterQdot( prhs, 3, nQdot); // S'assurer que le même nombre d'instants a été envoyé diff --git a/binding/matlab/Matlab_nBody.h b/binding/matlab/Matlab_nBody.h index bee1e98d..c1e8f4bf 100644 --- a/binding/matlab/Matlab_nBody.h +++ b/binding/matlab/Matlab_nBody.h @@ -13,7 +13,7 @@ void Matlab_nBody( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nControl.h b/binding/matlab/Matlab_nControl.h index 2bf3d6b2..e01dc8a4 100644 --- a/binding/matlab/Matlab_nControl.h +++ b/binding/matlab/Matlab_nControl.h @@ -13,7 +13,7 @@ void Matlab_nGeneralizedTorque(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nIMU.h b/binding/matlab/Matlab_nIMU.h index 202d6ecb..ed8b96a5 100644 --- a/binding/matlab/Matlab_nIMU.h +++ b/binding/matlab/Matlab_nIMU.h @@ -13,7 +13,7 @@ void Matlab_nIMU( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nMarkers.h b/binding/matlab/Matlab_nMarkers.h index e8042942..c0eb1964 100644 --- a/binding/matlab/Matlab_nMarkers.h +++ b/binding/matlab/Matlab_nMarkers.h @@ -13,7 +13,7 @@ void Matlab_nMarkers( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nMuscles.h b/binding/matlab/Matlab_nMuscles.h index 07dc998f..1f298a64 100644 --- a/binding/matlab/Matlab_nMuscles.h +++ b/binding/matlab/Matlab_nMuscles.h @@ -14,7 +14,7 @@ void Matlab_nMuscles( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Sortie des noms plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQ.h b/binding/matlab/Matlab_nQ.h index 7fbe027c..7d335dd1 100644 --- a/binding/matlab/Matlab_nQ.h +++ b/binding/matlab/Matlab_nQ.h @@ -13,7 +13,7 @@ void Matlab_nQ(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQddot.h b/binding/matlab/Matlab_nQddot.h index 2fd706c2..82fa3a81 100644 --- a/binding/matlab/Matlab_nQddot.h +++ b/binding/matlab/Matlab_nQddot.h @@ -13,7 +13,7 @@ void Matlab_nQddot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nQdot.h b/binding/matlab/Matlab_nQdot.h index 0b8b2600..7efe8b88 100644 --- a/binding/matlab/Matlab_nQdot.h +++ b/binding/matlab/Matlab_nQdot.h @@ -13,7 +13,7 @@ void Matlab_nQdot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nRoot.h b/binding/matlab/Matlab_nRoot.h index d28a4d4b..dcf5c750 100644 --- a/binding/matlab/Matlab_nRoot.h +++ b/binding/matlab/Matlab_nRoot.h @@ -13,7 +13,7 @@ void Matlab_nRoot( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_nameBody.h b/binding/matlab/Matlab_nameBody.h index 77c5d919..07cec5fa 100644 --- a/binding/matlab/Matlab_nameBody.h +++ b/binding/matlab/Matlab_nameBody.h @@ -14,7 +14,7 @@ void Matlab_nameBody( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int idx; if (nrhs>2) { diff --git a/binding/matlab/Matlab_nameDof.h b/binding/matlab/Matlab_nameDof.h index 3eec92c3..a2a5198c 100644 --- a/binding/matlab/Matlab_nameDof.h +++ b/binding/matlab/Matlab_nameDof.h @@ -14,7 +14,7 @@ void Matlab_nameDof( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF diff --git a/binding/matlab/Matlab_nameIMU.h b/binding/matlab/Matlab_nameIMU.h index ee5fce56..199c0470 100644 --- a/binding/matlab/Matlab_nameIMU.h +++ b/binding/matlab/Matlab_nameIMU.h @@ -14,10 +14,10 @@ void Matlab_nameIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->IMUsNames()); + std::vector allIMU(model->IMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); @@ -37,10 +37,10 @@ void Matlab_nameTechnicalIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->technicalIMUsNames()); + std::vector allIMU(model->technicalIMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); @@ -60,10 +60,10 @@ void Matlab_nameAnatomicalIMU( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allIMU(model->anatomicalIMUsNames()); + std::vector allIMU(model->anatomicalIMUsNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allIMU.size(), 1); diff --git a/binding/matlab/Matlab_nameMarkers.h b/binding/matlab/Matlab_nameMarkers.h index b210d7bd..aa2016e2 100644 --- a/binding/matlab/Matlab_nameMarkers.h +++ b/binding/matlab/Matlab_nameMarkers.h @@ -14,10 +14,10 @@ void Matlab_nameMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->markerNames()); + std::vector allMarkers(model->markerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); @@ -38,10 +38,10 @@ void Matlab_nameTechnicalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->technicalMarkerNames()); + std::vector allMarkers(model->technicalMarkerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); @@ -62,10 +62,10 @@ void Matlab_nameAnatomicalMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Trouver ou sont les marqueurs - std::vector allMarkers(model->anatomicalMarkerNames()); + std::vector allMarkers(model->anatomicalMarkerNames()); // Create a matrix for the return argument plhs[0] = mxCreateCellMatrix( allMarkers.size(), 1); diff --git a/binding/matlab/Matlab_new.h b/binding/matlab/Matlab_new.h index 15fcfd57..2cf3fb60 100644 --- a/binding/matlab/Matlab_new.h +++ b/binding/matlab/Matlab_new.h @@ -26,13 +26,13 @@ void Matlab_new( int nlhs, mxArray *plhs[], "You must catch the pointer address!"); } char *buf = mxArrayToString(prhs[1]); - biorbd::BIORBD_MATH_NAMESPACE::utils::String filepath(buf); // Copier le cstring dans un std::string + BIORBD_NAMESPACE::utils::String filepath(buf); // Copier le cstring dans un std::string // Loader le modèle musculosquelettique // Definition des variables globales du modele try { - plhs[0] = convertPtr2Mat( - new biorbd::BIORBD_MATH_NAMESPACE::Model(biorbd::BIORBD_MATH_NAMESPACE::Reader::readModelFile(filepath))); + plhs[0] = convertPtr2Mat( + new BIORBD_NAMESPACE::Model(BIORBD_NAMESPACE::Reader::readModelFile(filepath))); } catch (std::string m) { mexErrMsgTxt(m.c_str()); } diff --git a/binding/matlab/Matlab_parent.h b/binding/matlab/Matlab_parent.h index 2cce4b4d..56de4e54 100644 --- a/binding/matlab/Matlab_parent.h +++ b/binding/matlab/Matlab_parent.h @@ -13,7 +13,7 @@ void Matlab_parent( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 3, 3, "3 arguments are required where the 2nd is the handler on the model and the 3rd is the name of the segment"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/Matlab_rangeQ.h b/binding/matlab/Matlab_rangeQ.h index 02d90284..2b0a30b2 100644 --- a/binding/matlab/Matlab_rangeQ.h +++ b/binding/matlab/Matlab_rangeQ.h @@ -14,7 +14,7 @@ void Matlab_rangeQ(int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nbSegments = model->nbSegment(); mwSize outSize[2] = {nbSegments, 1}; diff --git a/binding/matlab/Matlab_segmentAngularMomentum.h b/binding/matlab/Matlab_segmentAngularMomentum.h index 47e853d6..32ebbc51 100644 --- a/binding/matlab/Matlab_segmentAngularMomentum.h +++ b/binding/matlab/Matlab_segmentAngularMomentum.h @@ -15,15 +15,15 @@ void Matlab_segmentAngularMomentum( int, mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot and 5th is the index of body segment"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir QDot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); unsigned int nFrame(static_cast(Q.size())); @@ -61,7 +61,7 @@ void Matlab_segmentAngularMomentum( int, mxArray *plhs[], // Préparer la sortie conditionnelle (tous si idx==-1, seulement celui demandé sinon) unsigned int cmp(0); for (unsigned int i=0; i am_all(model->CalcSegmentsAngularMomentum ( + std::vector am_all(model->CalcSegmentsAngularMomentum ( *model, *(Q.begin()+i), *(QDot.begin()+i), true)); if (idx==-1) { diff --git a/binding/matlab/Matlab_segmentCoM.h b/binding/matlab/Matlab_segmentCoM.h index 2761dd64..034722b0 100644 --- a/binding/matlab/Matlab_segmentCoM.h +++ b/binding/matlab/Matlab_segmentCoM.h @@ -14,11 +14,11 @@ void Matlab_segmentCOM( int, mxArray *plhs[], "3 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q and 4th is the index of body segment"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -29,7 +29,7 @@ void Matlab_segmentCOM( int, mxArray *plhs[], // Trouver la position du CoM if (i==-1) { - std::vector COM = model->CoMbySegment(Q,true); + std::vector COM = model->CoMbySegment(Q,true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); // Remplir l'output @@ -39,7 +39,7 @@ void Matlab_segmentCOM( int, mxArray *plhs[], tp[3*j+k] = COM[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment COM = model->CoMbySegment(Q, + BIORBD_NAMESPACE::rigidbody::NodeSegment COM = model->CoMbySegment(Q, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentCoMddot.h b/binding/matlab/Matlab_segmentCoMddot.h index 56ca83c8..27f53eb3 100644 --- a/binding/matlab/Matlab_segmentCoMddot.h +++ b/binding/matlab/Matlab_segmentCoMddot.h @@ -14,19 +14,19 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], "5 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot, 5th is Qddot and 6th is the index of body segment"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nQddot = model->nbQddot(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Recevoir Qddot - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot = *getParameterQddot(prhs, 4, + BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot = *getParameterQddot(prhs, 4, nQddot).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -37,7 +37,7 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], // Trouver la vitesse du CoM if (i==-1) { - std::vector COMddot = model->CoMddotBySegment(Q,QDot, + std::vector COMddot = model->CoMddotBySegment(Q,QDot, QDDot,true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); @@ -48,7 +48,7 @@ void Matlab_segmentCOMddot( int, mxArray *plhs[], tp[3*j+k] = COMddot[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d COMddot = model->CoMddotBySegment(Q,QDot,QDDot, + BIORBD_NAMESPACE::utils::Vector3d COMddot = model->CoMddotBySegment(Q,QDot,QDDot, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentCoMdot.h b/binding/matlab/Matlab_segmentCoMdot.h index 7195beeb..98eeab90 100644 --- a/binding/matlab/Matlab_segmentCoMdot.h +++ b/binding/matlab/Matlab_segmentCoMdot.h @@ -14,15 +14,15 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], "4 arguments are required (+1 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th is the Qdot and 5th is the index of body segment"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Recevoir le numéro du segment (optionnel) int i(0); @@ -33,7 +33,7 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], // Trouver la vitesse du CoM if (i==-1) { - std::vector COMdot = model->CoMdotBySegment(Q,QDot, + std::vector COMdot = model->CoMdotBySegment(Q,QDot, true); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 3, model->nbSegment(), mxREAL); @@ -44,7 +44,7 @@ void Matlab_segmentCOMdot( int, mxArray *plhs[], tp[3*j+k] = COMdot[j][k]; // Transférer le tout dans un tableau de sortie } } else { - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d COMdot = model->CoMdotBySegment(Q,QDot, + BIORBD_NAMESPACE::utils::Vector3d COMdot = model->CoMdotBySegment(Q,QDot, static_cast(i),true); // Create a matrix for the return argument diff --git a/binding/matlab/Matlab_segmentMarkers.h b/binding/matlab/Matlab_segmentMarkers.h index dd7a5225..095b40af 100644 --- a/binding/matlab/Matlab_segmentMarkers.h +++ b/binding/matlab/Matlab_segmentMarkers.h @@ -14,11 +14,11 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required (+2 optional) where the 2nd is the handler on the model, 3rd is the Q, 4th if you want to remove axes as specified in the model file [default = true] and 5th optional is a specific segment index"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); bool removeAxes(true); @@ -35,9 +35,9 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], if ( idx==-1) { // Si on a demande tous les segments // Trouver ou sont les marqueurs - std::vector> allMarkers; + std::vector> allMarkers; for (unsigned int i=0; inbSegment(); ++i) { - std::vector markers_tp = model->segmentMarkers( + std::vector markers_tp = model->segmentMarkers( Q, i, removeAxes); allMarkers.push_back(markers_tp); } @@ -49,7 +49,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], double *markers = mxGetPr(markers_out_tp); // Remplir le output - std::vector::iterator it=(* + std::vector::iterator it=(* (allMarkers.begin()+i_bone)).begin(); for (unsigned int i=0; (it+i)!=(*(allMarkers.begin()+i_bone)).end(); ++i) { markers[i*3] = (*(it+i))(0); @@ -61,7 +61,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], return; } else { // Si on a demande un segment precis - std::vector markers_tp = + std::vector markers_tp = model->segmentMarkers(Q, static_cast(idx), removeAxes); // Create a matrix for the return argument @@ -69,7 +69,7 @@ void Matlab_segmentMarkers( int, mxArray *plhs[], double *markers = mxGetPr(plhs[0]); // Remplir le output - std::vector::iterator it=markers_tp.begin(); + std::vector::iterator it=markers_tp.begin(); for (unsigned int i=0; (it+i)!=markers_tp.end(); ++i) { markers[i*3] = (*(it+i))(0); markers[i*3+1] = (*(it+i))(1); diff --git a/binding/matlab/Matlab_segmentMass.h b/binding/matlab/Matlab_segmentMass.h index eb210d88..f0b5711c 100644 --- a/binding/matlab/Matlab_segmentMass.h +++ b/binding/matlab/Matlab_segmentMass.h @@ -15,7 +15,7 @@ void Matlab_segmentMass( int, mxArray *plhs[], checkNombreInputParametres(nrhs, 2, 3, "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int idx; if (nrhs>2) { diff --git a/binding/matlab/Matlab_segmentsInertia.h b/binding/matlab/Matlab_segmentsInertia.h index 89df7de4..fd239afb 100644 --- a/binding/matlab/Matlab_segmentsInertia.h +++ b/binding/matlab/Matlab_segmentsInertia.h @@ -16,11 +16,11 @@ void Matlab_segmentsInertia( int, mxArray *plhs[], "3 arguments are required where the 2nd is the handler on the model and 3rd is the Q"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Update sur la cinématique (placer les segments) @@ -53,7 +53,7 @@ void Matlab_segmentsInertiaLocal( int, mxArray *plhs[], "2 arguments are required (+1 optional) where the 2nd is the handler on the model and optional third is the segment index"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nBones = model->nbSegment(); // Get the number of DoF diff --git a/binding/matlab/Matlab_segmentsVelocities.h b/binding/matlab/Matlab_segmentsVelocities.h index 688d335e..859c0a9f 100644 --- a/binding/matlab/Matlab_segmentsVelocities.h +++ b/binding/matlab/Matlab_segmentsVelocities.h @@ -15,15 +15,15 @@ void Matlab_segmentsVelocities( int, mxArray *plhs[], "4 arguments are required where the 2nd is the handler on the model, 3rd is the Q and 4th is QDot"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF // Recevoir Q - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates Q = *getParameterQ(prhs, 2, nQ).begin(); // Recevoir Qdot - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity QDot = *getParameterQdot(prhs, 3, nQdot).begin(); // Update sur la cinématique diff --git a/binding/matlab/Matlab_torqueActivation.h b/binding/matlab/Matlab_torqueActivation.h index ddc19b59..3358a5d5 100644 --- a/binding/matlab/Matlab_torqueActivation.h +++ b/binding/matlab/Matlab_torqueActivation.h @@ -14,20 +14,20 @@ void Matlab_torqueActivation( int, mxArray *plhs[], "5 or 5 arguments are required where the 2nd is the handler on the model, 3rd is the Q, 4th is QDot and 5th is GeneralizedTorque"); // Recevoir le model - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); unsigned int nQ = model->nbQ(); // Get the number of DoF unsigned int nQdot = model->nbQdot(); // Get the number of DoF unsigned int nGeneralizedTorque = model->nbGeneralizedTorque(); // Nombre de GeneralizedTorque // Recevoir Q - std::vector Q = getParameterQ(prhs, + std::vector Q = getParameterQ(prhs, 2, nQ); // Recevoir Qdot - std::vector QDot = getParameterQdot( + std::vector QDot = getParameterQdot( prhs, 3, nQdot); // Recevoir Qddot - std::vector act = + std::vector act = getParameterGeneralizedTorque(prhs, 4, model->nbGeneralizedTorque()); diff --git a/binding/matlab/Matlab_totalMass.h b/binding/matlab/Matlab_totalMass.h index b88d31dd..429911f2 100644 --- a/binding/matlab/Matlab_totalMass.h +++ b/binding/matlab/Matlab_totalMass.h @@ -14,7 +14,7 @@ void Matlab_totalMass( int, mxArray *plhs[], // Verifier les arguments d'entrée checkNombreInputParametres(nrhs, 2, 2, "2 arguments are required where the 2nd is the handler on the model"); - biorbd::BIORBD_MATH_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); + BIORBD_NAMESPACE::Model * model = convertMat2Ptr(prhs[1]); // Create a matrix for the return argument plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL); diff --git a/binding/matlab/biorbd.h b/binding/matlab/biorbd.h index 6f5cf4b2..b6cfeee7 100644 --- a/binding/matlab/biorbd.h +++ b/binding/matlab/biorbd.h @@ -274,7 +274,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Nombre de Markers if (!toLower(cmd).compare("nmimu")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"nmimu\" est obsolete. Remplacer par \"nimu\". Elle sera retirée prochainement"); Matlab_nIMU(nlhs, plhs, nrhs, prhs); return; @@ -286,7 +286,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers if (!toLower(cmd).compare("namemimu")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"namemimu\" est obsolete. Remplacer par \"nameimu\". Elle sera retirée prochainement"); Matlab_nameIMU(nlhs, plhs, nrhs, prhs); return; @@ -298,7 +298,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers techniques if (!toLower(cmd).compare("nametechnicalmimu")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"nametechnicalmimu\" est obsolete. Remplacer par \"nametechnicalimu\". Elle sera retirée prochainement"); Matlab_nameTechnicalIMU(nlhs, plhs, nrhs, prhs); return; @@ -310,7 +310,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Noms des Markers anatomiques if (!toLower(cmd).compare("nameanatomicalmimu")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"nameanatomicalmimu\" est obsolete. Remplacer par \"nameanatomicalimu\". Elle sera retirée prochainement"); Matlab_nameAnatomicalIMU(nlhs, plhs, nrhs, prhs); return; @@ -322,7 +322,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Fonction de cinématique directe if(!toLower(cmd).compare("mimu")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"mimu\" est obsolete. Remplacer par \"imu\". Elle sera retirée prochainement"); Matlab_IMU(nlhs, plhs, nrhs, prhs); return; @@ -334,7 +334,7 @@ void functionHub( int nlhs, mxArray *plhs[], // Fonction de cinématique directe if(!toLower(cmd).compare("mimujacobian")) { - biorbd::BIORBD_MATH_NAMESPACE::utils::Error::warning(0, + BIORBD_NAMESPACE::utils::Error::warning(0, "La fonction \"mimujacobian\" est obsolete. Remplacer par \"imujacobian\". Elle sera retirée prochainement"); Matlab_IMUJacobian(nlhs, plhs, nrhs, prhs); return; diff --git a/binding/matlab/processArguments.h b/binding/matlab/processArguments.h index f2c06741..7cb43359 100644 --- a/binding/matlab/processArguments.h +++ b/binding/matlab/processArguments.h @@ -27,7 +27,7 @@ void checkNombreInputParametres(int nrhs, int min, int max, } } -std::vector> getParameterAllMarkers( +std::vector> getParameterAllMarkers( const mxArray*prhs[], unsigned int idx, int nMark=-1) { @@ -80,15 +80,15 @@ std::vector> // Créer la sortie - std::vector> markersOverTime; + std::vector> markersOverTime; // Stocker les valeurs dans le format de sortie unsigned int cmp(0); for (unsigned int i=0; i markers_tp; // Markers a un temps i + std::vector markers_tp; // Markers a un temps i for (int j=0; j> // Retourner la matrice return markersOverTime; } -std::vector> getParameterAllIMUs( +std::vector> getParameterAllIMUs( const mxArray*prhs[], unsigned int idx) { // Check data type of input argument @@ -149,14 +149,14 @@ std::vector> getParam double *imus = mxGetPr(prhs[idx]); // Créer la sortie - std::vector> imuOverTime; + std::vector> imuOverTime; // Stocker les valeurs dans le format de sortie for (unsigned int i=0; i imus_tp; // IMUs a un temps i + std::vector imus_tp; // IMUs a un temps i for (unsigned int j=0; j> getParam imus[i*9*nIMUs+j*9+2], imus[i*9*nIMUs+j*9+5], imus[i*9*nIMUs+j*9+8]; trans.setZero(); } - imus_tp.push_back(biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans(rot, trans)); + imus_tp.push_back(BIORBD_NAMESPACE::utils::RotoTrans(rot, trans)); } imuOverTime.push_back(imus_tp); } @@ -180,7 +180,7 @@ std::vector> getParam } -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates getVector(const mxArray*prhs[], +BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates getVector(const mxArray*prhs[], unsigned int idx, std::string type = "") { // Check data type of input argument @@ -208,7 +208,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates getVector(const double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates vect(static_cast + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates vect(static_cast (length)); for (unsigned int i=0; i getParameterQ( +std::vector getParameterQ( const mxArray*prhs[], unsigned int idx, unsigned int nDof, std::string type = "q") { @@ -263,9 +263,9 @@ std::vector ge double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Q; + std::vector Q; for (unsigned int j=0; j ge return Q; } -std::vector getParameterQddot( +std::vector getParameterQddot( const mxArray*prhs[], unsigned int idx, unsigned int nbQddot, std::string type = "qddot") { @@ -301,9 +301,9 @@ std::vector g double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Qddot; + std::vector Qddot; for (unsigned int j=0; j g return Qddot; } -std::vector getParameterQdot( +std::vector getParameterQdot( const mxArray*prhs[], unsigned int idx, unsigned int nbQdot, std::string type = "qdot") { @@ -339,9 +339,9 @@ std::vector getPa double *q=mxGetPr(prhs[idx]); //matrice de position // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector Qdot; + std::vector Qdot; for (unsigned int j=0; j getPa return Qdot; } -std::vector getParameterGeneralizedTorque( +std::vector getParameterGeneralizedTorque( const mxArray*prhs[], unsigned int idx, unsigned int nControl) { - std::vector AllGeneralizedTorque_tp = + std::vector AllGeneralizedTorque_tp = getParameterQ(prhs, idx, nControl, "GeneralizedTorque"); - std::vector AllGeneralizedTorque; + std::vector AllGeneralizedTorque; for (unsigned int j=0; j getPara return AllGeneralizedTorque; } -std::vector> getForcePlate( +std::vector> getForcePlate( const mxArray*prhs[], unsigned int idx) { if (!(mxIsDouble(prhs[idx]))) { @@ -401,12 +401,12 @@ std::vector> ge double *pf = mxGetPr(prhs[idx]); // Matrice des plateforme de force // stockage des plateformes - std::vector> PF; + std::vector> PF; unsigned int cmp(0); for (unsigned int j=0; j PF_tp; + std::vector PF_tp; for (unsigned int i=0; i getDoubleArray(const mxArray*prhs[], unsigned int idx) return out; } -biorbd::BIORBD_MATH_NAMESPACE::utils::String getString(const mxArray*prhs[], unsigned int idx) +BIORBD_NAMESPACE::utils::String getString(const mxArray*prhs[], unsigned int idx) { // Check data type of input argument if (!(mxIsChar(prhs[idx]))) { @@ -525,7 +525,7 @@ bool isStateExist(const mxArray*prhs[], unsigned int nMus, int idx, return isThere; } -std::vector>> +std::vector>> getParameterMuscleState( const mxArray*prhs[], int idxExcitation, @@ -571,18 +571,18 @@ getParameterMuscleState( // Coordonnées généralisées du modèle envoyées vers lisible par le modèle - std::vector>> States; + std::vector>> States; for (unsigned int j=0; j> States_tp; + std::vector> States_tp; for (unsigned int i=0; i + States_tp.push_back( std::make_shared (stateExcitation[j*nMus+i], stateActivation[j*nMus+i])); } else if (!isThereExcitation && isThereActivation) { - States_tp.push_back( std::make_shared(0, + States_tp.push_back( std::make_shared(0, stateActivation[j*nMus+i])); } else if (isThereExcitation && !isThereActivation) { - States_tp.push_back( std::make_shared + States_tp.push_back( std::make_shared (stateExcitation[j*nMus+i], 0)); } @@ -592,7 +592,7 @@ getParameterMuscleState( } -std::vector> +std::vector> getParameterMuscleStateBuchanan(const mxArray*prhs[], int idxExcitation, int idxActivation, unsigned int nMus) { @@ -635,18 +635,18 @@ std::vector> States; + std::vector> States; for (unsigned int j=0; j States_tp; + std::vector States_tp; for (unsigned int i=0; i>> +std::vector>> getParameterMuscleStateActivation( const mxArray*prhs[], int idxActivation, unsigned int nMus) { return getParameterMuscleState(prhs, -1, idxActivation, nMus); } -std::vector>> +std::vector>> getParameterMuscleStateExcitation( const mxArray*prhs[], int idxExcitation, unsigned int nMus) { return getParameterMuscleState(prhs, idxExcitation, -1, nMus); } -std::vector> +std::vector> getParameterMuscleStateActivationBuchanan( const mxArray*prhs[], int idxActivation, unsigned int nMus) { return getParameterMuscleStateBuchanan(prhs, -1, idxActivation, nMus); } -std::vector> +std::vector> getParameterMuscleStateExcitationBuchanan( const mxArray*prhs[], int idxExcitation, unsigned int nMus) { @@ -719,7 +719,7 @@ std::vector getParameterMuscleForceNorm(const mxArray*prhs[], } -std::vector> getMusclePosition( +std::vector> getMusclePosition( const mxArray*prhs[], unsigned int idx, Eigen::VectorXd nPointsByMuscles) { @@ -755,13 +755,13 @@ std::vector> getMusc double *via=mxGetPr(prhs[idx]); //matrice de position // Préparer la matrice de sortie - std::vector> out; + std::vector> out; unsigned int cmpMus(0); for (unsigned int i=0; i mus; + std::vector mus; for (unsigned int j=0; j> getMusc return out; } -std::vector getMusclePointsJaco(const mxArray*prhs[], +std::vector getMusclePointsJaco(const mxArray*prhs[], unsigned int idx, Eigen::VectorXd nPointsByMuscles, unsigned int nQ) { @@ -807,12 +807,12 @@ std::vector getMusclePointsJaco(co double *jaco=mxGetPr(prhs[idx]); //matrice de position // Préparer la matrice de sortie - std::vector jacoOut; + std::vector jacoOut; unsigned int cmpMus(0); for (unsigned int i=0; i(nPointsByMuscles(i))*3, nQ); + BIORBD_NAMESPACE::utils::Matrix mus(static_cast(nPointsByMuscles(i))*3, nQ); for (unsigned int j=0; j(nPointsByMuscles(i)); ++j) { // Stocker for (unsigned int k1 = 0; k1<3; ++k1) diff --git a/binding/python3/biorbd_python.i.in b/binding/python3/biorbd_python.i.in index 8007af78..cf897518 100644 --- a/binding/python3/biorbd_python.i.in +++ b/binding/python3/biorbd_python.i.in @@ -26,36 +26,36 @@ %include "@CMAKE_BINARY_DIR@/include/biorbdConfig.h" #if defined(BIORBD_USE_CASADI_MATH) -#define SWIG_UTILS_STRING SWIGTYPE_p_biorbd__MathCasadi__utils__String -#define SWIG_UTILS_PATH SWIGTYPE_p_biorbd__MathCasadi__utils__Path -#define SWIG_UTILS_VECTOR SWIGTYPE_p_biorbd__MathCasadi__utils__Vector -#define SWIG_UTILS_NODE SWIGTYPE_p_biorbd__MathCasadi__utils__Node -#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_biorbd__MathCasadi__utils__SpatialVector - -#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Joints -#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedCoordinates -#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedVelocity -#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedAcceleration -#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_biorbd__MathCasadi__rigidbody__GeneralizedTorque -#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_biorbd__MathCasadi__rigidbody__Markers +#define SWIG_UTILS_STRING SWIGTYPE_p_BiorbdCasadi__utils__String +#define SWIG_UTILS_PATH SWIGTYPE_p_BiorbdCasadi__utils__Path +#define SWIG_UTILS_VECTOR SWIGTYPE_p_BiorbdCasadi__utils__Vector +#define SWIG_UTILS_NODE SWIGTYPE_p_BiorbdCasadi__utils__Node +#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_BiorbdCasadi__utils__SpatialVector + +#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_BiorbdCasadi__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_BiorbdCasadi__rigidbody__GeneralizedCoordinates +#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_BiorbdCasadi__rigidbody__GeneralizedVelocity +#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_BiorbdCasadi__rigidbody__GeneralizedAcceleration +#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_BiorbdCasadi__rigidbody__GeneralizedTorque +#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_BiorbdCasadi__rigidbody__Markers #elif defined(BIORBD_USE_EIGEN3_MATH) -#define SWIG_UTILS_STRING SWIGTYPE_p_biorbd__MathEigen3__utils__String -#define SWIG_UTILS_PATH SWIGTYPE_p_biorbd__MathEigen3__utils__Path -#define SWIG_UTILS_VECTOR SWIGTYPE_p_biorbd__MathEigen3__utils__Vector -#define SWIG_UTILS_NODE SWIGTYPE_p_biorbd__MathEigen3__utils__Node -#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_biorbd__MathEigen3__utils__SpatialVector - -#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Joints -#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedCoordinates -#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedVelocity -#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedAcceleration -#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_biorbd__MathEigen3__rigidbody__GeneralizedTorque -#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_biorbd__MathEigen3__rigidbody__Markers +#define SWIG_UTILS_STRING SWIGTYPE_p_BiorbdEigen3__utils__String +#define SWIG_UTILS_PATH SWIGTYPE_p_BiorbdEigen3__utils__Path +#define SWIG_UTILS_VECTOR SWIGTYPE_p_BiorbdEigen3__utils__Vector +#define SWIG_UTILS_NODE SWIGTYPE_p_BiorbdEigen3__utils__Node +#define SWIG_UTILS_SPATIAL_VECTOR SWIGTYPE_p_BiorbdEigen3__utils__SpatialVector + +#define SWIG_RIGIDBODY_JOINTS SWIGTYPE_p_BiorbdEigen3__rigidbody__Joints +#define SWIG_RIGIDBODY_GEN_COORD SWIGTYPE_p_BiorbdEigen3__rigidbody__GeneralizedCoordinates +#define SWIG_RIGIDBODY_GEN_VEL SWIGTYPE_p_BiorbdEigen3__rigidbody__GeneralizedVelocity +#define SWIG_RIGIDBODY_GEN_ACC SWIGTYPE_p_BiorbdEigen3__rigidbody__GeneralizedAcceleration +#define SWIG_RIGIDBODY_GEN_TORQUE SWIGTYPE_p_BiorbdEigen3__rigidbody__GeneralizedTorque +#define SWIG_RIGIDBODY_MARKERS SWIGTYPE_p_BiorbdEigen3__rigidbody__Markers #endif // -- STRING --// -%typemap(typecheck, precedence=2300) biorbd::BIORBD_MATH_NAMESPACE::utils::String &{ +%typemap(typecheck, precedence=2300) BIORBD_NAMESPACE::utils::String &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_STRING, 0 | 0)) && argp1) { // Test if it is a pointer to String already exists @@ -67,14 +67,14 @@ $1 = false; } }; -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::String &{ +%typemap(in) BIORBD_NAMESPACE::utils::String &{ void * argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_STRING, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::String * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::utils::String * >(argp1); } else if( PyUnicode_Check($input) ) { // Interpret the string - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::String(PyUnicode_AsUTF8($input)); + $1 = new BIORBD_NAMESPACE::utils::String(PyUnicode_AsUTF8($input)); } else { PyErr_SetString(PyExc_ValueError, "String must be a utils::String or string"); SWIG_fail; @@ -82,7 +82,7 @@ }; // --- Path --- // -%typemap(typecheck, precedence=2310) biorbd::BIORBD_MATH_NAMESPACE::utils::Path &{ +%typemap(typecheck, precedence=2310) BIORBD_NAMESPACE::utils::Path &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_PATH, 0 | 0)) && argp1) { // Test if it is a pointer to Path already exists @@ -94,14 +94,14 @@ $1 = false; } }; -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Path &{ +%typemap(in) BIORBD_NAMESPACE::utils::Path &{ void * argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_PATH, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Path * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::utils::Path * >(argp1); } else if( PyUnicode_Check($input) ) { // Interpret the string - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Path(PyUnicode_AsUTF8($input)); + $1 = new BIORBD_NAMESPACE::utils::Path(PyUnicode_AsUTF8($input)); } else { PyErr_SetString(PyExc_ValueError, "Path must be a Path or string"); SWIG_fail; @@ -109,7 +109,7 @@ }; // --- Joints --- // -%typemap(typecheck, precedence=2000) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &{ +%typemap(typecheck, precedence=2000) BIORBD_NAMESPACE::rigidbody::Joints &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_JOINTS, 0 | 0)) && argp1) { // Test if it is a pointer to SWIG_RIGIDBODY_JOINTS already exists @@ -118,11 +118,11 @@ $1 = false; } } -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &{ +%typemap(in) BIORBD_NAMESPACE::rigidbody::Joints &{ void * argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_RIGIDBODY_JOINTS, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::Joints * >(argp1); } else { PyErr_SetString(PyExc_ValueError, "rigidbody::Joints must be a rigidbody::Joints"); @@ -131,7 +131,7 @@ } // --- Allows for calling MX as scalar --- // -%typemap(typecheck, precedence=2400) biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar& { +%typemap(typecheck, precedence=2400) BIORBD_NAMESPACE::utils::Scalar& { void *argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { @@ -159,13 +159,13 @@ $1 = false; } }; -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar& { +%typemap(in) BIORBD_NAMESPACE::utils::Scalar& { void * argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { // Recast the pointer try{ - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::utils::Scalar(*reinterpret_cast(argp1)); } catch (...){ PyErr_SetString(PyExc_ValueError, "Scalar must be a 1x1 array or a float"); SWIG_fail; @@ -188,7 +188,7 @@ } // Copy the actual data - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR1((PyArrayObject*)data, 0)); + $1 = new BIORBD_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR1((PyArrayObject*)data, 0)); } else if (ndim == 2) { unsigned int scalar_dim0(dims[0]); @@ -200,7 +200,7 @@ } // Copy the actual data - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR2((PyArrayObject*)data, 0, 0)); + $1 = new BIORBD_NAMESPACE::utils::Scalar(*(double*)PyArray_GETPTR2((PyArrayObject*)data, 0, 0)); } else { PyErr_SetString(PyExc_ValueError, @@ -209,10 +209,10 @@ } } else if (PyFloat_Check($input)) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(PyFloat_AS_DOUBLE($input)); + $1 = new BIORBD_NAMESPACE::utils::Scalar(PyFloat_AS_DOUBLE($input)); } else if (PyLong_Check($input)) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar(static_cast(PyLong_AS_LONG($input))); + $1 = new BIORBD_NAMESPACE::utils::Scalar(static_cast(PyLong_AS_LONG($input))); } else { PyErr_SetString(PyExc_ValueError, "Scalar must be a 1x1 array or a float"); @@ -221,7 +221,7 @@ }; #ifdef BIORBD_USE_CASADI_MATH -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Scalar{ +%extend BIORBD_NAMESPACE::utils::Scalar{ casadi::MX to_mx(){ return *dynamic_cast($self); }; @@ -229,7 +229,7 @@ #endif // --- Quaternion --- // -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Quaternion{ +%extend BIORBD_NAMESPACE::utils::Quaternion{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -259,7 +259,7 @@ } // --- Matrix --- // -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Matrix{ +%extend BIORBD_NAMESPACE::utils::Matrix{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -372,7 +372,7 @@ #endif // --- Vector --- // -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector{ +%extend BIORBD_NAMESPACE::utils::Vector{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -394,7 +394,7 @@ }; #endif } -%typemap(typecheck, precedence=2150) biorbd::BIORBD_MATH_NAMESPACE::utils::Vector & { +%typemap(typecheck, precedence=2150) BIORBD_NAMESPACE::utils::Vector & { void *argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { @@ -412,18 +412,18 @@ $1 = false; } } -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Vector & { +%typemap(in) BIORBD_NAMESPACE::utils::Vector & { void * argp1 = 0; #ifdef BIORBD_USE_CASADI_MATH if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::utils::Vector(*reinterpret_cast(argp1)); #else if (SWIG_IsOK( SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_VECTOR, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Vector * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::utils::Vector * >(argp1); #endif } else if( PyArray_Check($input) ) { // Get dimensions of the data:: @@ -440,7 +440,7 @@ PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int n(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(n); + $1 = new BIORBD_NAMESPACE::utils::Vector(n); for (unsigned int i=0; i(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -501,13 +501,13 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQ(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); for (unsigned int q=0; q(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -556,7 +556,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQ(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates(nQ); for (unsigned int q=0; q(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -617,7 +617,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQdot(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); for (unsigned int qdot=0; qdot(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -673,7 +673,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity * { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQdot(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity(nQdot); for (unsigned int qdot=0; qdot(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -730,7 +730,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration & { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQddot(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); for (unsigned int qddot=0; qddot(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -786,7 +786,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * { PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data unsigned int nQddot(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration(nQddot); for (unsigned int qddot=0; qddot(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::GeneralizedTorque * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedTorque(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -843,7 +843,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * { // Copy the actual data unsigned int nGeneralizedTorque(dims[0]); - $1 = new biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque(nGeneralizedTorque); + $1 = new BIORBD_NAMESPACE::rigidbody::GeneralizedTorque(nGeneralizedTorque); for (unsigned int GeneralizedTorque=0; GeneralizedTorque(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::rigidbody::Markers * >(argp1); } else { PyErr_SetString(PyExc_ValueError, - "biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers must be " - "a biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Markers"); + "BIORBD_NAMESPACE::rigidbody::Markers must be " + "a BIORBD_NAMESPACE::rigidbody::Markers"); SWIG_fail; } } @@ -883,7 +883,7 @@ biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration * { // --- Vector3d --- // %typemap(typecheck, precedence=2130) -biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ +BIORBD_NAMESPACE::utils::Vector3d &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_NODE, 0 | 0)) && argp1) { // Test if it is a pointer to SWIG_UTILS_NODE already exists @@ -904,15 +904,15 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ $1 = false; } } -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ +%typemap(in) BIORBD_NAMESPACE::utils::Vector3d &{ void * argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_NODE, 0 | 0)) && argp1) { // Recast the pointer - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::utils::Vector3d * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d(biorbd::BIORBD_MATH_NAMESPACE::utils::Vector(*reinterpret_cast(argp1))); + $1 = new BIORBD_NAMESPACE::utils::Vector3d(BIORBD_NAMESPACE::utils::Vector(*reinterpret_cast(argp1))); } #endif else if( PyArray_Check($input) ) { @@ -929,7 +929,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ PyObject *data = PyArray_FROM_OTF((PyObject*)$input, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY); // Copy the actual data - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d(0, 0, 0); + $1 = new BIORBD_NAMESPACE::utils::Vector3d(0, 0, 0); for (unsigned int i=0; i<3; ++i) (*$1)[i] = *(double*)PyArray_GETPTR1((PyArrayObject*)data, i); @@ -939,7 +939,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ } }; -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d{ +%extend BIORBD_NAMESPACE::utils::Vector3d{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -962,7 +962,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ }; // --- Spatial Vector --- // -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector{ +%extend BIORBD_NAMESPACE::utils::SpatialVector{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -983,7 +983,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ } #endif }; -%typemap(typecheck, precedence=2010) biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector &{ +%typemap(typecheck, precedence=2010) BIORBD_NAMESPACE::utils::SpatialVector &{ void *argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_SPATIAL_VECTOR, 0 | 0)) && argp1) { $1 = true; @@ -1001,14 +1001,14 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ $1 = false; } } -%typemap(in) biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector &{ +%typemap(in) BIORBD_NAMESPACE::utils::SpatialVector &{ void * argp1 = 0; if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIG_UTILS_SPATIAL_VECTOR, 0 | 0)) && argp1) { - $1 = reinterpret_cast< biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector * >(argp1); + $1 = reinterpret_cast< BIORBD_NAMESPACE::utils::SpatialVector * >(argp1); } #ifdef BIORBD_USE_CASADI_MATH else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) { - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector(*reinterpret_cast(argp1)); + $1 = new BIORBD_NAMESPACE::utils::SpatialVector(*reinterpret_cast(argp1)); } #endif else if( PyArray_Check($input) ) { @@ -1033,7 +1033,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ "SpatialVector must be a (6,) vector when using a numpy array"); SWIG_fail; } - $1 = new biorbd::BIORBD_MATH_NAMESPACE::utils::SpatialVector(); + $1 = new BIORBD_NAMESPACE::utils::SpatialVector(); for (unsigned int sv=0; sv($self); @@ -1069,7 +1069,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ } #endif }; -%extend biorbd::BIORBD_MATH_NAMESPACE::rigidbody::IMU{ +%extend BIORBD_NAMESPACE::rigidbody::IMU{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); @@ -1094,7 +1094,7 @@ biorbd::BIORBD_MATH_NAMESPACE::utils::Vector3d &{ #endif }; -%extend biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation{ +%extend BIORBD_NAMESPACE::utils::Rotation{ #ifdef BIORBD_USE_CASADI_MATH casadi::MX to_mx(){ return *dynamic_cast($self); diff --git a/examples/WrappingObjectsExample.cpp b/examples/WrappingObjectsExample.cpp index f0118e14..10709402 100644 --- a/examples/WrappingObjectsExample.cpp +++ b/examples/WrappingObjectsExample.cpp @@ -13,7 +13,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/forwardDynamicsExample.cpp b/examples/forwardDynamicsExample.cpp index 846e9ee6..a29ede01 100644 --- a/examples/forwardDynamicsExample.cpp +++ b/examples/forwardDynamicsExample.cpp @@ -13,7 +13,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/forwardDynamicsFromMusclesExample.cpp b/examples/forwardDynamicsFromMusclesExample.cpp index 503a0949..24d3b8eb 100644 --- a/examples/forwardDynamicsFromMusclesExample.cpp +++ b/examples/forwardDynamicsFromMusclesExample.cpp @@ -14,7 +14,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/forwardKinematicsExample.cpp b/examples/forwardKinematicsExample.cpp index 3a89c97c..66054ec4 100644 --- a/examples/forwardKinematicsExample.cpp +++ b/examples/forwardKinematicsExample.cpp @@ -13,7 +13,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/inverseDynamicsExample.cpp b/examples/inverseDynamicsExample.cpp index 0b644f8d..c3b3c3bf 100644 --- a/examples/inverseDynamicsExample.cpp +++ b/examples/inverseDynamicsExample.cpp @@ -13,7 +13,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/inverseKinematicsKalmanExample.cpp b/examples/inverseKinematicsKalmanExample.cpp index ad68f1aa..6b126275 100644 --- a/examples/inverseKinematicsKalmanExample.cpp +++ b/examples/inverseKinematicsKalmanExample.cpp @@ -15,7 +15,7 @@ /// Please also note that kalman will be VERY slow if compiled in debug /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/examples/staticOptimizationExample.cpp b/examples/staticOptimizationExample.cpp index 01e52580..9d5fb760 100644 --- a/examples/staticOptimizationExample.cpp +++ b/examples/staticOptimizationExample.cpp @@ -15,7 +15,7 @@ /// Please note that this example will work only with the Eigen backend /// -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; int main() { diff --git a/include/Actuators/Actuator.h b/include/Actuators/Actuator.h index 3ff31069..802bc873 100644 --- a/include/Actuators/Actuator.h +++ b/include/Actuators/Actuator.h @@ -6,9 +6,7 @@ #include "Actuators/ActuatorEnums.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -109,7 +107,6 @@ class BIORBD_API Actuator }; -} } } diff --git a/include/Actuators/ActuatorConstant.h b/include/Actuators/ActuatorConstant.h index ce2695cf..50257143 100644 --- a/include/Actuators/ActuatorConstant.h +++ b/include/Actuators/ActuatorConstant.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Actuators/Actuator.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace actuator { @@ -82,7 +80,6 @@ class BIORBD_API ActuatorConstant : public Actuator }; -} } } diff --git a/include/Actuators/ActuatorEnums.h b/include/Actuators/ActuatorEnums.h index 7d7a51b3..599e4063 100644 --- a/include/Actuators/ActuatorEnums.h +++ b/include/Actuators/ActuatorEnums.h @@ -1,9 +1,7 @@ #ifndef BIORBD_ACTUATOR_ENUMS_H #define BIORBD_ACTUATOR_ENUMS_H -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace actuator { @@ -44,7 +42,6 @@ inline const char* TYPE_toStr(TYPE type) } -} } } diff --git a/include/Actuators/ActuatorGauss3p.h b/include/Actuators/ActuatorGauss3p.h index 74426c81..2fd38588 100644 --- a/include/Actuators/ActuatorGauss3p.h +++ b/include/Actuators/ActuatorGauss3p.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Actuators/Actuator.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -162,7 +160,6 @@ class BIORBD_API ActuatorGauss3p : public Actuator }; -} } } diff --git a/include/Actuators/ActuatorGauss6p.h b/include/Actuators/ActuatorGauss6p.h index 84cee9e3..b39e7f5c 100644 --- a/include/Actuators/ActuatorGauss6p.h +++ b/include/Actuators/ActuatorGauss6p.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Actuators/Actuator.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -177,7 +175,6 @@ class BIORBD_API ActuatorGauss6p : public Actuator }; -} } } diff --git a/include/Actuators/ActuatorLinear.h b/include/Actuators/ActuatorLinear.h index ad6c08cf..56beca06 100644 --- a/include/Actuators/ActuatorLinear.h +++ b/include/Actuators/ActuatorLinear.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Actuators/Actuator.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -109,7 +107,6 @@ class BIORBD_API ActuatorLinear : public Actuator }; -} } } diff --git a/include/Actuators/ActuatorSigmoidGauss3p.h b/include/Actuators/ActuatorSigmoidGauss3p.h index a100f68d..bfe65a94 100644 --- a/include/Actuators/ActuatorSigmoidGauss3p.h +++ b/include/Actuators/ActuatorSigmoidGauss3p.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Actuators/Actuator.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -132,7 +130,6 @@ class BIORBD_API ActuatorSigmoidGauss3p : public Actuator std::shared_ptr m_offset; ///< Height of the Sigmoid }; -} } } diff --git a/include/Actuators/Actuators.h b/include/Actuators/Actuators.h index c1ce0a5b..295a8dbb 100644 --- a/include/Actuators/Actuators.h +++ b/include/Actuators/Actuators.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -145,7 +143,6 @@ class BIORBD_API Actuators }; -} } } diff --git a/include/BiorbdModel.h b/include/BiorbdModel.h index ed0f8fb6..2e7f8400 100644 --- a/include/BiorbdModel.h +++ b/include/BiorbdModel.h @@ -57,11 +57,9 @@ /// \brief Returns the current version of biorbd /// \return The current version of biorbd /// -biorbd::BIORBD_MATH_NAMESPACE::utils::String getVersion(); +BIORBD_NAMESPACE::utils::String getVersion(); -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { /// /// \brief The actual musculoskeletal model that holds everything in biorbd @@ -102,7 +100,6 @@ class BIORBD_API Model : utils::Path path() const; }; -} } #endif // BIORBD_MODEL_H diff --git a/include/ModelReader.h b/include/ModelReader.h index 072f550d..d75cf13a 100644 --- a/include/ModelReader.h +++ b/include/ModelReader.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "rbdl/rbdl_math.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { class Model; @@ -66,7 +64,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the kinematics /// - static std::vector readQDataFile( + static std::vector readQDataFile( const utils::Path &path); /// @@ -157,7 +155,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileBiorbdSegments( + static rigidbody::Mesh readMeshFileBiorbdSegments( const utils::Path& path); /// @@ -165,7 +163,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFilePly( + static rigidbody::Mesh readMeshFilePly( const utils::Path& path); /// @@ -173,7 +171,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileObj( + static rigidbody::Mesh readMeshFileObj( const utils::Path& path); #ifdef MODULE_VTP_FILES_READER @@ -182,7 +180,7 @@ class BIORBD_API Reader /// \param path The path of the file /// \return Returns the mesh /// - static biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Mesh readMeshFileVtp( + static rigidbody::Mesh readMeshFileVtp( const utils::Path& path); #endif @@ -224,7 +222,6 @@ class BIORBD_API Reader utils::RotoTrans &RT); }; -} } #endif // BIORBD_UTILS_READ_H diff --git a/include/ModelWriter.h b/include/ModelWriter.h index 4da1061f..ae0086c7 100644 --- a/include/ModelWriter.h +++ b/include/ModelWriter.h @@ -2,9 +2,7 @@ #define BIORBD_UTILS_WRITER_H #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { class Model; @@ -31,7 +29,6 @@ class BIORBD_API Writer #endif }; -} } #endif // BIORBD_UTILS_WRITER_H diff --git a/include/Muscles/Characteristics.h b/include/Muscles/Characteristics.h index d1813107..6a68b0df 100644 --- a/include/Muscles/Characteristics.h +++ b/include/Muscles/Characteristics.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -228,7 +226,6 @@ class BIORBD_API Characteristics m_fatigueParameters; ///< Fatigue parameters }; -} } } diff --git a/include/Muscles/Compound.h b/include/Muscles/Compound.h index 002744c7..6a7d52e9 100644 --- a/include/Muscles/Compound.h +++ b/include/Muscles/Compound.h @@ -7,9 +7,7 @@ #include "Muscles/MusclesEnums.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -171,7 +169,6 @@ class BIORBD_API Compound }; -} } } diff --git a/include/Muscles/FatigueDynamicState.h b/include/Muscles/FatigueDynamicState.h index 25f26c53..345c6af2 100644 --- a/include/Muscles/FatigueDynamicState.h +++ b/include/Muscles/FatigueDynamicState.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/FatigueState.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -82,7 +80,6 @@ class BIORBD_API FatigueDynamicState : public FatigueState }; -} } } diff --git a/include/Muscles/FatigueDynamicStateXia.h b/include/Muscles/FatigueDynamicStateXia.h index 0aeda0c2..3cc820ea 100644 --- a/include/Muscles/FatigueDynamicStateXia.h +++ b/include/Muscles/FatigueDynamicStateXia.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/FatigueDynamicState.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -66,7 +64,6 @@ class BIORBD_API FatigueDynamicStateXia : public }; -} } } diff --git a/include/Muscles/FatigueModel.h b/include/Muscles/FatigueModel.h index b0c44508..8dcf66f5 100644 --- a/include/Muscles/FatigueModel.h +++ b/include/Muscles/FatigueModel.h @@ -6,9 +6,7 @@ #include "MusclesEnums.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -97,7 +95,6 @@ class BIORBD_API FatigueModel }; -} } } diff --git a/include/Muscles/FatigueParameters.h b/include/Muscles/FatigueParameters.h index ee2a815a..c91870e7 100644 --- a/include/Muscles/FatigueParameters.h +++ b/include/Muscles/FatigueParameters.h @@ -6,9 +6,7 @@ #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -107,7 +105,6 @@ class BIORBD_API FatigueParameters }; -} } } diff --git a/include/Muscles/FatigueState.h b/include/Muscles/FatigueState.h index e428bdeb..83b87216 100644 --- a/include/Muscles/FatigueState.h +++ b/include/Muscles/FatigueState.h @@ -6,9 +6,7 @@ #include "MusclesEnums.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -118,7 +116,6 @@ class BIORBD_API FatigueState }; -} } } diff --git a/include/Muscles/Geometry.h b/include/Muscles/Geometry.h index 5cef3e03..bb1d955c 100644 --- a/include/Muscles/Geometry.h +++ b/include/Muscles/Geometry.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -72,7 +70,7 @@ class BIORBD_API Geometry /// updateKinematics MUST be called before retreiving data that are dependent on Q and/or Qdot /// void updateKinematics( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates* Q = nullptr, const rigidbody::GeneralizedVelocity* Qdot = nullptr, int updateKin = 2); @@ -89,7 +87,7 @@ class BIORBD_API Geometry /// updateKinematics MUST be called before retreiving data that are dependent on Q and/or Qdot /// void updateKinematics( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const Characteristics& characteristics, PathModifiers& pathModifiers, const rigidbody::GeneralizedCoordinates* Q = nullptr, @@ -244,7 +242,7 @@ class BIORBD_API Geometry /// \return The origin position in the global reference /// const utils::Vector3d& originInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q); /// @@ -254,7 +252,7 @@ class BIORBD_API Geometry /// \return The origin position in the global reference /// const utils::Vector3d& insertionInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q); /// @@ -271,7 +269,7 @@ class BIORBD_API Geometry /// \param pathModifiers The set of path modifiers /// void setMusclesPointsInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, PathModifiers* pathModifiers = nullptr); @@ -299,7 +297,7 @@ class BIORBD_API Geometry /// \param model The joint model /// void setJacobianDimension( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model); + rigidbody::Joints &model); /// /// \brief Force a jacobian computed from the user @@ -314,7 +312,7 @@ class BIORBD_API Geometry /// \param Q The generalize coordinates /// void jacobian( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q); /// @@ -355,7 +353,6 @@ class BIORBD_API Geometry }; -} } } diff --git a/include/Muscles/HillThelenActiveOnlyType.h b/include/Muscles/HillThelenActiveOnlyType.h index d0b6d1f3..ac7245d7 100644 --- a/include/Muscles/HillThelenActiveOnlyType.h +++ b/include/Muscles/HillThelenActiveOnlyType.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/HillThelenType.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -118,7 +116,6 @@ class BIORBD_API HillThelenActiveOnlyType : public }; -} } } diff --git a/include/Muscles/HillThelenType.h b/include/Muscles/HillThelenType.h index 7e021122..f68389a5 100644 --- a/include/Muscles/HillThelenType.h +++ b/include/Muscles/HillThelenType.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/HillType.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -119,7 +117,6 @@ class BIORBD_API HillThelenType : public HillType }; -} } } diff --git a/include/Muscles/HillThelenTypeFatigable.h b/include/Muscles/HillThelenTypeFatigable.h index aa18a266..42178d0f 100644 --- a/include/Muscles/HillThelenTypeFatigable.h +++ b/include/Muscles/HillThelenTypeFatigable.h @@ -6,9 +6,7 @@ #include "Muscles/HillThelenType.h" #include "Muscles/FatigueModel.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -140,7 +138,6 @@ class BIORBD_API HillThelenTypeFatigable : public }; -} } } diff --git a/include/Muscles/HillType.h b/include/Muscles/HillType.h index 191d4336..9726db02 100644 --- a/include/Muscles/HillType.h +++ b/include/Muscles/HillType.h @@ -5,9 +5,7 @@ #include "Muscles/Muscle.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -131,7 +129,7 @@ class BIORBD_API HillType : public Muscle /// \return The force vector at origin and insertion /// virtual const utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const rigidbody::GeneralizedVelocity& Qdot, const State& emg, @@ -146,7 +144,7 @@ class BIORBD_API HillType : public Muscle /// \return The force vector at origin and insertion /// virtual const utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const State& emg, int updateKin = 2); @@ -249,7 +247,6 @@ class BIORBD_API HillType : public Muscle }; -} } } diff --git a/include/Muscles/IdealizedActuator.h b/include/Muscles/IdealizedActuator.h index 3c8fc61a..97ee37ba 100644 --- a/include/Muscles/IdealizedActuator.h +++ b/include/Muscles/IdealizedActuator.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/Muscle.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -118,7 +116,7 @@ class BIORBD_API IdealizedActuator : public Muscle /// \return The force vector at origin and insertion /// virtual const utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const rigidbody::GeneralizedVelocity& Qdot, const State& emg, @@ -133,7 +131,7 @@ class BIORBD_API IdealizedActuator : public Muscle /// \return The force vector at origin and insertion /// virtual const utils::Scalar& force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const State& emg, int updateKin = 2); @@ -152,7 +150,6 @@ class BIORBD_API IdealizedActuator : public Muscle }; -} } } diff --git a/include/Muscles/Muscle.h b/include/Muscles/Muscle.h index 9c8cc1a6..a453b098 100644 --- a/include/Muscles/Muscle.h +++ b/include/Muscles/Muscle.h @@ -5,9 +5,7 @@ #include "Muscles/Compound.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -123,7 +121,7 @@ class BIORBD_API Muscle : public Compound /// \return The length of the muscle /// const utils::Scalar& length( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, int updateKin = 2); @@ -135,7 +133,7 @@ class BIORBD_API Muscle : public Compound /// \return The musculo tendon length /// const utils::Scalar& musculoTendonLength( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, int updateKin = 2); @@ -148,7 +146,7 @@ class BIORBD_API Muscle : public Compound //// \return The velocity of the muscle /// const utils::Scalar& velocity( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const rigidbody::GeneralizedVelocity& Qdot, bool updateKin = true); @@ -160,7 +158,7 @@ class BIORBD_API Muscle : public Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// void updateOrientations( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, int updateKin = 2); @@ -172,7 +170,7 @@ class BIORBD_API Muscle : public Compound /// \param updateKin Update kinematics (0: don't update, 1:only muscles, [2: both kinematics and muscles]) /// void updateOrientations( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, int updateKin = 2); @@ -230,7 +228,7 @@ class BIORBD_API Muscle : public Compound /// \return The muscle points in global reference frame /// const std::vector& musclesPointsInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q); /// @@ -298,7 +296,6 @@ class BIORBD_API Muscle : public Compound }; -} } } diff --git a/include/Muscles/MuscleGroup.h b/include/Muscles/MuscleGroup.h index ea823d9c..62a3c75c 100644 --- a/include/Muscles/MuscleGroup.h +++ b/include/Muscles/MuscleGroup.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "Muscles/MusclesEnums.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -239,7 +237,6 @@ class BIORBD_API MuscleGroup }; -} } } diff --git a/include/Muscles/Muscles.h b/include/Muscles/Muscles.h index 9aa1ea08..fde82b54 100644 --- a/include/Muscles/Muscles.h +++ b/include/Muscles/Muscles.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -330,7 +328,6 @@ class BIORBD_API Muscles }; -} } } diff --git a/include/Muscles/MusclesEnums.h b/include/Muscles/MusclesEnums.h index 8364e57e..dcb9552c 100644 --- a/include/Muscles/MusclesEnums.h +++ b/include/Muscles/MusclesEnums.h @@ -1,9 +1,7 @@ #ifndef BIORBD_MUSCLES_ENUMS_H #define BIORBD_MUSCLES_ENUMS_H -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -91,7 +89,6 @@ inline const char* STATE_FATIGUE_TYPE_toStr(STATE_FATIGUE_TYPE } } -} } } diff --git a/include/Muscles/PathModifiers.h b/include/Muscles/PathModifiers.h index 1f042100..79bd91cd 100644 --- a/include/Muscles/PathModifiers.h +++ b/include/Muscles/PathModifiers.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -91,7 +89,6 @@ class BIORBD_API PathModifiers }; -} } } diff --git a/include/Muscles/State.h b/include/Muscles/State.h index e23a4c90..1d3d0177 100644 --- a/include/Muscles/State.h +++ b/include/Muscles/State.h @@ -6,9 +6,7 @@ #include "Muscles/MusclesEnums.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -135,7 +133,6 @@ class BIORBD_API State }; -} } } diff --git a/include/Muscles/StateDynamics.h b/include/Muscles/StateDynamics.h index 119580dd..c2d3bb7d 100644 --- a/include/Muscles/StateDynamics.h +++ b/include/Muscles/StateDynamics.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/State.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -143,7 +141,6 @@ class BIORBD_API StateDynamics : public State }; -} } } diff --git a/include/Muscles/StateDynamicsBuchanan.h b/include/Muscles/StateDynamicsBuchanan.h index 8e20ed5a..1eaf8087 100644 --- a/include/Muscles/StateDynamicsBuchanan.h +++ b/include/Muscles/StateDynamicsBuchanan.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/StateDynamics.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -112,7 +110,6 @@ class BIORBD_API StateDynamicsBuchanan : public StateDynamics }; -} } } diff --git a/include/Muscles/StateDynamicsDeGroote.h b/include/Muscles/StateDynamicsDeGroote.h index 90a9cba2..9ff92691 100644 --- a/include/Muscles/StateDynamicsDeGroote.h +++ b/include/Muscles/StateDynamicsDeGroote.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/StateDynamics.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -60,7 +58,6 @@ class BIORBD_API StateDynamicsDeGroote : public StateDynamics }; -} } } diff --git a/include/Muscles/StaticOptimization.h b/include/Muscles/StaticOptimization.h index 22f96c08..af953445 100644 --- a/include/Muscles/StaticOptimization.h +++ b/include/Muscles/StaticOptimization.h @@ -6,9 +6,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { class Model; @@ -197,7 +195,6 @@ class BIORBD_API StaticOptimization }; -} } } diff --git a/include/Muscles/StaticOptimizationIpopt.h b/include/Muscles/StaticOptimizationIpopt.h index 0d16678f..f2a6018f 100644 --- a/include/Muscles/StaticOptimizationIpopt.h +++ b/include/Muscles/StaticOptimizationIpopt.h @@ -6,9 +6,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { class Model; @@ -261,7 +259,6 @@ class BIORBD_API StaticOptimizationIpopt : public Ipopt::TNLP }; -} } } diff --git a/include/Muscles/StaticOptimizationIpoptLinearized.h b/include/Muscles/StaticOptimizationIpoptLinearized.h index 0f177afc..c1907969 100644 --- a/include/Muscles/StaticOptimizationIpoptLinearized.h +++ b/include/Muscles/StaticOptimizationIpoptLinearized.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/StaticOptimizationIpopt.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -98,7 +96,6 @@ class BIORBD_API StaticOptimizationIpoptLinearized : public StaticOptimizationIp }; -} } } diff --git a/include/Muscles/ViaPoint.h b/include/Muscles/ViaPoint.h index 127a0c89..ef218511 100644 --- a/include/Muscles/ViaPoint.h +++ b/include/Muscles/ViaPoint.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/Vector3d.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -90,7 +88,6 @@ class BIORBD_API ViaPoint : public utils::Vector3d } }; -} } } diff --git a/include/Muscles/WrappingHalfCylinder.h b/include/Muscles/WrappingHalfCylinder.h index 98be86d9..59a90dbe 100644 --- a/include/Muscles/WrappingHalfCylinder.h +++ b/include/Muscles/WrappingHalfCylinder.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/WrappingObject.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -103,7 +101,7 @@ class BIORBD_API WrappingHalfCylinder : public WrappingObject /// \param length Length of the muscle (ignored if no value is provided) /// void wrapPoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const utils::Vector3d& p1_bone, const utils::Vector3d& p2_bone, @@ -130,7 +128,7 @@ class BIORBD_API WrappingHalfCylinder : public WrappingObject /// \return The RotoTrans matrix of the half cylinder /// virtual const utils::RotoTrans& RT( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates& Q, bool updateKin = true); @@ -254,7 +252,6 @@ class BIORBD_API WrappingHalfCylinder : public WrappingObject }; -} } } diff --git a/include/Muscles/WrappingObject.h b/include/Muscles/WrappingObject.h index 322d930a..8ae8a4fb 100644 --- a/include/Muscles/WrappingObject.h +++ b/include/Muscles/WrappingObject.h @@ -5,9 +5,7 @@ #include "Utils/Scalar.h" #include "Utils/Vector3d.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -112,7 +110,7 @@ class BIORBD_API WrappingObject : public utils::Vector3d /// \param muscleLength Length of the muscle (ignored if no value is provided) /// virtual void wrapPoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const utils::Vector3d& p1_bone, const utils::Vector3d& p2_bone, @@ -141,7 +139,7 @@ class BIORBD_API WrappingObject : public utils::Vector3d /// \return The RotoTrans matrix of the wrapping object /// virtual const utils::RotoTrans& RT( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates& Q, bool updateKin = true) = 0; @@ -166,7 +164,6 @@ class BIORBD_API WrappingObject : public utils::Vector3d m_RT; ///< RotoTrans matrix of the wrapping object }; -} } } diff --git a/include/Muscles/WrappingSphere.h b/include/Muscles/WrappingSphere.h index 39beda3d..27cb8fbc 100644 --- a/include/Muscles/WrappingSphere.h +++ b/include/Muscles/WrappingSphere.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Muscles/WrappingObject.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace muscles { @@ -89,7 +87,7 @@ class BIORBD_API WrappingSphere : public WrappingObject /// \brief Not yet implemented /// virtual void wrapPoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints&, + rigidbody::Joints&, const rigidbody::GeneralizedCoordinates&, const utils::Vector3d&, const utils::Vector3d&, @@ -113,7 +111,7 @@ class BIORBD_API WrappingSphere : public WrappingObject /// \return The RotoTrans matrix of the sphere /// const utils::RotoTrans& RT( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates&Q , bool updateKin = true); @@ -136,7 +134,6 @@ class BIORBD_API WrappingSphere : public WrappingObject }; -} } } diff --git a/include/RigidBody/Contacts.h b/include/RigidBody/Contacts.h index f2ea778b..cd9f3d10 100644 --- a/include/RigidBody/Contacts.h +++ b/include/RigidBody/Contacts.h @@ -6,9 +6,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -161,7 +159,6 @@ class BIORBD_API Contacts : public RigidBodyDynamics::ConstraintSet }; -} } } diff --git a/include/RigidBody/GeneralizedAcceleration.h b/include/RigidBody/GeneralizedAcceleration.h index ea0c57d0..ba6b0273 100644 --- a/include/RigidBody/GeneralizedAcceleration.h +++ b/include/RigidBody/GeneralizedAcceleration.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/Vector.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -35,7 +33,7 @@ class BIORBD_API GeneralizedAcceleration : public utils::Vector /// \brief Create generalized acceleration vector from a joint Model /// \param j The joint model /// - GeneralizedAcceleration(const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& j); + GeneralizedAcceleration(const rigidbody::Joints& j); /// /// \brief Construct generalized acceleration vector @@ -126,7 +124,6 @@ class BIORBD_API GeneralizedAcceleration : public utils::Vector #endif }; -} } } diff --git a/include/RigidBody/GeneralizedCoordinates.h b/include/RigidBody/GeneralizedCoordinates.h index 6f2d9dce..218ef826 100644 --- a/include/RigidBody/GeneralizedCoordinates.h +++ b/include/RigidBody/GeneralizedCoordinates.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/Vector.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -127,7 +125,6 @@ class BIORBD_API GeneralizedCoordinates : public utils::Vector #endif }; -} } } diff --git a/include/RigidBody/GeneralizedTorque.h b/include/RigidBody/GeneralizedTorque.h index 66101afa..f2a2241d 100644 --- a/include/RigidBody/GeneralizedTorque.h +++ b/include/RigidBody/GeneralizedTorque.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/Vector.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -120,7 +118,6 @@ class BIORBD_API GeneralizedTorque : public utils::Vector #endif }; -} } } diff --git a/include/RigidBody/GeneralizedVelocity.h b/include/RigidBody/GeneralizedVelocity.h index 7b65ff1d..fde7d38e 100644 --- a/include/RigidBody/GeneralizedVelocity.h +++ b/include/RigidBody/GeneralizedVelocity.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/Vector.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -122,7 +120,6 @@ class BIORBD_API GeneralizedVelocity : public utils::Vector #endif }; -} } } diff --git a/include/RigidBody/IMU.h b/include/RigidBody/IMU.h index d9666440..93ec4c6b 100644 --- a/include/RigidBody/IMU.h +++ b/include/RigidBody/IMU.h @@ -5,9 +5,7 @@ #include "biorbdConfig.h" #include "Utils/RotoTransNode.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -111,7 +109,6 @@ class BIORBD_API IMU : public utils::RotoTransNode }; -} } } diff --git a/include/RigidBody/IMUs.h b/include/RigidBody/IMUs.h index c01e8df1..a9ce8097 100644 --- a/include/RigidBody/IMUs.h +++ b/include/RigidBody/IMUs.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -241,7 +239,6 @@ class BIORBD_API IMUs }; -} } } diff --git a/include/RigidBody/Joints.h b/include/RigidBody/Joints.h index 07ee57ed..398e65de 100644 --- a/include/RigidBody/Joints.h +++ b/include/RigidBody/Joints.h @@ -7,9 +7,7 @@ #include "biorbdConfig.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -279,7 +277,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model void UpdateKinematicsCustom( const GeneralizedCoordinates *Q = nullptr, const GeneralizedVelocity *Qdot = nullptr, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + const rigidbody::GeneralizedAcceleration *Qddot = nullptr); // -- POSITION INTERFACE OF THE MODEL -- // @@ -524,7 +522,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model utils::Vector3d CoMddot( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); /// @@ -564,7 +562,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model std::vector CoMddotBySegment( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin=true); /// @@ -579,7 +577,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model utils::Vector3d CoMddotBySegment( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin = true); @@ -705,7 +703,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model utils::Vector3d CalcAngularMomentum ( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); /// @@ -731,7 +729,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model std::vector CalcSegmentsAngularMomentum ( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin); // -------------------------------- // @@ -774,7 +772,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model GeneralizedTorque InverseDynamics( const GeneralizedCoordinates &Q, const GeneralizedVelocity &QDot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, + const rigidbody::GeneralizedAcceleration &QDDot, std::vector* f_ext = nullptr); /// @@ -797,7 +795,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamics( + rigidbody::GeneralizedAcceleration ForwardDynamics( const GeneralizedCoordinates& Q, const GeneralizedVelocity& QDot, const GeneralizedTorque& Tau, @@ -812,7 +810,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( + rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( const GeneralizedCoordinates& Q, const GeneralizedVelocity& QDot, const GeneralizedTorque& Tau, @@ -841,7 +839,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model /// \param f_ext External force acting on the system if there are any /// \return The Generalized Accelerations /// - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( + rigidbody::GeneralizedAcceleration ForwardDynamicsConstraintsDirect( const GeneralizedCoordinates& Q, const GeneralizedVelocity& QDot, const GeneralizedTorque& Tau, @@ -918,12 +916,11 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model void checkGeneralizedDimensions( const GeneralizedCoordinates *Q = nullptr, const GeneralizedVelocity *Qdot = nullptr, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, + const rigidbody::GeneralizedAcceleration *Qddot = nullptr, const GeneralizedTorque *torque = nullptr); }; } // namespace rigidbody -} // namespace BIORBD_MATH_NAMESPACE -} // namespace biorbd +} // namespace BIORBD_NAMESPACE #endif // BIORBD_RIGIDBODY_JOINTS_H diff --git a/include/RigidBody/KalmanRecons.h b/include/RigidBody/KalmanRecons.h index be267314..f7d2a0ae 100644 --- a/include/RigidBody/KalmanRecons.h +++ b/include/RigidBody/KalmanRecons.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { class Model; @@ -108,7 +106,7 @@ class BIORBD_API KalmanRecons void getState( GeneralizedCoordinates *Q = nullptr, GeneralizedVelocity *Qdot = nullptr, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + GeneralizedAcceleration *Qddot = nullptr); /// /// \brief Set the initial guess of the reconstruction @@ -119,7 +117,7 @@ class BIORBD_API KalmanRecons void setInitState( const GeneralizedCoordinates *Q = nullptr, const GeneralizedVelocity *Qdot = nullptr, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr); + const GeneralizedAcceleration *Qddot = nullptr); /// /// \brief Proceed to one iteration of the Kalman filter @@ -222,7 +220,6 @@ class BIORBD_API KalmanRecons }; -} } } diff --git a/include/RigidBody/KalmanReconsIMU.h b/include/RigidBody/KalmanReconsIMU.h index 187ded10..7cbc0cb5 100644 --- a/include/RigidBody/KalmanReconsIMU.h +++ b/include/RigidBody/KalmanReconsIMU.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "RigidBody/KalmanRecons.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -67,7 +65,7 @@ class BIORBD_API KalmanReconsIMU : public KalmanRecons const std::vector &IMUobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); + GeneralizedAcceleration *Qddot); /// /// \brief Reconstruct the kinematics @@ -82,7 +80,7 @@ class BIORBD_API KalmanReconsIMU : public KalmanRecons const utils::Vector &IMUobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot); + GeneralizedAcceleration *Qddot); /// /// \brief This function cannot be used to reconstruct frames @@ -117,7 +115,6 @@ class BIORBD_API KalmanReconsIMU : public KalmanRecons std::shared_ptr m_firstIteration; ///< If first iteration was done }; -} } } diff --git a/include/RigidBody/KalmanReconsMarkers.h b/include/RigidBody/KalmanReconsMarkers.h index c4be11e3..caf7a4d7 100644 --- a/include/RigidBody/KalmanReconsMarkers.h +++ b/include/RigidBody/KalmanReconsMarkers.h @@ -5,9 +5,7 @@ #include "RigidBody/KalmanRecons.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -63,7 +61,7 @@ class BIORBD_API KalmanReconsMarkers : public KalmanRecons const Markers &Tobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + GeneralizedAcceleration *Qddot, bool removeAxes=true); /// @@ -80,7 +78,7 @@ class BIORBD_API KalmanReconsMarkers : public KalmanRecons const std::vector &Tobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + GeneralizedAcceleration *Qddot, bool removeAxes=true); /// @@ -97,7 +95,7 @@ class BIORBD_API KalmanReconsMarkers : public KalmanRecons const utils::Vector &Tobs, GeneralizedCoordinates *Q = nullptr, GeneralizedVelocity *Qdot = nullptr, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot = nullptr, + GeneralizedAcceleration *Qddot = nullptr, bool removeAxes=true); /// @@ -133,7 +131,6 @@ class BIORBD_API KalmanReconsMarkers : public KalmanRecons std::shared_ptr m_firstIteration; ///< If first iteration was done }; -} } } diff --git a/include/RigidBody/Markers.h b/include/RigidBody/Markers.h index 7decb56c..725ea046 100644 --- a/include/RigidBody/Markers.h +++ b/include/RigidBody/Markers.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -212,7 +210,7 @@ class BIORBD_API Markers NodeSegment markerAcceleration( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, unsigned int idx, bool removeAxis = true, bool updateKin = true); @@ -230,7 +228,7 @@ class BIORBD_API Markers std::vector markerAcceleration( const GeneralizedCoordinates &Q, const GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &dQdot, + const rigidbody::GeneralizedAcceleration &dQdot, bool removeAxis=true, bool updateKin = true); @@ -394,7 +392,6 @@ class BIORBD_API Markers }; -} } } diff --git a/include/RigidBody/Mesh.h b/include/RigidBody/Mesh.h index d88fb16a..0a392248 100644 --- a/include/RigidBody/Mesh.h +++ b/include/RigidBody/Mesh.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -133,7 +131,6 @@ class BIORBD_API Mesh std::shared_ptr m_pathFile; ///< The path to the mesh file }; -} } } diff --git a/include/RigidBody/MeshFace.h b/include/RigidBody/MeshFace.h index 8f2fea87..aee1f0fc 100644 --- a/include/RigidBody/MeshFace.h +++ b/include/RigidBody/MeshFace.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -79,7 +77,6 @@ class BIORBD_API MeshFace }; -} } } diff --git a/include/RigidBody/NodeSegment.h b/include/RigidBody/NodeSegment.h index 96b292b5..3e9e1e19 100644 --- a/include/RigidBody/NodeSegment.h +++ b/include/RigidBody/NodeSegment.h @@ -5,9 +5,7 @@ #include "biorbdConfig.h" #include "Utils/Vector3d.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -205,7 +203,6 @@ class BIORBD_API NodeSegment : public utils::Vector3d }; -} } } diff --git a/include/RigidBody/RotoTransNodes.h b/include/RigidBody/RotoTransNodes.h index 928816a1..e731acfe 100644 --- a/include/RigidBody/RotoTransNodes.h +++ b/include/RigidBody/RotoTransNodes.h @@ -5,10 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ - -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -159,7 +156,6 @@ class BIORBD_API RotoTransNodes }; -} } } diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index b9dff078..ba9427fc 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -7,9 +7,7 @@ #include "biorbdConfig.h" #include "Utils/Node.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -48,7 +46,7 @@ class BIORBD_API Segment : public utils::Node /// \param PF Platform index attached to the body (-1 means no force platform acts on the body) /// Segment( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &name, const utils::String &parentName, const utils::String &seqT, @@ -74,7 +72,7 @@ class BIORBD_API Segment : public utils::Node /// \param PF Platform index attached to the body (-1 means no force platform acts on the body) /// Segment( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &name, const utils::String &parentName, const utils::String &seqR, @@ -226,7 +224,7 @@ class BIORBD_API Segment : public utils::Node /// inertia for these 3 segments as well. /// void updateCharacteristics( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const SegmentCharacteristics& characteristics); /// @@ -270,7 +268,7 @@ class BIORBD_API Segment : public utils::Node /// \param QDDotRanges Ranges of the translations and rotations dof acceleration. The length of QDDotRanges must be equal to length of translations and rotations /// void setDofs( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &seqT, const utils::String &seqR, const std::vector& QRanges, @@ -370,7 +368,7 @@ class BIORBD_API Segment : public utils::Node /// \param model The joint model /// virtual void setJoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model); + rigidbody::Joints& model); /// /// \brief Determine the rotation axis in relation to the requested sequence @@ -396,7 +394,6 @@ class BIORBD_API Segment : public utils::Node }; -} } } diff --git a/include/RigidBody/SegmentCharacteristics.h b/include/RigidBody/SegmentCharacteristics.h index 0f3a2c1c..d556cd3e 100644 --- a/include/RigidBody/SegmentCharacteristics.h +++ b/include/RigidBody/SegmentCharacteristics.h @@ -7,9 +7,7 @@ #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -126,7 +124,6 @@ class BIORBD_API SegmentCharacteristics : public RigidBodyDynamics::Body std::shared_ptr m_mesh; ///< Mesh of the segment }; -} } } diff --git a/include/Utils/Benchmark.h b/include/Utils/Benchmark.h index d58f75bf..7e7d6398 100644 --- a/include/Utils/Benchmark.h +++ b/include/Utils/Benchmark.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -94,7 +92,6 @@ class BIORBD_API Benchmark }; -} } } diff --git a/include/Utils/Equation.h b/include/Utils/Equation.h index c0b3c447..7c3f0261 100644 --- a/include/Utils/Equation.h +++ b/include/Utils/Equation.h @@ -6,9 +6,7 @@ #include "biorbdConfig.h" #include "Utils/String.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -126,7 +124,6 @@ class BIORBD_API Equation : public String static std::vector prepareMathSymbols(); }; -} } } diff --git a/include/Utils/Error.h b/include/Utils/Error.h index e65ccfed..692a5fff 100644 --- a/include/Utils/Error.h +++ b/include/Utils/Error.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "Utils/String.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -44,7 +42,6 @@ class BIORBD_API Error const String &message); }; -} } } diff --git a/include/Utils/IfStream.h b/include/Utils/IfStream.h index 2e2fa121..895cbc6f 100644 --- a/include/Utils/IfStream.h +++ b/include/Utils/IfStream.h @@ -7,9 +7,7 @@ #include #include "rbdl/rbdl_math.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -183,7 +181,6 @@ class BIORBD_API IfStream std::shared_ptr m_path;///< The path of the file }; -} } } diff --git a/include/Utils/Matrix.h b/include/Utils/Matrix.h index 17aa64b4..b05cf912 100644 --- a/include/Utils/Matrix.h +++ b/include/Utils/Matrix.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "rbdl/rbdl_math.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -102,7 +100,6 @@ class BIORBD_API Matrix : public RigidBodyDynamics::Math::MatrixNd #endif }; -} } } diff --git a/include/Utils/Node.h b/include/Utils/Node.h index 8a84e141..d5861b15 100644 --- a/include/Utils/Node.h +++ b/include/Utils/Node.h @@ -5,9 +5,7 @@ #include "biorbdConfig.h" #include "Utils/UtilsEnum.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -99,7 +97,6 @@ class BIORBD_API Node }; -} } } diff --git a/include/Utils/Path.h b/include/Utils/Path.h index 4985a834..8d84a327 100644 --- a/include/Utils/Path.h +++ b/include/Utils/Path.h @@ -7,9 +7,7 @@ #endif #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -256,7 +254,6 @@ class BIORBD_API Path std::shared_ptr m_extension; ///< The extension }; -} } } diff --git a/include/Utils/Quaternion.h b/include/Utils/Quaternion.h index db291f70..1d8fdd2f 100644 --- a/include/Utils/Quaternion.h +++ b/include/Utils/Quaternion.h @@ -6,9 +6,7 @@ #include "Utils/Scalar.h" #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -349,7 +347,6 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d }; -} } } diff --git a/include/Utils/Range.h b/include/Utils/Range.h index 8a55421b..6b363d6e 100644 --- a/include/Utils/Range.h +++ b/include/Utils/Range.h @@ -5,9 +5,7 @@ #include -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -67,7 +65,6 @@ class BIORBD_API Range std::shared_ptr m_max; ///< The maximal value allowed by the range }; -} } } diff --git a/include/Utils/Rotation.h b/include/Utils/Rotation.h index 929f54be..46d422a8 100644 --- a/include/Utils/Rotation.h +++ b/include/Utils/Rotation.h @@ -16,9 +16,7 @@ struct SpatialTransform; #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace rigidbody { @@ -214,7 +212,6 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d void checkUnitary(); }; -} } } @@ -224,7 +221,7 @@ class BIORBD_API Rotation : public RigidBodyDynamics::Math::Matrix3d /// \param os osstream /// \param rt The Rotation matrix /// - std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::Rotation &rt); + std::ostream& operator<<(std::ostream& os, const BIORBD_NAMESPACE::utils::Rotation &rt); #endif #endif // BIORBD_UTILS_ROTO_TRANS_H diff --git a/include/Utils/RotoTrans.h b/include/Utils/RotoTrans.h index 8f64a433..a53d20b9 100644 --- a/include/Utils/RotoTrans.h +++ b/include/Utils/RotoTrans.h @@ -16,9 +16,8 @@ struct SpatialTransform; #include "biorbdConfig.h" -namespace biorbd +namespace BIORBD_NAMESPACE { -namespace BIORBD_MATH_NAMESPACE { namespace rigidbody { class NodeSegment; @@ -137,11 +136,9 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \return The system of axes /// static RotoTrans fromMarkers( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::NodeSegment& origin, - const std::pair& - axis1markers, - const std::pair& - axis2markers, + const rigidbody::NodeSegment& origin, + const std::pair&axis1markers, + const std::pair&axis2markers, const std::pair &axesNames, const String& axisToRecalculate); @@ -257,7 +254,6 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d void checkUnitary(); }; -} } } @@ -267,7 +263,7 @@ class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d /// \param os osstream /// \param rt The RotoTrans matrix /// - std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::RotoTrans &rt); + std::ostream& operator<<(std::ostream& os, const BIORBD_NAMESPACE::utils::RotoTrans &rt); #endif #endif // BIORBD_UTILS_ROTO_TRANS_H diff --git a/include/Utils/RotoTransNode.h b/include/Utils/RotoTransNode.h index 03a82b92..f15cef5e 100644 --- a/include/Utils/RotoTransNode.h +++ b/include/Utils/RotoTransNode.h @@ -6,9 +6,7 @@ #include "Utils/RotoTrans.h" #include "Utils/Node.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -110,7 +108,6 @@ RotoTransNode operator*( const RotoTransNode& me); -} } } diff --git a/include/Utils/Scalar.h b/include/Utils/Scalar.h index e9f43643..d8030c79 100644 --- a/include/Utils/Scalar.h +++ b/include/Utils/Scalar.h @@ -4,9 +4,7 @@ #include "biorbdConfig.h" #include "rbdl/rbdl_math.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -57,7 +55,6 @@ class Scalar : public RigidBodyDynamics::Math::Scalar #endif -} } } diff --git a/include/Utils/SpatialVector.h b/include/Utils/SpatialVector.h index 4c7cdcb2..71998073 100644 --- a/include/Utils/SpatialVector.h +++ b/include/Utils/SpatialVector.h @@ -5,9 +5,7 @@ #include "rbdl/rbdl_math.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -102,7 +100,6 @@ class BIORBD_API SpatialVector : public RigidBodyDynamics::Math::SpatialVector #endif }; -} } } diff --git a/include/Utils/String.h b/include/Utils/String.h index eda228a0..b883b946 100644 --- a/include/Utils/String.h +++ b/include/Utils/String.h @@ -4,9 +4,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -161,7 +159,6 @@ class BIORBD_API String : public std::string const String& trailTag); }; -} } } @@ -171,7 +168,7 @@ class BIORBD_API String : public std::string /// \param os The ostream /// \param a The string to operate on /// - std::ostream& operator<<(std::ostream& os, const biorbd::BIORBD_MATH_NAMESPACE::utils::String &a); + std::ostream& operator<<(std::ostream& os, const BIORBD_NAMESPACE::utils::String &a); #endif #endif // BIORBD_UTILS_STRING_H diff --git a/include/Utils/Timer.h b/include/Utils/Timer.h index f4657359..3459b508 100644 --- a/include/Utils/Timer.h +++ b/include/Utils/Timer.h @@ -5,9 +5,7 @@ #include #include "biorbdConfig.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -79,7 +77,6 @@ class BIORBD_API Timer }; -} } } diff --git a/include/Utils/UtilsEnum.h b/include/Utils/UtilsEnum.h index bca0f93c..1b968018 100644 --- a/include/Utils/UtilsEnum.h +++ b/include/Utils/UtilsEnum.h @@ -1,9 +1,7 @@ #ifndef BIORBD_UTILS_ENUMS_H #define BIORBD_UTILS_ENUMS_H -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -53,7 +51,6 @@ inline const char* NODE_TYPE_toStr( } } -} } } diff --git a/include/Utils/Vector.h b/include/Utils/Vector.h index a0b055e5..81133fee 100644 --- a/include/Utils/Vector.h +++ b/include/Utils/Vector.h @@ -5,9 +5,7 @@ #include "rbdl/rbdl_math.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -141,7 +139,6 @@ class BIORBD_API Vector : public RigidBodyDynamics::Math::VectorNd #endif }; -} } } diff --git a/include/Utils/Vector3d.h b/include/Utils/Vector3d.h index 641732b6..f854b70e 100644 --- a/include/Utils/Vector3d.h +++ b/include/Utils/Vector3d.h @@ -7,9 +7,7 @@ #include "Utils/Node.h" #include "Utils/Scalar.h" -namespace biorbd -{ -namespace BIORBD_MATH_NAMESPACE +namespace BIORBD_NAMESPACE { namespace utils { @@ -233,7 +231,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, void setType(); }; -} } } diff --git a/include/biorbdConfig.h.in b/include/biorbdConfig.h.in index 2660ccde..341930b5 100644 --- a/include/biorbdConfig.h.in +++ b/include/biorbdConfig.h.in @@ -22,7 +22,7 @@ // Choice of linear algebra backend #cmakedefine BIORBD_USE_EIGEN3_MATH #cmakedefine BIORBD_USE_CASADI_MATH -#define BIORBD_MATH_NAMESPACE Math@MATH_LIBRARY_BACKEND@ +#define BIORBD_NAMESPACE Biorbd@MATH_LIBRARY_BACKEND@ namespace biorbd { @@ -77,19 +77,19 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ #ifdef BIORBD_USE_CASADI_MATH #define DECLARE_GENERALIZED_COORDINATES(varname, model) \ casadi::DM varname(model.nbQ(), 1);\ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates varname##_sym(casadi::MX::sym(#varname, model.nbQ(), 1)); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates varname##_sym(casadi::MX::sym(#varname, model.nbQ(), 1)); #define DECLARE_GENERALIZED_VELOCITY(varname, model) \ casadi::DM varname(model.nbQdot(), 1);\ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity varname##_sym(casadi::MX::sym(#varname, model.nbQdot(), 1)); + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity varname##_sym(casadi::MX::sym(#varname, model.nbQdot(), 1)); #define DECLARE_GENERALIZED_ACCELERATION(varname, model) \ casadi::DM varname(model.nbQddot(), 1);\ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration varname##_sym(casadi::MX::sym(#varname, model.nbQddot(), 1)); + BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration varname##_sym(casadi::MX::sym(#varname, model.nbQddot(), 1)); #define DECLARE_GENERALIZED_TORQUE(varname, model) \ casadi::DM varname(model.nbGeneralizedTorque(), 1);\ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname##_sym(casadi::MX::sym(#varname, model.nbGeneralizedTorque(), 1)); + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque varname##_sym(casadi::MX::sym(#varname, model.nbGeneralizedTorque(), 1)); #define DECLARE_VECTOR(varname, nbElements) \ casadi::DM varname(nbElements, 1);\ - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector varname##_sym(casadi::MX::sym(#varname, nbElements, 1)); + BIORBD_NAMESPACE::utils::Vector varname##_sym(casadi::MX::sym(#varname, nbElements, 1)); #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \ casadi::Function func_##funcname(#funcname, {arg1##_sym}, {model.funcname(arg1##_sym)}, {#arg1}, {#funcname});\ @@ -115,15 +115,15 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){ #else #define DECLARE_GENERALIZED_COORDINATES(varname, model) \ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedCoordinates varname(model); + BIORBD_NAMESPACE::rigidbody::GeneralizedCoordinates varname(model); #define DECLARE_GENERALIZED_VELOCITY(varname, model) \ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedVelocity varname(model); + BIORBD_NAMESPACE::rigidbody::GeneralizedVelocity varname(model); #define DECLARE_GENERALIZED_ACCELERATION(varname, model) \ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration varname(model); + BIORBD_NAMESPACE::rigidbody::GeneralizedAcceleration varname(model); #define DECLARE_GENERALIZED_TORQUE(varname, model) \ - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedTorque varname(model); + BIORBD_NAMESPACE::rigidbody::GeneralizedTorque varname(model); #define DECLARE_VECTOR(varname, nbElements) \ - biorbd::BIORBD_MATH_NAMESPACE::utils::Vector varname(nbElements); + BIORBD_NAMESPACE::utils::Vector varname(nbElements); #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \ auto varname = model.funcname(arg1); diff --git a/src/Actuators/Actuator.cpp b/src/Actuators/Actuator.cpp index b66f3099..c170000b 100644 --- a/src/Actuators/Actuator.cpp +++ b/src/Actuators/Actuator.cpp @@ -4,7 +4,7 @@ #include "Utils/Error.h" #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::Actuator::Actuator() : m_type(std::make_shared diff --git a/src/Actuators/ActuatorConstant.cpp b/src/Actuators/ActuatorConstant.cpp index 2397747b..1c445585 100644 --- a/src/Actuators/ActuatorConstant.cpp +++ b/src/Actuators/ActuatorConstant.cpp @@ -4,7 +4,7 @@ #include "Utils/String.h" #include "RigidBody/GeneralizedCoordinates.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::ActuatorConstant::ActuatorConstant() : Actuator(), diff --git a/src/Actuators/ActuatorGauss3p.cpp b/src/Actuators/ActuatorGauss3p.cpp index 4c608b5b..5aae8503 100644 --- a/src/Actuators/ActuatorGauss3p.cpp +++ b/src/Actuators/ActuatorGauss3p.cpp @@ -5,7 +5,7 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::ActuatorGauss3p::ActuatorGauss3p() : actuator::Actuator(), diff --git a/src/Actuators/ActuatorGauss6p.cpp b/src/Actuators/ActuatorGauss6p.cpp index 29adf395..533b8939 100644 --- a/src/Actuators/ActuatorGauss6p.cpp +++ b/src/Actuators/ActuatorGauss6p.cpp @@ -5,7 +5,7 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::ActuatorGauss6p::ActuatorGauss6p() : actuator::Actuator(), diff --git a/src/Actuators/ActuatorLinear.cpp b/src/Actuators/ActuatorLinear.cpp index fe2de7b6..94f8409e 100644 --- a/src/Actuators/ActuatorLinear.cpp +++ b/src/Actuators/ActuatorLinear.cpp @@ -4,7 +4,7 @@ #include "Utils/Error.h" #include "RigidBody/GeneralizedCoordinates.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::ActuatorLinear::ActuatorLinear() : Actuator(), diff --git a/src/Actuators/ActuatorSigmoidGauss3p.cpp b/src/Actuators/ActuatorSigmoidGauss3p.cpp index ac4b500c..9556ef0d 100644 --- a/src/Actuators/ActuatorSigmoidGauss3p.cpp +++ b/src/Actuators/ActuatorSigmoidGauss3p.cpp @@ -5,7 +5,7 @@ #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::ActuatorSigmoidGauss3p::ActuatorSigmoidGauss3p() : actuator::Actuator(), diff --git a/src/Actuators/Actuators.cpp b/src/Actuators/Actuators.cpp index 717a34c1..880b930e 100644 --- a/src/Actuators/Actuators.cpp +++ b/src/Actuators/Actuators.cpp @@ -14,7 +14,7 @@ #include "Actuators/ActuatorConstant.h" #include "Actuators/ActuatorLinear.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; actuator::Actuators::Actuators() : m_all(std::make_shared, std::shared_ptr>>>()), @@ -109,8 +109,7 @@ void actuator::Actuators::addActuator(const actuator::Actuator !*m_isClose, "You can't add actuator after closing the model"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = dynamic_cast(*this); // Verify that the target dof is associated to a dof that // already exists in the model @@ -194,8 +193,7 @@ void actuator::Actuators::addActuator(const actuator::Actuator void actuator::Actuators::closeActuator() { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = dynamic_cast(*this); utils::Error::check( model.nbDof()==m_all->size(), @@ -263,8 +261,7 @@ actuator::Actuators::torqueMax( "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = dynamic_cast(*this); std::pair maxGeneralizedTorque_all = @@ -345,8 +342,8 @@ rigidbody::GeneralizedTorque actuator::Actuators::torqueMax( "Close the actuator model before calling torqueMax"); // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = + dynamic_cast(*this); // Set qdot to be positive if concentric and negative if excentric rigidbody::GeneralizedVelocity QdotResigned(Qdot); diff --git a/src/BiorbdModel.cpp b/src/BiorbdModel.cpp index e4f76af0..edf73537 100644 --- a/src/BiorbdModel.cpp +++ b/src/BiorbdModel.cpp @@ -8,7 +8,7 @@ #include "RigidBody/NodeSegment.h" #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::String getVersion() { diff --git a/src/ModelReader.cpp b/src/ModelReader.cpp index 1985b561..5f1c92c0 100644 --- a/src/ModelReader.cpp +++ b/src/ModelReader.cpp @@ -42,7 +42,7 @@ #include "Muscles/StateDynamicsBuchanan.h" #endif // MODULE_MUSCLES -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; // ------ Public methods ------ // Model Reader::readModelFile(const utils::Path &path) diff --git a/src/ModelWriter.cpp b/src/ModelWriter.cpp index 2b4d50c7..06a36cc8 100644 --- a/src/ModelWriter.cpp +++ b/src/ModelWriter.cpp @@ -13,7 +13,7 @@ #include "RigidBody/Mesh.h" #include "RigidBody/SegmentCharacteristics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; #ifndef BIORBD_USE_CASADI_MATH void Writer::writeModel(Model & model, diff --git a/src/Muscles/Characteristics.cpp b/src/Muscles/Characteristics.cpp index c280e853..68d3a76e 100644 --- a/src/Muscles/Characteristics.cpp +++ b/src/Muscles/Characteristics.cpp @@ -4,7 +4,7 @@ #include "Muscles/State.h" #include "Muscles/FatigueParameters.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::Characteristics::Characteristics() : m_optimalLength(std::make_shared(0)), diff --git a/src/Muscles/Compound.cpp b/src/Muscles/Compound.cpp index 6aacab1a..801267aa 100644 --- a/src/Muscles/Compound.cpp +++ b/src/Muscles/Compound.cpp @@ -8,7 +8,7 @@ #include "Muscles/PathModifiers.h" #include "Muscles/State.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::Compound::Compound() : m_name(std::make_shared("")), diff --git a/src/Muscles/FatigueDynamicState.cpp b/src/Muscles/FatigueDynamicState.cpp index 1f0d3745..28da0a00 100644 --- a/src/Muscles/FatigueDynamicState.cpp +++ b/src/Muscles/FatigueDynamicState.cpp @@ -3,7 +3,7 @@ #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::FatigueDynamicState::FatigueDynamicState( const utils::Scalar& active, diff --git a/src/Muscles/FatigueDynamicStateXia.cpp b/src/Muscles/FatigueDynamicStateXia.cpp index 71276557..49a9e3a4 100644 --- a/src/Muscles/FatigueDynamicStateXia.cpp +++ b/src/Muscles/FatigueDynamicStateXia.cpp @@ -7,7 +7,7 @@ #include "Muscles/FatigueParameters.h" #include "Muscles/StateDynamics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::FatigueDynamicStateXia::FatigueDynamicStateXia( const utils::Scalar& active, diff --git a/src/Muscles/FatigueModel.cpp b/src/Muscles/FatigueModel.cpp index ff6ca7eb..04e9fcb3 100644 --- a/src/Muscles/FatigueModel.cpp +++ b/src/Muscles/FatigueModel.cpp @@ -5,7 +5,7 @@ #include "Muscles/Muscle.h" #include "Muscles/FatigueDynamicStateXia.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::FatigueModel::FatigueModel( muscles::STATE_FATIGUE_TYPE dynamicFatigueType) diff --git a/src/Muscles/FatigueParameters.cpp b/src/Muscles/FatigueParameters.cpp index 5ac1e54d..663b89da 100644 --- a/src/Muscles/FatigueParameters.cpp +++ b/src/Muscles/FatigueParameters.cpp @@ -1,7 +1,7 @@ #define BIORBD_API_EXPORTS #include "Muscles/FatigueParameters.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::FatigueParameters::FatigueParameters( const utils::Scalar& _fatigueRate, diff --git a/src/Muscles/FatigueState.cpp b/src/Muscles/FatigueState.cpp index fb0849bb..939da5d1 100644 --- a/src/Muscles/FatigueState.cpp +++ b/src/Muscles/FatigueState.cpp @@ -17,7 +17,7 @@ template < typename T > std::string to_string( const T& n ) } #endif -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::FatigueState::FatigueState( const utils::Scalar& active, diff --git a/src/Muscles/Geometry.cpp b/src/Muscles/Geometry.cpp index 18d73d21..e81646ae 100644 --- a/src/Muscles/Geometry.cpp +++ b/src/Muscles/Geometry.cpp @@ -16,7 +16,7 @@ #include "Muscles/ViaPoint.h" #include "Muscles/PathModifiers.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::Geometry::Geometry() : m_origin(std::make_shared()), @@ -99,7 +99,7 @@ void muscles::Geometry::DeepCopy(const muscles::Geometry &other) // ------ PUBLIC FUNCTIONS ------ // void muscles::Geometry::updateKinematics( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates *Q, const rigidbody::GeneralizedVelocity *Qdot, int updateKin) @@ -130,7 +130,7 @@ void muscles::Geometry::updateKinematics( _updateKinematics(Qdot); } -void muscles::Geometry::updateKinematics(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints +void muscles::Geometry::updateKinematics(rigidbody::Joints &model, const muscles::Characteristics& characteristics, muscles::PathModifiers &pathModifiers, @@ -329,7 +329,7 @@ void muscles::Geometry::_updateKinematics( } const utils::Vector3d &muscles::Geometry::originInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position @@ -340,7 +340,7 @@ const utils::Vector3d &muscles::Geometry::originInGlobal( } const utils::Vector3d &muscles::Geometry::insertionInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q) { // Return the position of the marker in function of the given position @@ -360,7 +360,7 @@ void muscles::Geometry::setMusclesPointsInGlobal( } void muscles::Geometry::setMusclesPointsInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, muscles::PathModifiers *pathModifiers) { @@ -491,7 +491,7 @@ const utils::Scalar& muscles::Geometry::velocity( return *m_velocity; } -void muscles::Geometry::setJacobianDimension(biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints +void muscles::Geometry::setJacobianDimension(rigidbody::Joints &model) { *m_jacobian = utils::Matrix::Zero(static_cast @@ -507,7 +507,7 @@ void muscles::Geometry::jacobian(const utils::Matrix &jaco) } void muscles::Geometry::jacobian( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q) { for (unsigned int i=0; isize(); ++i) { diff --git a/src/Muscles/HillThelenActiveOnlyType.cpp b/src/Muscles/HillThelenActiveOnlyType.cpp index cb656718..56e4749e 100644 --- a/src/Muscles/HillThelenActiveOnlyType.cpp +++ b/src/Muscles/HillThelenActiveOnlyType.cpp @@ -6,7 +6,7 @@ #include "Muscles/Geometry.h" #include "Muscles/Characteristics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::HillThelenActiveOnlyType::HillThelenActiveOnlyType() : muscles::HillThelenType() diff --git a/src/Muscles/HillThelenType.cpp b/src/Muscles/HillThelenType.cpp index b2c817b7..2adf73c1 100644 --- a/src/Muscles/HillThelenType.cpp +++ b/src/Muscles/HillThelenType.cpp @@ -6,7 +6,7 @@ #include "Muscles/Geometry.h" #include "Muscles/Characteristics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::HillThelenType::HillThelenType() : muscles::HillType() diff --git a/src/Muscles/HillThelenTypeFatigable.cpp b/src/Muscles/HillThelenTypeFatigable.cpp index 05114b04..78f3bde8 100644 --- a/src/Muscles/HillThelenTypeFatigable.cpp +++ b/src/Muscles/HillThelenTypeFatigable.cpp @@ -4,7 +4,7 @@ #include "Utils/String.h" #include "Muscles/FatigueState.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::HillThelenTypeFatigable::HillThelenTypeFatigable() : muscles::HillThelenType(), diff --git a/src/Muscles/HillType.cpp b/src/Muscles/HillType.cpp index a1128644..067b8b3a 100644 --- a/src/Muscles/HillType.cpp +++ b/src/Muscles/HillType.cpp @@ -8,7 +8,7 @@ #include "Muscles/Geometry.h" #include "Muscles/State.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::HillType::HillType() : muscles::Muscle(), @@ -207,7 +207,7 @@ const utils::Scalar& muscles::HillType::force( } const utils::Scalar& muscles::HillType::force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const muscles::State &emg, @@ -231,7 +231,7 @@ const utils::Scalar& muscles::HillType::force( } const utils::Scalar& muscles::HillType::force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, + rigidbody::Joints &, const rigidbody::GeneralizedCoordinates &, const muscles::State &, int) diff --git a/src/Muscles/IdealizedActuator.cpp b/src/Muscles/IdealizedActuator.cpp index 7c5a28b8..838c35a6 100644 --- a/src/Muscles/IdealizedActuator.cpp +++ b/src/Muscles/IdealizedActuator.cpp @@ -5,7 +5,7 @@ #include "Muscles/Characteristics.h" #include "Muscles/State.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::IdealizedActuator::IdealizedActuator() : muscles::Muscle() @@ -89,7 +89,7 @@ const utils::Scalar& muscles::IdealizedActuator::force( } const utils::Scalar& muscles::IdealizedActuator::force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, + rigidbody::Joints &, const rigidbody::GeneralizedCoordinates &, const rigidbody::GeneralizedVelocity &, const muscles::State &emg, @@ -100,7 +100,7 @@ const utils::Scalar& muscles::IdealizedActuator::force( } const utils::Scalar& muscles::IdealizedActuator::force( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, + rigidbody::Joints &, const rigidbody::GeneralizedCoordinates &, const muscles::State &emg, int) diff --git a/src/Muscles/Muscle.cpp b/src/Muscles/Muscle.cpp index 3732f442..ba789031 100644 --- a/src/Muscles/Muscle.cpp +++ b/src/Muscles/Muscle.cpp @@ -12,7 +12,7 @@ #include "Muscles/Characteristics.h" #include "Muscles/Geometry.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::Muscle::Muscle() : muscles::Compound(), @@ -113,7 +113,7 @@ void muscles::Muscle::DeepCopy(const muscles::Muscle &other) } void muscles::Muscle::updateOrientations( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -122,7 +122,7 @@ void muscles::Muscle::updateOrientations( updateKin); } void muscles::Muscle::updateOrientations( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, int updateKin) @@ -160,7 +160,7 @@ const muscles::Geometry &muscles::Muscle::position() const } const utils::Scalar& muscles::Muscle::length( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -176,7 +176,7 @@ const utils::Scalar& muscles::Muscle::length( } const utils::Scalar& muscles::Muscle::musculoTendonLength( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &m, + rigidbody::Joints &m, const rigidbody::GeneralizedCoordinates &Q, int updateKin) { @@ -192,7 +192,7 @@ const utils::Scalar& muscles::Muscle::musculoTendonLength( } const utils::Scalar& muscles::Muscle::velocity( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, bool updateKin) @@ -228,7 +228,7 @@ void muscles::Muscle::computeForce(const muscles::State &emg) const std::vector& muscles::Muscle::musclesPointsInGlobal( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q) { m_position->updateKinematics(model,*m_characteristics,*m_pathChanger,&Q, diff --git a/src/Muscles/MuscleGroup.cpp b/src/Muscles/MuscleGroup.cpp index b7cb5bf3..cff2f03b 100644 --- a/src/Muscles/MuscleGroup.cpp +++ b/src/Muscles/MuscleGroup.cpp @@ -10,7 +10,7 @@ #include "Muscles/StateDynamicsBuchanan.h" #include "Muscles/StateDynamicsDeGroote.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::MuscleGroup::MuscleGroup() : m_mus(std::make_shared>>()), diff --git a/src/Muscles/Muscles.cpp b/src/Muscles/Muscles.cpp index 6e93e2cd..a98876a5 100644 --- a/src/Muscles/Muscles.cpp +++ b/src/Muscles/Muscles.cpp @@ -12,7 +12,7 @@ #include "Muscles/MuscleGroup.h" #include "Muscles/StateDynamics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::Muscles::Muscles() : m_mus(std::make_shared>()) @@ -240,8 +240,7 @@ unsigned int muscles::Muscles::nbMuscleGroups() const utils::Matrix muscles::Muscles::musclesLengthJacobian() { // Assuming that this is also a Joints type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = dynamic_cast(*this); utils::Matrix tp(nbMuscleTotal(), model.nbDof()); unsigned int cmpMus(0); @@ -284,7 +283,7 @@ void muscles::Muscles::updateMuscles( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); // Update all the muscles @@ -306,7 +305,7 @@ void muscles::Muscles::updateMuscles( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); // Update all the muscles diff --git a/src/Muscles/PathModifiers.cpp b/src/Muscles/PathModifiers.cpp index 6d6d1ba0..cf62e364 100644 --- a/src/Muscles/PathModifiers.cpp +++ b/src/Muscles/PathModifiers.cpp @@ -7,7 +7,7 @@ #include "Muscles/WrappingSphere.h" #include "Muscles/WrappingHalfCylinder.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::PathModifiers::PathModifiers() : m_obj(std::make_shared>>()), diff --git a/src/Muscles/State.cpp b/src/Muscles/State.cpp index 46606c17..af7869aa 100644 --- a/src/Muscles/State.cpp +++ b/src/Muscles/State.cpp @@ -3,7 +3,7 @@ #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::State::State( const utils::Scalar& excitation, diff --git a/src/Muscles/StateDynamics.cpp b/src/Muscles/StateDynamics.cpp index b465982b..a0e67a43 100644 --- a/src/Muscles/StateDynamics.cpp +++ b/src/Muscles/StateDynamics.cpp @@ -5,7 +5,7 @@ #include "Utils/String.h" #include "Muscles/Characteristics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StateDynamics::StateDynamics( const utils::Scalar& excitation, diff --git a/src/Muscles/StateDynamicsBuchanan.cpp b/src/Muscles/StateDynamicsBuchanan.cpp index 1c8a7a8d..21c983d7 100644 --- a/src/Muscles/StateDynamicsBuchanan.cpp +++ b/src/Muscles/StateDynamicsBuchanan.cpp @@ -3,7 +3,7 @@ #include -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StateDynamicsBuchanan::StateDynamicsBuchanan( const utils::Scalar& neuralCommand, diff --git a/src/Muscles/StateDynamicsDeGroote.cpp b/src/Muscles/StateDynamicsDeGroote.cpp index 55d1c2be..b7c92dd6 100644 --- a/src/Muscles/StateDynamicsDeGroote.cpp +++ b/src/Muscles/StateDynamicsDeGroote.cpp @@ -6,7 +6,7 @@ #include "Utils/String.h" #include "Muscles/Characteristics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StateDynamicsDeGroote::StateDynamicsDeGroote( const utils::Scalar& excitation, diff --git a/src/Muscles/StaticOptimization.cpp b/src/Muscles/StaticOptimization.cpp index 9b760452..e57444f9 100644 --- a/src/Muscles/StaticOptimization.cpp +++ b/src/Muscles/StaticOptimization.cpp @@ -11,7 +11,7 @@ #include "Muscles/StateDynamics.h" #include "Muscles/StaticOptimizationIpoptLinearized.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StaticOptimization::StaticOptimization( Model& model, diff --git a/src/Muscles/StaticOptimizationIpopt.cpp b/src/Muscles/StaticOptimizationIpopt.cpp index fb6e9be2..1cd17d55 100644 --- a/src/Muscles/StaticOptimizationIpopt.cpp +++ b/src/Muscles/StaticOptimizationIpopt.cpp @@ -12,7 +12,7 @@ #include "RigidBody/GeneralizedTorque.h" #include "Muscles/StateDynamics.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StaticOptimizationIpopt::StaticOptimizationIpopt( Model &model, diff --git a/src/Muscles/StaticOptimizationIpoptLinearized.cpp b/src/Muscles/StaticOptimizationIpoptLinearized.cpp index 32eca568..99a7dba3 100644 --- a/src/Muscles/StaticOptimizationIpoptLinearized.cpp +++ b/src/Muscles/StaticOptimizationIpoptLinearized.cpp @@ -6,7 +6,7 @@ #include "RigidBody/GeneralizedTorque.h" #include "Muscles/State.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::StaticOptimizationIpoptLinearized::StaticOptimizationIpoptLinearized( Model &model, diff --git a/src/Muscles/ViaPoint.cpp b/src/Muscles/ViaPoint.cpp index f3a4abc9..30001329 100644 --- a/src/Muscles/ViaPoint.cpp +++ b/src/Muscles/ViaPoint.cpp @@ -3,7 +3,7 @@ #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::ViaPoint::ViaPoint() : utils::Vector3d() diff --git a/src/Muscles/WrappingHalfCylinder.cpp b/src/Muscles/WrappingHalfCylinder.cpp index d113e8c8..6ca821ce 100644 --- a/src/Muscles/WrappingHalfCylinder.cpp +++ b/src/Muscles/WrappingHalfCylinder.cpp @@ -5,7 +5,7 @@ #include "Utils/RotoTrans.h" #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::WrappingHalfCylinder::WrappingHalfCylinder() : muscles::WrappingObject (), @@ -149,7 +149,7 @@ void muscles::WrappingHalfCylinder::wrapPoints( } void muscles::WrappingHalfCylinder::wrapPoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::GeneralizedCoordinates& Q, const utils::Vector3d& p1_bone, const utils::Vector3d& p2_bone, @@ -175,7 +175,7 @@ void muscles::WrappingHalfCylinder::wrapPoints( } const utils::RotoTrans& muscles::WrappingHalfCylinder::RT( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model, + rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates& Q, bool updateKin) { diff --git a/src/Muscles/WrappingObject.cpp b/src/Muscles/WrappingObject.cpp index 2d799f6e..76c452b3 100644 --- a/src/Muscles/WrappingObject.cpp +++ b/src/Muscles/WrappingObject.cpp @@ -4,7 +4,7 @@ #include "Utils/String.h" #include "Utils/RotoTrans.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::WrappingObject::WrappingObject() : utils::Vector3d (), diff --git a/src/Muscles/WrappingSphere.cpp b/src/Muscles/WrappingSphere.cpp index 50fbec64..4f518a57 100644 --- a/src/Muscles/WrappingSphere.cpp +++ b/src/Muscles/WrappingSphere.cpp @@ -4,7 +4,7 @@ #include "Utils/String.h" #include "Utils/RotoTrans.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; muscles::WrappingSphere::WrappingSphere() : muscles::WrappingObject (), @@ -63,7 +63,7 @@ void muscles::WrappingSphere::DeepCopy(const } const utils::RotoTrans& muscles::WrappingSphere::RT( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &, + rigidbody::Joints &, const rigidbody::GeneralizedCoordinates &, bool) { diff --git a/src/RigidBody/Contacts.cpp b/src/RigidBody/Contacts.cpp index 3107f541..970895e6 100644 --- a/src/RigidBody/Contacts.cpp +++ b/src/RigidBody/Contacts.cpp @@ -11,7 +11,7 @@ #include "Utils/SpatialVector.h" #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::Contacts::Contacts() : RigidBodyDynamics::ConstraintSet (), diff --git a/src/RigidBody/GeneralizedAcceleration.cpp b/src/RigidBody/GeneralizedAcceleration.cpp index a4eef1a0..254792e5 100644 --- a/src/RigidBody/GeneralizedAcceleration.cpp +++ b/src/RigidBody/GeneralizedAcceleration.cpp @@ -3,7 +3,7 @@ #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::GeneralizedAcceleration::GeneralizedAcceleration() { @@ -18,7 +18,7 @@ rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( } rigidbody::GeneralizedAcceleration::GeneralizedAcceleration( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : + const rigidbody::Joints &j) : utils::Vector(j.nbQ()) { diff --git a/src/RigidBody/GeneralizedCoordinates.cpp b/src/RigidBody/GeneralizedCoordinates.cpp index 9382cba4..3abdf638 100644 --- a/src/RigidBody/GeneralizedCoordinates.cpp +++ b/src/RigidBody/GeneralizedCoordinates.cpp @@ -3,7 +3,7 @@ #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::GeneralizedCoordinates::GeneralizedCoordinates() { @@ -18,7 +18,7 @@ rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( } rigidbody::GeneralizedCoordinates::GeneralizedCoordinates( - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &j) : + const rigidbody::Joints &j) : utils::Vector(j.nbQ()) { diff --git a/src/RigidBody/GeneralizedTorque.cpp b/src/RigidBody/GeneralizedTorque.cpp index 01575322..106116c0 100644 --- a/src/RigidBody/GeneralizedTorque.cpp +++ b/src/RigidBody/GeneralizedTorque.cpp @@ -3,7 +3,7 @@ #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::GeneralizedTorque::GeneralizedTorque() { diff --git a/src/RigidBody/GeneralizedVelocity.cpp b/src/RigidBody/GeneralizedVelocity.cpp index e65bde07..6bfc8a23 100644 --- a/src/RigidBody/GeneralizedVelocity.cpp +++ b/src/RigidBody/GeneralizedVelocity.cpp @@ -3,7 +3,7 @@ #include "RigidBody/Joints.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::GeneralizedVelocity::GeneralizedVelocity() { diff --git a/src/RigidBody/IMU.cpp b/src/RigidBody/IMU.cpp index ed96885f..b30d61b5 100644 --- a/src/RigidBody/IMU.cpp +++ b/src/RigidBody/IMU.cpp @@ -3,7 +3,7 @@ #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::IMU::IMU( bool isTechnical, diff --git a/src/RigidBody/IMUs.cpp b/src/RigidBody/IMUs.cpp index 3c9e62b7..d579e6fe 100644 --- a/src/RigidBody/IMUs.cpp +++ b/src/RigidBody/IMUs.cpp @@ -11,7 +11,7 @@ #include "RigidBody/Segment.h" #include "RigidBody/IMU.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::IMUs::IMUs() : m_IMUs(std::make_shared>()) @@ -111,8 +111,7 @@ rigidbody::IMU rigidbody::IMUs::IMU( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast - (*this); + rigidbody::Joints &model = dynamic_cast(*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif @@ -181,7 +180,7 @@ std::vector rigidbody::IMUs::segmentIMU( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); // Segment name to find @@ -223,7 +222,7 @@ std::vector rigidbody::IMUs::IMUJacobian( bool lookForTechnical) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); std::vector G; diff --git a/src/RigidBody/Joints.cpp b/src/RigidBody/Joints.cpp index a232b388..d629a745 100644 --- a/src/RigidBody/Joints.cpp +++ b/src/RigidBody/Joints.cpp @@ -24,7 +24,7 @@ #include "RigidBody/SegmentCharacteristics.h" #include "RigidBody/Contacts.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::Joints::Joints() : RigidBodyDynamics::Model(), @@ -625,7 +625,7 @@ utils::Vector3d rigidbody::Joints::CoMdot( utils::Vector3d rigidbody::Joints::CoMddot( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -750,7 +750,7 @@ std::vector rigidbody::Joints::CoMddotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { std::vector out; @@ -765,7 +765,7 @@ rigidbody::Joints::CoMddotBySegment( utils::Vector3d rigidbody::Joints::CoMddotBySegment( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, const unsigned int idx, bool updateKin) { @@ -915,7 +915,7 @@ utils::Vector3d rigidbody::Joints::CalcAngularMomentum ( utils::Vector3d rigidbody::Joints::CalcAngularMomentum ( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { // Definition of the variables @@ -978,7 +978,7 @@ std::vector rigidbody::Joints::CalcSegmentsAngularMomentum ( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool updateKin) { #ifdef BIORBD_USE_CASADI_MATH @@ -1067,7 +1067,7 @@ rigidbody::GeneralizedVelocity rigidbody::Joints::computeQdot( rigidbody::GeneralizedTorque rigidbody::Joints::InverseDynamics( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &QDDot, + const rigidbody::GeneralizedAcceleration &QDDot, std::vector* f_ext) { rigidbody::GeneralizedTorque Tau(nbGeneralizedTorque()); @@ -1097,7 +1097,7 @@ rigidbody::GeneralizedTorque rigidbody::Joints::NonLinearEffect( return Tau; } -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamics( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, @@ -1108,7 +1108,7 @@ rigidbody::Joints::ForwardDynamics( UpdateKinematicsCustom(&Q, &QDot); #endif - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(*this); + rigidbody::GeneralizedAcceleration QDDot(*this); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1119,7 +1119,7 @@ rigidbody::Joints::ForwardDynamics( return QDDot; } -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamicsConstraintsDirect( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, @@ -1131,7 +1131,7 @@ rigidbody::Joints::ForwardDynamicsConstraintsDirect( UpdateKinematicsCustom(&Q, &QDot); #endif - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration QDDot(*this); + rigidbody::GeneralizedAcceleration QDDot(*this); if (f_ext) { std::vector f_ext_rbdl(dispatchedForce( *f_ext)); @@ -1157,7 +1157,7 @@ rigidbody::Joints::ContactForcesFromForwardDynamicsConstraintsDirect( return CS.getForce(); } -biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration +rigidbody::GeneralizedAcceleration rigidbody::Joints::ForwardDynamicsConstraintsDirect( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &QDot, @@ -1219,7 +1219,7 @@ unsigned int rigidbody::Joints::getDofIndex( void rigidbody::Joints::UpdateKinematicsCustom( const rigidbody::GeneralizedCoordinates *Q, const rigidbody::GeneralizedVelocity *Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) + const rigidbody::GeneralizedAcceleration *Qddot) { checkGeneralizedDimensions(Q, Qdot, Qddot); RigidBodyDynamics::UpdateKinematicsCustom(*this, Q, Qdot, Qddot); @@ -1301,7 +1301,7 @@ void rigidbody::Joints::CalcMatRotJacobian( void rigidbody::Joints::checkGeneralizedDimensions( const rigidbody::GeneralizedCoordinates *Q, const rigidbody::GeneralizedVelocity *Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + const rigidbody::GeneralizedAcceleration *Qddot, const rigidbody::GeneralizedTorque *torque) { #ifndef SKIP_ASSERT diff --git a/src/RigidBody/KalmanRecons.cpp b/src/RigidBody/KalmanRecons.cpp index 96440769..415df7db 100644 --- a/src/RigidBody/KalmanRecons.cpp +++ b/src/RigidBody/KalmanRecons.cpp @@ -8,7 +8,7 @@ #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedAcceleration.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::KalmanRecons::KalmanRecons() : m_params(std::make_shared()), @@ -99,7 +99,7 @@ void rigidbody::KalmanRecons::manageOcclusionDuringIteration( void rigidbody::KalmanRecons::getState( rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) + rigidbody::GeneralizedAcceleration *Qddot) { if (Q != nullptr) { @@ -215,7 +215,7 @@ rigidbody::KalmanRecons::initState( void rigidbody::KalmanRecons::setInitState( const rigidbody::GeneralizedCoordinates *Q, const rigidbody::GeneralizedVelocity *Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) + const rigidbody::GeneralizedAcceleration *Qddot) { if (Q != nullptr) { m_xp->block(0, 0, *m_nbDof, 1) = *Q; diff --git a/src/RigidBody/KalmanReconsIMU.cpp b/src/RigidBody/KalmanReconsIMU.cpp index 98145439..6ec36795 100644 --- a/src/RigidBody/KalmanReconsIMU.cpp +++ b/src/RigidBody/KalmanReconsIMU.cpp @@ -12,7 +12,7 @@ #include "RigidBody/GeneralizedAcceleration.h" #include "RigidBody/IMU.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::KalmanReconsIMU::KalmanReconsIMU() : rigidbody::KalmanRecons(), @@ -80,7 +80,7 @@ void rigidbody::KalmanReconsIMU::reconstructFrame( const std::vector &IMUobs, rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) + rigidbody::GeneralizedAcceleration *Qddot) { // Separate the IMUobs in a big vector utils::Vector T(static_cast @@ -100,7 +100,7 @@ void rigidbody::KalmanReconsIMU::reconstructFrame( const utils::Vector &IMUobs, rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot) + rigidbody::GeneralizedAcceleration *Qddot) { // An iteration of the Kalman filter if (*m_firstIteration) { diff --git a/src/RigidBody/KalmanReconsMarkers.cpp b/src/RigidBody/KalmanReconsMarkers.cpp index 09c6cc25..43636058 100644 --- a/src/RigidBody/KalmanReconsMarkers.cpp +++ b/src/RigidBody/KalmanReconsMarkers.cpp @@ -13,7 +13,7 @@ #include -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::KalmanReconsMarkers::KalmanReconsMarkers() : rigidbody::KalmanRecons(), @@ -83,7 +83,7 @@ void rigidbody::KalmanReconsMarkers::reconstructFrame( const rigidbody::Markers &Tobs, rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector @@ -101,7 +101,7 @@ void rigidbody::KalmanReconsMarkers::reconstructFrame( const std::vector &Tobs, rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // Separate the tobs in a big vector @@ -120,7 +120,7 @@ void rigidbody::KalmanReconsMarkers::reconstructFrame( const utils::Vector &Tobs, rigidbody::GeneralizedCoordinates *Q, rigidbody::GeneralizedVelocity *Qdot, - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration *Qddot, + rigidbody::GeneralizedAcceleration *Qddot, bool removeAxes) { // An iteration of the Kalman filter diff --git a/src/RigidBody/Markers.cpp b/src/RigidBody/Markers.cpp index 438c6f2c..468a040c 100644 --- a/src/RigidBody/Markers.cpp +++ b/src/RigidBody/Markers.cpp @@ -12,7 +12,7 @@ #include "RigidBody/NodeSegment.h" #include "RigidBody/Segment.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::Markers::Markers() : m_marks(std::make_shared>()) @@ -89,8 +89,7 @@ rigidbody::NodeSegment rigidbody::Markers::marker( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast - (*this); + rigidbody::Joints &model = dynamic_cast(*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif @@ -114,8 +113,7 @@ rigidbody::NodeSegment rigidbody::Markers::marker( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast - (*this); + rigidbody::Joints &model = dynamic_cast(*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif @@ -178,8 +176,7 @@ rigidbody::NodeSegment rigidbody::Markers::markerVelocity( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast - (*this); + rigidbody::Joints &model = dynamic_cast(*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif @@ -215,13 +212,13 @@ rigidbody::Markers::markersVelocity( rigidbody::NodeSegment rigidbody::Markers::markerAcceleration( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, unsigned int idx, bool removeAxis, bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -243,7 +240,7 @@ std::vector rigidbody::Markers::markerAcceleration( const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::GeneralizedAcceleration &Qddot, + const rigidbody::GeneralizedAcceleration &Qddot, bool removeAxis, bool updateKin) { @@ -323,7 +320,7 @@ rigidbody::Markers::segmentMarkers( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); // Name of the segment to find @@ -350,8 +347,8 @@ unsigned int rigidbody::Markers::nbMarkers(unsigned int idxSegment) const { // Assuming that this is also a joint type (via BiorbdModel) - const biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = - dynamic_cast(*this); + const rigidbody::Joints &model = + dynamic_cast(*this); // Name of the segment to find const utils::String& name(model.segment(idxSegment).name()); @@ -392,7 +389,7 @@ utils::Matrix rigidbody::Markers::markersJacobian( bool updateKin) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -413,7 +410,7 @@ bool rigidbody::Markers::inverseKinematics( rigidbody::GeneralizedCoordinates &Q, bool removeAxes) { - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); model.UpdateKinematicsCustom(&Q); // also assert for dimensions @@ -439,7 +436,7 @@ bool rigidbody::Markers::inverseKinematics( // Call the base function return RigidBodyDynamics::InverseKinematics( - dynamic_cast(*this), + dynamic_cast(*this), Qinit, body_id, body_pointEigen, markersInRbdl, Q); } #endif @@ -452,7 +449,7 @@ std::vector rigidbody::Markers::markersJacobian( bool lookForTechnical) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; @@ -499,7 +496,7 @@ unsigned int rigidbody::Markers::nbTechnicalMarkers( unsigned int idxSegment) { // Assuming that this is also a joint type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); unsigned int nTechMarkers = 0; diff --git a/src/RigidBody/Mesh.cpp b/src/RigidBody/Mesh.cpp index 549ed1e0..aa9fadd6 100644 --- a/src/RigidBody/Mesh.cpp +++ b/src/RigidBody/Mesh.cpp @@ -5,7 +5,7 @@ #include "Utils/Vector3d.h" #include "RigidBody/MeshFace.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::Mesh::Mesh() : m_vertex(std::make_shared>()), diff --git a/src/RigidBody/MeshFace.cpp b/src/RigidBody/MeshFace.cpp index 097cbc3d..50fba52d 100644 --- a/src/RigidBody/MeshFace.cpp +++ b/src/RigidBody/MeshFace.cpp @@ -3,7 +3,7 @@ #include "Utils/Vector3d.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::MeshFace::MeshFace(const std::vector& vertex) : m_face(std::make_shared>(vertex)) diff --git a/src/RigidBody/NodeSegment.cpp b/src/RigidBody/NodeSegment.cpp index f5fb10f7..d0deb696 100644 --- a/src/RigidBody/NodeSegment.cpp +++ b/src/RigidBody/NodeSegment.cpp @@ -3,7 +3,7 @@ #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::NodeSegment::NodeSegment() : utils::Vector3d(0, 0, 0), diff --git a/src/RigidBody/RotoTransNodes.cpp b/src/RigidBody/RotoTransNodes.cpp index 286556ae..da828d77 100644 --- a/src/RigidBody/RotoTransNodes.cpp +++ b/src/RigidBody/RotoTransNodes.cpp @@ -11,7 +11,7 @@ #include "RigidBody/Joints.h" #include "RigidBody/Segment.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::RotoTransNodes::RotoTransNodes() : m_RTs(std::make_shared>()) @@ -117,8 +117,7 @@ utils::RotoTransNode rigidbody::RotoTransNodes::RT( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast - (*this); + rigidbody::Joints &model = dynamic_cast(*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; #endif @@ -140,7 +139,7 @@ rigidbody::RotoTransNodes::segmentRTs( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); // Segment name to find @@ -164,7 +163,7 @@ rigidbody::RotoTransNodes::RTsJacobian( bool updateKin) { // Assuming that this is also a Joints type (via BiorbdModel) - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints &model = dynamic_cast + rigidbody::Joints &model = dynamic_cast (*this); #ifdef BIORBD_USE_CASADI_MATH updateKin = true; diff --git a/src/RigidBody/Segment.cpp b/src/RigidBody/Segment.cpp index 81c42b80..b1db57fd 100644 --- a/src/RigidBody/Segment.cpp +++ b/src/RigidBody/Segment.cpp @@ -11,7 +11,7 @@ #include "RigidBody/SegmentCharacteristics.h" #include "Utils/Range.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::Segment::Segment() : utils::Node(), @@ -47,7 +47,7 @@ rigidbody::Segment::Segment() : } rigidbody::Segment::Segment( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &name, const utils::String &parentName, const utils::String &seqT, @@ -95,7 +95,7 @@ rigidbody::Segment::Segment( setPF(PF); } rigidbody::Segment::Segment( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &name, // Name of segment const utils::String &parentName, // Name of segment const utils::String @@ -300,7 +300,7 @@ utils::RotoTrans rigidbody::Segment::localJCS() const } void rigidbody::Segment::updateCharacteristics( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const rigidbody::SegmentCharacteristics& characteristics) { @@ -322,7 +322,7 @@ rigidbody::Segment::characteristics() const } void rigidbody::Segment::setDofs( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model, + rigidbody::Joints& model, const utils::String &seqT, const utils::String &seqR, const std::vector& QRanges, @@ -522,7 +522,7 @@ void rigidbody::Segment::setJointAxis() } void rigidbody::Segment::setJoints( - biorbd::BIORBD_MATH_NAMESPACE::rigidbody::Joints& model) + rigidbody::Joints& model) { setDofCharacteristicsOnLastBody(); // Apply the segment caracteristics only to the last segment setJointAxis(); // Choose the axis order in relation to the selected sequence diff --git a/src/RigidBody/SegmentCharacteristics.cpp b/src/RigidBody/SegmentCharacteristics.cpp index a608d533..e888cc42 100644 --- a/src/RigidBody/SegmentCharacteristics.cpp +++ b/src/RigidBody/SegmentCharacteristics.cpp @@ -6,7 +6,7 @@ #include "RigidBody/MeshFace.h" #include "RigidBody/Mesh.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; rigidbody::SegmentCharacteristics::SegmentCharacteristics() : Body(), diff --git a/src/Utils/Benchmark.cpp b/src/Utils/Benchmark.cpp index 62b2057f..4d490db7 100644 --- a/src/Utils/Benchmark.cpp +++ b/src/Utils/Benchmark.cpp @@ -4,7 +4,7 @@ #include "Utils/Timer.h" #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Benchmark::Benchmark() : m_timers(std::map()), diff --git a/src/Utils/Equation.cpp b/src/Utils/Equation.cpp index 7898ff80..50b41fdc 100644 --- a/src/Utils/Equation.cpp +++ b/src/Utils/Equation.cpp @@ -8,7 +8,7 @@ #include #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; std::vector utils::Equation::prepareMathSymbols() diff --git a/src/Utils/Error.cpp b/src/Utils/Error.cpp index cc03a108..d2b891fb 100644 --- a/src/Utils/Error.cpp +++ b/src/Utils/Error.cpp @@ -1,7 +1,7 @@ #define BIORBD_API_EXPORTS #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; void utils::Error::raise( const utils::String &message) diff --git a/src/Utils/IfStream.cpp b/src/Utils/IfStream.cpp index c2823b24..ac763a0d 100644 --- a/src/Utils/IfStream.cpp +++ b/src/Utils/IfStream.cpp @@ -6,7 +6,7 @@ #include "Utils/Error.h" #include "Utils/Equation.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; // Constructor utils::IfStream::IfStream() : diff --git a/src/Utils/Matrix.cpp b/src/Utils/Matrix.cpp index b9beae0e..473b4b91 100644 --- a/src/Utils/Matrix.cpp +++ b/src/Utils/Matrix.cpp @@ -3,7 +3,7 @@ #include "Utils/Vector.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Matrix::Matrix() : RigidBodyDynamics::Math::MatrixNd() diff --git a/src/Utils/Node.cpp b/src/Utils/Node.cpp index 15f93828..41d48f0a 100644 --- a/src/Utils/Node.cpp +++ b/src/Utils/Node.cpp @@ -3,7 +3,7 @@ #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Node::Node() : m_name(std::make_shared("")), diff --git a/src/Utils/Path.cpp b/src/Utils/Path.cpp index c430b9a3..90846e5c 100644 --- a/src/Utils/Path.cpp +++ b/src/Utils/Path.cpp @@ -16,7 +16,7 @@ #include #endif -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Path::Path() : m_originalPath(std::make_shared()), diff --git a/src/Utils/Quaternion.cpp b/src/Utils/Quaternion.cpp index bed47829..d437ec80 100644 --- a/src/Utils/Quaternion.cpp +++ b/src/Utils/Quaternion.cpp @@ -7,7 +7,7 @@ #include "Utils/Error.h" #include "Utils/Rotation.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Quaternion::Quaternion ( double kStabilizer) : diff --git a/src/Utils/Range.cpp b/src/Utils/Range.cpp index 7c454a68..2bd700af 100644 --- a/src/Utils/Range.cpp +++ b/src/Utils/Range.cpp @@ -1,7 +1,7 @@ #define BIORBD_API_EXPORTS #include "Utils/Range.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Range::Range( double min, diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 0dcac383..3316a49b 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -9,7 +9,7 @@ #include "RigidBody/NodeSegment.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Rotation::Rotation( const RigidBodyDynamics::Math::Matrix3d& matrix) : diff --git a/src/Utils/RotoTrans.cpp b/src/Utils/RotoTrans.cpp index dbe46637..77911824 100644 --- a/src/Utils/RotoTrans.cpp +++ b/src/Utils/RotoTrans.cpp @@ -9,7 +9,7 @@ #include "RigidBody/NodeSegment.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::RotoTrans::RotoTrans( const RigidBodyDynamics::Math::Matrix4d& matrix) : diff --git a/src/Utils/RotoTransNode.cpp b/src/Utils/RotoTransNode.cpp index e1c41f76..b2932f2c 100644 --- a/src/Utils/RotoTransNode.cpp +++ b/src/Utils/RotoTransNode.cpp @@ -3,7 +3,7 @@ #include "Utils/String.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::RotoTransNode::RotoTransNode() : utils::RotoTrans(), diff --git a/src/Utils/Scalar.cpp b/src/Utils/Scalar.cpp index c6b85448..03313c28 100644 --- a/src/Utils/Scalar.cpp +++ b/src/Utils/Scalar.cpp @@ -4,7 +4,7 @@ #ifdef BIORBD_USE_CASADI_MATH #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Scalar::Scalar() : RigidBodyDynamics::Math::Scalar () diff --git a/src/Utils/SpatialVector.cpp b/src/Utils/SpatialVector.cpp index 33f1b887..19079d62 100644 --- a/src/Utils/SpatialVector.cpp +++ b/src/Utils/SpatialVector.cpp @@ -5,7 +5,7 @@ #include "Utils/String.h" #include "Utils/Vector3d.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::SpatialVector::SpatialVector() : RigidBodyDynamics::Math::SpatialVector() diff --git a/src/Utils/String.cpp b/src/Utils/String.cpp index c49b837c..8f9e3923 100644 --- a/src/Utils/String.cpp +++ b/src/Utils/String.cpp @@ -7,7 +7,7 @@ #include #include "Utils/Error.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::String::String() : std::string("") diff --git a/src/Utils/Timer.cpp b/src/Utils/Timer.cpp index 799ccfa1..b2d0f11f 100644 --- a/src/Utils/Timer.cpp +++ b/src/Utils/Timer.cpp @@ -1,7 +1,7 @@ #define BIORBD_API_EXPORTS #include "Utils/Timer.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Timer::Timer(bool startNow) : m_isStarted(false), diff --git a/src/Utils/Vector.cpp b/src/Utils/Vector.cpp index d317c4dc..13be1413 100644 --- a/src/Utils/Vector.cpp +++ b/src/Utils/Vector.cpp @@ -5,7 +5,7 @@ #include "Utils/String.h" #include "Utils/Vector3d.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Vector::Vector() : RigidBodyDynamics::Math::VectorNd() diff --git a/src/Utils/Vector3d.cpp b/src/Utils/Vector3d.cpp index 06efa15d..f0bf846e 100644 --- a/src/Utils/Vector3d.cpp +++ b/src/Utils/Vector3d.cpp @@ -4,7 +4,7 @@ #include "Utils/RotoTrans.h" #include "Utils/Vector.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; utils::Vector3d::Vector3d() : RigidBodyDynamics::Math::Vector3d (RigidBodyDynamics::Math::Vector3d::Zero()), diff --git a/test/binding/c/test_binder_c.cpp b/test/binding/c/test_binder_c.cpp index a70742cc..4d68f8b4 100644 --- a/test/binding/c/test_binder_c.cpp +++ b/test/binding/c/test_binder_c.cpp @@ -16,7 +16,7 @@ #include "RigidBody/KalmanReconsIMU.h" #endif -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; static double requiredPrecision(1e-10); diff --git a/test/test_actuators.cpp b/test/test_actuators.cpp index eaf6c12e..9df38f25 100644 --- a/test/test_actuators.cpp +++ b/test/test_actuators.cpp @@ -15,7 +15,7 @@ #include "Actuators/ActuatorLinear.h" #include "Actuators/ActuatorSigmoidGauss3p.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; static std::string modelPathForGeneralTesting("models/pyomecaman_withActuators.bioMod"); diff --git a/test/test_biorbd.cpp b/test/test_biorbd.cpp index eb46e804..090d4254 100644 --- a/test/test_biorbd.cpp +++ b/test/test_biorbd.cpp @@ -16,7 +16,7 @@ #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedTorque.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; static double requiredPrecision(1e-10); diff --git a/test/test_muscles.cpp b/test/test_muscles.cpp index ed5d2650..1d18205e 100644 --- a/test/test_muscles.cpp +++ b/test/test_muscles.cpp @@ -15,7 +15,7 @@ #include "Utils/String.h" #include "Utils/RotoTrans.h" -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; static double requiredPrecision(1e-10); diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index fb196364..a5ec9fb1 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -22,7 +22,7 @@ #include "RigidBody/KalmanReconsIMU.h" #endif -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; static double requiredPrecision(1e-10); #ifdef MODULE_ACTUATORS diff --git a/test/test_utils.cpp b/test/test_utils.cpp index 6dbf2eef..a7f46f06 100644 --- a/test/test_utils.cpp +++ b/test/test_utils.cpp @@ -16,7 +16,7 @@ #include "RigidBody/NodeSegment.h" static double requiredPrecision(1e-10); -using namespace biorbd::BIORBD_MATH_NAMESPACE; +using namespace BIORBD_NAMESPACE; TEST(ShallowCopy, DeepCopy) { From 50059fcd53de3aff0cabc18de3bcf23ea03495e7 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 14:03:55 -0400 Subject: [PATCH 12/14] Made the python test testing both backends --- .appveyor.yml | 9 +- test/CMakeLists.txt | 13 -- ....py.in => test_binder_python_rigidbody.py} | 126 ++++++++++-------- ...st_conversion.py.in => test_conversion.py} | 42 ++++-- 4 files changed, 105 insertions(+), 85 deletions(-) rename test/binding/Python3/{test_binder_python_rigidbody.py.in => test_binder_python_rigidbody.py} (74%) rename test/binding/Python3/{test_conversion.py.in => test_conversion.py} (55%) diff --git a/.appveyor.yml b/.appveyor.yml index 3c2ace6e..576ef2cb 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -75,10 +75,6 @@ test_script: - cmd: biorbd_casadi_tests.exe - cd .. - # Test for the python binder - - cd test/binding/Python3 - - pytest . - - cd ../../.. ############################################################################ @@ -111,7 +107,10 @@ test_script: - cmd: biorbd_eigen_tests_binding_c.exe - cd ../../.. - # Test for the python binder + + ############################################################################ + # Test for the python binder (both casadi and eigen) + ############################################################################ - cd test/binding/Python3 - pytest . - cd ../../.. diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 340b0371..95d7c5fa 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -113,19 +113,6 @@ if (BINDER_PYTHON3) set(PYTHON_TEST_IMPORT_BIORBD "import biorbd") endif() - - file(GLOB BIORBD_PYTHON3_TEST_CONFIG_FILES - "${CMAKE_SOURCE_DIR}/test/binding/Python3/*.py.in") - foreach(PYTHON_CONFIG_FILE ${BIORBD_PYTHON3_TEST_CONFIG_FILES}) - get_filename_component( - PYTHON_CONFIG_FILE_NAME ${PYTHON_CONFIG_FILE} - NAME_WE - ) - configure_file("${PYTHON_CONFIG_FILE}" - "${CMAKE_CURRENT_BINARY_DIR}/binding/Python3/${PYTHON_CONFIG_FILE_NAME}.py" @ONLY) - - endforeach() - file(GLOB BIORBD_PYTHON3_TEST_FILES "${CMAKE_SOURCE_DIR}/test/binding/Python3/*.py") file(COPY ${BIORBD_PYTHON3_TEST_FILES} diff --git a/test/binding/Python3/test_binder_python_rigidbody.py.in b/test/binding/Python3/test_binder_python_rigidbody.py similarity index 74% rename from test/binding/Python3/test_binder_python_rigidbody.py.in rename to test/binding/Python3/test_binder_python_rigidbody.py index 000ced43..69fffd1e 100644 --- a/test/binding/Python3/test_binder_python_rigidbody.py.in +++ b/test/binding/Python3/test_binder_python_rigidbody.py @@ -4,15 +4,27 @@ import pytest import numpy as np -@PYTHON_TEST_IMPORT_BIORBD@ - - -def test_load_model(): - biorbd.Model("../../models/pyomecaman.bioMod") - - -def test_dof_ranges(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +brbd_to_test = [] +try: + import biorbd + brbd_to_test.append(biorbd) +except: + pass +try: + import biorbd_casadi + brbd_to_test.append(biorbd_casadi) +except: + pass + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_load_model(brbd): + brbd.Model("../../models/pyomecaman.bioMod") + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_dof_ranges(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") pi = 3.14159265358979323846 # Pelvis @@ -69,26 +81,27 @@ def test_dof_ranges(): assert q_ranges[0].max() == pi / 2 -def test_forward_dynamics(): - m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_forward_dynamics(brbd): + m = brbd.Model("../../models/pyomecaman_withActuators.bioMod") q = np.array([i * 1.1 for i in range(m.nbQ())]) qdot = np.array([i * 1.1 for i in range(m.nbQ())]) tau = np.array([i * 1.1 for i in range(m.nbQ())]) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) qdot_sym = MX.sym("qdot", m.nbQdot(), 1) tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1) - forward_dynamics = biorbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym) + forward_dynamics = brbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym) qddot = forward_dynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] - elif biorbd.currentLinearAlgebraBackend() == 0: + elif brbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamics(q, qdot, tau).to_array() else: @@ -114,8 +127,9 @@ def test_forward_dynamics(): np.testing.assert_almost_equal(qddot, qddot_expected) -def test_forward_dynamics_with_external_forces(): - m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_forward_dynamics_with_external_forces(brbd): + m = brbd.Model("../../models/pyomecaman_withActuators.bioMod") q = np.array([i * 1.1 for i in range(m.nbQ())]) qdot = np.array([i * 1.1 for i in range(m.nbQ())]) @@ -125,20 +139,20 @@ def test_forward_dynamics_with_external_forces(): sv1 = np.array( ((11.1, 22.2, 33.3, 44.4, 55.5, 66.6), (11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) ).T - f_ext = biorbd.to_spatial_vector(sv1) + f_ext = brbd.to_spatial_vector(sv1) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) qdot_sym = MX.sym("qdot", m.nbQdot(), 1) tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1) - forward_dynamics = biorbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym, f_ext) + forward_dynamics = brbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym, f_ext) qddot = forward_dynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] - elif biorbd.currentLinearAlgebraBackend() == 0: + elif brbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array() else: @@ -164,8 +178,9 @@ def test_forward_dynamics_with_external_forces(): np.testing.assert_almost_equal(qddot, qddot_expected) -def test_com(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_com(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") q = np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) @@ -175,7 +190,7 @@ def test_com(): expected_com_dot = np.array([-0.05018973433722229, 1.4166208451420528, 1.4301750486035787]) expected_com_ddot = np.array([-0.7606169667295027, 11.508107073695976, 16.58853835505851]) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX @@ -183,15 +198,15 @@ def test_com(): q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1) q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1) - com_func = biorbd.to_casadi_func("Compute_CoM", m.CoM, q_sym) - com_dot_func = biorbd.to_casadi_func("Compute_CoM_dot", m.CoMdot, q_sym, q_dot_sym) - com_ddot_func = biorbd.to_casadi_func("Compute_CoM_ddot", m.CoMddot, q_sym, q_dot_sym, q_ddot_sym) + com_func = brbd.to_casadi_func("Compute_CoM", m.CoM, q_sym) + com_dot_func = brbd.to_casadi_func("Compute_CoM_dot", m.CoMdot, q_sym, q_dot_sym) + com_ddot_func = brbd.to_casadi_func("Compute_CoM_ddot", m.CoMddot, q_sym, q_dot_sym, q_ddot_sym) com = np.array(com_func(q)) com_dot = np.array(com_dot_func(q, q_dot)) com_ddot = np.array(com_ddot_func(q, q_dot, q_ddot)) - elif not biorbd.currentLinearAlgebraBackend(): + elif not brbd.currentLinearAlgebraBackend(): # If Eigen backend is used com = m.CoM(q).to_array() com_dot = m.CoMdot(q, q_dot).to_array() @@ -204,26 +219,28 @@ def test_com(): np.testing.assert_almost_equal(com_ddot.squeeze(), expected_com_ddot) -def test_set_vector3d(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_set_vector3d(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") m.setGravity(np.array((0, 0, -2))) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: from casadi import MX - get_gravity = biorbd.to_casadi_func("Compute_Markers", m.getGravity)()["o0"] + get_gravity = brbd.to_casadi_func("Compute_Markers", m.getGravity)()["o0"] else: get_gravity = m.getGravity().to_array() assert get_gravity[2] == -2 -def test_set_scalar(): +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_set_scalar(brbd): def check_value(target): - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: assert m.segment(0).characteristics().mass().to_mx() == target else: assert m.segment(0).characteristics().mass() == target - m = biorbd.Model("../../models/pyomecaman.bioMod") + m = brbd.Model("../../models/pyomecaman.bioMod") m.segment(0).characteristics().setMass(10) check_value(10) @@ -243,15 +260,16 @@ def check_value(target): with pytest.raises(ValueError, match="Scalar must be a 1x1 array or a float"): m.segment(0).characteristics().setMass(np.array([[[14]]])) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: from casadi import MX m.segment(0).characteristics().setMass(MX(15)) check_value(15.0) -def test_markers(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_markers(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") q = np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) @@ -259,20 +277,20 @@ def test_markers(): expected_markers_last = np.array([-0.11369, 0.63240501, -0.56253268]) expected_markers_last_dot = np.array([0.0, 4.16996219, 3.99459262]) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1) - markers_func = biorbd.to_casadi_func("Compute_Markers", m.markers, q_sym) - markers_velocity_func = biorbd.to_casadi_func("Compute_MarkersVelocity", m.markersVelocity, q_sym, q_dot_sym) + markers_func = brbd.to_casadi_func("Compute_Markers", m.markers, q_sym) + markers_velocity_func = brbd.to_casadi_func("Compute_MarkersVelocity", m.markersVelocity, q_sym, q_dot_sym) markers = np.array(markers_func(q)) markers_dot = np.array(markers_velocity_func(q, q_dot)) - elif not biorbd.currentLinearAlgebraBackend(): + elif not brbd.currentLinearAlgebraBackend(): # If Eigen backend is used markers = np.array([mark.to_array() for mark in m.markers(q)]).T markers_dot = np.array([mark.to_array() for mark in m.markersVelocity(q, q_dot)]).T @@ -284,8 +302,9 @@ def test_markers(): np.testing.assert_almost_equal(markers_dot[:, -1], expected_markers_last_dot) -def test_forward_dynamics_constraints_direct(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_forward_dynamics_constraints_direct(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") q = np.array([1.0 for _ in range(m.nbQ())]) qdot = np.array([1.0 for _ in range(m.nbQ())]) @@ -322,7 +341,7 @@ def test_forward_dynamics_constraints_direct(): np.testing.assert_almost_equal(cs.nbContacts(), contact_forces_expected.size) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import Function, MX @@ -341,7 +360,7 @@ def test_forward_dynamics_constraints_direct(): qddot = np.array(qddot) cs_forces = np.array(cs_forces) - elif biorbd.currentLinearAlgebraBackend() == 0: + elif brbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamicsConstraintsDirect(q, qdot, tau, cs).to_array() cs_forces = cs.getForce().to_array() @@ -353,18 +372,19 @@ def test_forward_dynamics_constraints_direct(): np.testing.assert_almost_equal(cs_forces.squeeze(), contact_forces_expected) -def test_name_to_index(): - m = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_name_to_index(brbd): + m = brbd.Model("../../models/pyomecaman.bioMod") # Index of a segment - np.testing.assert_equal(biorbd.segment_index(m, "Pelvis"), 0) - np.testing.assert_equal(biorbd.segment_index(m, "PiedG"), 10) + np.testing.assert_equal(brbd.segment_index(m, "Pelvis"), 0) + np.testing.assert_equal(brbd.segment_index(m, "PiedG"), 10) with pytest.raises(ValueError, match="dummy is not in the biorbd model"): - biorbd.segment_index(m, "dummy") + brbd.segment_index(m, "dummy") # Index of a marker - np.testing.assert_equal(biorbd.marker_index(m, "pelv1"), 0) - np.testing.assert_equal(biorbd.marker_index(m, "piedg6"), 96) + np.testing.assert_equal(brbd.marker_index(m, "pelv1"), 0) + np.testing.assert_equal(brbd.marker_index(m, "piedg6"), 96) with pytest.raises(ValueError, match="dummy is not in the biorbd model"): - biorbd.marker_index(m, "dummy") + brbd.marker_index(m, "dummy") diff --git a/test/binding/Python3/test_conversion.py.in b/test/binding/Python3/test_conversion.py similarity index 55% rename from test/binding/Python3/test_conversion.py.in rename to test/binding/Python3/test_conversion.py index fe52f228..9764916b 100644 --- a/test/binding/Python3/test_conversion.py.in +++ b/test/binding/Python3/test_conversion.py @@ -1,22 +1,34 @@ """ Test for file IO """ +import pytest import numpy as np -@PYTHON_TEST_IMPORT_BIORBD@ +brbd_to_test = [] +try: + import biorbd + brbd_to_test.append(biorbd) +except: + pass +try: + import biorbd_casadi + brbd_to_test.append(biorbd_casadi) +except: + pass # --- Options --- # -def test_np_mx_to_generalized(): - biorbd_model = biorbd.Model("../../models/pyomecaman.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_np_mx_to_generalized(brbd): + biorbd_model = brbd.Model("../../models/pyomecaman.bioMod") - q = biorbd.GeneralizedCoordinates(biorbd_model) - qdot = biorbd.GeneralizedVelocity((biorbd_model.nbQdot())) - qddot = biorbd.GeneralizedAcceleration((biorbd_model.nbQddot())) + q = brbd.GeneralizedCoordinates(biorbd_model) + qdot = brbd.GeneralizedVelocity((biorbd_model.nbQdot())) + qddot = brbd.GeneralizedAcceleration((biorbd_model.nbQddot())) tau = biorbd_model.InverseDynamics(q, qdot, qddot) biorbd_model.ForwardDynamics(q, qdot, tau) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: tau = biorbd_model.InverseDynamics(q.to_mx(), qdot.to_mx(), qddot.to_mx()) biorbd_model.ForwardDynamics(q, qdot, tau.to_mx()) else: @@ -25,15 +37,16 @@ def test_np_mx_to_generalized(): # --- Options --- # -def test_imu_to_array(): - m = biorbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod") +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_imu_to_array(brbd): + m = brbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod") q = np.zeros((m.nbQ(),)) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.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_func = brbd.to_casadi_func("imu", m.IMU, q_sym) imu = imu_func(q)[:, :4] else: @@ -52,14 +65,15 @@ def test_imu_to_array(): ) -def test_vector3d(): - biorbd_model = biorbd.Model() +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_vector3d(brbd): + biorbd_model = brbd.Model() vec = np.random.rand( 3, ) biorbd_model.setGravity(vec) - if biorbd.currentLinearAlgebraBackend() == 1: + if brbd.currentLinearAlgebraBackend() == 1: from casadi import MX vec = MX.ones(3, 1) From 20a0ca3f4f50ee98abf7d2e69327f2ef46de7dab Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 16:53:20 -0400 Subject: [PATCH 13/14] Adapted example installed --- CMakeLists.txt | 2 +- examples/cpp_casadi_installed/CMakeLists.txt | 8 ++++++++ .../cpp_casadi_installed/forwardDynamicsExample.cpp | 6 ++++-- examples/cpp_eigen_installed/CMakeLists.txt | 8 ++++++++ .../cpp_eigen_installed/forwardDynamicsExample.cpp | 12 +++++++----- 5 files changed, 28 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e830850..ada621ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,7 +25,7 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build.") # Set the possible values of build type for cmake-gui set_property(CACHE CMAKE_BUILD_TYPE PROPERTY - STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") + STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo" "Coverage") endif() # Choose the math's library backend to use diff --git a/examples/cpp_casadi_installed/CMakeLists.txt b/examples/cpp_casadi_installed/CMakeLists.txt index b9480b8b..d2a7e0a8 100644 --- a/examples/cpp_casadi_installed/CMakeLists.txt +++ b/examples/cpp_casadi_installed/CMakeLists.txt @@ -6,6 +6,14 @@ set(FILE "forwardDynamicsExample.cpp") get_filename_component(FILE_NAME ${FILE} NAME_WE) project(${FILE_NAME}) +# Set a default build type to 'Release' if none was specified +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build.") + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY + STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo" "Coverage") +endif() find_package(biorbd_casadi REQUIRED) find_package(RBDLCasadi REQUIRED) diff --git a/examples/cpp_casadi_installed/forwardDynamicsExample.cpp b/examples/cpp_casadi_installed/forwardDynamicsExample.cpp index 6330113e..ed36921d 100644 --- a/examples/cpp_casadi_installed/forwardDynamicsExample.cpp +++ b/examples/cpp_casadi_installed/forwardDynamicsExample.cpp @@ -11,11 +11,13 @@ /// 4. Print them to the console /// /// Please note that this example will work only with the Eigen backend -/// + +using namespace BIORBD_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Choose a position/velocity/torque to compute dynamics from DECLARE_GENERALIZED_COORDINATES(Q, model); diff --git a/examples/cpp_eigen_installed/CMakeLists.txt b/examples/cpp_eigen_installed/CMakeLists.txt index 7fcb880c..d13e4958 100644 --- a/examples/cpp_eigen_installed/CMakeLists.txt +++ b/examples/cpp_eigen_installed/CMakeLists.txt @@ -6,6 +6,14 @@ set(FILE "forwardDynamicsExample.cpp") get_filename_component(FILE_NAME ${FILE} NAME_WE) project(${FILE_NAME}) +# Set a default build type to 'Release' if none was specified +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build.") + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY + STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo" "Coverage") +endif() find_package(biorbd_eigen REQUIRED) find_package(RBDL REQUIRED) diff --git a/examples/cpp_eigen_installed/forwardDynamicsExample.cpp b/examples/cpp_eigen_installed/forwardDynamicsExample.cpp index f9513ef7..6b959d7c 100644 --- a/examples/cpp_eigen_installed/forwardDynamicsExample.cpp +++ b/examples/cpp_eigen_installed/forwardDynamicsExample.cpp @@ -11,16 +11,18 @@ /// 4. Print them to the console /// /// Please note that this example will work only with the Eigen backend -/// + +using namespace BIORBD_NAMESPACE; + int main() { // Load a predefined model - biorbd::Model model("pyomecaman.bioMod"); + Model model("pyomecaman.bioMod"); // Choose a position/velocity/torque to compute dynamics from - biorbd::rigidbody::GeneralizedCoordinates Q(model); - biorbd::rigidbody::GeneralizedVelocity Qdot(model); - biorbd::rigidbody::GeneralizedTorque Tau(model); + rigidbody::GeneralizedCoordinates Q(model); + rigidbody::GeneralizedVelocity Qdot(model); + rigidbody::GeneralizedTorque Tau(model); Q.setZero(); Qdot.setZero(); Tau.setZero(); From ccb6cf1ab6da650991ce3fe81c20c8021754543f Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 21 Jul 2021 16:53:38 -0400 Subject: [PATCH 14/14] Bumped version to 1.7.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ada621ca..1f10d893 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0") cmake_policy(SET CMP0042 NEW) endif() -project(biorbd VERSION 1.6.1) +project(biorbd VERSION 1.7.0) set(BIORBD_ROOT_FOLDER ${PROJECT_SOURCE_DIR}) set (BIORBD_NAME_NO_SUFFIX ${PROJECT_NAME}) set (CMAKE_CXX_STANDARD 11)