diff --git a/CHANGELOG.md b/CHANGELOG.md index 05a4ff2490..d284ed5b9b 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h index 34fafe6349..86a71c33ba 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h @@ -14,6 +14,8 @@ #include #include +#include + #include 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, 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 { - 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 handler); diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h index 652b0caf33..68528312fa 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h +++ b/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 ForwardEuler : public FixedStepIntegrator> @@ -61,7 +68,8 @@ class ForwardEuler : public FixedStepIntegrator> { static_assert(sizeof...(Tp) == sizeof...(Td)); - std::get(x) += std::get(dx) * dT; + // the order matters since we assume that all the velocities are left trivialized. + std::get(x) = (std::get(dx) * dT) + std::get(x); addArea(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); diff --git a/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp index 429b01e38c..d54350633e 100644 --- a/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp +++ b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp @@ -16,20 +16,15 @@ bool FloatingBaseSystemKinematics::initialize(std::weak_ptr 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& baseTwist = std::get<0>(m_controlInput); @@ -54,8 +49,7 @@ 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; } @@ -63,8 +57,7 @@ bool FloatingBaseSystemKinematics::dynamics(const double& time, 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; diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp index 32e11869ca..f4b0a3fe32 100644 --- a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp +++ b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp @@ -12,6 +12,8 @@ #include +#include + #include #include @@ -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,19 +46,20 @@ 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 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(); @@ -64,7 +67,7 @@ TEST_CASE("Integrator - Linear system") 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)); diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 83c2ae765c..372c4cae59 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -196,7 +196,7 @@ System getSystem(std::shared_ptr kinDyn) out.dynamics = std::make_shared(); out.dynamics->setState({basePose.topRightCorner<3, 1>(), - basePose.topLeftCorner<3, 3>(), + toManifRot(basePose.topLeftCorner<3, 3>()), jointPositions}); out.integrator = std::make_shared>(); @@ -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,