Merge pull request #56 from bemilio/retrieve_weight_from_unicycle_patch

Added weight retrieval function

CHANGELOG.md

@@ -4,11 +4,14 @@ All notable changes to this project are documented in this file.
 ## [Unreleased]
 ### Added
 - Adding the possibility to use Gazebo base data inside the walking controller
+- `TrajectoryGenerator` class of the `TrajectoryPlanner` library includes now
+  the method `getWeightPercentage` to retrieve the amount of weight on each foot
+  requested by the planner.
 
 ### Changed
 - Adding the `use_external_robot_base` parameter inside the `dcm_walking_with_joypad.ini`
 - Adding the Gazebo base data port inside the `robotControl.ini`
-- Tunning the `zmpControllerParams.ini` and `dcmReactiveControllerParams.ini` 
+- Tunning the `zmpControllerParams.ini` and `dcmReactiveControllerParams.ini`
 - Modifying the follwoing classes for geting and using Gazebo base data:
   - `/KinDynWrapper/Wrapper`
   - `RobotInterface/Helper`

CMakeLists.txt

@@ -7,7 +7,7 @@ set(CMAKE_CXX_STANDARD 14)
 
 ## MAIN project
 project(WalkingControllers
-  VERSION 0.4.101)
+  VERSION 0.4.102)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

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

@@ -216,6 +216,17 @@ namespace WalkingControllers
          */
         bool getMergePoints(std::vector<size_t>& mergePoints);
 
+        /**
+         * Get the weight percentage for the left and right foot
+         * @param weightInLeft vector containing the weight on the left foot (0 in case in case of
+         * stance foot during SS, 1 in case of swing foot)
+         * @param weightInRight vector containing the weight on the right foot (0 in case in case of
+         * stance foot during SS, 1 in case of swing foot)
+         * @return true/false in case of success/failure.
+         */
+        bool getWeightPercentage(std::vector<double> &weightInLeft, std::vector<double> &weightInRight);
+
+
         /**
          * Reset the planner
          */

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

@@ -612,6 +612,19 @@ bool TrajectoryGenerator::getMergePoints(std::vector<size_t>& mergePoints)
     return true;
 }
 
+bool TrajectoryGenerator::getWeightPercentage(std::vector<double> &weightInLeft,
+                                              std::vector<double> &weightInRight)
+{
+    if(!isTrajectoryComputed())
+    {
+        yError() << "[getWeightPercentage] No trajectories are available";
+        return false;
+    }
+
+    m_dcmGenerator->getWeightPercentage(weightInLeft, weightInRight);
+    return true;
+}
+
 void TrajectoryGenerator::reset()
 {
     // the mutex is automatically released when lock_guard goes out of its scope