jrl-umi3218 · rafaelxero · Sep 6, 2017 · Sep 28, 2017 · Sep 28, 2017 · Oct 25, 2017
diff --git a/src/RBDyn/CMakeLists.txt b/src/RBDyn/CMakeLists.txt
@@ -4,10 +4,11 @@
 
 set(SOURCES MultiBodyGraph.cpp MultiBody.cpp MultiBodyConfig.cpp
   FK.cpp FV.cpp FA.cpp Jacobian.cpp ID.cpp IK.cpp IS.cpp FD.cpp EulerIntegration.cpp
-  CoM.cpp Momentum.cpp ZMP.cpp IDIM.cpp VisServo.cpp Coriolis.cpp)
+  CoM.cpp Momentum.cpp ZMP.cpp IDIM.cpp VisServo.cpp Coriolis.cpp Friction.cpp TorqueFeedbackTerm.cpp)
 set(HEADERS RBDyn/Body.h RBDyn/Joint.h RBDyn/MultiBodyGraph.h RBDyn/MultiBody.h RBDyn/MultiBodyConfig.h
   RBDyn/FK.h RBDyn/FV.h RBDyn/FA.h RBDyn/Jacobian.h RBDyn/ID.h RBDyn/IK.h RBDyn/IS.h RBDyn/FD.h RBDyn/EulerIntegration.h RBDyn/CoM.h
-  RBDyn/Momentum.h RBDyn/ZMP.h RBDyn/IDIM.h RBDyn/VisServo.h RBDyn/util.hh RBDyn/util.hxx RBDyn/Coriolis.h)
+  RBDyn/Momentum.h RBDyn/ZMP.h RBDyn/IDIM.h RBDyn/VisServo.h RBDyn/util.hh RBDyn/util.hxx RBDyn/Coriolis.h RBDyn/Friction.h RBDyn/TorqueFeedbackTerm.h
+  RBDyn/LambertW/LambertW.h RBDyn/LambertW/LambertW.hxx RBDyn/LambertW/Horner.h)
 
 add_library(RBDyn SHARED ${SOURCES} ${HEADERS})
 target_include_directories(RBDyn PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}> $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/../include> $<INSTALL_INTERFACE:include>)

diff --git a/src/RBDyn/FD.cpp b/src/RBDyn/FD.cpp
@@ -10,15 +10,18 @@
 #include "RBDyn/MultiBody.h"
 #include "RBDyn/MultiBodyConfig.h"
 
+// #include <iostream>
+
 namespace rbd
 {
 
 ForwardDynamics::ForwardDynamics(const MultiBody & mb)
 : H_(mb.nrDof(), mb.nrDof()), C_(mb.nrDof()), I_st_(static_cast<size_t>(mb.nrBodies())),
-  F_(static_cast<size_t>(mb.nrJoints())), acc_(static_cast<size_t>(mb.nrBodies())),
+  F_(static_cast<size_t>(mb.nrJoints())), HIr_(mb.nrDof(), mb.nrDof()), acc_(static_cast<size_t>(mb.nrBodies())),
   f_(static_cast<size_t>(mb.nrBodies())), tmpFd_(mb.nrDof()), dofPos_(static_cast<size_t>(mb.nrJoints())),
   ldlt_(mb.nrDof())
 {
+  HIr_.setZero();
   int dofP = 0;
   for(int i = 0; i < mb.nrJoints(); ++i)
   {
@@ -41,6 +44,18 @@ void ForwardDynamics::forwardDynamics(const MultiBody & mb, MultiBodyConfig & mb
   vectorToParam(tmpFd_, mbc.alphaD);
 }
 
+void ForwardDynamics::computeHIr(const MultiBody& mb)
+{
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double gr = mb.joint(i).gearRatio();
+      HIr_(dofPos_[i], dofPos_[i]) = mb.joint(i).rotorInertia() * gr * gr;
+    }
+  }
+}
+
 void ForwardDynamics::computeH(const MultiBody & mb, const MultiBodyConfig & mbc)
 {
   const std::vector<Body> & bodies = mb.bodies();
@@ -67,8 +82,11 @@ void ForwardDynamics::computeH(const MultiBody & mb, const MultiBodyConfig & mbc
       F_[ui].col(dof).noalias() = (I_st_[ui] * sva::MotionVecd(mbc.motionSubspace[ui].col(dof))).vector();
     }
 
-    H_.block(dofPos_[ui], dofPos_[ui], joints[ui].dof(), joints[ui].dof()).noalias() =
+    if ( joints[i].dof() > 0)
+    {
+      H_.block(dofPos_[ui], dofPos_[ui], joints[ui].dof(), joints[ui].dof()).noalias() =
         mbc.motionSubspace[ui].transpose() * F_[ui];
+    }
 
     size_t j = ui;
     while(pred[j] != -1)
@@ -90,6 +108,8 @@ void ForwardDynamics::computeH(const MultiBody & mb, const MultiBodyConfig & mbc
       }
     }
   }
