Merge pull request #170 from dic-iit/TSID/joint_regularization

Implent JointRegularizationTask in TSID

CHANGELOG.md

@@ -29,7 +29,7 @@ All notable changes to this project are documented in this file.
 - Implement Continuous algebraic Riccati equation function (https://github.com/dic-iit/bipedal-locomotion-framework/pull/157)
 - Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/156)
 - Implement example YARP device `ROSPublisherTestDevice` for understanding the usage of `ROSPublisher`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/160)
-- Implement `TSID` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/167)
+- Implement `TSID` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/167, https://github.com/dic-iit/bipedal-locomotion-framework/pull/170)
 - Implement the `JointTrajectoryPlayer` application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/169)29ed234a1c
 - Implement `ContactDetectors` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/142)
 - Added `mas-imu-test` application to check the output of MAS IMUs (https://github.com/dic-iit/bipedal-locomotion-framework/pull/62)

src/TSID/CMakeLists.txt

@@ -8,8 +8,8 @@ if(FRAMEWORK_COMPILE_TSID)
 
  add_bipedal_locomotion_library(
  NAME TSID
- PUBLIC_HEADERS ${H_PREFIX}/Task.h ${H_PREFIX}/SE3Task.h
- SOURCES src/Task.cpp src/SE3Task.cpp
+ PUBLIC_HEADERS ${H_PREFIX}/Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/JointsTrackingTask.h
+ SOURCES src/Task.cpp src/SE3Task.cpp src/JointsTrackingTask.cpp
  PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System LieGroupControllers::LieGroupControllers MANIF::manif iDynTree::idyntree-high-level iDynTree::idyntree-model
  PRIVATE_LINK_LIBRARIES BipedalLocomotion::ManifConversions
  SUBDIRECTORIES tests)

src/TSID/include/BipedalLocomotion/TSID/JointsTrackingTask.h

@@ -0,0 +1,98 @@
+/**
+ * @file JointsTrackingTask.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 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_JOINT_REGULARIZATION_TASK_H
+#define BIPEDAL_LOCOMOTION_TSID_JOINT_REGULARIZATION_TASK_H
+
+#include <BipedalLocomotion/TSID/Task.h>
+
+#include <LieGroupControllers/ProportionalDerivativeController.h>
+
+namespace BipedalLocomotion
+{
+namespace TSID
+{
+
+/**
+ * JointsTrackingTask is a concrete implementation of the Task. Please use this element if you
+ * want to control the desired joint position of the robot.
+ * The task represents the following equation
+ * \f[
+ * \begin{bmatrix} 0_6 & I_n \end{bmatrix} \nu = \ddot{s} ^ *
+ * \f]
+ * where \f$0_6\f$ and \f$I_n\f$ are the zero and the identity matrix.
+ * The desired joint acceleration is chosen such that the joint will converge to the desired
+ * trajectory and it is computed with a standard standard PD controller in \f$\mathbb{R}^n\f$.
+ */
+class JointsTrackingTask : public Task
+{
+ Eigen::VectorXd m_kp; /**< Proportional gain. */
+ Eigen::VectorXd m_kd; /**< Derivative gain. */
+ Eigen::VectorXd m_jointVelocity; /**< Joints velocity in radians per second. */
+ Eigen::VectorXd m_jointPosition; /**< Joints position in radians */
+ Eigen::VectorXd m_desiredJointVelocity; /**< Desired joints velocity in radians per second. */
+ Eigen::VectorXd m_desiredJointAcceleration; /**< Desired joints acceleration in radians per
+ second square. */
+ Eigen::VectorXd m_desiredJointPosition; /**< Desired joints position in radians. */
+ Eigen::VectorXd m_zero; /**< Vector containing zero elements. */
+
+public:
+ /**
+ * Initialize the planner.
+ * @param paramHandler pointer to the parameters handler.
+ * @param variablesHandler reference to a variables 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 |
+ * | `kp ` | `vector` | Proportional controller gain | Yes |
+ * | `kd` | `vector` | Derivative Gain of the controller. if not specified \f$k_d = 2 \sqrt{k_p}\f$ | No |
+ * @return True in case of success, false otherwise.
+ */
+ bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler,
+ 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 setpoint.
+ * @param jointPosition vector containing the desired joint position in radians.
+ * @note The desired velocity and acceleration are implicitly set to zero.
+ * @return True in case of success, false otherwise.
+ */
+ bool setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition);
+
+ /**
+ * Set the desired setpoint.
+ * @param jointPosition vector containing the desired joint position in radians.
+ * @param jointVelocity vector containing the desired joint velocity in radians per second.
+ * @note The desired acceleration is implicitly set to zero.
+ * @return True in case of success, false otherwise.
+ */
+ bool setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
+ Eigen::Ref<const Eigen::VectorXd> jointVelocity);
+
+ /**
+ * Set the desired setpoint.
+ * @param jointPosition vector containing the desired joint position in radians.
+ * @param jointVelocity vector containing the desired joint velocity in radians per second.
+ * @param jointAcceleration vector containing the desired joint velocity in radians per second square.
+ * @return True in case of success, false otherwise.
+ */
+ bool setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
+ Eigen::Ref<const Eigen::VectorXd> jointVelocity,
+ Eigen::Ref<const Eigen::VectorXd> jointAcceleration);
+};
+
+} // namespace TSID
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_TSID_JOINT_REGULARIZATION_TASK_H

