[added dcm-estimator entity] does nothing for now

include/sot/talos_balance/dcm-estimator.hh

+/*
+ * Copyright 2019, 
+ * LAAS-CNRS,
+ * Gepetto team 
+ *
+ * This file is part of sot-talos-balance.
+ * See license file 
+ */
+
+#ifndef __sot_talos_balance_dcm_estimator_H__
+#define __sot_talos_balance_dcm_estimator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (dcm_estimator_EXPORTS)
+#    define DCMESTIMATOR_EXPORT __declspec(dllexport)
+#  else
+#    define DCMESTIMATOR_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define DCMESTIMATOR_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <sot/core/signal-helper.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/logger.hh>
+#include <map>
+#include "boost/assign.hpp"
+#include <boost/math/distributions/normal.hpp> // for normal_distribution
+
+/* Pinocchio */
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <sot/core/robot-utils.hh>
+
+namespace dynamicgraph 
+{
+  namespace sot 
+  {
+    namespace talos_balance 
+    {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class DCMESTIMATOR_EXPORT DcmEstimator
+	                         :public::dynamicgraph::Entity
+      {
+        typedef se3::SE3 SE3;
+        typedef Eigen::Vector2d Vector2;
+        typedef Eigen::Vector3d Vector3;
+        typedef Eigen::Vector4d Vector4;
+        typedef Eigen::Vector6d Vector6;
+        typedef Eigen::Vector7d Vector7;
+        typedef Eigen::Matrix3d Matrix3;
+        typedef boost::math::normal normal;
+
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        DcmEstimator(const std::string & name );
+
+        void init(const double & dt, const std::string& urdfFile);
+        void test_command(const int& test_int);
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(q,   dynamicgraph::Vector);
+
+        DECLARE_SIGNAL_OUT(c,  dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(dc, 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("[DcmEstimator-"+name+"] "+msg, t, file, line);
+        }
+
+      protected:
+        bool      m_initSucceeded;            /// true if the entity has been successfully initialized
+        RobotUtil*   m_robot_util;
+        se3::Data         *m_data;            /// Pinocchio robot data
+        Eigen::VectorXd   m_q_pin;            /// robot configuration according to pinocchio convention
+        Eigen::VectorXd   m_v_pin;            /// robot velocities according to pinocchio convention
+        Vector3        m_last_vel;
+        double               m_dt;            /// sampling time step
+        se3::Model        m_model;            /// Pinocchio robot model
+
+      }; // class DCMEstimator
+
+    }    // namespace talos_balance
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_dcm_estimator_H__

src/dcm-estimator.cpp

+/*
+ * Copyright 2019, 
+ * LAAS-CNRS
+ * François Bailly, 
+ *
+ * This file is part of sot-talos-balance.
+ * See license file.
+ */
+
+#include "sot/talos_balance/dcm-estimator.hh"
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+#include "sot/talos_balance/utils/commands-helper.hh"
+#include <sot/talos_balance/utils/stop-watch.hh>
+#include "pinocchio/algorithm/frames.hpp"
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace talos_balance
+    {
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+      using namespace dg::command;
+      using namespace std;
+      using namespace se3;
+      using boost::math::normal; // typedef provides default type is double.
+ //Size to be aligned                         "-------------------------------------------------------"
+     
+#define PROFILE_BASE_POSITION_ESTIMATION      "base-est position estimation"
+#define PROFILE_BASE_VELOCITY_ESTIMATION      "base-est velocity estimation"
+#define PROFILE_BASE_KINEMATICS_COMPUTATION   "base-est kinematics computation"
+
+#define INPUT_SIGNALS     m_qSIN
+#define OUTPUT_SIGNALS    m_cSOUT << m_dcSOUT
+
+      /// Define EntityClassName here rather than in the header file
+      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+      typedef DcmEstimator EntityClassName;
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DcmEstimator,
+                                         "DcmEstimator");      
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      DcmEstimator::DcmEstimator(const std::string& name)
+        : Entity(name)
+        ,CONSTRUCT_SIGNAL_IN( q,   dynamicgraph::Vector)
+        ,CONSTRUCT_SIGNAL_OUT(c,   dynamicgraph::Vector, m_qSIN)
+        ,CONSTRUCT_SIGNAL_OUT(dc,  dynamicgraph::Vector, m_qSIN)
+
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        /* Commands. */
+        addCommand("init",
+                   makeCommandVoid2(*this, &DcmEstimator::init,
+                                    docCommandVoid2("Initialize the entity.",
+                                                    "time step (double)",
+                                                    "URDF file path (string)")));
+
+        addCommand("test_command",
+                           makeCommandVoid1(*this, &DcmEstimator::test_command,
+                                            docCommandVoid1("Test dumb command",
+                                                            "integer (int)")));
+      }
+     void DcmEstimator::init(const double & dt, const std::string& robotRef)
+      {
+        m_dt = dt;
+        try
+        {
+          // Retrieve m_robot_util informations 
+          std::string localName(robotRef);
+          if (isNameInRobotUtil(localName))
+          {
+            m_robot_util = getRobotUtil(localName);
+            std::cerr << "m_robot_util:" << m_robot_util << std::endl;
+          }
+          else
+          {
+            SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
+            return;
+          }
+
+          se3::urdf::buildModel(m_robot_util->m_urdf_filename,
+                                se3::JointModelFreeFlyer(), m_model);
+
+          assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
+          assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
+          assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
+          m_q_pin.setZero(m_model.nq);
+          m_q_pin[6]= 1.; // for quaternion
+          m_v_pin.setZero(m_robot_util->m_nbJoints+6);
+
+          m_last_vel.setZero(); // ?
+        }
+        catch (const std::exception& e)
+        {
+          std::cout << e.what();
+          SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
+          return;
+        }
+        m_data = new se3::Data(m_model);
+        m_initSucceeded = true;
+      }
+    
+     void DcmEstimator::test_command(const int& test_int)     
+     {
+        std::cout << "test integer:" << test_int << std::endl;
+     }
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(c, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal com before initialization!");
+          return s;
+        }
+
+        s = m_q_pin;
+        return s;
+      }
+      
+      DEFINE_SIGNAL_OUT_FUNCTION(dc, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal dcom before initialization!");
+          return s;
+        }
+
+        s = m_q_pin;
+        return s;
+      }
+      void DcmEstimator::display(std::ostream& os) const
+      {
+        os << "DcmEstimator " << getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+    } // namespace talos_balance
+  } // namespace sot
+} // namespace dynamicgraph
\ No newline at end of file

unittest/python/test_com_estimator.py

+from sot_talos_balance.create_entities_utils import *
+from dynamic_graph import plug
+import dynamic_graph as dg
+from dynamic_graph.sot.core import SOT
+from numpy import eye
+from time import sleep
+import os
+from IPython import embed
+from sot_talos_balance.parameter_server             import ParameterServer
+from sot_talos_balance.utils.sot_utils              import Bunch
+from sot_talos_balance.dcm_estimator                import DcmEstimator
+from sot_talos_balance.utils.sot_utils              import Bunch
+import sot_talos_balance.talos.control_manager_conf as control_manager_conf
+
+
+dt = 0.001
+conf = Bunch()
+robot_name = 'robot'
+
+conf.param_server = control_manager_conf
+param_server = ParameterServer("param_server")     
+param_server.init(dt, conf.param_server.urdfFileName, robot_name)
+param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ)
+param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ)
+
+dcm_estimator = DcmEstimator('dcm_estimator')
+dcm_estimator.init(dt, robot_name)
+dcm_estimator.test_command(5)
\ No newline at end of file