Merge pull request #231 from pariterre/master

Added the smooth if_else in casadi

include/Utils/CasadiExpand.h

@@ -0,0 +1,92 @@
+#ifndef BIORBD_UTILS_CASADI_EXPAND_H
+#define BIORBD_UTILS_CASADI_EXPAND_H
+
+#include <map>
+#include "biorbdConfig.h"
+
+#include "casadi.hpp"
+
+namespace BIORBD_NAMESPACE
+{
+namespace utils
+{
+#ifdef USE_SMOOTH_IF_ELSE
+///
+/// \brief Lesser than that works with the if_else of CasadiExpand
+/// \param x First element
+/// \param y Second element
+/// \return The lesser than element
+///
+casadi::MX lt(
+        const casadi::MX& x,
+        const casadi::MX& y);
+
+///
+/// \brief Lesser or equal than that works with the if_else of CasadiExpand.
+/// This is strictly similar to lt
+/// \param x First element
+/// \param y Second element
+/// \return The lesser than element
+///
+casadi::MX le(
+        const casadi::MX& x,
+        const casadi::MX& y);
+
+///
+/// \brief Greater than that works with the if_else of CasadiExpand
+/// \param x First element
+/// \param y Second element
+/// \return The greater than element
+///
+casadi::MX gt(
+        const casadi::MX& x,
+        const casadi::MX& y);
+
+///
+/// \brief Greater or equal than that works with the if_else of CasadiExpand.
+/// This is strictly similar to lt
+/// \param x First element
+/// \param y Second element
+/// \return The greater than element
+///
+casadi::MX ge(
+        const casadi::MX& x,
+        const casadi::MX& y);
+
+///
+/// \brief A non-branching and continuous if_else based on tanh. This is not
+/// strictly equal to if_else when the values are near each other. The b parameter
+/// can be adjusted to increase the rate of change
+/// \param cond The condition (lt/le/gt/ge)
+/// \param if_true The value to return if true
+/// \param if_false The value to return if false
+/// \param b The slope parameter
+/// \return The value of the if_else function
+///
+casadi::MX if_else(
+        const casadi::MX& cond,
+        const casadi::MX& if_true,
+        const casadi::MX& if_false,
+        double b = 10000);
+
+///
+/// \brief A non-branching and continuous if_else_zero based on tanh. This is not
+/// strictly equal to if_else_zero when the values are near each other. The b parameter
+/// can be adjusted to increase the rate of change
+/// \param cond The condition (lt/le/gt/ge)
+/// \param if_true The value to return if true
+/// \param if_false The value to return if false
+/// \param b The slope parameter
+/// \return The value of the if_else function
+///
+casadi::MX if_else_zero(
+        const casadi::MX& cond,
+        const casadi::MX& if_true,
+        double b = 10000);
+#endif
+
+}
+}
+
+#endif // BIORBD_UTILS_CASADI_EXPAND_H
+

include/biorbd.h

@@ -17,5 +17,9 @@
     #include "Actuators/all.h"
 #endif
 
+#ifdef BIORBD_USE_CASADI_MATH
+#include "Utils/CasadiExpand.h"
+#endif
+
 #endif // BIORBD_ALL_H
 

include/biorbdConfig.h.in

