Improve com admittance

CMakeLists.txt

@@ -144,6 +144,7 @@ IF(BUILD_PYTHON_INTERFACE)
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/run_test_utils.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py

include/sot/talos_balance/com-admittance-controller.hh

@@ -73,6 +73,9 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
 
         DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
+
+        DECLARE_SIGNAL_INNER(stateRef, dynamicgraph::Vector);
+
         DECLARE_SIGNAL_OUT(comRef, dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(dcomRef, dynamicgraph::Vector);
 
@@ -87,9 +90,7 @@ namespace dynamicgraph {
 
       protected:
         bool m_initSucceeded;    /// true if the entity has been successfully initialized
-        dynamicgraph::Vector m_Kp;
-        dynamicgraph::Vector m_com; // internal state
-        dynamicgraph::Vector m_dcom; // internal state
+        dynamicgraph::Vector m_state; // internal state
         double m_dt;
 
       }; // class ComAdmittanceController

include/sot/talos_balance/dummy-dcm-estimator.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-talos-balance is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_dummy_dcm_estimator_H__
+#define __sot_talos_balance_dummy_dcm_estimator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (dummy_dcm_estimator_EXPORTS)
+#    define DUMMYDCMESTIMATOR_EXPORT __declspec(dllexport)
+#  else
+#    define DUMMYDCMESTIMATOR_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define DUMMYDCMESTIMATOR_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include "utils/signal-helper.hh"
+#include "utils/logger.hh"
+#include <map>
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace talos_balance {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class DUMMYDCMESTIMATOR_EXPORT DummyDcmEstimator
+	                         : public ::dynamicgraph::Entity
+      {
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        DummyDcmEstimator( const std::string & name );
+
+        void init();
+
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(omega, double);
+        DECLARE_SIGNAL_IN(mass, double);
+        DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(momenta, dynamicgraph::Vector);     
+
+        DECLARE_SIGNAL_OUT(dcm, dynamicgraph::Vector);
+
+        /* --- COMMANDS --- */
+        /* --- ENTITY INHERITANCE --- */
+        virtual void display( std::ostream& os ) const;
+
+        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
+        {
+          getLogger().sendMsg("[DummyDcmEstimator-"+name+"] "+msg, t, file, line);
+        }
+
+      protected:
+        bool m_initSucceeded;    /// true if the entity has been successfully initialized
+
+      }; // class DummyDcmEstimator
+
+    }    // namespace talos_balance
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_dummy_dcm_estimator_H__

python/sot_talos_balance/create_entities_utils.py

@@ -2,6 +2,8 @@ from dynamic_graph.sot.core.operator import Mix_of_vector
 from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
 from sot_talos_balance.joint_position_controller import JointPositionController
 from sot_talos_balance.joint_admittance_controller import JointAdmittanceController
+from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
+from sot_talos_balance.com_admittance_controller import ComAdmittanceController
 
 from dynamic_graph import plug
 
@@ -65,3 +67,32 @@ def create_admittance_controller(Kp,dt,robot):
     controller.init(dt, N_JOINTS+6)
     controller.setPosition(robot.device.state.value)
     return controller
+
+def create_dummy_dcm_estimator(robot):
+    from math import sqrt
+    estimator = DummyDcmEstimator("dummy")
+    mass = robot.dynamic.data.mass[0]
+    robot.dynamic.com.recompute(0)
+    h = robot.dynamic.com.value[2]
+    g = 9.81
+    omega = sqrt(g/h)
+
+    estimator.mass.value = mass
+    estimator.omega.value = omega
+    plug(robot.dynamic.com,estimator.com)
+    plug(robot.dynamic.momenta,estimator.momenta)
+    estimator.init()
+    return estimator
+
+def create_com_admittance_controller(Kp,dt,robot):
+    controller = ComAdmittanceController("comAdmCtrl")
+    controller.Kp.value = Kp
+    plug(robot.dynamic.zmp,controller.zmp)
+    robot.dynamic.zmp.recompute(0)
+    controller.zmpDes.value = robot.dynamic.zmp.value
+    controller.ddcomDes.value = [0.0,0.0,0.0]
+
+    controller.init(dt)
+    robot.dynamic.com.recompute(0)
+    controller.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
+    return controller

python/sot_talos_balance/test/test_comAdmittance.py

+from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+from numpy import eye
+from time import sleep
+import sys
+import os
+
+def main(robot):
+    dt = robot.timeStep;
+
+    # --- COM
+    robot.taskCom = MetaTaskKineCom(robot.dynamic)
+    robot.dynamic.com.recompute(0)
+    robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+    robot.taskCom.task.controlGain.value = 10
+
+    robot.estimator = create_dummy_dcm_estimator(robot)
+
+    # --- Admittance controller
+    Kp = [0.0,0.0,0.0]
+    robot.com_admittance_control = create_com_admittance_controller(Kp,dt,robot)
+
+    # --- CONTACTS
+    #define contactLF and contactRF
+    robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+    robot.contactLF.feature.frame('desired')
+    robot.contactLF.gain.setConstant(100)
+    robot.contactLF.keep()
+    locals()['contactLF'] = robot.contactLF
+
+    robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+    robot.contactRF.feature.frame('desired')
+    robot.contactRF.gain.setConstant(100)
+    robot.contactRF.keep()
+    locals()['contactRF'] = robot.contactRF
+
+    robot.sot = SOT('sot')
+    robot.sot.setSize(robot.dynamic.getDimension())
+    plug(robot.sot.control,robot.device.control)
+
+    robot.sot.push(robot.contactRF.task.name)
+    robot.sot.push(robot.contactLF.task.name)
+    robot.sot.push(robot.taskCom.task.name)
+    robot.device.control.recompute(0)
+
+    plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
+    sleep(1.0)
+    os.system("rosservice call \start_dynamic_graph")
+    sleep(2.0)
+
+    plug(robot.estimator.dcm,robot.com_admittance_control.zmpDes)
+
+    print("Activating controller...")
+    robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
+    robot.com_admittance_control.Kp.value = [10.0,10.0,0.0]
+
+    sleep(5.0)

src/CMakeLists.txt

@@ -36,6 +36,7 @@ ENDIF(UNIX)
 #This project will create many plugins as shared libraries, listed here
 SET(plugins
     example
+    dummy-dcm-estimator
     com-admittance-controller
     joint-admittance-controller
     joint-position-controller

src/com-admittance-controller.cpp

@@ -35,11 +35,14 @@ namespace dynamicgraph
 
 //Size to be aligned                                         "-------------------------------------------------------"
 #define PROFILE_COMADMITTANCECONTROLLER_DDCOMREF_COMPUTATION "ComAdmittanceController: ddcomRef computation          "
+#define PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION "ComAdmittanceController: stateRef computation          "
 #define PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION  "ComAdmittanceController: dcomRef computation           "
 #define PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION   "ComAdmittanceController: comRef computation            "
 
 #define INPUT_SIGNALS     m_KpSIN << m_zmpSIN << m_zmpDesSIN << m_ddcomDesSIN
 
+#define INNER_SIGNALS     m_stateRefSINNER
+
 #define OUTPUT_SIGNALS m_comRefSOUT << m_dcomRefSOUT << m_ddcomRefSOUT
 
       /// Define EntityClassName here rather than in the header file
@@ -60,12 +63,13 @@ namespace dynamicgraph
                       , CONSTRUCT_SIGNAL_IN(zmpDes, dynamicgraph::Vector)
                       , CONSTRUCT_SIGNAL_IN(ddcomDes, dynamicgraph::Vector)
                       , CONSTRUCT_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector, INPUT_SIGNALS)
-                      , CONSTRUCT_SIGNAL_OUT(comRef, dynamicgraph::Vector, m_ddcomRefSOUT)
-                      , CONSTRUCT_SIGNAL_OUT(dcomRef, dynamicgraph::Vector, m_comRefSOUT << m_ddcomRefSOUT)
+                      , CONSTRUCT_SIGNAL_INNER(stateRef, dynamicgraph::Vector, m_ddcomRefSOUT)
+                      , CONSTRUCT_SIGNAL_OUT(comRef, dynamicgraph::Vector, m_stateRefSINNER)
+                      , CONSTRUCT_SIGNAL_OUT(dcomRef, dynamicgraph::Vector, m_stateRefSINNER)
                       // dcomRef is set to depend from comRefSOUT to ensure position is updated before velocity
                       , m_initSucceeded(false)
       {
-        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+        Entity::signalRegistration( INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS );
 
         /* Commands. */
         addCommand("init", makeCommandVoid1(*this, &ComAdmittanceController::init, docCommandVoid1("Initialize the entity.","time step")));
@@ -86,19 +90,18 @@ namespace dynamicgraph
           return SEND_MSG("Init failed: signal zmpDes is not plugged", MSG_TYPE_ERROR);
 
         m_dt = dt;
-        m_com.setZero(3);
-        m_dcom.setZero(3);
+        m_state.setZero(6);
         m_initSucceeded = true;
       }
 
       void ComAdmittanceController::setPosition(const dynamicgraph::Vector & com)
       {
-        m_com = com;
+        m_state.head<3>() = com;
       }
 
       void ComAdmittanceController::setVelocity(const dynamicgraph::Vector & dcom)
       {
-        m_dcom = dcom;
+        m_state.tail<3>() = dcom;
       }
 
       void ComAdmittanceController::setState(const dynamicgraph::Vector & com, const dynamicgraph::Vector & dcom)
@@ -140,26 +143,28 @@ namespace dynamicgraph
         return s;
       }
 
