Add joint limits task in the IK

GiulioRomualdi · GiulioRomualdi · commit 9b9a4d843eb1 · 2024-09-18T17:40:06.000+02:00
diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -24,5 +26,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -24,5 +26,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.002
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -24,5 +26,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.002
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,17 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -24,5 +26,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -24,5 +26,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.001
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -20,6 +21,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -18,5 +18,14 @@ frame_name                      "l_sole"
 kp_linear                       4.0
 kp_angular                      2.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -26,6 +27,17 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,3 +1,5 @@
+use_joints_limits_task                          false
+
 [IK]
 robot_velocity_variable_name    "robot_velocity"
 
@@ -25,5 +27,14 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini
@@ -1,4 +1,6 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
+
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -20,6 +22,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini
@@ -1,4 +1,5 @@
 use_feedforward_term_for_joint_retargeting      false
+use_joints_limits_task                          false
 
 [IK]
 robot_velocity_variable_name    "robot_velocity"
@@ -20,6 +21,16 @@ frame_name                      "l_sole"
 kp_linear                       7.0
 kp_angular                      5.0
 
+
+[JOINT_LIMITS_TASK]
+sampling_time                   0.01
+klim                            (1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+use_model_limits                true
+
 [include TORSO_TASK "./tasks/torso.ini"]
 [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
 [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]
diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h
@@ -10,6 +10,7 @@
 #include <BipedalLocomotion/ContinuousDynamicalSystem/MultiStateWeightProvider.h>
 #include <BipedalLocomotion/IK/CoMTask.h>
 #include <BipedalLocomotion/IK/JointTrackingTask.h>
+#include <BipedalLocomotion/IK/JointLimitsTask.h>
 #include <BipedalLocomotion/IK/QPInverseKinematics.h>
 #include <BipedalLocomotion/IK/R3Task.h>
 #include <BipedalLocomotion/IK/SE3Task.h>
@@ -64,11 +65,13 @@ class BLFIK
     std::shared_ptr<BipedalLocomotion::IK::R3Task> m_rootTask;
     std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask> m_jointRetargetingTask;
     std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask> m_jointRegularizationTask;
+    std::shared_ptr<BipedalLocomotion::IK::JointLimitsTask> m_jointLimitsTask;
 
     iDynTree::VectorDynSize m_jointVelocity;
     bool m_usejointRetargeting{false};
     bool m_useRootLinkForHeight{false};
     bool m_useFeedforwardTermForJointRetargeting{false};
+    bool m_useJointLimitsTask{false};
 };
 
 } // namespace WalkingControllers
diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp
@@ -46,6 +46,9 @@ bool BLFIK::initialize(
     m_useRootLinkForHeight = false;
     ptr->getParameter("use_root_link_for_height", m_useRootLinkForHeight);
 
+    m_useJointLimitsTask = false;
+    ptr->getParameter("use_joints_limits_task", m_useJointLimitsTask);
+
     // weight providers
     bool ok = m_qpIK.initialize(ptr->getGroup("IK"));
     auto group = ptr->getGroup("IK").lock();
@@ -110,6 +113,15 @@ bool BLFIK::initialize(
                                m_jointRetargetingWeight);
     }
 
+
+    if (m_useJointLimitsTask)
+    {
+        m_jointLimitsTask = std::make_shared<BipedalLocomotion::IK::JointLimitsTask>();
+        ok = ok && m_jointLimitsTask->setKinDyn(kinDyn);
+        ok = ok && m_jointLimitsTask->initialize(ptr->getGroup("JOINT_LIMITS_TASK"));
+        ok = ok && m_qpIK.addTask(m_jointLimitsTask, "joint_limits_task", highPriority);
+    }
+
     if (m_useRootLinkForHeight)
     {
         m_rootTask = std::make_shared<BipedalLocomotion::IK::R3Task>();
