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();