CableJointMoment.h

#pragma once
 
#include <Eigen/Eigen>
 
#include <cmath>
 
namespace RbCable
{
constexpr double kBendingDampingRatio = 0.05;
 
constexpr double kTwistDampingRatio = 1.0;
 
struct JointLaw
{
   Eigen::Vector3d stiffness; 
   Eigen::Vector3d damping;   
 
   static JointLaw Of(double bendingStiffness, double torsionStiffness, double length, double inertiaBending, double inertiaTwist)
   {
      const auto   damping = [&](double ratio, double inertia, double stiffness) { return 2.0 * ratio * std::sqrt(inertia * stiffness / length); };
      const double bending = damping(kBendingDampingRatio, inertiaBending, bendingStiffness);
      return {{bendingStiffness, bendingStiffness, torsionStiffness}, {bending, bending, damping(kTwistDampingRatio, inertiaTwist, torsionStiffness)}};
   }
};
 
inline Eigen::Vector3d JointMomentOnNext(const JointLaw& law, double length, const Eigen::Quaterniond& q, const Eigen::Quaterniond& qNext,
      const Eigen::Vector3d& w, const Eigen::Vector3d& wNext)
{
   const Eigen::Matrix3d R              = q.toRotationMatrix();
   const Eigen::Vector3d relativeVector = (q.conjugate() * qNext).vec();
   const Eigen::Vector3d relativeRate   = R.transpose() * (wNext - w);
   const Eigen::Vector3d momentInFrame  = -2.0 / length * law.stiffness.cwiseProduct(relativeVector) - law.damping.cwiseProduct(relativeRate);
   return R * momentInFrame;
}
} // namespace RbCable