+
+  H_.noalias() = H_ + HIr_;
 }
 
 void ForwardDynamics::computeC(const MultiBody & mb, const MultiBodyConfig & mbc)
@@ -120,7 +140,11 @@ void ForwardDynamics::computeC(const MultiBody & mb, const MultiBodyConfig & mbc
   for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
   {
     const auto ui = static_cast<size_t>(i);
-    C_.segment(dofPos_[ui], joints[ui].dof()).noalias() = mbc.motionSubspace[ui].transpose() * f_[ui].vector();
+
+    if (joints[i].dof() > 0)
+    {
+      C_.segment(dofPos_[ui], joints[ui].dof()).noalias() = mbc.motionSubspace[ui].transpose() * f_[ui].vector();
+    }    
 
     if(pred[ui] != -1)
     {
@@ -148,7 +172,7 @@ void ForwardDynamics::sForwardDynamics(const MultiBody & mb, MultiBodyConfig & m
 void ForwardDynamics::sComputeH(const MultiBody & mb, const MultiBodyConfig & mbc)
 {
   checkMatchParentToSon(mb, mbc);
-  checkMatchMotionSubspace(mb, mbc);
+  // checkMatchMotionSubspace(mb, mbc);  // Commented out by Rafa as a test
 
   computeH(mb, mbc);
 }
@@ -165,4 +189,16 @@ void ForwardDynamics::sComputeC(const MultiBody & mb, const MultiBodyConfig & mb
   computeC(mb, mbc);
 }
 
+/*
+Eigen::Matrix3d ForwardDynamics::SkewSymmetric(const Eigen::Vector3d& v)
+{
+  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+  R(0,1) = -(R(1,0) = v[2]);
+  R(2,0) = -(R(0,2) = v[1]);
+  R(1,2) = -(R(2,1) = v[0]);
+
+  return R;
+}
+*/
+
 } // namespace rbd
diff --git a/src/RBDyn/Friction.cpp b/src/RBDyn/Friction.cpp
@@ -0,0 +1,225 @@
+/*
+ * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+// associated header
+#include "RBDyn/Friction.h"
+#include "RBDyn/LambertW/LambertW.h"
+
+#define EPSILON 1E-6
+
+namespace rbd
+{
+
+Friction::Friction(const MultiBody & mb)
+  : Fr_(mb.nrDof()), dofPos_(mb.nrJoints())
+{
+  int dofP = 0;
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    dofPos_[i] = dofP;
+    dofP += mb.joint(i).dof();
+  }
+}
+
+StaticModelFriction::StaticModelFriction(const MultiBody & mb)
+  : Friction(mb)
+{}
+
+void StaticModelFriction::computeFriction(const MultiBody & mb, const MultiBodyConfig & mbc)
+{
+  Fr_.setZero();
+
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double Ts = mb.joint(i).staticFriction();
+      double Tc = mb.joint(i).kineticFriction();
+      double Tv = mb.joint(i).viscousFrictionCoeff();
+
+      double wbrk = mb.joint(i).breakawayVelocity();
+
+      double w = mbc.alpha[i][0];
+      unsigned short sign = w > EPSILON ? 1 : (w < -EPSILON ? -1 : 0);
+
+      Fr_(dofPos_[i]) =
+	(Ts - Tc) * exp(pow(-abs(w / wbrk), 2)) * sign +
+	Tc * sign + Tv * w;
+    }
+  }
+}
+
+StribeckModelFriction::StribeckModelFriction(const MultiBody & mb)
+  : Friction(mb)
+{}
+
+void StribeckModelFriction::computeFriction(const MultiBody & mb, const MultiBodyConfig & mbc)
+{
+  Fr_.setZero();
+
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double Ts = mb.joint(i).staticFriction();
+      double Tc = mb.joint(i).kineticFriction();
+      double Tv = mb.joint(i).viscousFrictionCoeff();
+
+      double wbrk = mb.joint(i).breakawayVelocity();
+      double wst = wbrk * sqrt(2);
+      double wcoul = wbrk / 10;
+
+      double w = mbc.alpha[i][0];
+
+      Fr_(dofPos_[i]) =
+	sqrt(2 * exp(1)) * (Ts - Tc) * exp(pow(-abs(w / wst), 2)) * (w / wst) +
+	Tc * tanh(w / wcoul) + Tv * w;
+    }
+  }
+}
+
+ImplEulerIntModelFriction::ImplEulerIntModelFriction(const MultiBody & mb, double Kf, double dt)
+  : Friction(mb), Kf_(Kf), dt_(dt), e_(Eigen::VectorXd::Zero(mb.nrDof()))
+{}
+
+ImplEulerIntModelCoulombFriction::ImplEulerIntModelCoulombFriction(const MultiBody & mb, double Kf, double dt)
+  : ImplEulerIntModelFriction(mb, Kf, dt)
+{}
+
+void ImplEulerIntModelCoulombFriction::computeFriction(const MultiBody & mb, const MultiBodyConfig & mbc)
+{
+  Fr_.setZero();
+
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double Tc = mb.joint(i).kineticFriction();
+      double Tv = mb.joint(i).viscousFrictionCoeff();
+
+      double w = mbc.alpha[i][0];
+
+      double Bf = 1 / dt_;
+      if (Tv >= Kf_ * dt_ + Bf)
+	Bf = Tv - Kf_ * dt_ + EPSILON;
+
+      double Z = 1 / (Kf_ * dt_ + Bf);
+
+      double w_ast = w + Z * Kf_ * e_(dofPos_[i]);
+      double T_ast = w_ast / Z;
+
+      double den = 1 + Z * Tv;
+
+      if (T_ast > Tc)
+        Fr_(dofPos_[i]) = (Tc + Tv * w_ast) / den;
+      else if (T_ast < -Tc)
+        Fr_(dofPos_[i]) = (-Tc + Tv * w_ast) / den;
+      else
+        Fr_(dofPos_[i]) = T_ast;
+
+      e_(dofPos_[i]) = Z * (Bf * e_(dofPos_[i]) + Fr_(dofPos_[i]) * dt_);
+    }
+  }
+}
+
+ImplEulerIntModelExpStictionFriction::ImplEulerIntModelExpStictionFriction(const MultiBody & mb, double Kf, double dt)
+  : ImplEulerIntModelFriction(mb, Kf, dt)
+{}
+
+void ImplEulerIntModelExpStictionFriction::computeFriction(const MultiBody & mb, const MultiBodyConfig & mbc)
+{
+  Fr_.setZero();
+
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double Ts = mb.joint(i).staticFriction();
+      double Tc = mb.joint(i).kineticFriction();
+      double Tv = mb.joint(i).viscousFrictionCoeff();
+      double wbrk = mb.joint(i).breakawayVelocity();
+
+      double Tsc = Ts - Tc;
+
+      double w = mbc.alpha[i][0];
+
+      double Bf = Ts / wbrk - Kf_ * dt_;
+      double Z = 1 / (Kf_ * dt_ + Bf);
+
+      double w_ast = w + Z * Kf_ * e_(dofPos_[i]);
+      double T_ast = w_ast / Z;
+
+      double den = 1 + Z * Tv;
+
+      if (T_ast > Ts) {
+        double lambArg = -Z/wbrk * Tsc/den * exp(Z/wbrk * (Tc + Tv*w_ast) / den - w_ast/wbrk);
+        Fr_(dofPos_[i]) = -(wbrk/Z) * utl::LambertW<0>(lambArg) + (Tc + Tv*w_ast) / den;
+      }
+      else if (T_ast < -Ts) {
+        double lambArg = -Z/wbrk * Tsc/den * exp(-Z/wbrk * (-Tc + Tv*w_ast) / den + w_ast/wbrk);
+        Fr_(dofPos_[i]) = (wbrk/Z) * utl::LambertW<0>(lambArg) + (-Tc + Tv*w_ast) / den;
+      }
+      else
+        Fr_(dofPos_[i]) = T_ast;
+
+      e_(dofPos_[i]) = Z * (Bf * e_(dofPos_[i]) + Fr_(dofPos_[i]) * dt_);
+    }
+  }
+}
+
+ImplEulerIntModelRatStictionFriction::ImplEulerIntModelRatStictionFriction(const MultiBody & mb, double Kf, double dt)
+  : ImplEulerIntModelFriction(mb, Kf, dt)
+{}
+
+void ImplEulerIntModelRatStictionFriction::computeFriction(const MultiBody & mb, const MultiBodyConfig & mbc)
+{
+  Fr_.setZero();
+
+  for(int i = 0; i < mb.nrJoints(); ++i)
+  {
+    if(mb.joint(i).type() == Joint::Rev)
+    {
+      double Ts = mb.joint(i).staticFriction();
+      double Tc = mb.joint(i).kineticFriction();
+      double Tv = mb.joint(i).viscousFrictionCoeff();
+      double wbrk = mb.joint(i).breakawayVelocity();
+
+      double Tsc = Ts - Tc;
+      double r   = Tsc / wbrk - Tv;
+
+      double w = mbc.alpha[i][0];
+
+      double Bf = Ts / wbrk - Kf_ * dt_;
+      if (r >= Kf_ * dt_ + Bf)
+	Bf = r - Kf_ * dt_ + EPSILON;
+
+      double Z = 1 / (Kf_ * dt_ + Bf);
+
+      double w_ast = w + Z * Kf_ * e_(dofPos_[i]);
+      double T_ast = w_ast / Z;
+
+      double delta = abs(r + Tv) > EPSILON ? Tsc / (r + Tv) : 0.0;
+      double alpha = Tv * delta + Tc;
+      double beta  = Ts * delta;
+      double a = Tv * pow(Z, 2) + Z;
+
+      if (T_ast > Ts) {
+	double b = -(w_ast + delta + 2 * Tv * Z * w_ast + alpha * Z);
+	double c = Tv * pow(w_ast, 2) + alpha * w_ast + beta;
+	Fr_(dofPos_[i]) = (-b - sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
+      }
+      else if (T_ast < -Ts) {
+	double b = -(-w_ast + delta - 2 * Tv * Z * w_ast + alpha * Z);
+	double c = Tv * pow(w_ast, 2) - alpha * w_ast + beta;
+	Fr_(dofPos_[i]) = ( b + sqrt(pow(b, 2) - 4 * a * c)) / (2 * a);
+      }
+      else
+	Fr_(dofPos_[i]) = T_ast;
+
+      e_(dofPos_[i]) = Z * (Bf * e_(dofPos_[i]) + Fr_(dofPos_[i]) * dt_);
+    }
+  }
+}
+
+} // namespace rbd
diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp
@@ -14,6 +14,8 @@
 // RBDyn
 #include "RBDyn/MultiBodyConfig.h"
 
+#include <iostream>
+
 namespace rbd
 {
 

diff --git a/src/RBDyn/MultiBodyConfig.cpp b/src/RBDyn/MultiBodyConfig.cpp
@@ -406,7 +406,7 @@ void checkMatchJointTorque(const MultiBody & mb, const MultiBodyConfig & mbc)
     {
       std::ostringstream str;
       str << "Bad number of torque variable for Joint " << mb.joint(i) << " at position " << i << ": expected size "
-          << mb.joint(i).dof() << " gived " << mbc.jointTorque[ui].size();
+          << mb.joint(i).dof() << " gave " << mbc.jointTorque[ui].size();
       throw std::domain_error(str.str());
     }
   }
@@ -423,7 +423,7 @@ void checkMatchMotionSubspace(const MultiBody & mb, const MultiBodyConfig & mbc)
     {
       std::ostringstream str;
       str << "Bad motionSubspace matrix size for Joint " << mb.joint(i) << " at position " << i
-          << ": expected column number " << mb.joint(i).dof() << " gived " << mbc.motionSubspace[ui].cols();
+          << ": expected column number " << mb.joint(i).dof() << " gave " << mbc.motionSubspace[ui].cols();
       throw std::domain_error(str.str());
     }
   }
@@ -440,7 +440,7 @@ void checkMatchQ(const MultiBody & mb, const MultiBodyConfig & mbc)
     {
       std::ostringstream str;
       str << "Bad number of generalized position variable for Joint " << mb.joint(i) << " at position " << i
-          << ": expected size " << mb.joint(i).params() << " gived " << mbc.q[ui].size();
+          << ": expected size " << mb.joint(i).params() << " gave " << mbc.q[ui].size();
       throw std::domain_error(str.str());
     }
   }
@@ -457,7 +457,7 @@ void checkMatchAlpha(const MultiBody & mb, const MultiBodyConfig & mbc)
     {
       std::ostringstream str;
       str << "Bad number of generalized velocity variable for Joint " << mb.joint(i) << " at position " << i
-          << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alpha[ui].size();
+          << ": expected size " << mb.joint(i).dof() << " gave " << mbc.alpha[ui].size();
       throw std::domain_error(str.str());
     }
   }
@@ -474,7 +474,7 @@ void checkMatchAlphaD(const MultiBody & mb, const MultiBodyConfig & mbc)
     {
       std::ostringstream str;
       str << "Bad number of generalized acceleration variable for Joint " << mb.joint(i) << " at position " << i
-          << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alphaD[ui].size();
+          << ": expected size " << mb.joint(i).dof() << " gave " << mbc.alphaD[ui].size();
       throw std::domain_error(str.str());
     }
   }