adding the step adaptation feature

cmake/WalkingControllersFindDependencies.cmake

@@ -131,7 +131,7 @@ checkandset_dependency(ICUBcontrib)
 find_package(iDynTree QUIET)
 checkandset_dependency(iDynTree)
 
-find_package(UnicyclePlanner 0.1.102 QUIET)
+find_package(UnicyclePlanner 0.2.102 QUIET)
 checkandset_dependency(UnicyclePlanner)
 
 find_package(osqp QUIET)
@@ -162,8 +162,11 @@ walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_RetargetingHelp
                                     "WALKING_CONTROLLERS_HAS_iDynTree;WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_HAS_ICUB" OFF)
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_LoggerClient "Compile LoggerClient library?" ON WALKING_CONTROLLERS_COMPILE_YarpUtilities OFF)
 
+walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_StepAdaptationController "Compile StepAdaptationController library?" ON
+                                    "WALKING_CONTROLLERS_HAS_iDynTree;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_HAS_osqp;WALKING_CONTROLLERS_HAS_OsqpEigen" OFF)
+
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_WalkingModule "Compile WalkingModule app?" ON
-  "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_RobotInterface;WALKING_CONTROLLERS_COMPILE_KinDynWrapper;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_COMPILE_SimplifiedModelControllers;WALKING_CONTROLLERS_COMPILE_WholeBodyControllers;WALKING_CONTROLLERS_COMPILE_RetargetingHelper;WALKING_CONTROLLERS_COMPILE_LoggerClient;WALKING_CONTROLLERS_HAS_ICUBcontrib" OFF)
+  "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities;WALKING_CONTROLLERS_COMPILE_RobotInterface;WALKING_CONTROLLERS_COMPILE_KinDynWrapper;WALKING_CONTROLLERS_COMPILE_TrajectoryPlanner;WALKING_CONTROLLERS_COMPILE_SimplifiedModelControllers;WALKING_CONTROLLERS_COMPILE_WholeBodyControllers;WALKING_CONTROLLERS_COMPILE_RetargetingHelper;WALKING_CONTROLLERS_COMPILE_LoggerClient;WALKING_CONTROLLERS_HAS_ICUBcontrib;WALKING_CONTROLLERS_COMPILE_StepAdaptationController" OFF)
 
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_JoypadModule "Compile JoypadModule app?" ON "WALKING_CONTROLLERS_COMPILE_YarpUtilities;WALKING_CONTROLLERS_HAS_ICUBcontrib" OFF)
 

src/CMakeLists.txt

@@ -16,3 +16,4 @@ add_subdirectory(LoggerClient)
 add_subdirectory(WalkingModule)
 add_subdirectory(JoypadModule)
 add_subdirectory(LoggerModule)
+add_subdirectory(StepAdaptationController)

src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h

@@ -21,6 +21,8 @@
 #include <yarp/dev/IPositionDirect.h>
 #include <yarp/dev/IVelocityControl.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IImpedanceControl.h>
+
 #include <yarp/sig/Vector.h>
 #include <yarp/os/Timer.h>
 
@@ -53,7 +55,7 @@ namespace WalkingControllers
         yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */
         yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */
         yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */
-
+        yarp::dev::IImpedanceControl *m_impedanceControlInterface{nullptr};
         std::unique_ptr<WalkingPIDHandler> m_PIDHandler; /**< Pointer to the PID handler object. */
 
         yarp::os::Bottle m_remoteControlBoards; /**< Contain all the name of the controlled joints. */
@@ -71,6 +73,9 @@ namespace WalkingControllers
         iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */
         iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */
         iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */
+         std::vector<yarp::dev::InteractionModeEnum> m_isJointModeStiffVector;/**< Joint is in the stiff or compliance mode */
+         std::vector<yarp::dev::InteractionModeEnum> m_JointModeStiffVectorDefult;/**< All the joints are in the stiff  mode */
+         std::vector<yarp::dev::InteractionModeEnum> m_currentModeofJoints;/**< Joint is in the stiff or compliance mode based on the walking architecture phases */
         // yarp::sig::Vector m_positionFeedbackDegFiltered;
         yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_positionFilter; /**< Joint position low pass filter .*/
@@ -89,6 +94,9 @@ namespace WalkingControllers
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_rightWrenchFilter; /**< Right wrench low pass filter.*/
         bool m_useWrenchFilter; /**< True if the wrench filter is used. */
 
+        iDynTree::VectorDynSize  m_stiffnessGainVector; /**< the vector of stifness gains for the joint */
+        iDynTree::VectorDynSize  m_dampingGainVector; /**< the vector of damping gains for the joint */
+        std::vector<bool>  m_jointModes; /**< True if the joint is in the stiff mode */
         double m_startingPositionControlTime;
         bool m_positionMoveSkipped;
 
@@ -254,6 +262,11 @@ namespace WalkingControllers
          */
         bool isExternalRobotBaseUsed();
 
+        /**
+         * Set the impedance control gains of the joints(stiffness/damping).
+         * @return true in case of success and false otherwise.
+         */
+        bool setImpedanceControlGain();
     };
 };
 #endif

src/RobotInterface/src/Helper.cpp

@@ -227,13 +227,27 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
     m_jointInteractionMode.resize(m_actuatedDOFs);
     m_currentJointInteractionMode.resize(m_actuatedDOFs);
     std::vector<bool> isJointInStiffMode(m_actuatedDOFs);
+    m_stiffnessGainVector.resize(m_actuatedDOFs);
+    m_dampingGainVector.resize(m_actuatedDOFs);
     if(!YarpUtilities::getVectorOfBooleanFromSearchable(config, "joint_is_stiff_mode",
                                                         isJointInStiffMode))
     {
         yError() << "[RobotInterface::configureRobot] Unable to find joint_is_stiff_mode into config file.";
         return false;
     }
 
+    if(!YarpUtilities::getVectorFromSearchable(config,"joint_stiffness_gain",m_stiffnessGainVector))
+    {
+        yError() << "[RobotInterface::configureRobot] Unable to find joint_stiffness_gain into config file.";
+        return false;
+    }
+
+    if(!YarpUtilities::getVectorFromSearchable(config,"joint_damping_gain",m_dampingGainVector))
+    {
+        yError() << "[RobotInterface::configureRobot] Unable to find joint_damping_gain into config file.";
+        return false;
+    }
+
     for (unsigned int i = 0; i < m_actuatedDOFs; i++)
     {
         if(isJointInStiffMode[i])
@@ -306,6 +320,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         return false;
     }
 
+    if(!m_robotDevice.view(m_impedanceControlInterface) || !m_impedanceControlInterface)
+    {
+             yError() << "[configureRobot] Cannot obtain ImpedanceControl interface";
+            return false;
+    }
+
     // resize the buffers
     m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0);
     m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0);
@@ -637,6 +657,12 @@ bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desire
         return false;
     }
 
+    if(m_impedanceControlInterface == nullptr)
+    {
+        yError() << "[RobotInterface::setPositionReferences] IImpedanceControlInterface interface is not ready.";
+        return false;
+    }
+
     m_desiredJointPositionRad = desiredJointPositionsRad;
 
     std::pair<int, double> worstError(0, 0.0);
@@ -921,3 +947,17 @@ bool RobotInterface::loadCustomInteractionMode()
 {
     return setInteractionMode(m_jointInteractionMode);
 }
+
+bool RobotInterface::setImpedanceControlGain()
+{
+    for (unsigned i = 0; i < m_actuatedDOFs; i++)
+    {
+        if(!m_impedanceControlInterface->setImpedance(i,m_stiffnessGainVector(i),m_dampingGainVector(i)))
+        {
+            yError() << "[RobotInterface::setImpedanceControlGain] Error while setting the impedance control gains";
+            return false;
+        }
+    }
+
+    return true;
+}

src/StepAdaptationController/CMakeLists.txt

