From bf55149cddfd2ff28eca1404d9a4e34ea7f3aee8 Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Thu, 20 Dec 2018 18:38:09 +0100
Subject: [PATCH] DCM controller [second version]
---
CMakeLists.txt | 2 +
.../sot/talos_balance/dcm-com-controller.hh | 104 ++++++++
.../create_entities_utils.py | 27 ++
.../test/test_dcmComControl.py | 149 ++++++++++++
.../test/test_dcmComZmpControl.py | 130 ++++++++++
src/CMakeLists.txt | 1 +
src/dcm-com-controller.cpp | 230 ++++++++++++++++++
7 files changed, 643 insertions(+)
create mode 100644 include/sot/talos_balance/dcm-com-controller.hh
create mode 100644 python/sot_talos_balance/test/test_dcmComControl.py
create mode 100644 python/sot_talos_balance/test/test_dcmComZmpControl.py
create mode 100644 src/dcm-com-controller.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95a4e11..8a0aa5d 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/sot/talos_balance/dcm-com-controller.hh b/include/sot/talos_balance/dcm-com-controller.hh
new file mode 100644
index 0000000..126fbbc
--- /dev/null
+++ b/include/sot/talos_balance/dcm-com-controller.hh
@@ -0,0 +1,104 @@
+/*
+ * 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__
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index 7bfc6c4..8586203 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/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)
diff --git a/python/sot_talos_balance/test/test_dcmComControl.py b/python/sot_talos_balance/test/test_dcmComControl.py
new file mode 100644
index 0000000..33d278c
--- /dev/null
+++ b/python/sot_talos_balance/test/test_dcmComControl.py
@@ -0,0 +1,149 @@
+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()
+
diff --git a/python/sot_talos_balance/test/test_dcmComZmpControl.py b/python/sot_talos_balance/test/test_dcmComZmpControl.py
new file mode 100644
index 0000000..0ddc168
--- /dev/null
+++ b/python/sot_talos_balance/test/test_dcmComZmpControl.py
@@ -0,0 +1,130 @@
+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()
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 26271ae..a4c9caf 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/dcm-com-controller.cpp b/src/dcm-com-controller.cpp
new file mode 100644
index 0000000..99f546b
--- /dev/null
+++ b/src/dcm-com-controller.cpp
@@ -0,0 +1,230 @@
+/*
+ * 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
+
--
GitLab