DCM controller [second version]

CMakeLists.txt

@@ -144,6 +144,8 @@ 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_dcmComControl.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py

include/sot/talos_balance/dcm-com-controller.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_dcm_com_controller_H__
+#define __sot_talos_balance_dcm_com_controller_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (dcm_com_controller_EXPORTS)
+#    define DCMCOMCONTROLLER_EXPORT __declspec(dllexport)
+#  else
+#    define DCMCOMCONTROLLER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define DCMCOMCONTROLLER_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 DCMCOMCONTROLLER_EXPORT DcmComController
+	                         : public ::dynamicgraph::Entity
+      {
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        DcmComController( const std::string & name );
+
+        void init(const double & dt);
+
+        void resetDcmIntegralError();
+
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(decayFactor, double);
+        DECLARE_SIGNAL_IN(omega, double);
+        DECLARE_SIGNAL_IN(mass, double);
+        DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(comDes, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
+
+        DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(wrenchRef, 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("[DcmComController-"+name+"] "+msg, t, file, line);
+        }
+
+      protected:
+        bool m_initSucceeded;    /// true if the entity has been successfully initialized
+        dynamicgraph::Vector m_dcmIntegralError; // internal state
+        double m_dt;
+
+      }; // class DcmComController
+
+    }    // namespace talos_balance
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_dcm_com_controller_H__

python/sot_talos_balance/create_entities_utils.py

@@ -5,6 +5,7 @@ from sot_talos_balance.joint_admittance_controller import JointAdmittanceControl
 from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
 from sot_talos_balance.com_admittance_controller import ComAdmittanceController
 from sot_talos_balance.dcm_controller import DcmController
+from sot_talos_balance.dcm_com_controller import DcmComController
 
 from time import sleep
 from dynamic_graph import plug
@@ -124,6 +125,32 @@ def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
     controller.init(dt)
     return controller
 
+def create_dcm_com_controller(Kp,Ki,dt,robot,dcmSignal):
+    from math import sqrt
+    controller = DcmComController("dcmComCtrl")
+    mass = robot.dynamic.data.mass[0]
+    robot.dynamic.com.recompute(0)
+    h = robot.dynamic.com.value[2]
+    g = 9.81
+    omega = sqrt(g/h)
+
+    controller.Kp.value = Kp
+    controller.Ki.value = Ki
+    controller.decayFactor.value = 0.2
+    controller.mass.value = mass
+    controller.omega.value = omega
+
+    controller.ddcomDes.value = [0.0,0.0,0.0]
+
+    plug(dcmSignal,controller.dcm)
+
+    robot.dynamic.com.recompute(0)
+    controller.comDes.value = robot.dynamic.com.value
+    controller.dcmDes.value = (robot.dynamic.com.value[0], robot.dynamic.com.value[1], 0.0)
+
+    controller.init(dt)
+    return controller
+
 def addTrace(tracer, entity, signalName):
     signal = '{0}.{1}'.format(entity.name, signalName)
     filename = '{0}-{1}'.format(entity.name, signalName)

python/sot_talos_balance/test/test_dcmComControl.py

+from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_com_controller
+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
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+import matplotlib.pyplot as plt
+import numpy as np
+
+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 = 600
+
+    # --- Dummy estimator
+    robot.estimator = create_dummy_dcm_estimator(robot)
+
+    # --- DCM controller
+    Kp_dcm = [5.0,5.0,0.0]
+    Ki_dcm = [0.0,0.0,0.0]
+    robot.dcm_control = create_dcm_com_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
+
+    # --- Admittance controller
+    Kp_adm = [0.0,0.0,0.0] # must stay zero
+    robot.com_admittance_control = create_com_admittance_controller(Kp_adm,dt,robot)
+
+    # -- Gains = 0, ddcomDes = ddcomRef --> admittance controller is a double integrator
+    plug(robot.dcm_control.ddcomRef,robot.com_admittance_control.ddcomDes)
+
+    # --- 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(300)
+    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(300)
+    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)
+
+    # --- TRACER
+    robot.tracer = TracerRealTime("zmp_tracer")
+    robot.tracer.setBufferSize(80*(2**20))
+    robot.tracer.open('/tmp','dg_','.dat')
+    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+
+    addTrace(robot.tracer, robot.dynamic, 'zmp')
+    addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
+    addTrace(robot.tracer, robot.estimator, 'dcm')
+    addTrace(robot.tracer, robot.dynamic, 'com')
+    addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
+
+    # SIMULATION
+
+    plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
+    plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)
+    robot.dcm_control.Ki.value = [1.0,1.0,0.0]
+    sleep(1.0)
+
+    robot.tracer.start()
+
+    os.system("rosservice call \start_dynamic_graph")
+
+    sleep(5.0)
+
+    dump_tracer(robot.tracer)
+
+	  # --- DISPLAY
+    dcm_data = np.loadtxt('/tmp/dg_'+robot.estimator.name+'-dcm.dat')
+    zmp_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-zmp.dat')
+    zmpDes_data = np.loadtxt('/tmp/dg_'+robot.dcm_control.name+'-zmpRef.dat')
+    com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
+    comDes_data = np.loadtxt('/tmp/dg_'+robot.com_admittance_control.name+'-comRef.dat')
+
+    plt.figure()
+    plt.plot(dcm_data[:,1],'b-')
+    plt.plot(dcm_data[:,2],'r-')
+    plt.title('DCM')
+    plt.legend(['x','y'])
+
+    plt.figure()
+    plt.plot(com_data[:,1],'b-')
+    plt.plot(comDes_data[:,1],'b--')
+    plt.plot(com_data[:,2],'r-')
+    plt.plot(comDes_data[:,2],'r--')
+    plt.plot(com_data[:,3],'g-')
+    plt.plot(comDes_data[:,3],'g--')
+    plt.title('COM real vs desired')
+    plt.legend(['Real x','Desired x','Real y','Desired y','Real z','Desired z'])
+
+    plt.figure()
+    plt.plot(com_data[:,1],'b-')
+    plt.title('COM real x')
+    plt.figure()
+    plt.plot(comDes_data[:,1],'b--')
+    plt.title('COM desired x')
+
+    plt.figure()
+    plt.plot(com_data[:,2],'r-')
+    plt.title('COM real y')
+    plt.figure()
+    plt.plot(comDes_data[:,2],'r--')
+    plt.title('COM desired y')
+
+    plt.figure()
+    plt.plot(com_data[:,3],'g-')
+    plt.title('COM real z')
+    plt.figure()
+    plt.plot(comDes_data[:,3],'g--')
+    plt.title('COM desired z')
+
+    plt.figure()
+    plt.plot(zmp_data[:,1],'b-')
+    plt.plot(zmpDes_data[:,1],'b--')
+    plt.plot(zmp_data[:,2],'r-')
+    plt.plot(zmpDes_data[:,2],'r--')
+    plt.title('ZMP real vs desired')
+    plt.legend(['Real x','Desired x','Real y','Desired y'])
+
+    plt.figure()
+    plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
+    plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
+    plt.title('ZMP error')
+    plt.legend(['Error on x','Error on y'])
+
+    plt.show()
+

python/sot_talos_balance/test/test_dcmComZmpControl.py

