first skeleton of tsid

src/CMakeLists.txt

@@ -13,3 +13,5 @@ add_subdirectory(KinDynWrapper)
 add_subdirectory(RetargetingHelper)
 add_subdirectory(WalkingModule)
 add_subdirectory(JoypadModule)
+add_subdirectory(JointControllers)
+add_subdirectory(Integrators)

src/Integrators/CMakeLists.txt

@@ -0,0 +1,11 @@
+# Copyright (C) 2025 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Lorenzo Moretti <[email protected]>
+
+add_walking_controllers_library(
+  NAME Integrators
+  SOURCES src/jointIntegrators.cpp
+  PUBLIC_HEADERS include/WalkingControllers/Integrators/jointIntegrators.h
+  PUBLIC_LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem
+                        BipedalLocomotion::TextLogging
+                        Eigen3::Eigen) 

src/Integrators/include/WalkingControllers/Integrators/jointIntegrators.h

@@ -0,0 +1,79 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/RK4.h>
+
+#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS
+#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS
+
+namespace WalkingControllers {
+
+class JointAccelerationIntegrator {
+
+  private:
+    std::shared_ptr<
+        BipedalLocomotion::ContinuousDynamicalSystem::LinearTimeInvariantSystem>
+        dynamics;
+    std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::RK4<
+        BipedalLocomotion::ContinuousDynamicalSystem::
+            LinearTimeInvariantSystem>>
+        integrator;
+
+    bool m_isInitialized{
+        false}; /**< true if the integrator is successfully initialized */
+
+    int m_numberOfJoints{0}; /**< number of joints */
+
+    std::chrono::nanoseconds m_dT{0}; /**< time step */
+
+    Eigen::VectorXd m_jointPosition; /**< joint position */
+    Eigen::VectorXd m_jointVelocity; /**< joint velocity */
+
+  public:
+    /**
+     * @brief Initialize the integrator.
+     * @param numberOfJoints number of joints.
+     * @param dt time step in seconds.
+     * @return true if successful.
+     */
+    bool initialize(int numberOfJoints, double dt);
+
+    /**
+     * @brief Set the input of the integrator.
+     * @param jointAcceleration joint acceleration.
+     * @return true if successful.
+     */
+    bool setInput(const Eigen::Ref<const Eigen::VectorXd> &jointAcceleration);
+
+    /**
+     * @brief Set the state of the integrator.
+     * @param jointposition joint position.
+     * @param jointVelocity joint velocity.
+     * @return true if successful.
+     */
+    bool setState(const Eigen::Ref<const Eigen::VectorXd> &jointposition,
+                  const Eigen::Ref<const Eigen::VectorXd> &jointVelocity);
+
+    /**
+     * @brief Perform one step integration.
+     * @return true if successful.
+     */
+    bool oneStepIntegration();
+
+    /**
+     * @brief Get the joint position.
+     * @return joint position.
+     */
+    const Eigen::VectorXd &getJointPosition() const;
+
+    /**
+     * @brief Get the joint velocity.
+     * @return joint velocity.
+     */
+    const Eigen::VectorXd &getJointVelocity() const;
+};
+
+} // namespace WalkingControllers
+
+#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS

src/Integrators/src/jointIntegrators.cpp