@@ -56,6 +56,16 @@ inline LINEAR_ALGEBRA_BACKEND currentLinearAlgebraBackend(){
 #cmakedefine MODULE_STATIC_OPTIM
 #cmakedefine MODULE_VTP_FILES_READER
 
+
+#ifdef BIORBD_USE_CASADI_MATH
+#cmakedefine USE_SMOOTH_IF_ELSE
+#ifdef USE_SMOOTH_IF_ELSE
+#define IF_ELSE_NAMESPACE utils
+#else
+#define IF_ELSE_NAMESPACE casadi::MX
+#endif
+#endif
+
 // Define some skip if ones doesn't want to compile them
 #cmakedefine SKIP_ASSERT
 #cmakedefine SKIP_LONG_TESTS

src/Actuators/ActuatorGauss3p.cpp

@@ -5,6 +5,10 @@
 #include "RigidBody/GeneralizedCoordinates.h"
 #include "RigidBody/GeneralizedVelocity.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 actuator::ActuatorGauss3p::ActuatorGauss3p() :
@@ -152,7 +156,7 @@ utils::Scalar actuator::ActuatorGauss3p::torqueMax(
 
     utils::Scalar Tw;
 #ifdef BIORBD_USE_CASADI_MATH
-    Tw = casadi::MX::if_else(casadi::MX::ge(speed, 0),
+    Tw = IF_ELSE_NAMESPACE::if_else(IF_ELSE_NAMESPACE::ge(speed, 0),
                              C / ( *m_wc + speed )  - Tc,
                              E / ( we - speed ) + *m_Tmax);
 #else

src/Actuators/ActuatorGauss6p.cpp

@@ -5,6 +5,10 @@
 #include "RigidBody/GeneralizedCoordinates.h"
 #include "RigidBody/GeneralizedVelocity.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 actuator::ActuatorGauss6p::ActuatorGauss6p() :
@@ -170,7 +174,7 @@ utils::Scalar actuator::ActuatorGauss6p::torqueMax(
 
     utils::Scalar Tw;
 #ifdef BIORBD_USE_CASADI_MATH
-    Tw = casadi::MX::if_else(casadi::MX::ge(speed, 0),
+    Tw = IF_ELSE_NAMESPACE::if_else(IF_ELSE_NAMESPACE::ge(speed, 0),
                              C / ( *m_wc + speed )  - Tc,
                              E / ( we - speed ) + *m_Tmax);
 #else

src/Actuators/Actuators.cpp

@@ -14,6 +14,10 @@
 #include "Actuators/ActuatorConstant.h"
 #include "Actuators/ActuatorLinear.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 actuator::Actuators::Actuators() :
@@ -349,8 +353,8 @@ rigidbody::GeneralizedTorque actuator::Actuators::torqueMax(
     rigidbody::GeneralizedVelocity QdotResigned(Qdot);
     for (unsigned int i=0; i<Qdot.size(); ++i) {
 #ifdef BIORBD_USE_CASADI_MATH
-        QdotResigned(i) = casadi::MX::if_else(
-                              casadi::MX::lt(activation(i), 0),
+        QdotResigned(i) = IF_ELSE_NAMESPACE::if_else(
+                              IF_ELSE_NAMESPACE::lt(activation(i), 0),
                               -Qdot(i), Qdot(i));
 #else
         if (activation(i)<0) {
@@ -363,8 +367,8 @@ rigidbody::GeneralizedTorque actuator::Actuators::torqueMax(
 
     for (unsigned int i=0; i<model.nbDof(); ++i) {
 #ifdef BIORBD_USE_CASADI_MATH
-        maxGeneralizedTorque_all[i] = casadi::MX::if_else(
-                                          casadi::MX::ge(activation(i, 0), 0),
+        maxGeneralizedTorque_all[i] = IF_ELSE_NAMESPACE::if_else(
+                                          IF_ELSE_NAMESPACE::ge(activation(i, 0), 0),
                                           getTorqueMaxDirection(actuator(i).first, Q, QdotResigned),
                                           getTorqueMaxDirection(actuator(i).second, Q, QdotResigned));
 #else

src/Muscles/HillThelenType.cpp

@@ -6,6 +6,10 @@
 #include "Muscles/Geometry.h"
 #include "Muscles/Characteristics.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 muscles::HillThelenType::HillThelenType() :
@@ -84,8 +88,8 @@ void muscles::HillThelenType::DeepCopy(
 void muscles::HillThelenType::computeFlPE()
 {
 #ifdef BIORBD_USE_CASADI_MATH
-    *m_FlPE = casadi::MX::if_else(
-                  casadi::MX::gt(position().length(), characteristics().tendonSlackLength()),
+    *m_FlPE = IF_ELSE_NAMESPACE::if_else(
+                  IF_ELSE_NAMESPACE::gt(position().length(), characteristics().tendonSlackLength()),
                   (exp( *m_cste_FlPE_1 * (position().length()/characteristics().optimalLength()
                                           -1)) -1)
                   /

src/Muscles/HillType.cpp

@@ -8,6 +8,10 @@
 #include "Muscles/Geometry.h"
 #include "Muscles/State.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 muscles::HillType::HillType() :
@@ -294,8 +298,8 @@ void muscles::HillType::computeFvCE()
     // The relation is different if velocity< 0  or > 0
     utils::Scalar v = m_position->velocity();
 #ifdef BIORBD_USE_CASADI_MATH
-    *m_FvCE = casadi::MX::if_else(
-                  casadi::MX::le(v, 0),
+    *m_FvCE = IF_ELSE_NAMESPACE::if_else(
+                  IF_ELSE_NAMESPACE::le(v, 0),
                   (1.0-std::fabs(v) / *m_cste_maxShorteningSpeed) /
                   (1.0+std::fabs(v) / *m_cste_maxShorteningSpeed / *m_cste_FvCE_1),
                   (1.0-1.33*v / *m_cste_maxShorteningSpeed / *m_cste_FvCE_2) /
@@ -315,8 +319,8 @@ void muscles::HillType::computeFlPE()
 {
 
 #ifdef BIORBD_USE_CASADI_MATH
-    *m_FlPE = casadi::MX::if_else_zero(
-                  casadi::MX::gt(position().length(), characteristics().tendonSlackLength()),
+    *m_FlPE = IF_ELSE_NAMESPACE::if_else_zero(
+                  IF_ELSE_NAMESPACE::gt(position().length(), characteristics().tendonSlackLength()),
                   exp(*m_cste_FlPE_1*(position().length()/characteristics().optimalLength()-1) -
                       *m_cste_FlPE_2));
 #else

src/Muscles/StateDynamics.cpp

@@ -5,6 +5,10 @@
 #include "Utils/String.h"
 #include "Muscles/Characteristics.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 muscles::StateDynamics::StateDynamics(
@@ -82,11 +86,11 @@ muscles::StateDynamics::timeDerivativeActivation(
     // Implémentation de la fonction da/dt = (u-a)/GeneralizedTorque(u,a)
     // ou GeneralizedTorque(u,a) = t_act(0.5+1.5*a) is u>a et GeneralizedTorque(u,a)=t_deact(0.5+1.5*a) sinon
 #ifdef BIORBD_USE_CASADI_MATH
-    *m_activation = casadi::MX::if_else(
-                        casadi::MX::lt(*m_activation, characteristics.minActivation()),
+    *m_activation = IF_ELSE_NAMESPACE::if_else(
+                        IF_ELSE_NAMESPACE::lt(*m_activation, characteristics.minActivation()),
                         characteristics.minActivation(), *m_activation);
-    *m_excitation = casadi::MX::if_else(
-                        casadi::MX::lt(*m_excitation, characteristics.minActivation()),
+    *m_excitation = IF_ELSE_NAMESPACE::if_else(
+                        IF_ELSE_NAMESPACE::lt(*m_excitation, characteristics.minActivation()),
                         characteristics.minActivation(), *m_excitation);
 #else
     if (*m_activation < characteristics.minActivation()) {
@@ -116,8 +120,8 @@ muscles::StateDynamics::timeDerivativeActivation(
 
     utils::Scalar denom; // dénominateur
 #ifdef BIORBD_USE_CASADI_MATH
-    denom = casadi::MX::if_else(
-                casadi::MX::gt(num, 0),
+    denom = IF_ELSE_NAMESPACE::if_else(
+                IF_ELSE_NAMESPACE::gt(num, 0),
                 characteristics.torqueActivation()   * (0.5+1.5* *m_activation),
                 characteristics.torqueDeactivation() / (0.5+1.5* *m_activation));
 #else

src/Muscles/WrappingHalfCylinder.cpp

@@ -5,6 +5,10 @@
 #include "Utils/RotoTrans.h"
 #include "RigidBody/Joints.h"
 
+#ifdef USE_SMOOTH_IF_ELSE
+#include "Utils/CasadiExpand.h"
+#endif
+
 using namespace BIORBD_NAMESPACE;
 
 muscles::WrappingHalfCylinder::WrappingHalfCylinder() :
@@ -234,7 +238,12 @@ void muscles::WrappingHalfCylinder::findTangentToCircle(
     const utils::Vector3d& p,
     utils::Vector3d& p_tan) const
 {
+    // This function ignores the Z axis of the vector p to create the circle
+#ifdef BIORBD_USE_EIGEN3_MATH
+    utils::Scalar p_dot = static_cast<Eigen::Vector2d>(p.block(0,0,2,1)).dot(static_cast<Eigen::Vector2d>(p.block(0,0,2,1)));
+#else
     utils::Scalar p_dot = p.block(0,0,2,1).dot(p.block(0,0,2,1));
+#endif
 
     const RigidBodyDynamics::Math::Vector2d& Q0(radius()*radius()/p_dot*p.block(0,0,
             2,1));
@@ -259,8 +268,8 @@ void muscles::WrappingHalfCylinder::selectTangents(
     utils::Vector3d &p_tan) const
 {
 #ifdef BIORBD_USE_CASADI_MATH
-    p_tan = casadi::MX::if_else(
-                casadi::MX::ge((*p1.m_p2)(0), (*p1.m_p1)(0)),
+    p_tan = IF_ELSE_NAMESPACE::if_else(
+                IF_ELSE_NAMESPACE::ge((*p1.m_p2)(0), (*p1.m_p1)(0)),
                 *p1.m_p2, *p1.m_p1);
 #else
     if ((*p1.m_p2)(0) >= (*p1.m_p1)(0)) {

src/Utils/CMakeLists.txt

@@ -24,6 +24,11 @@ set(SRC_LIST_MODULE
     "${CMAKE_CURRENT_SOURCE_DIR}/Vector.cpp"
 )
 
+if(${MATH_LIBRARY_BACKEND} STREQUAL "Casadi")
+    list(APPEND SRC_LIST_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/CasadiExpand.cpp")
+    option(USE_SMOOTH_IF_ELSE "If biorbd should be compiled with branching if_else (from CasADi) or using the tanh approximation" OFF)
+endif()
+
 # Create the library
 if (WIN32)
     add_library(${PROJECT_NAME} STATIC "${SRC_LIST_MODULE}")

src/Utils/CasadiExpand.cpp

@@ -0,0 +1,45 @@
+#define BIORBD_API_EXPORTS
+#include "Utils/CasadiExpand.h"
+
+using namespace BIORBD_NAMESPACE;
+
+#ifdef USE_SMOOTH_IF_ELSE
+casadi::MX utils::lt(
+        const casadi::MX& x,
+        const casadi::MX& y){
+    return x - y;
+}
+
+casadi::MX utils::le(
+        const casadi::MX& x,
+        const casadi::MX& y){
+    return utils::lt(x, y);
+}
+
+casadi::MX utils::gt(
+        const casadi::MX& x,
+        const casadi::MX& y){
+    return utils::lt(y, x);
+}
+
+casadi::MX utils::ge(
+        const casadi::MX& x,
+        const casadi::MX& y){
+    return utils::le(y, x);
+}
+
+casadi::MX utils::if_else(
+        const casadi::MX& cond,
+        const casadi::MX& if_true,
+        const casadi::MX& if_false,
+        double b){
+    return if_true + (if_false - if_true) * (0.5 + 0.5 * casadi::MX::tanh(b * cond));
+}
+
+casadi::MX utils::if_else_zero(
+        const casadi::MX& cond,
+        const casadi::MX& if_true,
+        double b){
+    return utils::if_else(cond, if_true, 0, b);
+}
+#endif