From 8e8fe181f81c2f78f5eeab29354ea595935c4f5e Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Apr 2022 11:20:09 +0200 Subject: [PATCH 01/11] Bumped UnicyclePlanner requested version and bumped minor version. --- cmake/WalkingControllersFindDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/WalkingControllersFindDependencies.cmake b/cmake/WalkingControllersFindDependencies.cmake index 03db26fb..9d5c636d 100644 --- a/cmake/WalkingControllersFindDependencies.cmake +++ b/cmake/WalkingControllersFindDependencies.cmake @@ -134,7 +134,7 @@ checkandset_dependency(iDynTree) find_package(Eigen3 3.2.92 QUIET) checkandset_dependency(Eigen3) -find_package(UnicyclePlanner 0.4.0 QUIET) +find_package(UnicyclePlanner 0.4.2 QUIET) checkandset_dependency(UnicyclePlanner) find_package(osqp QUIET) From e5b8de9cf27320303991809cc09ca36a17ed04a9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Apr 2022 12:13:47 +0200 Subject: [PATCH 02/11] Using two parameters to set the inner ellipse. --- src/TrajectoryPlanner/src/TrajectoryGenerator.cpp | 6 ++++-- .../dcm_walking/common/freeSpaceEllipseParams.ini | 7 +++++-- .../dcm_walking/common/freeSpaceEllipseParams.ini | 7 +++++-- .../dcm_walking/common/freeSpaceEllipseParams.ini | 7 +++++-- .../dcm_walking/common/freeSpaceEllipseParams.ini | 7 +++++-- .../dcm_walking/common/freeSpaceEllipseParams.ini | 7 +++++-- 6 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index c973a1b8..712a5d38 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -117,7 +117,9 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) yarp::os::Bottle ellipseMethodGroup = config.findGroup("ELLIPSE_METHOD_SETTINGS"); double freeSpaceConservativeFactor = ellipseMethodGroup.check("conservative_factor", yarp::os::Value(2.0)).asFloat64(); - double innerEllipseOffset = ellipseMethodGroup.check("inner_ellipse_offset", yarp::os::Value(0.0)).asFloat64(); + double innerEllipseSemiMajorOffset = ellipseMethodGroup.check("inner_offset_major", yarp::os::Value(0.0)).asFloat64(); + double innerEllipseSemiMinorOffset = ellipseMethodGroup.check("inner_offset_minor", yarp::os::Value(0.0)).asFloat64(); + // try to configure the planner std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); @@ -143,7 +145,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) unicyclePlanner->startWithLeft(m_swingLeft); unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); ok = ok && unicyclePlanner->setFreeSpaceEllipseConservativeFactor(freeSpaceConservativeFactor); - ok = ok && unicyclePlanner->setInnerFreeSpaceEllipseOffset(innerEllipseOffset); + ok = ok && unicyclePlanner->setInnerFreeSpaceEllipseOffsets(innerEllipseSemiMajorOffset, innerEllipseSemiMinorOffset); ok = ok && m_trajectoryGenerator.setSwitchOverSwingRatio(switchOverSwingRatio); ok = ok && m_trajectoryGenerator.setTerminalHalfSwitchTime(lastStepSwitchTime); diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/freeSpaceEllipseParams.ini index 76c2bac0..77b6e0d3 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/freeSpaceEllipseParams.ini @@ -5,8 +5,11 @@ #Greater values make this behavior more sensitive. conservative_factor 2.0 -#This offset is subtracted from the semi_minor_axis and semi_major_axis to obtain the inner ellipse -inner_ellipse_offset 0.5 +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/freeSpaceEllipseParams.ini index 76c2bac0..77b6e0d3 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/freeSpaceEllipseParams.ini @@ -5,8 +5,11 @@ #Greater values make this behavior more sensitive. conservative_factor 2.0 -#This offset is subtracted from the semi_minor_axis and semi_major_axis to obtain the inner ellipse -inner_ellipse_offset 0.5 +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/freeSpaceEllipseParams.ini index 76c2bac0..77b6e0d3 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/common/freeSpaceEllipseParams.ini @@ -5,8 +5,11 @@ #Greater values make this behavior more sensitive. conservative_factor 2.0 -#This offset is subtracted from the semi_minor_axis and semi_major_axis to obtain the inner ellipse -inner_ellipse_offset 0.5 +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini index b462af8e..6f146f88 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini @@ -5,8 +5,11 @@ #Greater values make this behavior more sensitive. conservative_factor 2.0 -#This offset is subtracted from the semi_minor_axis and semi_major_axis to obtain the inner ellipse -inner_ellipse_offset 0.5 +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in diff --git a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/freeSpaceEllipseParams.ini index 76c2bac0..77b6e0d3 100644 --- a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/freeSpaceEllipseParams.ini @@ -5,8 +5,11 @@ #Greater values make this behavior more sensitive. conservative_factor 2.0 -#This offset is subtracted from the semi_minor_axis and semi_major_axis to obtain the inner ellipse -inner_ellipse_offset 0.5 +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in From 1a8b6b5e0d8023c5738eb8492e04df6b2fef009e Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Wed, 6 Apr 2022 10:46:35 +0000 Subject: [PATCH 03/11] Change joint regularization for using the trackers without iFeel suit --- .../iFeel_joint_retargeting/qpInverseKinematics.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini index 65beaf8b..ace3bba9 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini @@ -30,8 +30,8 @@ joint_regularization_weight_stance (0.0, 0.0, 0.0, joint_regularization_weight_walking (0.0, 0.0, 0.0, 1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) @@ -45,8 +45,8 @@ joint_retargeting_gains (5.0, 5.0, 5.0, joint_retargeting_weight_walking (2.0, 2.0, 2.0, 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) From aacf3da9233bf16642b1fe6b224ed3503295dff2 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Wed, 6 Apr 2022 10:47:06 +0000 Subject: [PATCH 04/11] Decrease walking velocity --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 64cb2270..627033f8 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -20,8 +20,8 @@ minWidth 0.16 maxAngleVariation 15.0 minAngleVariation 8.0 #Timings -maxStepDuration 1.0 -minStepDuration 0.7 +maxStepDuration 1.2 +minStepDuration 0.9 ##Nominal Values #Width @@ -32,7 +32,7 @@ stepLandingVelocity 0.0 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 0.9 +nominalDuration 1.1 lastStepSwitchTime 0.8 switchOverSwingRatio 0.7 From b78531fdcd312381a31212ee269e1834616ca396 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Wed, 6 Apr 2022 10:47:18 +0000 Subject: [PATCH 05/11] Disable ellipse --- .../iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini index 6f146f88..c82b8413 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/freeSpaceEllipseParams.ini @@ -14,7 +14,7 @@ inner_offset_minor 1.0 [ELLIPSE_MANAGER_SETTINGS] port_name freeSpaceEllipse:in -use_initial_ellipse 1 +use_initial_ellipse 0 [INITIAL_ELLIPSE] From 1a33729f6481309f4e64f29905723d8d57d868db Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Tue, 7 Jun 2022 16:54:51 +0000 Subject: [PATCH 06/11] Increased turning velocity of iCubGenova09. --- .../dcm_walking/common/plannerParams.ini | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 627033f8..62260387 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -6,7 +6,7 @@ unicycleGain 10.0 referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 -slowWhenTurningGain 4.0 +slowWhenTurningGain 2.0 slowWhenBackwardFactor 0.7 ##Bounds @@ -14,14 +14,14 @@ slowWhenBackwardFactor 0.7 maxStepLength 0.25 minStepLength 0.05 #Width -minWidth 0.16 +minWidth 0.12 #Angle Variations in DEGREES #maxAngleVariation 12.0 -maxAngleVariation 15.0 -minAngleVariation 8.0 +maxAngleVariation 18.0 +minAngleVariation 5.0 #Timings -maxStepDuration 1.2 -minStepDuration 0.9 +maxStepDuration 1.1 +minStepDuration 0.8 ##Nominal Values #Width @@ -32,7 +32,7 @@ stepLandingVelocity 0.0 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.1 +nominalDuration 1.0 lastStepSwitchTime 0.8 switchOverSwingRatio 0.7 From 8b2e66f29b718ef106ec23dc40c22a1c6346bcdc Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Thu, 7 Jul 2022 09:50:49 +0000 Subject: [PATCH 07/11] modify stepDuration and zmpDelta for iCubGenova09 --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 62260387..b578b73d 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -20,8 +20,8 @@ minWidth 0.12 maxAngleVariation 18.0 minAngleVariation 5.0 #Timings -maxStepDuration 1.1 -minStepDuration 0.8 +maxStepDuration 1.4 +minStepDuration 1.1 ##Nominal Values #Width @@ -32,13 +32,13 @@ stepLandingVelocity 0.0 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.0 +nominalDuration 1.1 lastStepSwitchTime 0.8 switchOverSwingRatio 0.7 #ZMP Delta leftZMPDelta (0.01 -0.0) -rightZMPDelta (0.01 -0.0) +rightZMPDelta (0.01 0.005) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 From 28fbd9046ffee022199c5a6cdf7c422cbe0f47ca Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Tue, 12 Jul 2022 17:56:17 +0000 Subject: [PATCH 08/11] Restored fast walking velocity and used landing velocity We noticed that using a landing velocity of -0.15 makes less visible the impact on the feet. -0.2 was better, but it causes the knee to go in HF becaue of overcurrent. --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index b578b73d..50f195e4 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -20,19 +20,19 @@ minWidth 0.12 maxAngleVariation 18.0 minAngleVariation 5.0 #Timings -maxStepDuration 1.4 -minStepDuration 1.1 +maxStepDuration 1.1 +minStepDuration 0.8 ##Nominal Values #Width nominalWidth 0.17 #Height stepHeight 0.035 -stepLandingVelocity 0.0 +stepLandingVelocity -0.15 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.1 +nominalDuration 1.0 lastStepSwitchTime 0.8 switchOverSwingRatio 0.7 From 68d783f0cecd1630958a307263ed4846f120fedb Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Jul 2022 17:20:54 +0000 Subject: [PATCH 09/11] [iCubGenova09] Update the planner parameters --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 50f195e4..58245e4a 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -11,7 +11,7 @@ slowWhenBackwardFactor 0.7 ##Bounds #Step length -maxStepLength 0.25 +maxStepLength 0.27 minStepLength 0.05 #Width minWidth 0.12 @@ -21,7 +21,7 @@ maxAngleVariation 18.0 minAngleVariation 5.0 #Timings maxStepDuration 1.1 -minStepDuration 0.8 +minStepDuration 0.7 ##Nominal Values #Width @@ -32,9 +32,9 @@ stepLandingVelocity -0.15 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.0 +nominalDuration 0.9 lastStepSwitchTime 0.8 -switchOverSwingRatio 0.7 +switchOverSwingRatio 0.3 #ZMP Delta leftZMPDelta (0.01 -0.0) From 80ce9f850b24410d5bec32ba9c0f97794083997d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 15 Jul 2022 17:34:46 +0000 Subject: [PATCH 10/11] [iCubGenova09] Update the rightZMPDelta --- .../robots/iCubGenova09/dcm_walking/common/plannerParams.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 58245e4a..3738b18a 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -38,7 +38,7 @@ switchOverSwingRatio 0.3 #ZMP Delta leftZMPDelta (0.01 -0.0) -rightZMPDelta (0.01 0.005) +rightZMPDelta (0.01 0.01) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 From 4532ecba0fd08497ebcbedb0f8ddf4ae5bf47c06 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Tue, 19 Jul 2022 13:44:27 +0000 Subject: [PATCH 11/11] Change knee low level PID --- .../app/robots/iCubGenova09/dcm_walking/common/pidParams.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini index b9aa6bb0..fe6023c1 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini @@ -15,8 +15,8 @@ useGainScheduling 0 [DEFAULT] #NAME P I D #torso_pitch 5.0 1.0 0.5 -l_knee -25000.0 -1000.0 0.0 -r_knee 25000.0 1000.0 0.0 +l_knee -30000.0 -1000.0 0.0 +r_knee 30000.0 1000.0 0.0 l_ankle_pitch -15000.0 -1000.0 0.0 r_ankle_pitch 15000.0 1000.0 0.0 l_ankle_roll -8000.0 -1000.0 0.0