@@ -0,0 +1,168 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <WalkingControllers/Integrators/jointIntegrators.h>
+
+#include <BipedalLocomotion/TextLogging/Logger.h>
+#include <chrono>
+
+using namespace WalkingControllers;
+
+bool JointAccelerationIntegrator::initialize(int numberOfJoints, double dt) {
+
+    constexpr auto logPrefix = "[JointAccelerationIntegrator::initialize]";
+
+    dynamics = std::make_shared<BipedalLocomotion::ContinuousDynamicalSystem::
+                                    LinearTimeInvariantSystem>();
+
+    m_numberOfJoints = numberOfJoints;
+
+    // Set joints state space system matrices.
+    //
+    // [s_dot; s_ddot] = [0, I; 0, 0] * [s; s_dot] + [0; I] * sddot
+    //
+    // therefore
+    //
+    // A = [0, I; 0, 0]
+    // b = [0; 1]
+    Eigen::MatrixXd A =
+        Eigen::MatrixXd::Zero(2 * m_numberOfJoints, 2 * m_numberOfJoints);
+    Eigen::MatrixXd B =
+        Eigen::MatrixXd::Zero(2 * m_numberOfJoints, m_numberOfJoints);
+    A.block(0, m_numberOfJoints, m_numberOfJoints, m_numberOfJoints)
+        .setIdentity();
+    B.block(m_numberOfJoints, 0, m_numberOfJoints, m_numberOfJoints)
+        .setIdentity();
+
+    if (!dynamics->setSystemMatrices(A, B)) {
+        BipedalLocomotion::log()->error(
+            "{} Failed to set system matrices of the dynamical system",
+            logPrefix);
+        return false;
+    };
+
+    // set dynamical system of the integrator
+    integrator =
+        std::make_shared<BipedalLocomotion::ContinuousDynamicalSystem::RK4<
+            BipedalLocomotion::ContinuousDynamicalSystem::
+                LinearTimeInvariantSystem>>();
+    if (!integrator->setDynamicalSystem(dynamics)) {
+        BipedalLocomotion::log()->error(
+            "{} Failed to set dynamical system of the integrator", logPrefix);
+        return false;
+    };
+
+    // set integration step
+    m_dT = std::chrono::milliseconds(static_cast<int>(dt * 1000));
+    if (!integrator->setIntegrationStep(m_dT)) {
+        BipedalLocomotion::log()->error(
+            "{} Failed to set integration step of the integrator", logPrefix);
+        return false;
+    };
+
+    m_jointPosition.resize(m_numberOfJoints);
+    m_jointPosition.setZero();
+    m_jointVelocity.resize(m_numberOfJoints);
+    m_jointVelocity.setZero();
+
+    m_isInitialized = true;
+    return true;
+}
+
+bool JointAccelerationIntegrator::setInput(
+    const Eigen::Ref<const Eigen::VectorXd> &jointAcceleration) {
+
+    constexpr auto logPrefix = "[JointAccelerationIntegrator::setInput]";
+
+    if (!m_isInitialized) {
+        BipedalLocomotion::log()->error("{} Integrator is not initialized",
+                                        logPrefix);
+        return false;
+    }
+
+    if (jointAcceleration.size() != m_numberOfJoints) {
+        BipedalLocomotion::log()->error("{} Size of joint acceleration is not "
+                                        "equal to the number of joints",
+                                        logPrefix);
+        return false;
+    }
+
+    if (!dynamics->setControlInput({jointAcceleration})) {
+        BipedalLocomotion::log()->error(
+            "{} Failed to set input of the dynamical system", logPrefix);
+        return false;
+    }
+
+    return true;
+}
+
+bool JointAccelerationIntegrator::setState(
+    const Eigen::Ref<const Eigen::VectorXd> &jointposition,
+    const Eigen::Ref<const Eigen::VectorXd> &jointVelocity) {
+
+    constexpr auto logPrefix = "[JointAccelerationIntegrator::setState]";
+
+    if (!m_isInitialized) {
+        BipedalLocomotion::log()->error("{} Integrator is not initialized",
+                                        logPrefix);
+        return false;
+    }
+
+    if (jointposition.size() != m_numberOfJoints) {
+        BipedalLocomotion::log()->error(
+            "{} Size of joint position is not equal to the number of joints",
+            logPrefix);
+        return false;
+    }
+
+    if (jointVelocity.size() != m_numberOfJoints) {
+        BipedalLocomotion::log()->error(
+            "{} Size of joint velocity is not equal to the number of joints",
+            logPrefix);
+        return false;
+    }
+
+    Eigen::VectorXd state(2 * m_numberOfJoints);
+    state << jointposition, jointVelocity;
+
+    if (!dynamics->setState({state})) {
+        BipedalLocomotion::log()->error(
+            "{} Failed to set state of the dynamical system",
+            logPrefix);
+        return false;
+    }
+
+    return true;
+}
+
+bool JointAccelerationIntegrator::oneStepIntegration() {
+
+    constexpr auto logPrefix =
+        "[JointAccelerationIntegrator::oneStepIntegration]";
+
+    if (!m_isInitialized) {
+        BipedalLocomotion::log()->error("{} Integrator is not initialized",
+                                        logPrefix);
+        return false;
+    }
+
+    if (!integrator->oneStepIntegration(std::chrono::nanoseconds(0), m_dT)) {
+        BipedalLocomotion::log()->error("{} Failed to advance the integrator",
+                                        logPrefix);
+        return false;
+    }
+
+    const auto &[stateX] = integrator->getSolution();
+    m_jointPosition = stateX.head(m_numberOfJoints);
+    m_jointVelocity = stateX.tail(m_numberOfJoints);
+
+    return true;
+}
+
+const Eigen::VectorXd &JointAccelerationIntegrator::getJointPosition() const {
+    return m_jointPosition;
+}
+
+const Eigen::VectorXd &JointAccelerationIntegrator::getJointVelocity() const {
+    return m_jointVelocity;
+}

