adding the step adaptation library

changing the name of QPSolver class

remove small bug

combining the two class of step adaptation into one class

small  modifications of step adaptation library

modifications of step adaptation library

modifications of step adaptation library

small modifications of step adaptation library

removing some useless methods

removing some useless methods

small modifications

adding an enum for recognizing of swing foot in solve method

Using  step adaptation library inside the walking controller

fixing some bugs

 fixing  the bug related to the  size of hessian matrix of step adaptation optimization

push recovery by two arms in saggital and lateral direction

cleaning the code

some primary modifications

separation of the step adaptation config files

some primary modifications

adding the documentation link to the simple estimator

modification of the configuration file

make an object of TrajectoryGeneration for step adjustment

some modifications in trajectoryGenerator and module

some small modifications in Module.cpp

moving the arms push detection inside the stepAdjustment module

modifying some comments

dubeg of moving the step adjustment stuff from walkingModule to step adaptation library

Keeping  the dcm replanning part inside the walkingModule

remove some magic numbers

modify the input of runstepAdaptation as a reference

small modifications

step adaptation documentation and configuration files modification

Bring the arm joints name for  push detection inside the configuration file

some modification on walkingModule and exceptions

some modifications in walkingModule

defining  the output and input of step adaptation structure as a private member

removing some small bugs!

 add missing trajectoryPlanner inside cmake lists

remove the bug related to zmp offset

copy  the retargeting planner parameters to reduce the  shaking

tuning pull request on the robot

step adaptation left arm lateral direction

tuning the planner parametr for yaw movement

tuning unicycle planner parameter for rotational movement

tuning step adaptaion on the robot

set stiffness and damping joint again inside the code

tune the step adaptation parameters

 tuning step adaptation

 tuning step adjustment

tune the step adaptation parameters

activating push recovery every two steps

lateral direction is working

remove the yaw error

remove some comments

tuning step adaptaion parameters

remove the bug related to the yaw orientation

small modification

removing the bug related to the push detection during rotational walking

parameter tuning

some small modifications

solve the problem of joystick command during step adaptation

solve the problem of joystick

increasing speed of walking

some modifications

try to solve joystick problem

small modification

try to solve the last step impact

saturation of the com error

tunning parameters

saturation of the com error

video recorded version

pull request modifications

Fix some rebase issues.

Author: Milad

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 <milad.shafiee@iit.it>
+
+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 <milad.shafiee@iit.it>
+ * @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