-      DEFINE_SIGNAL_OUT_FUNCTION(dcomRef, dynamicgraph::Vector)
+      DEFINE_SIGNAL_INNER_FUNCTION(stateRef, dynamicgraph::Vector)
       {
         if(!m_initSucceeded)
         {
-          SEND_WARNING_STREAM_MSG("Cannot compute signal dcomRef before initialization!");
+          SEND_WARNING_STREAM_MSG("Cannot compute signal stateRef before initialization!");
           return s;
         }
 
-        getProfiler().start(PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION);
+        getProfiler().start(PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION);
 
-        const Vector & comRef = m_comRefSOUT(iter); // ficticious call to force recomputation
         const Vector & ddcomRef = m_ddcomRefSOUT(iter);
 
         assert(ddcomRef.size()==3 && "Unexpected size of signal ddcomRef");
 
-        m_dcom += ddcomRef*m_dt;
+        const Vector & dcomRef = m_state.tail<3>();
 
-        s = m_dcom;
+        m_state.head<3>() +=  dcomRef*m_dt + 0.5*ddcomRef*m_dt*m_dt;
+        m_state.tail<3>() += ddcomRef*m_dt;
 
-        getProfiler().stop(PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION);
+        s = m_state;
+
+        getProfiler().stop(PROFILE_COMADMITTANCECONTROLLER_STATEREF_COMPUTATION);
 
         return s;
       }
@@ -174,19 +179,37 @@ namespace dynamicgraph
 
         getProfiler().start(PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION);
 
-        const Vector & ddcomRef = m_ddcomRefSOUT(iter);
+        const Vector & stateRef = m_stateRefSINNER(iter);
 
-        assert(ddcomRef.size()==3 && "Unexpected size of signal ddcomRef");
-
-        m_com += m_dcom*m_dt + 0.5*ddcomRef*m_dt*m_dt;
+        assert(stateRef.size()==3 && "Unexpected size of signal stateRef");
 
-        s = m_com;
+        s = stateRef.head<3>();
 
         getProfiler().stop(PROFILE_COMADMITTANCECONTROLLER_COMREF_COMPUTATION);
 
         return s;
       }
 
+      DEFINE_SIGNAL_OUT_FUNCTION(dcomRef, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal dcomRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION);
+
+        const Vector & stateRef = m_stateRefSINNER(iter);
+
+        assert(stateRef.size()==3 && "Unexpected size of signal stateRef");
+
+        s = stateRef.tail<3>();
+
+        getProfiler().stop(PROFILE_COMADMITTANCECONTROLLER_DCOMREF_COMPUTATION);
+
+        return s;
+      }
 
       /* --- COMMANDS ---------------------------------------------------------- */
 

