Merge pull request #52 from MiladShafiee/devel

Use gazebo base data in position control walking
robotology · Mar 26, 2020 · 73b8add · 73b8add
2 parents 5f53955 + eb66623
commit 73b8add
Show file tree

Hide file tree

Showing 13 changed files with 299 additions and 122 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -3,8 +3,17 @@ 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
 
 ### 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`
 
 ## [0.3.2] - 2020-03-21
 ### Changed
@@ -19,6 +28,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`

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

diff --git a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/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. */
@@ -77,6 +86,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 +115,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

diff --git a/src/KinDynWrapper/src/Wrapper.cpp b/src/KinDynWrapper/src/Wrapper.cpp
@@ -40,7 +40,7 @@ bool WalkingFK::setRobotModel(const iDynTree::Model& model)
     return true;
 }
 
-bool WalkingFK::setBaseFrames(const std::string& lFootFrame, const std::string& rFootFrame)
+bool WalkingFK::setBaseFrame(const std::string& baseFrame, const std::string& name)
 {
     if(!m_kinDyn.isValid())
     {
@@ -52,25 +52,19 @@ bool WalkingFK::setBaseFrames(const std::string& lFootFrame, const std::string&
     // note: in the following the base frames will be:
     // - left_foot when the left foot is the stance foot;
     // - right_foot when the right foot is the stance foot.
-    m_frameLeftIndex = m_kinDyn.model().getFrameIndex(lFootFrame);
-    if(m_frameLeftIndex == iDynTree::FRAME_INVALID_INDEX)
+    //.-.root when the external base supposed to be used
+    m_frameBaseIndex = m_kinDyn.model().getFrameIndex(baseFrame);
+    if(m_frameBaseIndex == iDynTree::FRAME_INVALID_INDEX)
     {
-        yError() << "[WalkingFK::setBaseFrames] Unable to find the frame named: " << lFootFrame;
+        yError() << "[setBaseFrames] Unable to find the frame named: " << baseFrame;
         return false;
     }
-    iDynTree::LinkIndex linkLeftIndex = m_kinDyn.model().getFrameLink(m_frameLeftIndex);
-    m_baseFrameLeft = m_kinDyn.model().getLinkName(linkLeftIndex);
-    m_frameHlinkLeft = m_kinDyn.getRelativeTransform(m_frameLeftIndex, linkLeftIndex);
+    iDynTree::LinkIndex linkBaseIndex = m_kinDyn.model().getFrameLink(m_frameBaseIndex);
 
-    m_frameRightIndex = m_kinDyn.model().getFrameIndex(rFootFrame);
-    if(m_frameRightIndex == iDynTree::FRAME_INVALID_INDEX)
-    {
-        yError() << "[WalkingFK::setBaseFrames] Unable to find the frame named: " << rFootFrame;
-        return false;
-    }
-    iDynTree::LinkIndex linkRightIndex = m_kinDyn.model().getFrameLink(m_frameRightIndex);
-    m_baseFrameRight = m_kinDyn.model().getLinkName(linkRightIndex);
-    m_frameHlinkRight = m_kinDyn.getRelativeTransform(m_frameRightIndex, linkRightIndex);
+    std::string baseFrameName = m_kinDyn.model().getLinkName(linkBaseIndex);
+    m_baseFrames.insert({name, std::make_pair(baseFrameName,
+                                              m_kinDyn.getRelativeTransform(m_frameBaseIndex,
+                                                                            linkBaseIndex))});
 
     return true;
 }
@@ -107,10 +101,19 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config,
         return false;
     }
 
+    m_frameLeftIndex = m_kinDyn.model().getFrameIndex(lFootFrame);
+    if(m_frameLeftIndex == iDynTree::FRAME_INVALID_INDEX)
+    {
+        yError() << "[WalkingFK::initialize] Unable to find the frame named: " << lFootFrame;
+        return false;
+    }
+
+
     // set base frames
-    if(!setBaseFrames(lFootFrame, rFootFrame))
+    m_frameRightIndex = m_kinDyn.model().getFrameIndex(rFootFrame);
+    if(m_frameRightIndex == iDynTree::FRAME_INVALID_INDEX)
     {
-        yError() << "[WalkingFK::initialize] Unable to set the base frames.";
+        yError() << "[WalkingFK::initialize] Unable to find the frame named: " << rFootFrame;
         return false;
     }
 
@@ -181,6 +184,42 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config,
         return false;
     }
 
+    m_useExternalRobotBase = config.check("use_external_robot_base", yarp::os::Value("False")).asBool();
+
+    if(!m_useExternalRobotBase)
+    {
+        if(!setBaseFrame(lFootFrame, "leftFoot"))
+        {
+            yError() << "[initialize] Unable to set the leftFootFrame.";
+            return false;
+        }
+
+        if(!setBaseFrame(rFootFrame, "rightFoot"))
+        {
+            yError() << "[initialize] Unable to set the rightFootFrame.";
+            return false;
+        }
+
+        // Since the base is attached to the stance foot its velocity is always equal to zero
+        // (stable contact hypothesis)
+        m_baseTwist.zero();
+    }
+    else
+    {
+        if(!setBaseFrame(rootFrame, "root"))
+        {
+            yError() << "[initialize] Unable to set the rightFootFrame.";
+            return false;
+        }
+
+        // in this specific case the base is always the root link
+        if(!m_kinDyn.setFloatingBase(m_baseFrames["root"].first))
+        {
+            yError() << "[initialize] Unable to set the floating base";
+            return false;
+        }
+    }
+
     double comHeight;
     if(!YarpUtilities::getNumberFromSearchable(config, "com_height", comHeight))
     {
@@ -226,71 +265,44 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config,
     return true;
 }
 
-bool WalkingFK::evaluateFirstWorldToBaseTransformation(const iDynTree::Transform& leftFootTransform)
-{
-    m_worldToBaseTransform = leftFootTransform * m_frameHlinkLeft;
-    if(!m_kinDyn.setFloatingBase(m_baseFrameLeft))
+void WalkingFK::evaluateWorldToBaseTransformation(const iDynTree::Transform& rootTransform,
+                                                  const iDynTree::Twist& rootTwist){
+    if(!m_useExternalRobotBase)
     {
-        yError() << "[WalkingFK::evaluateFirstWorldToBaseTransformation] Error while setting the floating "
-                 << "base on link " << m_baseFrameLeft;
-        return false;
+        yWarning() << "[evaluateWorldToBaseTransformation] The base position is not retrieved from external. There is no reason to call this function.";
+                       return;
     }
-    m_prevContactLeft = true;
-    return true;
-}
+    auto& base = m_baseFrames["root"];
+    m_worldToBaseTransform = rootTransform * base.second;
+    m_baseTwist = rootTwist;
 
-bool WalkingFK::evaluateWorldToBaseTransformation(const bool& isLeftFixedFrame)
-{
-    if(isLeftFixedFrame)
-    {
-        // evaluate the new world to base transformation only if the previous fixed frame was
-        // the right foot
-        if(!m_prevContactLeft)
-        {
-            m_worldToBaseTransform =  this->getLeftFootToWorldTransform() * m_frameHlinkLeft;
-            if(!m_kinDyn.setFloatingBase(m_baseFrameLeft))
-            {
-                yError() << "[WalkingFK::evaluateWorldToBaseTransformation] Error while setting the floating "
-                         << "base on link " << m_baseFrameLeft;
-                return false;
-            }
-            m_prevContactLeft = true;
-        }
-    }
-    else
-    {
-        // evaluate the new world to base transformation only if the previous if the previous fixed frame was
-        // the left foot
-        if(m_prevContactLeft)
-        {
-            m_worldToBaseTransform = this->getRightFootToWorldTransform() * m_frameHlinkRight;
-            if(!m_kinDyn.setFloatingBase(m_baseFrameRight))
-            {
-                yError() << "[WalkingFK::evaluateWorldToBaseTransformation] Error while setting the floating "
-                         << "base on link " << m_baseFrameRight;
-                return false;
-            }
-            m_prevContactLeft = false;
-        }
-    }
-    return true;
+    m_comEvaluated = false;
+    m_dcmEvaluated = false;
+    return;
 }
 
 bool WalkingFK::evaluateWorldToBaseTransformation(const iDynTree::Transform& leftFootTransform,
                                                   const iDynTree::Transform& rightFootTransform,
                                                   const bool& isLeftFixedFrame)
 {
+    if(m_useExternalRobotBase)
+       {
+           yWarning() << "[evaluateWorldToBaseTransformation] The base position is retrieved from external. There is no reason on using odometry.";
+           return true;
+       }
+
     if(isLeftFixedFrame)
     {
         // evaluate the new world to base transformation only if the previous fixed frame was
         // the right foot
         if(!m_prevContactLeft || m_firstStep)
         {
-            m_worldToBaseTransform = leftFootTransform * m_frameHlinkLeft;
-            if(!m_kinDyn.setFloatingBase(m_baseFrameLeft))
+            auto& base = m_baseFrames["leftFoot"];
+            m_worldToBaseTransform = leftFootTransform * base.second;
+            if(!m_kinDyn.setFloatingBase(base.first))
             {
-                yError() << "[WalkingFK::evaluateWorldToBaseTransformation] Error while setting the floating "
-                         << "base on link " << m_baseFrameLeft;
+                yError() << "[evaluateWorldToBaseTransformation] Error while setting the floating "
+                         << "base on link " << base.first;
                 return false;
             }
             m_prevContactLeft = true;
@@ -302,18 +314,22 @@ bool WalkingFK::evaluateWorldToBaseTransformation(const iDynTree::Transform& lef
         // the left foot
         if(m_prevContactLeft || m_firstStep)
         {
-            m_worldToBaseTransform = rightFootTransform * m_frameHlinkRight;
-            if(!m_kinDyn.setFloatingBase(m_baseFrameRight))
+            auto base = m_baseFrames["rightFoot"];
+            m_worldToBaseTransform = rightFootTransform * base.second;
+            if(!m_kinDyn.setFloatingBase(base.first))
             {
                 yError() << "[WalkingFK::evaluateWorldToBaseTransformation] Error while setting the floating "
-                         << "base on link " << m_baseFrameRight;
+                         << "base on link " << base.first;
                 return false;
             }
             m_prevContactLeft = false;
         }
     }
 
     m_firstStep = false;
+
+    m_comEvaluated = false;
+    m_dcmEvaluated = false;
     return true;
 }
 
@@ -325,7 +341,7 @@ bool WalkingFK::setInternalRobotState(const iDynTree::VectorDynSize& positionFee
     gravity(2) = -9.81;
 
     if(!m_kinDyn.setRobotState(m_worldToBaseTransform, positionFeedbackInRadians,
-                               iDynTree::Twist::Zero(), velocityFeedbackInRadians,
+                               m_baseTwist, velocityFeedbackInRadians,
                                gravity))
     {
         yError() << "[WalkingFK::setInternalRobotState] Error while updating the state.";
@@ -414,11 +430,18 @@ const iDynTree::Vector3& WalkingFK::getCoMVelocity()
 
 bool WalkingFK::setBaseOnTheFly()
 {
-    m_worldToBaseTransform = m_frameHlinkLeft;
-    if(!m_kinDyn.setFloatingBase(m_baseFrameLeft))
+    if(m_useExternalRobotBase)
+    {
+            yError() << "[setBaseOnTheFly] If the base comes from the external you cannot use the onthefly. (This feature will be implemented in a second moment)";
+            return false;
+    }
+
+    auto base = m_baseFrames["leftFoot"];
+    m_worldToBaseTransform = base.second;
+    if(!m_kinDyn.setFloatingBase(base.first))
     {
-        yError() << "[WalkingFK::setBaseOnTheFly] Error while setting the floating base on link "
-                 << m_baseFrameLeft;
+        yError() << "[setBaseOnTheFly] Error while setting the floating base on link "
+                 << base.first;
         return false;
     }