Using the feetYawDelta and the slowWhenBackwardFactor.

cmake/WalkingControllersFindDependencies.cmake

@@ -134,7 +134,7 @@ checkandset_dependency(iDynTree)
 find_package(Eigen3 3.2.92 QUIET)
 checkandset_dependency(Eigen3)
 
-find_package(UnicyclePlanner 0.3.0 QUIET)
+find_package(UnicyclePlanner 0.3.101 QUIET)
 checkandset_dependency(UnicyclePlanner)
 
 find_package(osqp QUIET)

src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h

@@ -49,6 +49,8 @@ namespace WalkingControllers
         double m_plannerHorizon; /**< Horizon of the planner. */
         std::size_t m_stancePhaseDelay; /**< Delay in ticks of the beginning of the stance phase. */
 
+        double m_leftYawDeltaInRad; /**< Offset for the left foot rotation around the z axis. */
+        double m_rightYawDeltaInRad; /**< Offset for the right foot rotation around the z axis. */
         double m_nominalWidth; /**< Nominal width between two feet. */
         double m_initTime; /**< Init time of the current trajectory. */
 

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -83,6 +83,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asDouble();
     double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asDouble();
     double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble();
+    double slowWhenBackwardFactor = config.check("slowWhenBackwardFactor", yarp::os::Value(1.0)).asDouble();
     double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asDouble();
     double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asDouble();
     double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble();
@@ -103,6 +104,8 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
                                                yarp::os::Value(0.4)).asDouble();
     double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble();
     double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asDouble();
+    m_leftYawDeltaInRad = iDynTree::deg2rad(config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asDouble());
+    m_rightYawDeltaInRad = iDynTree::deg2rad(config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asDouble());
 
     m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble();
 
@@ -129,6 +132,9 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
     ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation);
     ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength);
     ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain);
+    ok = ok && unicyclePlanner->setSlowWhenBackwardFactor(slowWhenBackwardFactor);
+    unicyclePlanner->setLeftFootYawOffsetInRadians(m_leftYawDeltaInRad);
+    unicyclePlanner->setRightFootYawOffsetInRadians(m_rightYawDeltaInRad);
     unicyclePlanner->addTerminalStep(true);
     unicyclePlanner->startWithLeft(m_swingLeft);
     unicyclePlanner->resetStartingFootIfStill(startWithSameFoot);
@@ -192,9 +198,10 @@ void TrajectoryGenerator::computeThread()
 
         bool correctLeft;
 
-        iDynTree::Vector2 desiredPoint;
+        iDynTree::Vector2 desiredPointInRelativeFrame, desiredPointInAbsoluteFrame;
         iDynTree::Vector2 measuredPositionLeft, measuredPositionRight;
         double measuredAngleLeft, measuredAngleRight;
+        double leftYawDeltaInRad, rightYawDeltaInRad;
 
         iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition;
         iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity;
@@ -217,7 +224,7 @@ void TrajectoryGenerator::computeThread()
             endTime = initTime + m_plannerHorizon;
 
             // set desired point
-            desiredPoint = m_desiredPoint;
+            desiredPointInRelativeFrame = m_desiredPoint;
 
             // dcm boundary conditions
             DCMBoundaryConditionAtMergePointPosition = m_DCMBoundaryConditionAtMergePointPosition;
@@ -227,11 +234,13 @@ void TrajectoryGenerator::computeThread()
             measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0);
             measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1);
             measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2);
+            leftYawDeltaInRad = m_leftYawDeltaInRad;
 
             // right foot
             measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0);
             measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1);
             measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2);
+            rightYawDeltaInRad = m_rightYawDeltaInRad;
 
             correctLeft = m_correctLeft;
 
@@ -246,12 +255,55 @@ void TrajectoryGenerator::computeThread()
             }
         }
 