+from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_com_controller
+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
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+import matplotlib.pyplot as plt
+import numpy as np
+
+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 = 600
+
+    # --- Dummy estimator
+    robot.estimator = create_dummy_dcm_estimator(robot)
+
+    # --- DCM controller
+    Kp_dcm = [5.0,5.0,5.0]
+    Ki_dcm = [0.0,0.0,0.0]
+    robot.dcm_control = create_dcm_com_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
+
+    # --- Admittance controller
+    Kp_adm = [0.0,0.0,0.0]
+    robot.com_admittance_control = create_com_admittance_controller(Kp_adm,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(300)
+    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(300)
+    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)
+
+    # --- TRACER
+    robot.tracer = TracerRealTime("zmp_tracer")
+    robot.tracer.setBufferSize(80*(2**20))
+    robot.tracer.open('/tmp','dg_','.dat')
+    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+
+    addTrace(robot.tracer, robot.dynamic, 'zmp')
+    addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
+    addTrace(robot.tracer, robot.estimator, 'dcm')
+    addTrace(robot.tracer, robot.dynamic, 'com')
+    addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
+
+    # SIMULATION
+
+    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.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)
+
+    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]
+    robot.dcm_control.resetDcmIntegralError()
+    robot.dcm_control.Ki.value = [1.0,1.0,0.0]
+
+    robot.tracer.start()
+
+    sleep(5.0)
+
+    dump_tracer(robot.tracer)
+
+	  # --- DISPLAY
+    dcm_data = np.loadtxt('/tmp/dg_'+robot.estimator.name+'-dcm.dat')
+    zmp_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-zmp.dat')
+    zmpDes_data = np.loadtxt('/tmp/dg_'+robot.dcm_control.name+'-zmpRef.dat')
+    com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
+    comDes_data = np.loadtxt('/tmp/dg_'+robot.com_admittance_control.name+'-comRef.dat')
+
+    plt.figure()
+    plt.plot(dcm_data[:,1],'b-')
+    plt.plot(dcm_data[:,2],'r-')
+    plt.title('DCM')
+    plt.legend(['x','y'])
+
+    plt.figure()
+    plt.plot(com_data[:,1],'b-')
+    plt.plot(comDes_data[:,1],'b--')
+    plt.plot(com_data[:,2],'r-')
+    plt.plot(comDes_data[:,2],'r--')
+    plt.plot(com_data[:,3],'g-')
+    plt.plot(comDes_data[:,3],'g--')
+    plt.title('COM real vs desired')
+    plt.legend(['Real x','Desired x','Real y','Desired y','Real z','Desired z'])
+
+    plt.figure()
+    plt.plot(zmp_data[:,1],'b-')
+    plt.plot(zmpDes_data[:,1],'b--')
+    plt.plot(zmp_data[:,2],'r-')
+    plt.plot(zmpDes_data[:,2],'r--')
+    plt.title('ZMP real vs desired')
+    plt.legend(['Real x','Desired x','Real y','Desired y'])
+
+    plt.figure()
+    plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
+    plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
+    plt.title('ZMP error')
+    plt.legend(['Error on x','Error on y'])
+
+    plt.show()
+

src/CMakeLists.txt

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

