Merge pull request #131 from paulWegiel/test_actuators

Tests for actuators

Author: pariterre

binding/python3/__init__.py

@@ -1,3 +1,2 @@
 from .biorbd import *
 from ._version import __version__
-

binding/python3/_version.py

@@ -1,2 +1,3 @@
 import biorbd
+
 __version__ = biorbd.getVersion().to_string()

include/Actuators/Actuators.h

@@ -4,6 +4,7 @@
 #include <vector>
 #include <memory>
 #include "biorbdConfig.h"
+#include "Utils/Scalar.h"
 
 namespace biorbd {
 namespace utils {
@@ -122,6 +123,18 @@ class BIORBD_API Actuators
     std::shared_ptr<std::vector<bool>> m_isDofSet;///< If DoF all dof are set
     std::shared_ptr<bool> m_isClose; ///< If the set is ready
 
+    ///
+    /// \brief getTorqueMaxDirection Get the max torque of a specific actuator (interface necessary because of CasADi)
+    /// \param actuator The actuator to gather from
+    /// \param Q The Generalized coordinates
+    /// \param Qdot The Generalized velocity
+    /// \return The torque max
+    ///
+    biorbd::utils::Scalar getTorqueMaxDirection(
+            const std::shared_ptr<Actuator> actuator,
+            const rigidbody::GeneralizedCoordinates &Q,
+            const rigidbody::GeneralizedVelocity &Qdot) const;
+
 };
 
 }}

include/biorbdConfig.h.in

@@ -86,6 +86,9 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){
 #define DECLARE_GENERALIZED_TORQUE(varname, model) \
     casadi::DM varname(model.nbGeneralizedTorque(), 1);\
     biorbd::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));
 
 #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \
     casadi::Function func_##funcname(#funcname, {arg1##_sym}, {model.funcname(arg1##_sym)}, {#arg1}, {#funcname});\