+        iDynTree::Vector2 measuredPosition;
+        double measuredAngle;
+        measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight;
+        measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight;
+
+        DCMInitialState initialState;
+        initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition;
+        initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity;
+
+        Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition;
+        unicyclePositionFromStanceFoot(0) = 0.0;
+
+        Eigen::Matrix2d unicycleRotation;
+        double unicycleAngle;
+
+        if (correctLeft)
+        {
+            unicyclePositionFromStanceFoot(1) = -nominalWidth/2;
+            unicycleAngle = measuredAngleLeft - leftYawDeltaInRad;
+            footPosition = iDynTree::toEigen(measuredPositionLeft);
+        }
+        else
+        {
+            unicyclePositionFromStanceFoot(1) = nominalWidth/2;
+            unicycleAngle = measuredAngleRight - rightYawDeltaInRad;
+            footPosition = iDynTree::toEigen(measuredPositionRight);
+        }
+
+        double s_theta = std::sin(unicycleAngle);
+        double c_theta = std::cos(unicycleAngle);
+
+        unicycleRotation(0,0) = c_theta;
+        unicycleRotation(0,1) = -s_theta;
+        unicycleRotation(1,0) = s_theta;
+        unicycleRotation(1,1) = c_theta;
+
+        unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot + footPosition;
+
+        // apply the homogeneous transformation w_H_{unicycle}
+        iDynTree::toEigen(desiredPointInAbsoluteFrame) = unicycleRotation * (iDynTree::toEigen(m_referencePointDistance) +
+                                                                             iDynTree::toEigen(desiredPointInRelativeFrame))
+                + unicyclePosition;
+
         // clear the old trajectory
         std::shared_ptr<UnicyclePlanner> unicyclePlanner = m_trajectoryGenerator.unicyclePlanner();
         unicyclePlanner->clearDesiredTrajectory();
 
         // add new point
-        if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint))
+        if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPointInAbsoluteFrame))
         {
             // something goes wrong
             std::lock_guard<std::mutex> guard(m_mutex);
@@ -260,15 +312,6 @@ void TrajectoryGenerator::computeThread()
             break;
         }
 
-        iDynTree::Vector2 measuredPosition;
-        double measuredAngle;
-        measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight;
-        measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight;
-
-        DCMInitialState initialState;
-        initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition;
-        initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity;
-
         if (!m_dcmGenerator->setDCMInitialState(initialState)) {
             // something goes wrong
             std::lock_guard<std::mutex> guard(m_mutex);
@@ -282,35 +325,6 @@ void TrajectoryGenerator::computeThread()
             iDynTree::MatrixFixSize<2,2> ellipseImage = freeSpaceEllipse.imageMatrix(), newEllipseImage;
             iDynTree::VectorFixSize<2> centerOffset = freeSpaceEllipse.centerOffset(), newCenterOffset;
 
-            Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition;
-            unicyclePositionFromStanceFoot(0) = 0.0;
-
-            Eigen::Matrix2d unicycleRotation;
-            double theta;
-
-            if (correctLeft)
-            {
-                unicyclePositionFromStanceFoot(1) = -nominalWidth/2;
-                theta = measuredAngleLeft;
-                footPosition = iDynTree::toEigen(measuredPositionLeft);
-            }
-            else
-            {
-                unicyclePositionFromStanceFoot(1) = nominalWidth/2;
-                theta = measuredAngleRight;
-                footPosition = iDynTree::toEigen(measuredPositionRight);
-            }
-
-            double s_theta = std::sin(theta);
-            double c_theta = std::cos(theta);
-
-            unicycleRotation(0,0) = c_theta;
-            unicycleRotation(0,1) = -s_theta;
-            unicycleRotation(1,0) = s_theta;
-            unicycleRotation(1,1) = c_theta;
-
-            unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot + footPosition;
-
             iDynTree::toEigen(newEllipseImage) = unicycleRotation * iDynTree::toEigen(ellipseImage);
 
             iDynTree::toEigen(newCenterOffset) = unicycleRotation * iDynTree::toEigen(centerOffset) + unicyclePosition;
@@ -489,7 +503,10 @@ bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &l
         return false;
     }
 
-    m_generatorState = GeneratorState::Returned;
+    {
+        std::lock_guard<std::mutex> guard(m_mutex);
+        m_generatorState = GeneratorState::Returned;
+    }
     return true;
 }
 
