Admittance controller sketch

include/sot/talos_balance/admittance-controller-single-joint.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_admittance_controller_single_joint_H__
+#define __sot_talos_balance_admittance_controller_single_joint_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (admittance_controller_single_joint_EXPORTS)
+#    define ADMITTANCECONTROLLERSINGLEJOINT_EXPORT __declspec(dllexport)
+#  else
+#    define ADMITTANCECONTROLLERSINGLEJOINT_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define ADMITTANCECONTROLLERSINGLEJOINT_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 ADMITTANCECONTROLLERSINGLEJOINT_EXPORT AdmittanceControllerSingleJoint
+	                         : public ::dynamicgraph::Entity
+      {
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        AdmittanceControllerSingleJoint( const std::string & name );
+
+        void init(const unsigned & n);
+
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(qDes, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(dqDes, dynamicgraph::Vector);
+
+        DECLARE_SIGNAL_OUT(dqRef, 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("[AdmittanceControllerSingleJoint-"+name+"] "+msg, t, file, line);
+        }
+
+      protected:
+        int m_n;
+        bool m_initSucceeded;    /// true if the entity has been successfully initialized
+        dynamicgraph::Vector m_Kp;
+
+      }; // class AdmittanceControllerSingleJoint
+
+    }    // namespace talos_balance
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_admittance_controller_single_joint_H__

src/CMakeLists.txt

@@ -36,6 +36,7 @@ ENDIF(UNIX)
 #This project will create many plugins as shared libraries, listed here
 SET(plugins
     example
+    admittance-controller-single-joint
     joint-position-controller
     nd-trajectory-generator
   )

src/admittance-controller-single-joint.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/admittance-controller-single-joint.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_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION "AdmittanceControllerSingleJoint: dqRef computation     "
+
+#define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_qDesSIN << m_dqDesSIN
+
+#define OUTPUT_SIGNALS m_dqRefSOUT
+
+      /// Define EntityClassName here rather than in the header file
+      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+      typedef AdmittanceControllerSingleJoint EntityClassName;
+
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerSingleJoint,
+                                         "AdmittanceControllerSingleJoint");
+
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      AdmittanceControllerSingleJoint::AdmittanceControllerSingleJoint(const std::string& name)
+                      : Entity(name)
+                      , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(state, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(qDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(dqDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_OUT(dqRef, dynamicgraph::Vector, INPUT_SIGNALS)
+                      , m_initSucceeded(false)
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        /* Commands. */
+        addCommand("init", makeCommandVoid1(*this, &AdmittanceControllerSingleJoint::init, docCommandVoid1("Initialize the entity.","Control gains")));
+      }
+
+      void AdmittanceControllerSingleJoint::init(const unsigned & n)
+      {
+        if(n<1)
+          return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
+        if(!m_KpSIN.isPlugged())
+          return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
+        if(!m_stateSIN.isPlugged())
+          return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
+        if(!m_qDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal qDes is not plugged", MSG_TYPE_ERROR);
+        if(!m_dqDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal dqDes is not plugged", MSG_TYPE_ERROR);
+
+        m_n = n;
+        m_initSucceeded = true;
+      }
+
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(dqRef, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal dqRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
+
+        const Vector & state = m_stateSIN(iter);
+        const Vector & qDes = m_qDesSIN(iter);
+        const Vector & dqDes = m_dqDesSIN(iter);
+        const Vector & Kp = m_KpSIN(iter);
+
+        assert(state.size()==m_n+6 && "Unexpected size of signal state");
+        assert(qDes.size()==m_n    && "Unexpected size of signal qDes");
+        assert(dqDes.size()==m_n   && "Unexpected size of signal dqDes");
+        assert(Kp.size()==m_n      && "Unexpected size of signal Kp");
+
+        const Vector & q = state.tail(m_n);
+
+        s = dqDes + Kp.cwiseProduct(qDes-q);
+
+        getProfiler().stop(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
+
+        return s;
+      }
+
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      /* ------------------------------------------------------------------- */
+      /* --- ENTITY -------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      void AdmittanceControllerSingleJoint::display(std::ostream& os) const
+      {
+        os << "AdmittanceControllerSingleJoint " << getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+    } // namespace talos_balance
+  } // namespace sot
+} // namespace dynamicgraph
+