Distance and gravity tasks

.gitignore

@@ -11,6 +11,7 @@ build*
 
 # IDEs
 .idea*
+.vscode*
 
 # ======
 # Python (https://github.com/github/gitignore/blob/master/Python.gitignore)

CHANGELOG.md

@@ -2,6 +2,7 @@
 All notable changes to this project are documented in this file.
 
 ## [Unreleased]
+- Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717)
 
 ### Added
 - Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727)

bindings/python/IK/CMakeLists.txt

@@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::IK)
 
   add_bipedal_locomotion_python_module(
     NAME IKBindings
-    SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp  src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp
-    HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h
+    SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp  src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp
+    HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h
     LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif
     TESTS tests/test_QP_inverse_kinematics.py
     TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models

bindings/python/IK/include/BipedalLocomotion/bindings/IK/DistanceTask.h

@@ -0,0 +1,26 @@
+/**
+ * @file DistanceTask.h
+ * @authors Stefano Dafarra
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateDistanceTask(pybind11::module& module);
+
+} // namespace IK
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_DISTANCE_TASK_H

bindings/python/IK/include/BipedalLocomotion/bindings/IK/GravityTask.h

@@ -0,0 +1,26 @@
+/**
+ * @file GravityTask.h
+ * @authors Stefano Dafarra
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateGravityTask(pybind11::module& module);
+
+} // namespace IK
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_GRAVITY_TASK_H

bindings/python/IK/src/DistanceTask.cpp

@@ -0,0 +1,39 @@
+/**
+ * @file DistanceTask.cpp
+ * @authors Stefano Dafarra
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/IK/DistanceTask.h>
+
+#include <BipedalLocomotion/bindings/IK/DistanceTask.h>
+#include <BipedalLocomotion/bindings/IK/IKLinearTask.h>
+#include <BipedalLocomotion/bindings/System/LinearTask.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateDistanceTask(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::IK;
+
+    py::class_<DistanceTask, std::shared_ptr<DistanceTask>, IKLinearTask>(module, "DistanceTask")
+        .def(py::init())
+        .def("set_desired_distance", &DistanceTask::setDesiredDistance, py::arg("desired_distance"))
+        .def("set_set_point", &DistanceTask::setSetPoint, py::arg("desired_distance"))
+        .def("get_distance", &DistanceTask::getDistance);
+}
+
+} // namespace IK
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/IK/src/GravityTask.cpp

@@ -0,0 +1,46 @@
+/**
+ * @file GravityTask.cpp
+ * @authors Stefano Dafarra
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/IK/GravityTask.h>
+
+#include <BipedalLocomotion/bindings/IK/GravityTask.h>
+#include <BipedalLocomotion/bindings/IK/IKLinearTask.h>
+#include <BipedalLocomotion/bindings/System/LinearTask.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateGravityTask(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::IK;
+
+    py::class_<GravityTask, std::shared_ptr<GravityTask>, IKLinearTask>(module, "GravityTask")
+        .def(py::init())
+        .def("set_desired_gravity_direction_in_target_frame",
+             &GravityTask::setDesiredGravityDirectionInTargetFrame,
+             py::arg("desired_gravity_direction"))
+        .def("set_feedforward_velocity_in_target_frame",
+             &GravityTask::setFeedForwardVelocityInTargetFrame,
+             py::arg("feedforward_velocity"))
+        .def("set_set_point",
+             &GravityTask::setSetPoint,
+             py::arg("desired_gravity_direction"),
+             py::arg("feedforward_velocity") = Eigen::Vector3d::Zero());
+}
+
+} // namespace IK
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/IK/src/Module.cpp

@@ -9,6 +9,8 @@
 
 #include <BipedalLocomotion/bindings/IK/AngularMomentumTask.h>
 #include <BipedalLocomotion/bindings/IK/CoMTask.h>
+#include <BipedalLocomotion/bindings/IK/DistanceTask.h>
+#include <BipedalLocomotion/bindings/IK/GravityTask.h>
 #include <BipedalLocomotion/bindings/IK/IKLinearTask.h>
 #include <BipedalLocomotion/bindings/IK/IntegrationBasedIK.h>
 #include <BipedalLocomotion/bindings/IK/JointLimitsTask.h>
@@ -37,6 +39,8 @@ void CreateModule(pybind11::module& module)
     CreateJointTrackingTask(module);
     CreateJointLimitsTask(module);
     CreateAngularMomentumTask(module);
+    CreateDistanceTask(module);
+    CreateGravityTask(module);
     CreateIntegrationBasedIK(module);
     CreateQPInverseKinematics(module);
 }

bindings/python/IK/tests/test_QP_inverse_kinematics.py

@@ -213,6 +213,53 @@ def test_angular_momentum_task():
     assert angular_momentum_task.set_variables_handler(variables_handler=angular_momentum_var_handler)
     assert angular_momentum_task.set_set_point([0., 0., 0.])
 
+def test_distance_task():
+    # create KinDynComputationsDescriptor
+    joints_list, kindyn = get_kindyn()
+
+    # Set the parameters
+    distance_task_param_handler = blf.parameters_handler.StdParametersHandler()
+    distance_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity")
+    distance_task_param_handler.set_parameter_string(name="target_frame_name", value="chest")
+    distance_task_param_handler.set_parameter_string(name="reference_frame_name", value="root_link")
+    distance_task_param_handler.set_parameter_float(name="kp",value=5.0)
+
+    # Initialize the task
+    distance_task = blf.ik.DistanceTask()
+    assert distance_task.set_kin_dyn(kindyn)
+    assert distance_task.initialize(param_handler=distance_task_param_handler)
+    distance_task_var_handler = blf.system.VariablesHandler()
+    assert distance_task_var_handler.add_variable("robotVelocity", 32) is True  # robot velocity size = 26 (joints) + 6 (base)
+    assert distance_task.set_variables_handler(variables_handler=distance_task_var_handler)
+
+    # Set desired distance
+    assert distance_task.set_desired_distance(desired_distance=np.random.uniform(-0.5,0.5))
+    assert distance_task.set_set_point(desired_distance=np.random.uniform(-0.5,0.5))
+
+def test_gravity_task():
+    # create KinDynComputationsDescriptor
+    joints_list, kindyn = get_kindyn()
+
+    # Set the parameters
+    gravity_task_param_handler = blf.parameters_handler.StdParametersHandler()
+    gravity_task_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity")
+    gravity_task_param_handler.set_parameter_string(name="target_frame_name", value="chest")
+    gravity_task_param_handler.set_parameter_string(name="base_frame_name", value="root_link")
+    gravity_task_param_handler.set_parameter_float(name="kp",value=5.0)
+
+    # Initialize the task
+    gravity_task = blf.ik.GravityTask()
+    assert gravity_task.set_kin_dyn(kindyn)
+    assert gravity_task.initialize(param_handler=gravity_task_param_handler)
+    gravity_task_var_handler = blf.system.VariablesHandler()
+    assert gravity_task_var_handler.add_variable("robotVelocity", 32) is True  # robot velocity size = 26 (joints) + 6 (base)
+    assert gravity_task.set_variables_handler(variables_handler=gravity_task_var_handler)
+
+    # Set desired distance
+    assert gravity_task.set_desired_gravity_direction_in_target_frame(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)])
+    assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)])
+    assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)])
+
 def test_integration_based_ik_state():
 
     state = blf.ik.IntegrationBasedIKState()

src/IK/CMakeLists.txt

@@ -10,9 +10,9 @@ if(FRAMEWORK_COMPILE_IK)
     NAME                   IK
     PUBLIC_HEADERS         ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h
                            ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/QPFixedBaseInverseKinematics.h
-                           ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h
+                           ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h
     SOURCES                src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/JointLimitsTask.cpp
-                           src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp
+                           src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen
                            BipedalLocomotion::ParametersHandler BipedalLocomotion::System
                            LieGroupControllers::LieGroupControllers