@@ -513,32 +530,8 @@ bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Ve
                      << "Please call 'generateFirstTrajectories()' method.";
             return false;
         }
-    }
-    // if correctLeft is true the stance foot is the true.
-    // The vector (expressed in the unicycle reference frame from the left foot to the center of the
-    // unicycle is [0, width/2]')
-    iDynTree::Vector2 unicyclePositionFromStanceFoot;
-    unicyclePositionFromStanceFoot(0) = 0.0;
-    unicyclePositionFromStanceFoot(1) = correctLeft ? -m_nominalWidth/2 : m_nominalWidth/2;
 
-    iDynTree::Vector2 desredPositionFromStanceFoot;
-    iDynTree::toEigen(desredPositionFromStanceFoot) = iDynTree::toEigen(unicyclePositionFromStanceFoot)
-        + iDynTree::toEigen(m_referencePointDistance) + iDynTree::toEigen(desiredPosition);
-
-    // prepare the rotation matrix w_R_{unicycle}
-    double theta = measured.getRotation().asRPY()(2);
-    double s_theta = std::sin(theta);
-    double c_theta = std::cos(theta);
-
-    // save the data
-    {
-        std::lock_guard<std::mutex> guard(m_mutex);
-
-        // apply the homogeneous transformation w_H_{unicycle}
-        m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0)
-            - s_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(0);
-        m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0)
-            + c_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(1);
+        m_desiredPoint = desiredPosition;
 
         m_initTime = initTime;
 

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -39,6 +40,10 @@ switchOverSwingRatio    0.7
 leftZMPDelta           (0.03 -0.005)
 rightZMPDelta          (0.03 0.005)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;
 # If it is 0 the DCM position coincides with the stance foot ZMP;

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -39,6 +40,10 @@ switchOverSwingRatio    1.0
 leftZMPDelta           (-0.015 -0.01)
 rightZMPDelta          (-0.015  0.01)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;
 # If it is 0 the DCM position coincides with the stance foot ZMP;

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -19,7 +20,7 @@ minWidth                0.15
 maxAngleVariation       20.0
 minAngleVariation       8.0
 #Timings
-maxStepDuration		0.95
+maxStepDuration         0.95
 minStepDuration         0.80
 
 ##Nominal Values
@@ -39,6 +40,10 @@ switchOverSwingRatio    0.7
 leftZMPDelta           (0.03 -0.005)
 rightZMPDelta          (0.03 0.005)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;
 # If it is 0 the DCM position coincides with the stance foot ZMP;

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -39,6 +40,10 @@ switchOverSwingRatio    0.7
 leftZMPDelta           (0.03 -0.005)
 rightZMPDelta          (0.03 0.005)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 #MergePoint
 mergePointRatio         0.4
 

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -39,6 +40,10 @@ switchOverSwingRatio    0.7
 leftZMPDelta           (0.03 -0.005)
 rightZMPDelta          (0.03 0.005)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 #MergePoint
 mergePointRatio         0.4
 

src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini

@@ -7,6 +7,7 @@ referencePosition       (0.1 0.0)
 timeWeight              2.5
 positionWeight          1.0
 slowWhenTurningGain     5.0
+slowWhenBackwardFactor  1.0
 
 ##Bounds
 #Step length
@@ -39,6 +40,10 @@ switchOverSwingRatio    0.7
 leftZMPDelta           (0.03 -0.005)
 rightZMPDelta          (0.03 0.005)
 
+#Feet cartesian offset on the yaw
+leftYawDeltaInDeg      0.0
+rightYawDeltaInDeg     0.0
+
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;
 # If it is 0 the DCM position coincides with the stance foot ZMP;

src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini

@@ -25,5 +25,5 @@ l_hip_pitch         -5000.0  -1500.0   0.0
 r_hip_pitch          5000.0   1500.0   0.0
 l_hip_roll          25000.0   1000.0   0.0
 r_hip_roll         -25000.0  -1000.0   0.0
-r_hip_yaw            5000.0    2000.0   0.0
-l_hip_yaw           -5000.0    -2000.0   0.0
+r_hip_yaw            10000.0    2000.0   0.0
+l_hip_yaw           -7000.0    -2000.0   0.0