Implement SO3 task in TSID

CHANGELOG.md

@@ -28,6 +28,7 @@ All notable changes to this project are documented in this file.
 - Implement `handleQuitSignals()` function in System component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/277)
 - Implement TaskSpaceInverseDynamics interface (https://github.com/dic-iit/bipedal-locomotion-framework/pull/279)
 - Implement `Wrench` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/279)
+- Implement `SO3Task` in `TSID` component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/281)
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

src/TSID/CMakeLists.txt

@@ -8,15 +8,11 @@ if(FRAMEWORK_COMPILE_TSID)
 
   add_bipedal_locomotion_library(
     NAME                   TSID
-    PUBLIC_HEADERS         ${H_PREFIX}/SE3Task.h
-                           ${H_PREFIX}/JointTrackingTask.h
-                           ${H_PREFIX}/BaseDynamicsTask.h
-                           ${H_PREFIX}/JointDynamicsTask.h
+    PUBLIC_HEADERS         ${H_PREFIX}/SO3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/JointTrackingTask.h
+                           ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/JointDynamicsTask.h
                            ${H_PREFIX}/TaskSpaceInverseDynamics.h
-    SOURCES                src/SE3Task.cpp
-                           src/JointTrackingTask.cpp
-                           src/BaseDynamicsTask.cpp
-                           src/JointDynamicsTask.cpp
+    SOURCES                src/SO3Task.cpp src/SE3Task.cpp src/JointTrackingTask.cpp
+                           src/BaseDynamicsTask.cpp src/JointDynamicsTask.cpp
                            src/TaskSpaceInverseDynamics.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen
                            BipedalLocomotion::Contacts

src/TSID/include/BipedalLocomotion/TSID/SO3Task.h

@@ -0,0 +1,139 @@
+/**
+ * @file SO3Task.h
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_TSID_SO3_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_SO3_TASK_H
+
+#include <manif/manif.h>
+
+#include <BipedalLocomotion/System/LinearTask.h>
+
+#include <iDynTree/KinDynComputations.h>
+
+#include <LieGroupControllers/ProportionalDerivativeController.h>
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+
+/**
+ * SO3Task is a concrete implementation of the System::LinearTask. Please use this element if you
+ * want to control the orientation of a given frame rigidly attached to the robot.
+ * The task assumes perfect control of the robot acceleration \f$\dot{\nu}\f$ that contains the base
+ * linear and angular acceleration expressed in mixed representation and the joints acceleration.
+ * The task represents the following equation
+ * \f[
+ * J \dot{\nu} + \dot{J} \nu = \dot{\mathrm{v}} ^ *
+ * \f]
+ * where \f$J\f$ is the robot jacobian and \f$\dot{\mathrm{v}} ^ *\f$ is the desired acceleration.
+ * The desired acceleration is chosen such that the frame orientation will asymptotically converge
+ * to the desired trajectory. The angular acceleration is computed by a PD controller in
+ * \f$SO(3)\f$.
+ * @note Please refer to https://github.com/dic-iit/lie-group-controllers if you are interested in
+ * the implementation of the PD controllers.
+ */
+class SO3Task : public System::LinearTask
+{
+    LieGroupControllers::ProportionalDerivativeControllerSO3d m_SO3Controller; /**< PD Controller in
+                                                                                  SO(3) */
+
+    System::VariablesHandler::VariableDescription m_robotAccelerationVariable; /**< Variable
+                                                                                  describing the
+                                                                                  robot acceleration
+                                                                                  (base + joint) */
+
+    iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the task */
+
+    static constexpr std::size_t m_angularVelocitySize{3}; /**< Size of the angular velocity vector. */
+    static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */
+
+    bool m_isInitialized{false}; /**< True if the task has been initialized. */
+    bool m_isValid{false}; /**< True if the task is valid. */
+
+    std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
+                                                               object */
+
+    Eigen::MatrixXd m_jacobian; /**< Jacobian of the frame expressed in mixed representation */
+
+public:
+    /**
+     * Initialize the SO3Task.
+     * @param paramHandler pointer to the parameters handler.
+     * @note the following parameters are required by the class
+     * |           Parameter Name           |   Type   |                                       Description                                      | Mandatory |
+     * |:----------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:|
+     * | `robot_acceleration_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot acceleration |    Yes    |
+     * |            `frame_name`            | `string` |                       Name of the frame controlled by the SO3Task                      |    Yes    |
+     * |            `kp_angular`            | `double` |                           Gain of the orientation controller                           |    Yes    |
+     * |            `kd_angular`            | `double` |                         Gain of the angular velocity controller                        |    Yes    |
+     * @return True in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler);
+
+    /**
+     * Set the kinDynComputations object.
+     * @param kinDyn pointer to a kinDynComputations object.
+     * @return True in case of success, false otherwise.
+     */
+    bool setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
+
+    /**
+     * Set the set of variables required by the task. The variables are stored in the
+     * System::VariablesHandler.
+     * @param variablesHandler reference to a variables handler.
+     * @note The handler must contain a variable named as the parameter
+     * `robot_acceleration_variable_name` stored in the parameter handler. The variable represents
+     * the generalized acceleration of the robot, where the generalized robot acceleration is a
+     * vector containing the base spatial-acceleration (expressed in mixed representation) and the
+     * joints acceleration.
+     * @return True in case of success, false otherwise.
+     */
+    bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
+
+    /**
+     * Update the content of the element.
+     * @return True in case of success, false otherwise.
+     */
+    bool update() override;
+
+    /**
+     * Set the desired reference point.
+     * @param I_R_F Rotation between the link and the inertial frame.
+     * @param angularVelocity angular velocity of the frame F wr.t. the inertial frame expressed in
+     * the inertial frame.
+     * @param angularAcceleration angular acceleration of the frame F wr.t. the inertial frame
+     * expressed in the inertial frame.
+     * @return True in case of success, false otherwise.
+     */
+    bool setSetPoint(const manif::SO3d& I_R_F,
+                     const manif::SO3d::Tangent& angularVelocity,
+                     const manif::SO3d::Tangent& angularAcceleration);
+
+    /**
+     * Get the size of the task. (I.e the number of rows of the vector b)
+     * @return the size of the task.
+     */
+    std::size_t size() const override;
+
+    /**
+     * The SO3Task is an equality task.
+     * @return the type of the task.
+     */
+    Type type() const override;
+
+    /**
+     * Determines the validity of the objects retrieved with getA() and getB()
+     * @return True if the objects are valid, false otherwise.
+     */
+    bool isValid() const override;
+};
+
+} // namespace TSID
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_TSID_SO3_TASK_H