src/dcm-com-controller.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/dcm-com-controller.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_DCMCOMCONTROLLER_DDCOMREF_COMPUTATION  "DcmComController: ddcomRef computation                 "
+#define PROFILE_DCMCOMCONTROLLER_ZMPREF_COMPUTATION    "DcmComController: zmpRef computation                   "
+#define PROFILE_DCMCOMCONTROLLER_WRENCHREF_COMPUTATION "DcmComController: wrenchRef computation                "
+
+#define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_decayFactorSIN << m_omegaSIN << m_massSIN << m_dcmSIN << m_dcmDesSIN << m_comDesSIN << m_ddcomDesSIN
+
+#define OUTPUT_SIGNALS m_ddcomRefSOUT << m_zmpRefSOUT << m_wrenchRefSOUT
+
+      /// Define EntityClassName here rather than in the header file
+      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+      typedef DcmComController EntityClassName;
+
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DcmComController,
+                                         "DcmComController");
+
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      DcmComController::DcmComController(const std::string& name)
+                      : Entity(name)
+                      , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(decayFactor, double)
+                      , CONSTRUCT_SIGNAL_IN(omega, double)
+                      , CONSTRUCT_SIGNAL_IN(mass, double)
+                      , CONSTRUCT_SIGNAL_IN(dcm, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(dcmDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(comDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(ddcomDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector, INPUT_SIGNALS)
+                      , CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector,    m_ddcomRefSOUT << m_comDesSIN << m_omegaSIN )
+                      , CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_ddcomRefSOUT << m_comDesSIN << m_massSIN )
+                      , m_initSucceeded(false)
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        /* Commands. */
+        addCommand("init", makeCommandVoid1(*this, &DcmComController::init, docCommandVoid1("Initialize the entity.","time step")));
+        addCommand("resetDcmIntegralError", makeCommandVoid0(*this, &DcmComController::resetDcmIntegralError, docCommandVoid0("Set dcm integral error to zero.")));
+      }
+
+      void DcmComController::init(const double & dt)
+      {
+        if(!m_KpSIN.isPlugged())
+          return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
+        if(!m_KiSIN.isPlugged())
+          return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
+        if(!m_decayFactorSIN.isPlugged())
+          return SEND_MSG("Init failed: signal decayFactor is not plugged", MSG_TYPE_ERROR);
+        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_dcmSIN.isPlugged())
+          return SEND_MSG("Init failed: signal dcm is not plugged", MSG_TYPE_ERROR);
+        if(!m_dcmDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal dcmDes is not plugged", MSG_TYPE_ERROR);
+        if(!m_comDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal comDes is not plugged", MSG_TYPE_ERROR);
+        if(!m_ddcomDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal ddcomDes is not plugged", MSG_TYPE_ERROR);
+
+        m_dt = dt;
+        resetDcmIntegralError();
+        m_initSucceeded = true;
+      }
+
+      void DcmComController::resetDcmIntegralError()
+      {
+        m_dcmIntegralError.setZero(3);
+      }
+
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(ddcomRef, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal ddcomRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_DCMCOMCONTROLLER_DDCOMREF_COMPUTATION);
+
+        
+        const Vector & Kp = m_KpSIN(iter);
+        const Vector & Ki = m_KiSIN(iter);
+        const double & decayFactor = m_decayFactorSIN(iter);
+        const double & omega = m_omegaSIN(iter);
+        const Vector & dcm = m_dcmSIN(iter);
+        const Vector & dcmDes = m_dcmDesSIN(iter);
+        const Vector & ddcomDes = m_ddcomDesSIN(iter);
+
+        assert(Kp.size()==3       && "Unexpected size of signal Kp");
+        assert(Ki.size()==3       && "Unexpected size of signal Ki");
+        assert(dcm.size()==3      && "Unexpected size of signal dcm");
+        assert(dcmDes.size()==3   && "Unexpected size of signal dcmDes");
+        assert(ddcomDes.size()==3 && "Unexpected size of signal ddcomDes");
+
+        Vector dcmError = dcmDes - dcm;
+
+        m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;
+
+        Vector ddcomRef = ddcomDes + omega * Kp.cwiseProduct(dcmError) + omega * Ki.cwiseProduct(m_dcmIntegralError);
+
+        s = ddcomRef;
+
+        getProfiler().stop(PROFILE_DCMCOMCONTROLLER_DDCOMREF_COMPUTATION);
+
+        return s;
+      }
+
+      DEFINE_SIGNAL_OUT_FUNCTION(zmpRef, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal zmpRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_DCMCOMCONTROLLER_ZMPREF_COMPUTATION);
+
+        const double & omega = m_omegaSIN(iter);
+        const Vector & comDes = m_comDesSIN(iter);
+
+        const Vector & ddcomRef = m_ddcomRefSOUT(iter);
+
+        assert(comDes.size()==3 && "Unexpected size of signal comDes");
+
+        Vector zmpRef = comDes - ddcomRef/(omega*omega);
+
+        s = zmpRef;
+
+        getProfiler().stop(PROFILE_DCMCOMCONTROLLER_ZMPREF_COMPUTATION);
+
+        return s;
+      }
+
+      DEFINE_SIGNAL_OUT_FUNCTION(wrenchRef, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal wrenchRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_DCMCOMCONTROLLER_WRENCHREF_COMPUTATION);
+
+        const double & mass   = m_massSIN(iter);
+        const Vector & comDes = m_comDesSIN(iter);
+
+        const Vector & ddcomRef = m_ddcomRefSOUT(iter);
+
+        assert(comDes.size()==3 && "Unexpected size of signal comDes");
+
+        Vector gravity(3);
+        gravity << 0.0, 0.0, -9.81;
+
+        Vector forceRef = mass * ( ddcomRef - gravity );
+
+        Vector wrenchRef(6);
+        wrenchRef.head<3>() = forceRef;
+        Eigen::Vector3d comDes3 = comDes;
+        wrenchRef.tail<3>() = comDes3.cross(wrenchRef.head<3>());
+
+        s = wrenchRef;
+
+        getProfiler().stop(PROFILE_DCMCOMCONTROLLER_WRENCHREF_COMPUTATION);
+
+        return s;
+      }
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      /* ------------------------------------------------------------------- */
+      /* --- ENTITY -------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      void DcmComController::display(std::ostream& os) const
+      {
+        os << "DcmComController " << getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+    } // namespace talos_balance
+  } // namespace sot
+} // namespace dynamicgraph
+