src/JointControllers/CMakeLists.txt

@@ -0,0 +1,11 @@
+# Copyright (C) 2025 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Lorenzo Moretti <[email protected]>
+
+add_walking_controllers_library(
+  NAME JointControllers
+  SOURCES src/AdmittanceController.cpp
+  PUBLIC_HEADERS include/WalkingControllers/JointControllers/AdmittanceController.h
+  PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler
+                        BipedalLocomotion::TextLogging
+                        Eigen3::Eigen) 

src/JointControllers/include/WalkingControllers/JointControllers/AdmittanceController.h

@@ -0,0 +1,80 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER
+#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER
+
+#include <Eigen/Dense>
+#include <memory>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+
+namespace WalkingControllers {
+
+class AdmittanceController {
+
+    struct Input {
+        Eigen::VectorXd jointTorqueFeedforward; /**< Joint position desired */
+        Eigen::VectorXd jointPositionDesired;   /**< Joint position desired */
+        Eigen::VectorXd jointVelocityDesired;   /**< Joint velocity desired*/
+        Eigen::VectorXd jointPosition;          /**< Joint position */
+        Eigen::VectorXd jointVelocity;          /**< Joint velocity */
+    };
+    Input m_input; /**< Input of the admittance controller */
+
+    struct Output {
+        Eigen::VectorXd jointTorque; /**< Joint Torque */
+    };
+    Output m_output; /**< Output of the admittance controller */
+
+    bool m_isInitialized{false}; /**< true if the admittance controller is
+                                    successfully initialized */
+    struct Gains {
+        Eigen::VectorXd kp; /**< Proportional gains */
+        Eigen::VectorXd kd; /**< Derivative gains */
+    };
+    Gains m_gains; /**< gains of the admittance controller */
+
+    int m_numberOfJoints; /**< Number of controlled joints */
+
+  public:
+    AdmittanceController() = default;
+
+    ~AdmittanceController() = default;
+
+    /**
+     * @brief Initialize the admittance controller.
+     * @param parametersHandler parameters handler.
+     * @return true if successful.
+     */
+    bool
+    initialize(std::shared_ptr<
+               const BipedalLocomotion::ParametersHandler::IParametersHandler>
+                   parametersHandler);
+    /**
+     * @brief Advance the admittance controller.
+     * @return true if successful.
+     */
+    bool advance();
+
+    /**
+     * @brief Set the input to the admittance controller.
+     * @return output of the admittance controller.
+     */
+    bool
+    setInput(const Eigen::Ref<const Eigen::VectorXd> &jointTorqueFeedforward,
+             const Eigen::Ref<const Eigen::VectorXd> &jointPositionDesired,
+             const Eigen::Ref<const Eigen::VectorXd> &jointVelocityDesired,
+             const Eigen::Ref<const Eigen::VectorXd> &jointPosition,
+             const Eigen::Ref<const Eigen::VectorXd> &jointVelocity);
+
+    /**
+     * @brief Get the output of the admittance controller (i.e., joint torques).
+     * @return output of the admittance controller.
+     */
+     const Eigen::VectorXd &getOutput() const;
+};
+
+} // namespace WalkingControllers
+
+#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER