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) 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..c82b8413 100644 --- 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 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 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..3738b18a 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -6,21 +6,21 @@ 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 @@ -28,17 +28,17 @@ minStepDuration 0.7 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/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) 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