src/TSID/src/SO3Task.cpp

@@ -0,0 +1,208 @@
+/**
+ * @file SO3Task.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <BipedalLocomotion/Conversions/ManifConversions.h>
+#include <BipedalLocomotion/TSID/SO3Task.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Model/Model.h>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::System;
+using namespace BipedalLocomotion::TSID;
+
+bool SO3Task::setVariablesHandler(const System::VariablesHandler& variablesHandler)
+{
+    if (!m_isInitialized)
+    {
+        log()->error("[SO3Task::setVariablesHandler] The task is not initialized. Please call "
+                     "initialize method.");
+        return false;
+    }
+
+    // get the variable
+    if (!variablesHandler.getVariable(m_robotAccelerationVariable.name,
+                                      m_robotAccelerationVariable))
+    {
+        log()->error("[SO3Task::setVariablesHandler] Unable to get the variable named {}.",
+                     m_robotAccelerationVariable.name);
+        return false;
+    }
+
+    // get the variable
+    if (m_robotAccelerationVariable.size
+        != m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize)
+    {
+        log()->error("[SO3Task::setVariablesHandler] The size of the robot velocity variable is "
+                     "different from the one expected. Expected size: {}. Given size: {}.",
+                     m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize,
+                     m_robotAccelerationVariable.size);
+        return false;
+    }
+
+    // resize the matrices
+    m_A.resize(m_angularVelocitySize, variablesHandler.getNumberOfVariables());
+    m_A.setZero();
+    m_b.resize(m_angularVelocitySize);
+    m_jacobian.resize(m_spatialVelocitySize,
+                      m_kinDyn->getNrOfDegreesOfFreedom() + m_spatialVelocitySize);
+
+    return true;
+}
+
+bool SO3Task::setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
+{
+    if ((kinDyn == nullptr) || (!kinDyn->isValid()))
+    {
+        log()->error("[SO3Task::setKinDyn] Invalid kinDyn object.");
+        return false;
+    }
+
+    m_kinDyn = kinDyn;
+    return true;
+}
+
+bool SO3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler)
+{
+    constexpr std::string_view errorPrefix = "[SO3Task::initialize] ";
+
+    std::string frameName = "Unknown";
+    constexpr auto descriptionPrefix = "SO3Task Optimal Control Element - Frame name: ";
+
+    if (m_kinDyn == nullptr || !m_kinDyn->isValid())
+    {
+        log()->error("{}, [{} {}] KinDynComputations object is not valid.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    if (m_kinDyn->getFrameVelocityRepresentation()
+        != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)
+    {
+        log()->error("{}, [{} {}] The task supports only quantities expressed in MIXED "
+                     "representation. Please provide a KinDynComputations with Frame velocity "
+                     "representation set to MIXED_REPRESENTATION.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    auto ptr = paramHandler.lock();
+    if (ptr == nullptr)
+    {
+        log()->error("{}, [{} {}] The parameter handler is not valid.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    if (!ptr->getParameter("robot_acceleration_variable_name", m_robotAccelerationVariable.name))
+    {
+        log()->error("{}, [{} {}] Error while retrieving the robot velocity variable.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    if (!ptr->getParameter("frame_name", frameName)
+        || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName))
+               == iDynTree::FRAME_INVALID_INDEX)
+    {
+        log()->error("{}, [{} {}] Error while retrieving the frame that should be controlled.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    // set the gains for the controllers
+    double kpAngular, kdAngular;
+    if (!ptr->getParameter("kp_angular", kpAngular) || !ptr->getParameter("kd_angular", kdAngular))
+    {
+        log()->error("{}, [{} {}] Unable to get the proportional and derivative angular gain.",
+                     errorPrefix,
+                     descriptionPrefix,
+                     frameName);
+        return false;
+    }
+
+    m_SO3Controller.setGains({kpAngular, kdAngular});
+
+    // set the description
+    m_description = std::string(descriptionPrefix) + frameName + ".";
+
+    m_isInitialized = true;
+
+    return true;
+}
+
+bool SO3Task::update()
+{
+    m_isValid = false;
+
+    if (!m_isInitialized)
+    {
+        log()->error("[SO3Task::update] Please call initialize() before update().");
+        return m_isValid;
+    }
+
+
+    m_SO3Controller.setState(
+        {BipedalLocomotion::Conversions::toManifRot(
+             m_kinDyn->getWorldTransform(m_frameIndex).getRotation()),
+         iDynTree::toEigen(m_kinDyn->getFrameVel(m_frameIndex).getAngularVec3())});
+
+    m_SO3Controller.computeControlLaw();
+
+
+    m_b = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex)).tail<3>();
+    m_b += m_SO3Controller.getControl().coeffs();
+
+    if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, m_jacobian))
+    {
+        log()->error("[SO3Task::update] Unable to get the jacobian.");
+        return m_isValid;
+    }
+
+    iDynTree::toEigen(this->subA(m_robotAccelerationVariable)) = m_jacobian.bottomRows<3>();
+
+    m_isValid = true;
+    return m_isValid;
+}
+
+bool SO3Task::setSetPoint(const manif::SO3d& I_R_F,
+                          const manif::SO3d::Tangent& angularVelocity,
+                          const manif::SO3d::Tangent& angularAcceleration)
+{
+    bool ok = true;
+
+    ok = ok && m_SO3Controller.setDesiredState({I_R_F, angularVelocity});
+    ok = ok && m_SO3Controller.setFeedForward(angularAcceleration);
+
+    return ok;
+}
+
+std::size_t SO3Task::size() const
+{
+    return m_angularVelocitySize;
+}
+
+SO3Task::Type SO3Task::type() const
+{
+    return Type::equality;
+}
+
+bool SO3Task::isValid() const
+{
+    return m_isValid;
+}

src/TSID/tests/CMakeLists.txt

@@ -2,6 +2,11 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
+add_bipedal_test(
+  NAME SO3TaskTSID
+  SOURCES SO3TaskTest.cpp
+  LINKS BipedalLocomotion::TSID BipedalLocomotion::ManifConversions)
+
 add_bipedal_test(
   NAME SE3TaskTSID
   SOURCES SE3TaskTest.cpp