src/TSID/src/JointsTrackingTask.cpp

@@ -0,0 +1,150 @@
+/**
+ * @file JointsTrackingTask.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 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/TSID/JointsTrackingTask.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::TSID;
+
+bool JointsTrackingTask::initialize(
+ std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler,
+ const System::VariablesHandler& variablesHandler)
+{
+ constexpr std::string_view errorPrefix = "[JointsTrackingTask::initialize] ";
+
+ if (m_kinDyn == nullptr || !m_kinDyn->isValid())
+ {
+ std::cerr << errorPrefix << "KinDynComputations object is not valid." << std::endl;
+ return false;
+ }
+
+ auto ptr = paramHandler.lock();
+ if (ptr == nullptr)
+ {
+ std::cerr << errorPrefix << "The parameter handler is not valid." << std::endl;
+ return false;
+ }
+
+ System::VariablesHandler::VariableDescription robotAccelerationVariable;
+ std::string robotAccelerationVariableName;
+ if (!ptr->getParameter("robot_acceleration_variable_name", robotAccelerationVariableName)
+ || !variablesHandler.getVariable(robotAccelerationVariableName, robotAccelerationVariable))
+ {
+ std::cerr << errorPrefix << "Error while retrieving the robot acceleration variable."
+ << std::endl;
+ return false;
+ }
+
+ if (robotAccelerationVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6)
+ {
+ std::cerr << errorPrefix << "Error while retrieving the robot acceleration variable."
+ << std::endl;
+ return false;
+ }
+
+ // set the gains for the controllers
+ m_kp.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_kd.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+ if (!ptr->getParameter("kp", m_kp))
+ {
+ std::cerr << errorPrefix << "Error while retrieving the proportional gain." << std::endl;
+ return false;
+ }
+
+ if (!ptr->getParameter("kd", m_kd))
+ {
+ std::cout << errorPrefix << "Error while retrieving the derivative gain." << std::endl;
+ std::cout << errorPrefix << "The default kd will be set." << std::endl;
+
+ m_kd = 2 * m_kp.cwiseSqrt();
+ }
+
+ // set the description
+ m_description = "Joint tracking task";
+
+ // resize the matrices
+ m_A.resize(m_kinDyn->getNrOfDegreesOfFreedom(), variablesHandler.getNumberOfVariables());
+ m_A.setZero();
+ m_b.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+
+ m_zero = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_desiredJointPosition = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_desiredJointVelocity = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_desiredJointAcceleration = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_jointPosition = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+ m_jointVelocity = Eigen::VectorXd::Zero(m_kinDyn->getNrOfDegreesOfFreedom());
+
+ // A is constant
+ // here we assume that the first robot acceleration is stored as [base_acceleration;
+ // joint_acceleration]
+ iDynTree::toEigen(this->subA(robotAccelerationVariable))
+ .rightCols(m_kinDyn->getNrOfDegreesOfFreedom())
+ .setIdentity();
+
+ return true;
+}
+
+bool JointsTrackingTask::update()
+{
+ constexpr std::string_view errorPrefix = "[JointsTrackingTask::update] ";
+
+ if (!m_kinDyn->getJointPos(m_jointPosition))
+ {
+ std::cerr << errorPrefix << " Unable to get the joint position" << std::endl;
+ return false;
+ }
+
+ if (!m_kinDyn->getJointVel(m_jointVelocity))
+ {
+ std::cerr << errorPrefix << " Unable to get the joint velocity" << std::endl;
+ return false;
+ }
+
+ m_b = m_desiredJointAcceleration
+ + m_kp.asDiagonal() * (m_desiredJointPosition - m_jointPosition)
+ + m_kd.asDiagonal() * (m_desiredJointVelocity - m_jointVelocity);
+
+ return true;
+}
+
+bool JointsTrackingTask::setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition)
+{
+ return this->setSetpoint(jointPosition, m_zero, m_zero);
+}
+
+bool JointsTrackingTask::setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
+ Eigen::Ref<const Eigen::VectorXd> jointVelocity)
+{
+ return this->setSetpoint(jointPosition, jointVelocity, m_zero);
+}
+
+bool JointsTrackingTask::setSetpoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
+ Eigen::Ref<const Eigen::VectorXd> jointVelocity,
+ Eigen::Ref<const Eigen::VectorXd> jointAcceleration)
+{
+ constexpr std::string_view errorPrefix = "[JointsTrackingTask::setSetpoint] ";
+
+ if (jointPosition.size() != m_kinDyn->getNrOfDegreesOfFreedom()
+ || jointVelocity.size() != m_kinDyn->getNrOfDegreesOfFreedom()
+ || jointAcceleration.size() != m_kinDyn->getNrOfDegreesOfFreedom())
+ {
+ std::cerr << errorPrefix << "Wrong size of the desired reference trajectory:" << std::endl
+ << "Expected size: " << m_kinDyn->getNrOfDegreesOfFreedom() << std::endl
+ << "Joint position size: " << jointPosition.size() << std::endl
+ << "Joint velocity size: " << jointVelocity.size() << std::endl
+ << "Joint acceleration size: " << jointAcceleration.size() << std::endl;
+ return false;
+ }
+
+ m_desiredJointPosition = jointPosition;
+ m_desiredJointVelocity = jointVelocity;
+ m_desiredJointAcceleration = jointVelocity;
+
+ return true;
+}

src/TSID/src/Task.cpp

@@ -13,13 +13,13 @@ using namespace BipedalLocomotion::TSID;
 
 iDynTree::MatrixView<double> Task::subA(const VariablesHandler::VariableDescription& description)
 {
- return iDynTree::MatrixView<double>(m_A).block(0, description.offset, 6, description.size);
+ return iDynTree::MatrixView<double>(m_A).block(0, description.offset, m_A.rows(), description.size);
 }
 
 iDynTree::MatrixView<const double>
 Task::subA(const VariablesHandler::VariableDescription& description) const
 {
- return iDynTree::MatrixView<const double>(m_A).block(0, description.offset, 6, description.size);
+ return iDynTree::MatrixView<const double>(m_A).block(0, description.offset, m_A.rows(), description.size);
 }
 
 bool Task::setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn)

src/TSID/tests/CMakeLists.txt

@@ -6,3 +6,8 @@ add_bipedal_test(
  NAME SE3Task
  SOURCES SE3TaskTest.cpp
  LINKS BipedalLocomotion::TSID BipedalLocomotion::ManifConversions)
+
+add_bipedal_test(
+ NAME JointsTrackingTask
+ SOURCES JointsTrackingTaskTest.cpp
+ LINKS BipedalLocomotion::TSID)