@@ -118,6 +121,8 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){
     biorbd::rigidbody::GeneralizedAcceleration varname(model);
 #define DECLARE_GENERALIZED_TORQUE(varname, model) \
     biorbd::rigidbody::GeneralizedTorque varname(model);
+#define DECLARE_VECTOR(varname, nbElements) \
+    biorbd::utils::Vector varname(nbElements);
 
 #define CALL_BIORBD_FUNCTION_1ARG(varname, model, funcname, arg1) \
     auto varname = model.funcname(arg1);

src/Actuators/Actuators.cpp

@@ -284,36 +284,36 @@ biorbd::rigidbody::GeneralizedTorque biorbd::actuator::Actuators::torqueMax(
     biorbd::rigidbody::GeneralizedTorque maxGeneralizedTorque_all(model);
 
     for (unsigned int i=0; i<model.nbDof(); ++i){
-        std::shared_ptr<Actuator> GeneralizedTorque_tp;
-
 #ifdef BIORBD_USE_CASADI_MATH
-        casadi::MX useFirst = casadi::MX::if_else(
-                    casadi::MX::ge(activation[i], 0),
-                    1, 0);
-        if (!useFirst.is_zero()){
-            GeneralizedTorque_tp = actuator(i).first;
-        } else {
-            GeneralizedTorque_tp = actuator(i).second;
-        }
+        maxGeneralizedTorque_all[i] = casadi::MX::if_else(
+                    casadi::MX::ge(activation(i, 0), 0),
+                    getTorqueMaxDirection(actuator(i).first, Q, Qdot),
+                    getTorqueMaxDirection(actuator(i).second, Q, Qdot));
 #else
         if (activation[i] >= 0) // First
-            GeneralizedTorque_tp = actuator(i).first;
+            maxGeneralizedTorque_all[i] = getTorqueMaxDirection(actuator(i).first, Q, Qdot);
         else
-            GeneralizedTorque_tp = actuator(i).second;
+            maxGeneralizedTorque_all[i] = getTorqueMaxDirection(actuator(i).second, Q, Qdot);
 #endif
-
-        if (std::dynamic_pointer_cast<ActuatorGauss3p> (GeneralizedTorque_tp))
-            maxGeneralizedTorque_all[i] = std::static_pointer_cast<ActuatorGauss3p> (GeneralizedTorque_tp)->torqueMax(Q, Qdot);
-        else if (std::dynamic_pointer_cast<ActuatorConstant> (GeneralizedTorque_tp))
-            maxGeneralizedTorque_all[i] = std::static_pointer_cast<ActuatorConstant> (GeneralizedTorque_tp)->torqueMax();
-        else if (std::dynamic_pointer_cast<ActuatorLinear> (GeneralizedTorque_tp))
-            maxGeneralizedTorque_all[i] = std::static_pointer_cast<ActuatorLinear> (GeneralizedTorque_tp)->torqueMax(Q);
-        else if (std::dynamic_pointer_cast<ActuatorGauss6p> (GeneralizedTorque_tp))
-            maxGeneralizedTorque_all[i] = std::static_pointer_cast<ActuatorGauss6p> (GeneralizedTorque_tp)->torqueMax(Q, Qdot);
-        else
-            biorbd::utils::Error::raise("Wrong type (should never get here because of previous safety)");
-
     }
 
     return maxGeneralizedTorque_all;
 }
+
+biorbd::utils::Scalar biorbd::actuator::Actuators::getTorqueMaxDirection(
+        const std::shared_ptr<biorbd::actuator::Actuator> actuator,
+        const biorbd::rigidbody::GeneralizedCoordinates& Q,
+        const biorbd::rigidbody::GeneralizedVelocity& Qdot) const
+{
+    if (std::dynamic_pointer_cast<ActuatorGauss3p> (actuator))
+        return std::static_pointer_cast<ActuatorGauss3p> (actuator)->torqueMax(Q, Qdot);
+    else if (std::dynamic_pointer_cast<ActuatorConstant> (actuator))
+        return std::static_pointer_cast<ActuatorConstant> (actuator)->torqueMax();
+    else if (std::dynamic_pointer_cast<ActuatorLinear> (actuator))
+        return std::static_pointer_cast<ActuatorLinear> (actuator)->torqueMax(Q);
+    else if (std::dynamic_pointer_cast<ActuatorGauss6p> (actuator))
+        return std::static_pointer_cast<ActuatorGauss6p> (actuator)->torqueMax(Q, Qdot);
+    else
+        biorbd::utils::Error::raise("Wrong type (should never get here because of previous safety)");
+
+}

src/ModelReader.cpp

@@ -628,7 +628,7 @@ void biorbd::Reader::readModelFile(
                 int int_direction = 0;
                 double Tmax(-1);            bool isTmaxSet  = false;
                 double T0(-1);              bool isT0Set    = false;
-                double pente(-1);           bool isPenteSet = false;
+                double slope(-1);           bool isSlopeSet = false;
                 double wmax(-1);            bool iswmaxSet  = false;
                 double wc(-1);              bool iswcSet    = false;
                 double amin(-1);            bool isaminSet  = false;
@@ -671,9 +671,9 @@ void biorbd::Reader::readModelFile(
                         file.read(T0, variable);
                         isT0Set = true;
                     }
-                    else if (!property_tag.tolower().compare("pente")){
-                        file.read(pente, variable);
-                        isPenteSet = true;
+                    else if (!property_tag.tolower().compare("pente") || !property_tag.tolower().compare("slope")){
+                        file.read(slope, variable);
+                        isSlopeSet = true;
                     }
                     else if (!property_tag.tolower().compare("wmax")){
                         file.read(wmax, variable);
@@ -732,9 +732,9 @@ void biorbd::Reader::readModelFile(
                     actuator = new biorbd::actuator::ActuatorConstant(int_direction,Tmax,dofIdx,name);
                 }
                 else if (!type.tolower().compare("linear")){
-                    biorbd::utils::Error::check(isDofSet && isDirectionSet && isPenteSet && isT0Set,
+                    biorbd::utils::Error::check(isDofSet && isDirectionSet && isSlopeSet && isT0Set,
                                         "Make sure all parameters are defined");
-                    actuator = new biorbd::actuator::ActuatorLinear(int_direction,T0,pente,dofIdx,name);
+                    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 && iswmaxSet && iswcSet && isaminSet &&

test/CMakeLists.txt

@@ -42,7 +42,19 @@ add_subdirectory(
 # Unit Tests
 ##############
 enable_testing()
-file(GLOB TEST_SRC_FILES "${CMAKE_SOURCE_DIR}/test/*.cpp")
+
+set(TEST_SRC_FILES
+    "${CMAKE_SOURCE_DIR}/test/test_biorbd.cpp"
+    "${CMAKE_SOURCE_DIR}/test/test_rigidbody.cpp"
+    "${CMAKE_SOURCE_DIR}/test/test_utils.cpp"
+    "${CMAKE_SOURCE_DIR}/test/test_actuators.cpp"
+)
+if(MODULE_MUSCLES)
+    list(APPEND TEST_SRC_FILES "${CMAKE_SOURCE_DIR}/test/test_muscles.cpp")
+endif()
+if(MODULE_ACTUATORS)
+    list(APPEND TEST_SRC_FILES "${CMAKE_SOURCE_DIR}/test/test_actuators.cpp")
+endif()
 add_executable(${PROJECT_NAME} "${TEST_SRC_FILES}")
 add_dependencies(${PROJECT_NAME} ${BIORBD_NAME})