@@ -0,0 +1,59 @@
+# Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Milad Shafiee <[email protected]>
+
+if(WALKING_CONTROLLERS_COMPILE_StepAdaptationController)
+
+# set target name
+  set(LIBRARY_TARGET_NAME StepAdaptationController)
+
+  # set cpp files
+  set(${LIBRARY_TARGET_NAME}_SRC
+    src/StepAdaptationController.cpp
+    src/DCMSimpleEstimator.cpp
+    )
+
+  # set hpp files
+  set(${LIBRARY_TARGET_NAME}_HDR
+    include/WalkingControllers/StepAdaptationController/StepAdaptationController.hpp
+    include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp
+    )
+
+  # add an executable to the project using the specified source files.
+  add_library(${LIBRARY_TARGET_NAME} SHARED ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR})
+  set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES OUTPUT_NAME "${PROJECT_NAME}${LIBRARY_TARGET_NAME}")
+
+  target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC
+    WalkingControllers::YarpUtilities
+    WalkingControllers::iDynTreeUtilities
+    WalkingControllers::TrajectoryPlanner
+    osqp::osqp
+    OsqpEigen::OsqpEigen
+    ${qpOASES_LIBRARIES})
+
+
+  add_library(WalkingControllers::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME})
+
+  set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION ${WalkingControllers_VERSION}
+    PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}")
+
+  #Specify include directories for both compilation and installation process.
+  #The $<INSTALL_PREFIX> generator expression is useful to ensure to create
+  #relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages
+  target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+  # Specify installation targets, typology and destination folders.
+  install(TARGETS    ${LIBRARY_TARGET_NAME}
+    EXPORT     ${PROJECT_NAME}
+    COMPONENT  runtime
+    LIBRARY       DESTINATION "${CMAKE_INSTALL_LIBDIR}"                                          COMPONENT shlib
+    ARCHIVE       DESTINATION "${CMAKE_INSTALL_LIBDIR}"                                          COMPONENT lib
+    RUNTIME       DESTINATION "${CMAKE_INSTALL_BINDIR}"                                          COMPONENT bin
+    PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WalkingControllers/StepAdaptationController" COMPONENT dev)
+
+  set_property(GLOBAL APPEND PROPERTY WalkingControllers_TARGETS ${LIBRARY_TARGET_NAME})
+
+  message(STATUS "Created target ${LIBRARY_TARGET_NAME} for export ${PROJECT_NAME}.")
+
+endif()

src/StepAdaptationController/include/WalkingControllers/StepAdaptationController/DCMSimpleEstimator.hpp

@@ -0,0 +1,63 @@
+/**
+ * @file DCMSimpleEstimator.hpp
+ * @authors Milad Shafiee <[email protected]>
+ * @copyright 2020 iCub Facility - Istituto Italiano di Tecnologia
+ *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2020
+ */
+
+#ifndef WALKING_CONTROLLERS_DCM_SIMPLE_ESTIMATOR_H
+#define WALKING_CONTROLLERS_DCM_SIMPLE_ESTIMATOR_H
+
+// YARP
+#include <yarp/os/Searchable.h>
+#include <yarp/sig/Vector.h>
+
+//iDynTree
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Core/LinearMotionVector3.h>
+
+
+namespace WalkingControllers
+{
+/**
+ * The theory related to this simple estimator has been discussed in the following github issue:
+ *https://github.com/robotology/walking-controllers/issues/61
+*/
+    class DCMSimpleEstimator
+    {
+        double m_omega; /**< Inverted time constant of the 3D-LIPM. */
+        double m_mass; /**< Mass of the robot. */
+        iDynTree::Vector2 m_dcmEstimatedPosition; /**< Position of the estimated DCM. */
+        iDynTree::Vector2 m_dcmPosition; /**< Position of the DCM. */
+        iDynTree::Vector2 m_dcmVelocity; /**< Velocity of the dcm. */
+
+    public:
+
+        /**
+         * Config the DCMEstimator.
+         * @param config config of the simple DCM estimator;
+         * @return true on success, false otherwise.
+         */
+        bool configure(const yarp::os::Searchable& config);
+
+        /**
+         * Get the position of the DCM.
+         * @return position of the DCM.
+         */
+        const iDynTree::Vector2& getDCMPosition() const;
+
+        /**
+         * run the pendulum estimator
+         * @param footOrientation the orientation of stance foot.
+         * @param zmp the vector of zmp position with respect to the inertial frame.
+         * @param com com the com position obtained as if the foot is not rotated.
+         * @param CoMVelocity3d the vector of com velocity that is simple time derivative of the com position.
+         * @return true/false in case of success/failure
+         */
+        bool update(const iDynTree::Rotation& footOrientation,const iDynTree::Vector3& zmp,const iDynTree::Vector3& com,const iDynTree::LinVelocity& CoMVelocity3d);
+    };
+};
+
+#endif