Merge pull request #114 from robotology/paris_demo

Paris and minister demo
robotology · Aug 24, 2022 · 082c36e · 082c36e
2 parents b2e6aa9 + 4532ecb
commit 082c36e
Show file tree

Hide file tree

Showing 10 changed files with 46 additions and 29 deletions.
diff --git 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)

diff --git 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> 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
@@ -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
@@ -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
@@ -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
@@ -5,13 +5,16 @@
 #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
 
-use_initial_ellipse    1
+use_initial_ellipse    0    
 
 [INITIAL_ELLIPSE]
 

diff --git 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

diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini
@@ -6,39 +6,39 @@ unicycleGain            10.0
 referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
-slowWhenTurningGain     4.0
+slowWhenTurningGain     2.0
 slowWhenBackwardFactor  0.7
 
 ##Bounds
 #Step length
-maxStepLength           0.25
+maxStepLength           0.27
 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.0
+maxStepDuration         1.1
 minStepDuration         0.7
 
 ##Nominal Values
 #Width
 nominalWidth            0.17
 #Height
 stepHeight              0.035
-stepLandingVelocity     0.0
+stepLandingVelocity     -0.15
 footApexTime            0.5
 comHeightDelta          0.01
 #Timings
 nominalDuration         0.9
 lastStepSwitchTime      0.8
-switchOverSwingRatio    0.7
+switchOverSwingRatio    0.3
 
 #ZMP Delta
 leftZMPDelta           (0.01 -0.0)
-rightZMPDelta          (0.01 -0.0)
+rightZMPDelta          (0.01 0.01)
 
 #Feet cartesian offset on the yaw
 leftYawDeltaInDeg      0.0

diff --git a/...odule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini b/...odule/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)
 

diff --git 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