Streaming odometry info, CoM and footsteps

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

@@ -264,6 +264,23 @@ namespace WalkingControllers
          * Reset the planner
          */
         void reset();
+
+        /**
+         * Get the footprints planned by the unicycle generator
+         * @param leftFootprints vector containing the desired poses of the left foot on the xy plane
+         * @param rightFootprints vector containing the desired poses of the right foot on the xy plane
+         * @return true/false in case of success/failure.
+         */
+        bool getFootprints(std::vector<iDynTree::Vector3>& leftFootprints, std::vector<iDynTree::Vector3>& rightFootprints);
+
+        /**
+         * Get the unicycle state from the feet of the robot in the odom frame (aka world frame)
+         * @param virtualUnicyclePose pose of the virtual unicycle in the odom frame
+         * @param referenceUnicyclePose pose of the reference virtual unicycle in the odom frame
+         * @param stanceFoot returns a string with the stance foot used for the computation
+         * @return true/false in case of success/failure.
+         */
+        bool getUnicycleState(iDynTree::Vector3& virtualUnicyclePose, iDynTree::Vector3& referenceUnicyclePose, const std::string& stanceFoot);
     };
 };
 

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -853,3 +853,104 @@ bool TrajectoryGenerator::getDesiredZMPPosition(std::vector<iDynTree::Vector2> &
     desiredZMP = m_dcmGenerator->getZMPPosition();
     return true;
 }
+
+//TODO improve conversion
+bool TrajectoryGenerator::getFootprints(std::vector<iDynTree::Vector3>& leftFootprints, std::vector<iDynTree::Vector3>& rightFootprints)
+{
+    auto leftDequeue = m_trajectoryGenerator.getLeftFootPrint()->getSteps();
+    for (size_t i = 0; i < leftDequeue.size(); ++i)
+    {
+        iDynTree::Vector3 pose;
+        pose(0) = leftDequeue.at(i).position(0);
+        pose(1) = leftDequeue.at(i).position(1);
+        pose(2) = leftDequeue.at(i).angle;
+        leftFootprints.push_back(pose);
+    }
+
+    auto rightDeque = m_trajectoryGenerator.getRightFootPrint()->getSteps();
+    for (size_t i = 0; i < rightDeque.size(); ++i)
+    {
+        iDynTree::Vector3 pose;
+        pose(0) = rightDeque.at(i).position(0);
+        pose(1) = rightDeque.at(i).position(1);
+        pose(2) = rightDeque.at(i).angle;
+        rightFootprints.push_back(pose);
+    }
+
+    return true;
+}
+
+bool TrajectoryGenerator::getUnicycleState(iDynTree::Vector3& virtualUnicyclePose, iDynTree::Vector3& referenceUnicyclePose, const std::string& stanceFoot)
+{
+    double nominalWidth;
+    bool correctLeft;
+    iDynTree::Vector2 measuredPositionLeft, measuredPositionRight;
+    iDynTree::Vector3 desiredDirectControl;
+    double measuredAngleLeft, measuredAngleRight;
+    double leftYawDeltaInRad, rightYawDeltaInRad;
+    // left foot
+    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;
+    m_newFreeSpaceEllipse = false;
+    nominalWidth = m_nominalWidth;
+
+    iDynTree::Vector2 measuredPosition;
+    double measuredAngle;
+    measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight;
+    measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight;
+
+    Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition, unicycleOdom;
+    unicyclePositionFromStanceFoot(0) = 0.0;
+    Eigen::Matrix2d unicycleRotation; //rotation world -> unicycle
+    double unicycleAngle, unicycleAngleOdom;
+
+    if (stanceFoot == "left")
+    {
+        unicyclePositionFromStanceFoot(1) = -nominalWidth/2;
+        unicycleAngle = measuredAngleLeft;  //- leftYawDeltaInRad
+        unicycleAngleOdom = measuredAngleLeft - leftYawDeltaInRad;
+        footPosition = iDynTree::toEigen(measuredPositionLeft);
+        //stanceFoot = "left";
+    }
+    else if (stanceFoot == "right")
+    {
+        unicyclePositionFromStanceFoot(1) = nominalWidth/2;
+        unicycleAngle = measuredAngleRight;    //- rightYawDeltaInRad
+        unicycleAngleOdom = measuredAngleRight - rightYawDeltaInRad;
+        footPosition = iDynTree::toEigen(measuredPositionRight);
+        //stanceFoot = "right";
+    }
+    else
+    {
+        return false;
+    }
+
+    //Odom (expressed in odom frame)
+    double s_theta = std::sin(unicycleAngleOdom);
+    double c_theta = std::cos(unicycleAngleOdom);
+    unicycleRotation(0,0) = c_theta;
+    unicycleRotation(0,1) = -s_theta;
+    unicycleRotation(1,0) = s_theta;
+    unicycleRotation(1,1) = c_theta;
+    unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot;   // position in respect to the support foot
+    unicycleOdom = unicyclePosition + footPosition;     //postion in odom frame
+    virtualUnicyclePose(0) = unicycleOdom(0);
+    virtualUnicyclePose(1) = unicycleOdom(1);
+    virtualUnicyclePose(2) = unicycleAngleOdom;
+
+    iDynTree::Vector2 referencePointInAbsoluteFrame;
+    iDynTree::toEigen(referencePointInAbsoluteFrame) = unicycleRotation * (iDynTree::toEigen(m_referencePointDistance)) + unicycleOdom;
+    referenceUnicyclePose(0) = referencePointInAbsoluteFrame(0);
+    referenceUnicyclePose(1) = referencePointInAbsoluteFrame(1);
+    referenceUnicyclePose(2) = unicycleAngleOdom;
+
+    return true;
+}

