From 97dc12e8fa38f39d4a16f5c44c6ddcbc9822ac01 Mon Sep 17 00:00:00 2001
From: fbailly <fbailly@laas.fr>
Date: Tue, 15 Jan 2019 17:14:09 +0100
Subject: [PATCH] [added dcm-estimator entity] does nothing for now
---
include/sot/talos_balance/dcm-estimator.hh | 111 +++++++++++++++
src/dcm-estimator.cpp | 150 +++++++++++++++++++++
unittest/python/test_com_estimator.py | 28 ++++
3 files changed, 289 insertions(+)
create mode 100644 include/sot/talos_balance/dcm-estimator.hh
create mode 100644 src/dcm-estimator.cpp
create mode 100644 unittest/python/test_com_estimator.py
diff --git a/include/sot/talos_balance/dcm-estimator.hh b/include/sot/talos_balance/dcm-estimator.hh
new file mode 100644
index 0000000..a144513
--- /dev/null
+++ b/include/sot/talos_balance/dcm-estimator.hh
@@ -0,0 +1,111 @@
+/*
+ * 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__
diff --git a/src/dcm-estimator.cpp b/src/dcm-estimator.cpp
new file mode 100644
index 0000000..d89e8f1
--- /dev/null
+++ b/src/dcm-estimator.cpp
@@ -0,0 +1,150 @@
+/*
+ * 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
diff --git a/unittest/python/test_com_estimator.py b/unittest/python/test_com_estimator.py
new file mode 100644
index 0000000..4baa1cc
--- /dev/null
+++ b/unittest/python/test_com_estimator.py
@@ -0,0 +1,28 @@
+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
--
GitLab