Merge pull request #475 from ami-iit/ik-tsid/weights

Add the possibility to change the weight in TSID/IK

CHANGELOG.md

@@ -4,7 +4,7 @@ All notable changes to this project are documented in this file.
 ## [Unreleased]
 ### Added
 - Add the reading of the orientation of the head IMU in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/471)
-
+- Add the possibility to change the weight in TSID/IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/475)
 ### Changed
 - Use yarp clock instead of system clock in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/473)
 

bindings/python/IK/src/IntegrationBasedIK.cpp

@@ -42,6 +42,30 @@ void CreateIntegrationBasedIK(pybind11::module& module)
  py::arg("task_name"),
  py::arg("priority"),
  py::arg("weight") = Eigen::VectorXd())
+ .def("set_task_weight",
+ &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::setTaskWeight,
+ py::arg("task_name"),
+ py::arg("weight"))
+ .def(
+ "get_task_weight",
+ [](const ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>& impl,
+ const std::string& name) {
+ auto task = impl.getTask("name").lock();
+ if(task == nullptr)
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ Eigen::VectorXd weight(task->size());
+
+ if (!impl.getTaskWeight(name, weight))
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ return weight;
+ },
+ py::arg("task_name"))
  .def("get_task_names",
  &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getTaskNames)
  .def("finalize",

bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp

@@ -43,22 +43,43 @@ void CreateTaskSpaceInverseDynamics(pybind11::module& module)
  py::arg("task_name"),
  py::arg("priority"),
  py::arg("weight") = Eigen::VectorXd())
- .def("get_task_names",
- &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getTaskNames)
+ .def("set_task_weight",
+ &ILinearTaskSolver<TSIDLinearTask, TSIDState>::setTaskWeight,
+ py::arg("task_name"),
+ py::arg("weight"))
+ .def(
+ "get_task_weight",
+ [](const ILinearTaskSolver<TSIDLinearTask, TSIDState>& impl, const std::string& name) {
+ auto task = impl.getTask("name").lock();
+ if(task == nullptr)
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ Eigen::VectorXd weight(task->size());
+
+ if (!impl.getTaskWeight(name, weight))
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ return weight;
+ },
+ py::arg("task_name"))
+ .def("get_task_names", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getTaskNames)
  .def("finalize",
  &ILinearTaskSolver<TSIDLinearTask, TSIDState>::finalize,
  py::arg("handler"))
  .def("advance", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::advance)
  .def("get_output", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getOutput)
- .def("is_output_valid",
- &ILinearTaskSolver<TSIDLinearTask, TSIDState>::isOutputValid)
+ .def("is_output_valid", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::isOutputValid)
  .def(
  "initialize",
  [](ILinearTaskSolver<TSIDLinearTask, TSIDState>& impl,
  std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
  handler) -> bool { return impl.initialize(handler); },
  py::arg("handler"))
- .def("__repr__", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::toString);
+ .def("__repr__", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::toString);
 
  py::class_<TaskSpaceInverseDynamics, //
  ILinearTaskSolver<TSIDLinearTask, TSIDState>>(module, "TaskSpaceInverseDynamics");

src/IK/include/BipedalLocomotion/IK/QPInverseKinematics.h

@@ -75,6 +75,24 @@ class QPInverseKinematics : public IntegrationBasedIK
  std::size_t priority,
  std::optional<Eigen::Ref<const Eigen::VectorXd>> weight = {}) override;
 
