Merge pull request #379 from dic-iit/ContinuousDynamicalSystem/manif

Add the possibility to use manif objects in the ForwardEuler integrator

CHANGELOG.md

@@ -22,6 +22,7 @@ All notable changes to this project are documented in this file.
 - `QPFixedBaseTSID` now inherits from `QPTSID` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/366)
 - Enable the Current control in `RobotInterface` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/375)
 - Add the possibility to disable and enable the PD controllers in `IK::SE3Task` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/373).
+- Add the possibility to use manif objects in the ForwardEuler integrator (https://github.com/dic-iit/bipedal-locomotion-framework/pull/379).
 
 ### Fix
 - Fixed the crashing of `YarpSensorBridge` while trying to access unconfigured control board sensors data by adding some checks (https://github.com/dic-iit/bipedal-locomotion-framework/pull/378)

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/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h

@@ -35,6 +35,13 @@ namespace ContinuousDynamicalSystem
  * @tparam _DynamicalSystem a class derived from DynamicalSystem
  * @warning We assume that the operator + is defined for the objects contained in the
  * DynamicalSystem::State and DynamicalSystem::StateDerivative.
+ * @warning The ForwardEuler integrator is compatible with Lie groups defined by the `manif` library.
+ * Since the _box-plus_ operator is not commutative for a Lie group, here we consider only manifold
+ * left plus
+ * \f[
+ * X + \psi = X \circ \exp(\psi)
+ * \f]
+ * where \f$X\f$ belongs to a Lie group and $\f\psi\f$ belongs to the tangent space.
  */
 template <class _DynamicalSystem>
 class ForwardEuler : public FixedStepIntegrator<ForwardEuler<_DynamicalSystem>>
@@ -61,7 +68,8 @@ class ForwardEuler : public FixedStepIntegrator<ForwardEuler<_DynamicalSystem>>
  {
  static_assert(sizeof...(Tp) == sizeof...(Td));
 
- std::get<I>(x) += std::get<I>(dx) * dT;
+ // the order matters since we assume that all the velocities are left trivialized.
+ std::get<I>(x) = (std::get<I>(dx) * dT) + std::get<I>(x);
  addArea<I + 1>(dx, dT, x);
  }
 
@@ -91,7 +99,7 @@ bool ForwardEuler<_DynamicalSystem>::oneStepIntegration(double t0, double dT)
  return false;
  }
 
- // x = x0 + dT * dx
+ // x = dT * dx + x0
  this->m_computationalBufferState = this->m_dynamicalSystem->getState();
  this->addArea(this->m_computationalBufferStateDerivative, dT, this->m_computationalBufferState);
 

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;
 

src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp

@@ -12,6 +12,8 @@
 
 #include <Eigen/Dense>
 
+#include <manif/SO3.h>
+
 #include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
 #include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
 
@@ -31,7 +33,7 @@ TEST_CASE("Integrator - Linear system")
  Eigen::VectorXd jointVelocity(20);
  jointVelocity.setRandom();
 
- Eigen::Matrix3d rotation0;
+ manif::SO3d rotation0;
  rotation0.setIdentity();
 
  Eigen::Vector3d position0;
@@ -44,27 +46,28 @@ TEST_CASE("Integrator - Linear system")
  Eigen::Vector3d pos = position0 + t * twist.head<3>();
  Eigen::Matrix3d rot
  = Eigen::AngleAxisd(twist.tail<3>().norm() * t, twist.tail<3>().normalized())
-  * rotation0;
+ * rotation0.rotation();
 
  Eigen::VectorXd jointPos = jointPosition0 + t * jointVelocity;
  return std::make_tuple(pos, rot, jointPos);
  };
 
- system->setControlInput({twist, jointVelocity});
  system->setState({position0, rotation0, jointPosition0});
 
  ForwardEuler<FloatingBaseSystemKinematics> integrator;
  integrator.setIntegrationStep(dT);
  integrator.setDynamicalSystem(system);
 
+ system->setControlInput({twist, jointVelocity});
+
  for (int i = 0; i < simulationTime / dT; i++)
  {
  const auto& [basePosition, baseRotation, jointPosition] = integrator.getSolution();
 
  const auto& [basePositionExact, baseRotationExact, jointPositionExact]
  = closeFormSolution(dT * i);
 
- REQUIRE(baseRotation.isApprox(baseRotationExact, tolerance));
+ REQUIRE(baseRotation.rotation().isApprox(baseRotationExact, tolerance));
  REQUIRE(basePosition.isApprox(basePositionExact, tolerance));
  REQUIRE(jointPosition.isApprox(jointPositionExact, tolerance));
 

src/IK/tests/QPInverseKinematicsTest.cpp

@@ -196,7 +196,7 @@ System getSystem(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
 
  out.dynamics = std::make_shared<FloatingBaseSystemKinematics>();
  out.dynamics->setState({basePose.topRightCorner<3, 1>(),
- basePose.topLeftCorner<3, 3>(),
+ toManifRot(basePose.topLeftCorner<3, 3>()),
  jointPositions});
 
  out.integrator = std::make_shared<ForwardEuler<FloatingBaseSystemKinematics>>();
@@ -266,7 +266,7 @@ TEST_CASE("QP-IK")
  = system.integrator->getSolution();
 
  // update the KinDynComputations object
- baseTransform.topLeftCorner<3, 3>() = baseRotation;
+ baseTransform.topLeftCorner<3, 3>() = baseRotation.rotation();
  baseTransform.topRightCorner<3, 1>() = basePosition;
  REQUIRE(kinDyn->setRobotState(baseTransform,
  jointPosition,