src/dummy-dcm-estimator.cpp

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-talos-balance is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "sot/talos_balance/dummy-dcm-estimator.hh"
+
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/command-bind.h>
+
+#include "sot/talos_balance/utils/commands-helper.hh"
+#include "sot/talos_balance/utils/stop-watch.hh"
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace talos_balance
+    {
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+      using namespace dg::command;
+
+//Size to be aligned                              "-------------------------------------------------------"
+#define PROFILE_DUMMYDCMESTIMATOR_DCM_COMPUTATION "DummyDcmEstimator: dcm computation                     "
+
+#define INPUT_SIGNALS     m_omegaSIN << m_massSIN << m_comSIN << m_momentaSIN
+
+#define OUTPUT_SIGNALS m_dcmSOUT
+
+      /// Define EntityClassName here rather than in the header file
+      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+      typedef DummyDcmEstimator EntityClassName;
+
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DummyDcmEstimator,
+                                         "DummyDcmEstimator");
+
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      DummyDcmEstimator::DummyDcmEstimator(const std::string& name)
+                      : Entity(name)
+                      , CONSTRUCT_SIGNAL_IN(omega, double)
+                      , CONSTRUCT_SIGNAL_IN(mass, double)
+                      , CONSTRUCT_SIGNAL_IN(com, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(momenta, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_OUT(dcm, dynamicgraph::Vector, INPUT_SIGNALS)
+                      , m_initSucceeded(false)
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        /* Commands. */
+        addCommand("init", makeCommandVoid0(*this, &DummyDcmEstimator::init, docCommandVoid0("Initialize the entity.")));
+      }
+
+      void DummyDcmEstimator::init()
+      {
+        if(!m_omegaSIN.isPlugged())
+          return SEND_MSG("Init failed: signal omega is not plugged", MSG_TYPE_ERROR);
+        if(!m_massSIN.isPlugged())
+          return SEND_MSG("Init failed: signal mass is not plugged", MSG_TYPE_ERROR);
+        if(!m_comSIN.isPlugged())
+          return SEND_MSG("Init failed: signal com is not plugged", MSG_TYPE_ERROR);
+        if(!m_momentaSIN.isPlugged())
+          return SEND_MSG("Init failed: signal momenta is not plugged", MSG_TYPE_ERROR);
+
+        m_initSucceeded = true;
+      }
+
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(dcm, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal com_dcom before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_DUMMYDCMESTIMATOR_DCM_COMPUTATION);
+
+        const double & omega = m_omegaSIN(iter);
+        const double & mass = m_massSIN(iter);
+        const Vector & com = m_comSIN(iter);
+        const Vector & momenta = m_momentaSIN(iter);
+
+        assert(com.size()==3 && "Unexpected size of signal com");
+        assert(momenta.size()==6 && "Unexpected size of signal momenta");
+
+        const Vector dcom = momenta.head<3>()/mass;
+
+        const Vector dcm = com + dcom/omega;
+
+        s = dcm;
+
+        getProfiler().stop(PROFILE_DUMMYDCMESTIMATOR_DCM_COMPUTATION);
+
+        return s;
+      }
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      /* ------------------------------------------------------------------- */
+      /* --- ENTITY -------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      void DummyDcmEstimator::display(std::ostream& os) const
+      {
+        os << "DummyDcmEstimator " << getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+    } // namespace talos_balance
+  } // namespace sot
+} // namespace dynamicgraph
+