Add the possibility to control a subset of coordinates in IK::SE3Task

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 ### Changed
 - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338)
 - General improvements of `blf-calibration-delta-updater` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/361)
+- Add the possibility to control a subset of coordinates in `IK::SE3Task` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/356)
 
 ### Fix
 

src/IK/include/BipedalLocomotion/IK/SE3Task.h

@@ -44,6 +44,8 @@ namespace IK
  * different definitions of exponential maps and logarithm maps. Please consider that here the MIXED
  * representation is used to define the 6d-velocity. You can find further details in Section 2.3.4
  * of https://traversaro.github.io/phd-thesis/traversaro-phd-thesis.pdf.
+ * @note SE3Task can be used to control also a subset of element of the linear part of the SE3.
+ * Please refer to `mask` parameter in IK::SE3Task::initialize method.
  */
 class SE3Task : public System::LinearTask
 {
@@ -58,13 +60,20 @@ class SE3Task : public System::LinearTask
  iDynTree::FrameIndex m_frameIndex; /**< Frame controlled by the OptimalControlElement */
 
  static constexpr std::size_t m_spatialVelocitySize{6}; /**< Size of the spatial velocity vector. */
+ static constexpr std::size_t m_linearVelocitySize{3}; /**< Size of the linear 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 */
 
+ /** Mask used to select the DoFs controlled by the task */
+ std::array<bool, m_linearVelocitySize> m_mask{true, true, true};
+ std::size_t m_linearDoFs{m_linearVelocitySize}; /**< DoFs associated to the linear task */
+ std::size_t m_DoFs{m_spatialVelocitySize}; /**< DoFs associated to the entire task */
+
+ Eigen::MatrixXd m_jacobian; /**< Jacobian matrix in MIXED representation */
 public:
 
  /**
@@ -77,6 +86,7 @@ class SE3Task : public System::LinearTask
  * | `frame_name` | `string` | Name of the frame controlled by the SE3Task | Yes |
  * | `kp_linear` | `double` | Gain of the position controller | Yes |
  * | `kp_angular` | `double` | Gain of the orientation controller | Yes |
+ * | `mask` | `vector<bool>` | Mask representing the linear DoFs controlled. E.g. [1,0,1] will enable the control on the x and z coordinates only and the angular part. (Default value, [1,1,1]) | No |
  * @return True in case of success, false otherwise.
  * Where the generalized robot velocity is a vector containing the base spatial-velocity
  * (expressed in mixed representation) and the joint velocities.

src/IK/src/SE3Task.cpp

@@ -57,23 +57,32 @@ bool SE3Task::setVariablesHandler(const System::VariablesHandler& variablesHandl
  }
 
  // resize the matrices
- m_A.resize(m_spatialVelocitySize, variablesHandler.getNumberOfVariables());
+ m_A.resize(m_DoFs, variablesHandler.getNumberOfVariables());
  m_A.setZero();
- m_b.resize(m_spatialVelocitySize);
+ m_b.resize(m_DoFs);
+ m_jacobian.resize(m_spatialVelocitySize, m_robotVelocityVariable.size);
 
  return true;
 }
 
 bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> paramHandler)
 {
- constexpr std::string_view errorPrefix = "[SE3Task::initialize] ";
+ constexpr auto errorPrefix = "[SE3Task::initialize] ";
 
  std::string frameName = "Unknown";
- constexpr std::string_view descriptionPrefix = "SE3Task Optimal Control Element - Frame name: ";
+ constexpr auto descriptionPrefix = "IK-SE3Task - Frame name: ";
+
+ std::string maskDescription = "";
+ auto boolToString = [](bool b) { return b ? " true" : " false"; };
+ for(const auto flag : m_mask)
+ {
+ maskDescription += boolToString(flag);
+ }
+
 
  if (m_kinDyn == nullptr || !m_kinDyn->isValid())
  {
- log()->error("{}, [{} {}] KinDynComputations object is not valid.",
+ log()->error("{} [{} {}] KinDynComputations object is not valid.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -83,7 +92,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
  if (m_kinDyn->getFrameVelocityRepresentation()
  != iDynTree::FrameVelocityRepresentation::MIXED_REPRESENTATION)
  {
- log()->error("{}, [{} {}] The task supports only quantities expressed in MIXED "
+ log()->error("{} [{} {}] The task supports only quantities expressed in MIXED "
  "representation. Please provide a KinDynComputations with Frame velocity "
  "representation set to MIXED_REPRESENTATION.",
  errorPrefix,
@@ -95,7 +104,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
  auto ptr = paramHandler.lock();
  if (ptr == nullptr)
  {
- log()->error("{}, [{} {}] The parameter handler is not valid.",
+ log()->error("{} [{} {}] The parameter handler is not valid.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -104,7 +113,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
 
  if (!ptr->getParameter("robot_velocity_variable_name", m_robotVelocityVariable.name))
  {
- log()->error("{}, [{} {}] Error while retrieving the robot velocity variable.",
+ log()->error("{} [{} {}] Error while retrieving the robot velocity variable.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -115,7 +124,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
  || (m_frameIndex = m_kinDyn->model().getFrameIndex(frameName))
  == iDynTree::FRAME_INVALID_INDEX)
  {
- log()->error("{}, [{} {}] Error while retrieving the frame that should be controlled.",
+ log()->error("{} [{} {}] Error while retrieving the frame that should be controlled.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -127,7 +136,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
  double kpAngular;
  if (!ptr->getParameter("kp_linear", kpLinear))
  {
- log()->error("{}, [{} {}] Unable to get the proportional linear gain.",
+ log()->error("{} [{} {}] Unable to get the proportional linear gain.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -136,7 +145,7 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
 
  if (!ptr->getParameter("kp_angular", kpAngular))
  {
- log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
+ log()->error("{} [{} {}] Unable to get the proportional angular gain.",
  errorPrefix,
  descriptionPrefix,
  frameName);
@@ -146,8 +155,33 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> pa
  m_R3Controller.setGains(kpLinear);
  m_SO3Controller.setGains(kpAngular);
 
- // set the description
- m_description = std::string(descriptionPrefix) + frameName + ".";
+ std::vector<bool> mask;
+ if (!ptr->getParameter("mask", mask) || (mask.size() != m_linearVelocitySize))
+ {
+ log()->info("{} [{} {}] Unable to find the mask parameter. The default value is used:{}.",
+ errorPrefix,
+ descriptionPrefix,
+ frameName,
+ maskDescription);
+ }
+ else
+ {
+ // covert an std::vector in a std::array
+ std::copy(mask.begin(), mask.end(), m_mask.begin());
+ // compute the DoFs associated to the task
+ m_linearDoFs = std::count(m_mask.begin(), m_mask.end(), true);
+
+ m_DoFs = m_linearDoFs + m_linearVelocitySize;
+
+ // Update the mask description
+ maskDescription.clear();
+ for(const auto flag : m_mask)
+ {
+ maskDescription += boolToString(flag);
+ }
+ }
+
+ m_description = descriptionPrefix + frameName + " Mask:" + maskDescription + ".";
 
  m_isInitialized = true;
 
@@ -169,14 +203,47 @@ bool SE3Task::update()
  m_SO3Controller.computeControlLaw();
  m_R3Controller.computeControlLaw();
 
- m_b.head<3>() = m_R3Controller.getControl().coeffs();
+ // the angular part is always enabled
  m_b.tail<3>() = m_SO3Controller.getControl().coeffs();
 
- if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
- this->subA(m_robotVelocityVariable)))
+ // if we want to control all 6 DoF we avoid to lose performances
+ if (m_linearDoFs == m_linearVelocitySize)
+ {
+ m_b.head<3>() = m_R3Controller.getControl().coeffs();
+
+ if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex,
+ this->subA(m_robotVelocityVariable)))
+ {
+ log()->error("[SE3Task::update] Unable to get the jacobian.");
+ return m_isValid;
+ }
+ } else
  {
- log()->error("[SE3Task::update] Unable to get the jacobian.");
- return m_isValid;
+ // store the jacobian associated to the given frame
+ if (!m_kinDyn->getFrameFreeFloatingJacobian(m_frameIndex, m_jacobian))
+ {
+ log()->error("[SE3Task::update] Unable to get the jacobian.");
+ return m_isValid;
+ }
+
+ // take only the required components
+ std::size_t index = 0;
+
+ // linear components
+ for (std::size_t i = 0; i < 3; i++)
+ {
+ if (m_mask[i])
+ {
+ m_b(index) = m_R3Controller.getControl().coeffs()(i);
+ iDynTree::toEigen(this->subA(m_robotVelocityVariable)).row(index)
+ = m_jacobian.row(i);
+ index++;
+ }
+ }
+
+ // take the all angular part
+ iDynTree::toEigen(this->subA(m_robotVelocityVariable)).bottomRows<3>()
+ = m_jacobian.bottomRows<3>();
  }
 
  m_isValid = true;
@@ -199,7 +266,7 @@ bool SE3Task::setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent&
 
 std::size_t SE3Task::size() const
 {
- return m_spatialVelocitySize;
+ return m_DoFs;
 }
 
 SE3Task::Type SE3Task::type() const