From df77cef9418380f0ccb626acab871a0e03d94321 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 18 Sep 2024 17:40:06 +0200 Subject: [PATCH 1/2] Add joint limits task in the IK --- .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../autonomous/qpInverseKinematicsBlf.ini | 12 ++++++++++++ .../qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../autonomous/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joint_retargeting/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 9 +++++++++ .../joint_retargeting/qpInverseKinematicsBlf.ini | 12 ++++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../qpInverseKinematicsBlf.ini | 12 ++++++++++++ .../joypad_control/qpInverseKinematicsBlf.ini | 11 +++++++++++ .../WalkingControllers/WholeBodyControllers/BLFIK.h | 3 +++ src/WholeBodyControllers/src/BLFIK.cpp | 12 ++++++++++++ 18 files changed, 192 insertions(+) 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 index 70c48fa81..6a56c3156 100644 --- 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 index 70c48fa81..6a56c3156 100644 --- 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 index b16519e48..6f4f88e6a 100644 --- 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 index 82acb692e..0a85a3017 100644 --- 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 index b16519e48..e52bda034 100644 --- 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 index b16519e48..4e672692d 100644 --- 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 index 82acb692e..f3d757330 100644 --- 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 index b16519e48..4e672692d 100644 --- 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 index b16519e48..4e672692d 100644 --- 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 index 82acb692e..f3d757330 100644 --- 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 index 36c1d741c..69128c99b 100644 --- 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 index e695941e4..a3e174a66 100644 --- 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 index b16519e48..af4534b28 100644 --- 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 index 5f4115837..3a2f64a4e 100644 --- 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 index 36c1d741c..d3e4a5f59 100644 --- 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 index 36c1d741c..b934c501c 100644 --- 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 index d11452b26..7b7abbbef 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -64,11 +65,13 @@ class BLFIK std::shared_ptr m_rootTask; std::shared_ptr m_jointRetargetingTask; std::shared_ptr m_jointRegularizationTask; + std::shared_ptr 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 index bef835ceb..d81e1dee7 100644 --- 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(); + 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(); From 04eb0b32e08d4c144d9387fc7edd90a0bf648329 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 20 Sep 2024 09:27:36 +0200 Subject: [PATCH 2/2] Remove the need to specify the 'robot_velocity_variable_name' in the QPIK --- .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../autonomous/qpInverseKinematicsBlf.ini | 4 --- .../autonomous/tasks/regularization.ini | 1 - .../autonomous/tasks/retargeting.ini | 1 - .../dcm_walking/autonomous/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../autonomous/qpInverseKinematicsBlf.ini | 4 --- .../autonomous/tasks/regularization.ini | 1 - .../autonomous/tasks/retargeting.ini | 1 - .../dcm_walking/autonomous/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 3 -- .../tasks/regularization.ini | 1 - .../joint_retargeting/tasks/retargeting.ini | 1 - .../joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 3 -- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../joint_retargeting/tasks/retargeting.ini | 1 - .../joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 3 -- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 3 -- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../qpInverseKinematicsBlf.ini | 4 --- .../tasks/regularization.ini | 1 - .../tasks/retargeting.ini | 1 - .../iFeel_joint_retargeting/tasks/torso.ini | 1 - .../joypad_control/qpInverseKinematicsBlf.ini | 4 --- .../joypad_control/tasks/regularization.ini | 1 - .../joypad_control/tasks/torso.ini | 1 - .../WholeBodyControllers/BLFIK.h | 4 +++ src/WholeBodyControllers/src/BLFIK.cpp | 36 ++++++++++++++----- 69 files changed, 32 insertions(+), 128 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini index b16519e48..b015457b7 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_feedforward_term_for_joint_retargeting false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 048583577..f342f102e 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 1dce65fae..adb5854c4 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index d17d7a447..0eede3d1a 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 6a56c3156..f4d18e0f1 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 0.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..a7ed0265b 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..2d3c450c2 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 6a56c3156..f4d18e0f1 100644 --- 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 @@ -4,24 +4,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 0.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..a7ed0265b 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..2d3c450c2 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 6f4f88e6a..d4a870864 100644 --- 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 @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 8611abc63..b154631f6 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 45f04e11e..d77c26417 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index 01d7def3d..873d8fd73 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 0a85a3017..97e9f74af 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..a7ed0265b 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..2d3c450c2 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini index e52bda034..c4bba50ec 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/qpInverseKinematicsBlf.ini @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/regularization.ini index 47fdc9b78..c27bb6602 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/retargeting.ini index af2c08826..9982f299e 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/torso.ini index dc626bbb2..a826514ef 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/autonomous/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 4e672692d..8a85579bd 100644 --- 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 @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 070a4bb94..7e9d9209e 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index b7b403e37..831dedf06 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index dc626bbb2..a826514ef 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index f3d757330..fbd458666 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini index 758dfc72d..6aeb373aa 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini index b804b6a85..dd87092f0 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini index 4e672692d..8a85579bd 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini index 47fdc9b78..c27bb6602 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini index af2c08826..9982f299e 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini index dc626bbb2..a826514ef 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 4e672692d..8a85579bd 100644 --- 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 @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 070a4bb94..7e9d9209e 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index b7b403e37..831dedf06 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index dc626bbb2..a826514ef 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index f3d757330..fbd458666 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini index 758dfc72d..6aeb373aa 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini index b804b6a85..dd87092f0 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 69128c99b..4b84a783b 100644 --- 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 @@ -5,18 +5,15 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/regularization.ini index 048583577..f342f102e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/retargeting.ini index 1dce65fae..adb5854c4 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/torso.ini index d17d7a447..0eede3d1a 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index a3e174a66..3e5d00f98 100644 --- 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 @@ -2,18 +2,15 @@ robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 1.0 mask (true, true, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 4.0 kp_angular 2.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 4.0 kp_angular 2.0 diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/regularization.ini index 87c2081f9..800f7a96a 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/torso.ini index 8258c0af5..016d0d6df 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index af4534b28..a0d71453e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/qpInverseKinematicsBlf.ini @@ -5,24 +5,20 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/regularization.ini index 8611abc63..b154631f6 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/retargeting.ini index 45f04e11e..d77c26417 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/torso.ini index 01d7def3d..873d8fd73 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index 3a2f64a4e..a16a02d53 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -4,25 +4,21 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 1.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 1.0 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini index 4505b5a1a..cf9513110 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini index 7231b689c..318122c63 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index d3e4a5f59..7beb347e0 100644 --- 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 @@ -6,18 +6,15 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 048583577..f342f102e 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 1dce65fae..adb5854c4 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index d17d7a447..0eede3d1a 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 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 index b934c501c..949b93cc7 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -5,18 +5,15 @@ use_joints_limits_task false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/regularization.ini index 4505b5a1a..cf9513110 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/torso.ini index 7231b689c..318122c63 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini index b16519e48..b015457b7 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini @@ -4,24 +4,20 @@ use_feedforward_term_for_joint_retargeting false robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 8611abc63..b154631f6 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 45f04e11e..d77c26417 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index 01d7def3d..873d8fd73 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 98b440464..2dc917a96 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -2,24 +2,20 @@ robot_velocity_variable_name "robot_velocity" [COM_TASK] -robot_velocity_variable_name "robot_velocity" kp_linear 2.0 mask (true, true, false) [ROOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "root_link" kp_linear 0.5 mask (false, false, true) [RIGHT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "r_sole" kp_linear 7.0 kp_angular 5.0 [LEFT_FOOT_TASK] -robot_velocity_variable_name "robot_velocity" frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..a7ed0265b 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..2d3c450c2 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini @@ -1,4 +1,3 @@ -robot_velocity_variable_name "robot_velocity" frame_name "chest" kp_angular 5.0 diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 7b7abbbef..381f7a4c6 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -47,6 +47,10 @@ class BLFIK bool setTorsoSetPoint(const iDynTree::Rotation& rotation); const iDynTree::VectorDynSize& getDesiredJointVelocity() const; + bool initializeTask(std::shared_ptr task, + const std::string robotVelocityVariableName, + const std::weak_ptr handler); + private: std::shared_ptr m_torsoWeight; diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index d81e1dee7..bbc82d113 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -22,6 +22,24 @@ using namespace WalkingControllers; +bool BLFIK::initializeTask(std::shared_ptr task, + const std::string robotVelocityVariableName, + const std::weak_ptr handler) +{ + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("[BLFIK::initializeTask] Invalid parameter handler"); + return false; + } + + auto clone = ptr->clone(); + clone->setParameter("robot_velocity_variable_name", robotVelocityVariableName); + + return task->initialize(clone); +} + + bool BLFIK::initialize( std::weak_ptr handler, std::shared_ptr kinDyn) @@ -74,27 +92,27 @@ bool BLFIK::initialize( // CoM Task m_comTask = std::make_shared(); ok = ok && m_comTask->setKinDyn(kinDyn); - ok = ok && m_comTask->initialize(ptr->getGroup("COM_TASK")); + ok = ok && this->initializeTask(m_comTask, variable, ptr->getGroup("COM_TASK")); ok = ok && m_qpIK.addTask(m_comTask, "com_task", highPriority); m_rightFootTask = std::make_shared(); ok = ok && m_rightFootTask->setKinDyn(kinDyn); - ok = ok && m_rightFootTask->initialize(ptr->getGroup("RIGHT_FOOT_TASK")); + ok = ok && this->initializeTask(m_rightFootTask, variable, ptr->getGroup("RIGHT_FOOT_TASK")); ok = ok && m_qpIK.addTask(m_rightFootTask, "right_foot_task", highPriority); m_leftFootTask = std::make_shared(); ok = ok && m_leftFootTask->setKinDyn(kinDyn); - ok = ok && m_leftFootTask->initialize(ptr->getGroup("LEFT_FOOT_TASK")); + ok = ok && this->initializeTask(m_leftFootTask, variable, ptr->getGroup("LEFT_FOOT_TASK")); ok = ok && m_qpIK.addTask(m_leftFootTask, "left_foot_task", highPriority); m_torsoTask = std::make_shared(); ok = ok && m_torsoTask->setKinDyn(kinDyn); - ok = ok && m_torsoTask->initialize(ptr->getGroup("TORSO_TASK")); + ok = ok && this->initializeTask(m_torsoTask, variable, ptr->getGroup("TORSO_TASK")); ok = ok && m_qpIK.addTask(m_torsoTask, "torso_task", lowPriority, m_torsoWeight); m_jointRegularizationTask = std::make_shared(); ok = ok && m_jointRegularizationTask->setKinDyn(kinDyn); - ok = ok && m_jointRegularizationTask->initialize(ptr->getGroup("JOINT_REGULARIZATION_TASK")); + ok = ok && this->initializeTask(m_jointRegularizationTask, variable, ptr->getGroup("JOINT_REGULARIZATION_TASK")); ok = ok && m_qpIK.addTask(m_jointRegularizationTask, "joint_regularization_task", @@ -105,7 +123,7 @@ bool BLFIK::initialize( { m_jointRetargetingTask = std::make_shared(); ok = ok && m_jointRetargetingTask->setKinDyn(kinDyn); - ok = ok && m_jointRetargetingTask->initialize(ptr->getGroup("JOINT_RETARGETING_TASK")); + ok = ok && this->initializeTask(m_jointRetargetingTask, variable, ptr->getGroup("JOINT_RETARGETING_TASK")); ok = ok && m_qpIK.addTask(m_jointRetargetingTask, "joint_retargeting_task", @@ -118,7 +136,7 @@ bool BLFIK::initialize( { m_jointLimitsTask = std::make_shared(); ok = ok && m_jointLimitsTask->setKinDyn(kinDyn); - ok = ok && m_jointLimitsTask->initialize(ptr->getGroup("JOINT_LIMITS_TASK")); + ok = ok && this->initializeTask(m_jointLimitsTask, variable, ptr->getGroup("JOINT_LIMITS_TASK")); ok = ok && m_qpIK.addTask(m_jointLimitsTask, "joint_limits_task", highPriority); } @@ -126,7 +144,7 @@ bool BLFIK::initialize( { m_rootTask = std::make_shared(); ok = ok && m_rootTask->setKinDyn(kinDyn); - ok = ok && m_rootTask->initialize(ptr->getGroup("ROOT_TASK")); + ok = ok && this->initializeTask(m_rootTask, variable, ptr->getGroup("ROOT_TASK")); ok = ok && m_qpIK.addTask(m_rootTask, "root_task", highPriority); } @@ -165,7 +183,9 @@ bool BLFIK::setPhase(const std::string& phase) ok = ok && m_jointRegularizationWeight->setState(phase); if (m_usejointRetargeting) + { ok = ok && m_jointRetargetingWeight->setState(phase); + } return ok; }