+ /**
+ * Set the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight new Weight associated to the task.
+ * @return true if the weight has been updated
+ */
+ bool setTaskWeight(const std::string& taskName,
+ Eigen::Ref<const Eigen::VectorXd> weight) override;
+
+ /**
+ * Get the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight the weight associated to the task.
+ * @return true in case of success and false otherwise
+ */
+ bool getTaskWeight(const std::string& taskName,
+ Eigen::Ref<Eigen::VectorXd> weight) const override;
+
  /**
  * Finalize the IK.
  * @param handler parameter handler.

src/IK/src/QPInverseKinematics.cpp

@@ -211,6 +211,77 @@ bool QPInverseKinematics::addTask(std::shared_ptr<QPInverseKinematics::Task> tas
  return true;
 }
 
+bool QPInverseKinematics::setTaskWeight(const std::string& taskName,
+ Eigen::Ref<const Eigen::VectorXd> weight)
+{
+ constexpr auto logPrefix = "[QPInverseKinematics::setTaskWeight]";
+
+ auto tmp = m_pimpl->tasks.find(taskName);
+
+ const bool taskExist = (tmp != m_pimpl->tasks.end());
+ if (!taskExist)
+ {
+ log()->error("{} The task named {} does not exist.", logPrefix, taskName);
+ return false;
+ }
+
+ auto taskWithPriority = tmp->second;
+
+ if (taskWithPriority.priority != 1)
+ {
+ log()->error("{} - [Task name: '{}'] The weight can be set only to a task with priority "
+ "equal to 1.",
+ logPrefix,
+ taskName);
+ return false;
+ }
+
+ if (weight.size() != taskWithPriority.task->size())
+ {
+ log()->error("{} - [Task name: '{}'] The size of the weight is not coherent with the "
+ "size of the task. Expected: {}. Given: {}.",
+ logPrefix,
+ taskName,
+ taskWithPriority.task->size(),
+ weight.size());
+ return false;
+ }
+
+ // update the weight
+ m_pimpl->tasks[taskName].weight = weight;
+
+ return true;
+}
+
+bool QPInverseKinematics::getTaskWeight(const std::string& taskName,
+ Eigen::Ref<Eigen::VectorXd> weight) const
+{
+ constexpr auto logPrefix = "[QPInverseKinematics::getTaskWeight]";
+
+ auto taskWithPriority = m_pimpl->tasks.find(taskName);
+ const bool taskExist = (taskWithPriority != m_pimpl->tasks.end());
+ if (!taskExist)
+ {
+ log()->error("{} The task named {} does not exist.", logPrefix, taskName);
+ return false;
+ }
+
+ if (weight.size() != taskWithPriority->second.task->size())
+ {
+ log()->error("{} - [Task name: '{}'] The size of the weight is not coherent with the "
+ "size of the task. Expected: {}. Given: {}.",
+ logPrefix,
+ taskName,
+ taskWithPriority->second.task->size(),
+ weight.size());
+ return false;
+ }
+
+ weight = taskWithPriority->second.weight;
+
+ return true;
+}
+
 bool QPInverseKinematics::finalize(const System::VariablesHandler& handler)
 {
  constexpr auto logPrefix = "[QPInverseKinematics::finalize]";

src/IK/tests/QPInverseKinematicsTest.cpp

@@ -126,6 +126,17 @@ InverseKinematicsAndTasks createIK(std::shared_ptr<IParametersHandler> handler,
  lowPriority,
  weightRegularization));
 
+
+
+ Eigen::VectorXd newWeight = 10 * weightRegularization;
+ REQUIRE(out.ik->setTaskWeight("regularization_task", newWeight));
+
+ Eigen::VectorXd weight(newWeight.size());
+ REQUIRE(out.ik->getTaskWeight("regularization_task", weight));
+ REQUIRE(weight.isApprox(newWeight));
+
+ REQUIRE(out.ik->setTaskWeight("regularization_task", weightRegularization));
+
  REQUIRE(out.ik->finalize(variables));
 
  return out;

src/System/include/BipedalLocomotion/System/ILinearTaskSolver.h

@@ -44,6 +44,7 @@ template <class _Task, class _State> class ILinearTaskSolver : public Source<_St
  /**
  * Add a linear task in the solver.
  * @param task pointer to a given linear task
+ * @param taskName unique name associated to the task.
  * @param priority Priority associated to the task. The lower the number the higher the
  * priority.
  * @param weight Weight associated to the task. This parameter is optional. The default value is
@@ -56,6 +57,25 @@ template <class _Task, class _State> class ILinearTaskSolver : public Source<_St
  std::optional<Eigen::Ref<const Eigen::VectorXd>> weight = {})
  = 0;
 
+ /**
+ * Set the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight new Weight associated to the task.
+ * @return true if the weight has been updated
+ */
+ virtual bool setTaskWeight(const std::string& taskName,
+ Eigen::Ref<const Eigen::VectorXd> weight) = 0;
+
+ /**
+ * Get the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight the weight associated to the task.
+ * @return true in case of success and false otherwise
+ */
+ virtual bool getTaskWeight(const std::string& taskName,
+ Eigen::Ref<Eigen::VectorXd> weight) const = 0;
+
+
  /**
  * Get a vector containing the name of the tasks.
  * @return an std::vector containing all the names associated to the tasks

src/TSID/include/BipedalLocomotion/TSID/QPTSID.h

@@ -71,6 +71,24 @@ class QPTSID : public TaskSpaceInverseDynamics
  std::size_t priority,
  std::optional<Eigen::Ref<const Eigen::VectorXd>> weight = {}) override;
 
+ /**
+ * Set the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight new Weight associated to the task.
+ * @return true if the weight has been updated
+ */
+ bool setTaskWeight(const std::string& taskName,
+ Eigen::Ref<const Eigen::VectorXd> weight) override;
+
+ /**
+ * Get the weight associated to an already existing task
+ * @param taskName name associated to the task
+ * @param weight the weight associated to the task.
+ * @return true in case of success and false otherwise
+ */
+ bool getTaskWeight(const std::string& taskName,
+ Eigen::Ref<Eigen::VectorXd> weight) const override;
+
  /**
  * Get a vector containing the name of the tasks.
  * @return an std::vector containing all the names associated to the tasks

src/TSID/src/QPTSID.cpp

@@ -226,6 +226,75 @@ bool QPTSID::addTask(std::shared_ptr<QPTSID::Task> task,
  return true;
 }
 
+bool QPTSID::setTaskWeight(const std::string& taskName, Eigen::Ref<const Eigen::VectorXd> weight)
+{
+ constexpr auto logPrefix = "[QPTSID::setTaskWeight]";
+
+ auto tmp = m_pimpl->tasks.find(taskName);
+
+ const bool taskExist = (tmp != m_pimpl->tasks.end());
+ if (!taskExist)
+ {
+ log()->error("{} The task named {} does not exist.", logPrefix, taskName);
+ return false;
+ }
+
+ auto taskWithPriority = tmp->second;
+
+ if (taskWithPriority.priority != 1)
+ {
+ log()->error("{} - [Task name: '{}'] The weight can be set only to a task with priority "
+ "equal to 1.",
+ logPrefix,
+ taskName);
+ return false;
+ }
+
+ if (weight.size() != taskWithPriority.task->size())
+ {
+ log()->error("{} - [Task name: '{}'] The size of the weight is not coherent with the "
+ "size of the task. Expected: {}. Given: {}.",
+ logPrefix,
+ taskName,
+ taskWithPriority.task->size(),
+ weight.size());
+ return false;
+ }
+
+ // update the weight
+ m_pimpl->tasks[taskName].weight = weight;
+
+ return true;
+}
+
+bool QPTSID::getTaskWeight(const std::string& taskName, Eigen::Ref<Eigen::VectorXd> weight) const
+{
+ constexpr auto logPrefix = "[QPTSID::getTaskWeight]";
+
+ auto taskWithPriority = m_pimpl->tasks.find(taskName);
+ const bool taskExist = (taskWithPriority != m_pimpl->tasks.end());
+ if (!taskExist)
+ {
+ log()->error("{} The task named {} does not exist.", logPrefix, taskName);
+ return false;
+ }
+
+ if (weight.size() != taskWithPriority->second.task->size())
+ {
+ log()->error("{} - [Task name: '{}'] The size of the weight is not coherent with the "
+ "size of the task. Expected: {}. Given: {}.",
+ logPrefix,
+ taskName,
+ taskWithPriority->second.task->size(),
+ weight.size());
+ return false;
+ }
+
+ weight = taskWithPriority->second.weight;
+
+ return true;
+}
+
 std::vector<std::string> QPTSID::getTaskNames() const
 {
  std::vector<std::string> tasksName;

src/TSID/tests/QPFixedBaseTSIDTest.cpp

@@ -124,6 +124,15 @@ TSIDAndTasks createTSID(std::shared_ptr<IParametersHandler> handler,
  lowPriority,
  weightRegularization));
 
+ Eigen::VectorXd newWeight = 10 * weightRegularization;
+ REQUIRE(out.tsid->setTaskWeight("regularization_task", newWeight));
+
+ Eigen::VectorXd weight(newWeight.size());
+ REQUIRE(out.tsid->getTaskWeight("regularization_task", weight));
+ REQUIRE(weight.isApprox(newWeight));
+ REQUIRE(out.tsid->setTaskWeight("regularization_task", weightRegularization));
+
+
  REQUIRE(out.tsid->finalize(variables));
 
  return out;