Avoid to use Baumgarte in FloatingBaseSystemKinematics class

src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h

@@ -14,6 +14,8 @@
 #include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 
+#include <manif/SO3.h>
+
 #include <Eigen/Dense>
 
 namespace BipedalLocomotion
@@ -30,12 +32,12 @@ class FloatingBaseSystemKinematics;
 // BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(
 // FloatingBaseSystemKinematics,
 // (base position, base orientation, joint positions),
-// (base linear velocity, rate of change of base rotation matrix, joint velocities),
+// (base linear velocity, base angular velocity, joint velocities),
 // (base twist expressed in mixed representation, joint velocities))
 BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(
  FloatingBaseSystemKinematics,
- (Eigen::Vector3d, Eigen::Matrix3d, Eigen::VectorXd),
- (Eigen::Vector3d, Eigen::Matrix3d, Eigen::VectorXd),
+ (Eigen::Vector3d, manif::SO3d, Eigen::VectorXd),
+ (Eigen::Vector3d, manif::SO3d::Tangent, Eigen::VectorXd),
  (Eigen::Matrix<double, 6, 1>, Eigen::VectorXd));
 
 namespace BipedalLocomotion
@@ -48,12 +50,12 @@ namespace ContinuousDynamicalSystem
  * The FloatingBaseSystemKinematics inherits from a generic DynamicalSystem where:
  * - DynamicalSystem::State is described by an std::tuple containing:
  * - Eigen::Vector3d: position of the base w.r.t. the inertial frame
- * - Eigen::Matrix3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector
+ * - manif::SO3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector
  * whose coordinates are expressed in the base frame in the inertial frame;
  * - Eigen::VectorXd: the joint positions [in rad].
  * - DynamicalSystem::StateDerivative is described by an std::tuple containing:
  * - Eigen::Vector3d: base linear velocity w.r.t. the inertial frame;
- * - Eigen::Matrix3d: rate of change of the rotation matrix \f${} ^ I \dot{R} _ {b}\f$.
+ * - manif::SO3d::Tangent: base angular velocity w.r.t. the inertial frame; (Left trivialized)
  * whose coordinates are expressed in the base frame in the inertial frame;
  * - Eigen::VectorXd: the joint velocities [in rad/s].
  * - DynamicalSystem::Input is described by an std::tuple containing:
@@ -62,21 +64,15 @@ namespace ContinuousDynamicalSystem
  */
 class FloatingBaseSystemKinematics : public DynamicalSystem<FloatingBaseSystemKinematics>
 {
- double m_rho{0.01}; /**< Regularization term used for the Baumgarte stabilization over the SO(3)
- group */
-
  State m_state;
  Input m_controlInput;
 
 public:
  /**
- * Initialize the FloatingBaseSystemKinematics system.
+ * Initialize the Dynamical system.
  * @param handler pointer to the parameter handler.
- * @note The following parameters are used
- * | Parameter Name | Type | Description | Mandatory |
- * |:--------------:|:--------:|:---------------------------------------------------------------------------------:|:---------:|
- * | `rho` | `double` | Baumgarte stabilization parameter over the SO(3) group. The default value is 0.01 | No |
  * @return true in case of success/false otherwise.
+ * @note This function does nothing but it is required for CRTP.
  */
  bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
 

src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp

@@ -16,20 +16,15 @@ bool FloatingBaseSystemKinematics::initialize(std::weak_ptr<IParametersHandler>
  constexpr auto logPrefix = "[FloatingBaseSystemKinematics::initialize]";
 
  auto ptr = handler.lock();
- if (ptr == nullptr)
+ if (ptr != nullptr)
  {
- log()->error("{} The parameter handler is expired. Please call the function passing a "
- "pointer pointing an already allocated memory.",
- logPrefix);
- return false;
- }
-
- if (!ptr->getParameter("rho", m_rho))
- {
- log()->info("{} The Baumgarte stabilization parameter not found. The default one will be "
- "used {}.",
- logPrefix,
- m_rho);
+ double baumgarte = 0;
+ if (ptr->getParameter("rho", baumgarte))
+ {
+ log()->info("{} The rho parameter of the Baumgarte stabilization is no more required. "
+ "The integration is computed on the manifold directly",
+ logPrefix);
+ }
  }
 
  return true;
@@ -40,12 +35,12 @@ bool FloatingBaseSystemKinematics::dynamics(const double& time,
 {
  // get the state
  const Eigen::Vector3d& basePosition = std::get<0>(m_state);
- const Eigen::Matrix3d& baseRotation = std::get<1>(m_state);
+ const manif::SO3d& baseRotation = std::get<1>(m_state);
  const Eigen::VectorXd& jointPositions = std::get<2>(m_state);
 
  // get the state derivative
  Eigen::Vector3d& baseLinearVelocity = std::get<0>(stateDerivative);
- Eigen::Matrix3d& baseRotationRate = std::get<1>(stateDerivative);
+ manif::SO3d::Tangent& baseAngularVelocity = std::get<1>(stateDerivative);
  Eigen::VectorXd& jointVelocityOutput = std::get<2>(stateDerivative);
 
  const Eigen::Matrix<double, 6, 1>& baseTwist = std::get<0>(m_controlInput);
@@ -54,17 +49,15 @@ bool FloatingBaseSystemKinematics::dynamics(const double& time,
  // check the size of the vectors
  if (jointVelocity.size() != jointPositions.size())
  {
- std::cerr << "[FloatingBaseSystemKinematics::dynamics] Wrong size of the vectors."
- << std::endl;
+ log()->error("[FloatingBaseSystemKinematics::dynamics] Wrong size of the vectors.");
  return false;
  }
 
  // compute the base linear velocity
  baseLinearVelocity = baseTwist.head<3>();
 
  // here we assume that the velocity is expressed using the mixed representation
- baseRotationRate = -baseRotation.colwise().cross(baseTwist.tail<3>())
- + m_rho / 2.0 * (baseRotation.transpose().inverse() - baseRotation);
+ baseAngularVelocity = baseTwist.tail<3>();
 
  jointVelocityOutput = jointVelocity;