src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini

@@ -54,6 +54,9 @@ sampling_time           0.01
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link
 height_reference_frame  root_link
 
+# Loop rate for the thread computing the navigation trigger
+navigationTriggerLoopRate   100
+
 # include robot control parameters
 [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"]
 

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -123,6 +123,7 @@ namespace WalkingControllers
         std::deque<double> m_comHeightVelocity; /**< Deque containing the CoM height velocity. */
         std::deque<size_t> m_mergePoints; /**< Deque containing the time position of the merge points. */
         std::deque<bool> m_isStancePhase; /**< if true the robot is not walking */
+        std::deque<iDynTree::Vector3> m_desiredCoM_Trajectory;  /**< Deque containing the desired CoM trajectory projected on the ground in pose x, y, theta. */
 
         std::deque<bool> m_isLeftFixedFrame; /**< Deque containing when the main frame of the left foot is the fixed frame
                                                 In general a main frame of a foot is the fix frame only during the
@@ -141,6 +142,7 @@ namespace WalkingControllers
         yarp::os::BufferedPort<yarp::sig::Vector> m_desiredUnyciclePositionPort; /**< Desired robot position port. */
 
         bool m_newTrajectoryRequired; /**< if true a new trajectory will be merged soon. (after m_newTrajectoryMergeCounter - 2 cycles). */
+        bool m_newTrajectoryMerged; /**< true if a new trajectory has been just merged. */
         size_t m_newTrajectoryMergeCounter; /**< The new trajectory will be merged after m_newTrajectoryMergeCounter - 2 cycles. */
 
         bool m_useRootLinkForHeight;
@@ -155,10 +157,18 @@ namespace WalkingControllers
         size_t m_feedbackAttempts;
         double m_feedbackAttemptDelay;
 
+        std::thread m_virtualUnicyclePubliserThread; /**< Thread for publishing the state of the unicycle used in the TrajectoryGenerator. */
+        bool m_runThreads;
+        yarp::os::BufferedPort<yarp::sig::Vector> m_plannedCoMPositionPort; /**< Desired CoM position port for naviagation purposes. */
+        bool m_wasInDoubleSupport;  /**< Flag that symbolizes the previous status of the double support. */
+        int m_navigationTriggerLoopRate;    /**< Loop rate for the thread computing the navigation trigger*/
+
         // debug
         std::unique_ptr<iCub::ctrl::Integrator> m_velocityIntegral{nullptr};
 
         yarp::os::BufferedPort<BipedalLocomotion::YarpUtilities::VectorsCollection> m_loggerPort; /**< Logger port. */
+        yarp::os::BufferedPort<yarp::os::Bottle> m_feetPort; /**< Feet port vector of feet positions (left, right). */
+        yarp::os::BufferedPort<yarp::os::Bottle> m_unicyclePort; /**< Feet port vector of feet positions (left, right). */
 
         /**
          * Get the robot model from the resource finder and set it.
@@ -277,6 +287,11 @@ namespace WalkingControllers
          */
         void applyGoalScaling(yarp::sig::Vector &plannerInput);
 
+        /**
+         * Parallel thread for publishing the unicycle status.
+         */
+        void computeVirtualUnicycleThread();
+
     public:
 
         /**