Merge pull request #74 from robotology/devel

Merge devel into master

Author: GiulioRomualdi

CHANGELOG.md

@@ -6,6 +6,23 @@ All notable changes to this project are documented in this file.
 
 ### Changed
 
+## [0.4.0] - 2020-12-01
+### 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`
+- Modifying the follwoing classes for geting and using Gazebo base data:
+  - `/KinDynWrapper/Wrapper`
+  - `RobotInterface/Helper`
+  - `TrajectoryPlanner/TrajectoryGenerator`
+  - `WalkingModule`
+- Tune gains for iCubGenova04
 
 ## [0.3.3] - 2020-11-23
 ### Added
@@ -27,6 +44,7 @@ All notable changes to this project are documented in this file.
 - The `CHANGELOG.md` file
 - Implement the `WalkingControllersFindDepencies.cmake`
 - Adding the possibility of selecting Stiff/Compliant mode in joint level.
+
 ### Changed
 - General refactoring of the library. The WalkingModule is now split in several library. Namelly:
    - `YarpUtilities`: utilities for using `YARP`
@@ -93,7 +111,8 @@ All notable changes to this project are documented in this file.
 - Implement the first version of the `WalkingLoggerModule`
 - Implement the first version of the `WalkingJoypadModule`
 
-[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.3.3...devel
+[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.4.0...devel
+[0.4.0]: https://github.com/robotology/walking-controllers/compare/v0.3.3...v0.4.0
 [0.3.3]: https://github.com/robotology/walking-controllers/compare/v0.3.2...v0.3.3
 [0.3.2]: https://github.com/robotology/walking-controllers/compare/v0.3.1...v0.3.2
 [0.3.1]: https://github.com/robotology/walking-controllers/compare/v0.3.0...v0.3.1

CMakeLists.txt

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

src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h

@@ -17,21 +17,28 @@
 
 //iDynTree
 #include <iDynTree/KinDynComputations.h>
+#include <iDynTree/Model/FreeFloatingState.h>
 
 // iCub-ctrl
 #include <iCub/ctrl/filters.h>
 
+#include <unordered_map>
+
 namespace WalkingControllers
 {
 
     class WalkingFK
     {
         iDynTree::KinDynComputations m_kinDyn; /**< KinDynComputations solver. */
 
+        bool m_useExternalRobotBase; /**< is external estimator for the base of robot used? */
+        iDynTree::FreeFloatingGeneralizedTorques m_generalizedBiasForces;
+
         bool m_prevContactLeft; /**< Boolean is the previous contact foot the left one? */
         bool m_dcmEvaluated; /**< is the DCM evaluated? */
         bool m_comEvaluated; /**< is the CoM evaluated? */
 
+        iDynTree::FrameIndex m_frameBaseIndex;
         iDynTree::FrameIndex m_frameLeftIndex; /**< Index of the frame attached to the left foot in which all the left foot transformations are expressed. */
         iDynTree::FrameIndex m_frameRightIndex; /**< Index of the frame attached to the right foot in which all the right foot transformations are expressed. */
         iDynTree::FrameIndex m_frameRootIndex; /**< Index of the frame attached to the root_link. */
@@ -46,6 +53,8 @@ namespace WalkingControllers
         iDynTree::Transform m_frameHlinkLeft; /**< Transformation between the l_sole and the l_foot frame (l_ankle_2?!). */
         iDynTree::Transform m_frameHlinkRight; /**< Transformation between the l_sole and the l_foot frame (l_ankle_2?!). */
         iDynTree::Transform m_worldToBaseTransform; /**< World to base transformation. */
+        std::unordered_map<std::string, std::pair<const std::string, const iDynTree::Transform>> m_baseFrames;/**< Transform related to the base frame */
+        iDynTree::Twist m_baseTwist;/**< twist related to base frame */
 
         iDynTree::Position m_comPosition; /**< Position of the CoM. */
         iDynTree::Vector3 m_comVelocity; /**< Velocity of the CoM. */
@@ -60,6 +69,8 @@ namespace WalkingControllers
 
         bool m_firstStep; /**< True only during the first step. */
 
+        iDynTree::VectorDynSize m_jointPositions; /**< joint positions in radians. */
+
         /**
          * Set the model of the robot.
          * @param model iDynTree model.
@@ -77,6 +88,14 @@ namespace WalkingControllers
          */
         bool setBaseFrames(const std::string& lFootFrame, const std::string& rFootFrame);
 
+        /**
+         * Set The base frames for enabling using Gazebo as a base estimator.
+         * @param baseFrame the frame name inside model;
+         * @param name label for storing the frame information;
+         * @return true/false in case of success/failure.
+         */
+        bool setBaseFrame(const std::string& baseFrame, const std::string& name);
+
         /**
          * Evaluate the Divergent component of motion.
          */
@@ -98,24 +117,14 @@ namespace WalkingControllers
         bool initialize(const yarp::os::Searchable& config,
                         const iDynTree::Model& model);
 
-        /**
-         * Evaluate the first world to base transformation.
-         * @note: The The first reference frame is always the left foot.
-         * @note: please use this method only with evaluateWorldToBaseTransformation(const bool& isLeftFixedFrame);
-         * @param leftFootTransform transformation from the world to the left foot frame (l_sole);
-         * @return true/false in case of success/failure.
-         */
-        bool evaluateFirstWorldToBaseTransformation(const iDynTree::Transform& leftFootTransform);
-
         /**
          * Evaluate the world to base transformation
-         * @note: During the walking task the frame shift from the left to the right foot.
-         * the new base frame is attached where the foot is.
-         * @note: please use this method only with evaluateFirstWorldToBaseTransformation();
-         * @param isLeftFixedFrame true if the main frame of the left foot is fixed one.
+         * @param rootTransform transformation from the world to the root frame.
+         * @param rootTwist twist of the root frame.
          * @return true/false in case of success/failure.
          */
-        bool evaluateWorldToBaseTransformation(const bool& isLeftFixedFrame);
+        void evaluateWorldToBaseTransformation(const iDynTree::Transform& rootTransform,
+                                               const iDynTree::Twist& rootTwist);
 
         /**
          * Evaluate the world to base transformation
@@ -251,6 +260,12 @@ namespace WalkingControllers
          * @return true/false in case of success/failure.
          */
         bool getCoMJacobian(iDynTree::MatrixDynSize &jacobian);
+
+        /**
+         * Get the joint position
+         * @return the joint position expressed in radians
+         */
+        const iDynTree::VectorDynSize& getJointPos();
     };
 };
 #endif