Simple position controller

include/sot/talos_balance/example.hh

 /*
- * Copyright 2015, Andrea Del Prete, LAAS-CNRS
+ * 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
@@ -14,8 +14,8 @@
  * with sot-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef __test_example_H__
-#define __test_example_H__
+#ifndef __sot_talos_balance_example_H__
+#define __sot_talos_balance_example_H__
 
 /* --------------------------------------------------------------------- */
 /* --- API ------------------------------------------------------------- */
@@ -88,4 +88,4 @@ namespace dynamicgraph {
 
 
 
-#endif // #ifndef __test_example_H__
+#endif // #ifndef __sot_talos_balance_example_H__

include/sot/talos_balance/joint-position-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_joint_position_controller_H__
+#define __sot_talos_balance_joint_position_controller_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (position_controller_EXPORTS)
+#    define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
+#  else
+#    define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define JOINTPOSITIONCONTROLLER_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 JOINTPOSITIONCONTROLLER_EXPORT JointPositionController
+	                         : public ::dynamicgraph::Entity
+      {
+        DYNAMIC_GRAPH_ENTITY_DECL();
+
+      public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        JointPositionController( const std::string & name );
+
+        void init(const Eigen::VectorXd & Kp);
+
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(q, Eigen::VectorXd);
+        DECLARE_SIGNAL_IN(qDes, Eigen::VectorXd);
+        DECLARE_SIGNAL_IN(dqDes, Eigen::VectorXd);
+
+        DECLARE_SIGNAL_OUT(dqRef, Eigen::VectorXd);
+
+        /* --- 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("[JointPositionController-"+name+"] "+msg, t, file, line);
+        }
+
+      protected:
+        bool m_initSucceeded;    /// true if the entity has been successfully initialized
+        Eigen::VectorXd m_Kp;
+
+      }; // class JointPositionController
+
+    }    // namespace talos_balance
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_talos_balance_joint_position_controller_H__

src/CMakeLists.txt

@@ -36,6 +36,7 @@ ENDIF(UNIX)
 #This project will create many plugins as shared libraries, listed here
 SET(plugins
     example
+    joint-position-controller
   )
 
 #set(ADDITIONAL_feature-task_LIBS feature-generic task)

src/example.cpp

 /*
- * Copyright 2015, Andrea Del Prete, LAAS-CNRS
+ * 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

src/joint-position-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/joint-position-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_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION "JointPositionController: dqRef computation             "
+
+#define INPUT_SIGNALS     m_qSIN << 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 JointPositionController EntityClassName;
+
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointPositionController,
+                                         "JointPositionController");
+
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      JointPositionController::JointPositionController(const std::string& name)
+                      : Entity(name)
+                      , CONSTRUCT_SIGNAL_IN(q, Eigen::VectorXd)
+                      , CONSTRUCT_SIGNAL_IN(qDes, Eigen::VectorXd)
+                      , CONSTRUCT_SIGNAL_IN(dqDes, Eigen::VectorXd)
+                      , CONSTRUCT_SIGNAL_OUT(dqRef, Eigen::VectorXd, INPUT_SIGNALS)
+                      , m_initSucceeded(false)
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        /* Commands. */
+        addCommand("init", makeCommandVoid1(*this, &JointPositionController::init, docCommandVoid1("Initialize the entity.","Control gains")));
+      }
+
+      void JointPositionController::init(const Eigen::VectorXd & Kp)
+      {
+        if(!m_qDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal firstAddend is not plugged", MSG_TYPE_ERROR);
+        if(!m_dqDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal secondAddend is not plugged", MSG_TYPE_ERROR);
+
+        m_Kp = Kp;
+        m_initSucceeded = true;
+      }
+
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(dqRef, Eigen::VectorXd)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal dqRef before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
+
+        Eigen::VectorXd q = m_qSIN(iter);
+        Eigen::VectorXd qDes = m_qDesSIN(iter);
+        Eigen::VectorXd dqDes = m_dqDesSIN(iter);
+
+        s = dqDes + m_Kp.cwiseProduct(qDes-q);
+
+        getProfiler().stop(PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
+
+        return s;
+      }
+
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      /* ------------------------------------------------------------------- */
+      /* --- ENTITY -------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      void JointPositionController::display(std::ostream& os) const
+      {
+        os << "JointPositionController " << getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+    } // namespace talos_balance
+  } // namespace sot
+} // namespace dynamicgraph
+

unittest/python/test_control.py

+from __future__ import print_function
+from sot_talos_balance.joint_position_controller import JointPositionController
+
+controller = JointPositionController("ciao")
+
+print("Commands:")
+print(controller.commands())
+
+print("\nSignals (at creation):")
+controller.displaySignals()
+
+controller.q.value = (0.0,0.0)
+controller.qDes.value = (1.0,1.0)
+controller.dqDes.value = (0.0,0.0)
+
+Kp = (10.0,10.0)
+
+controller.init(Kp)
+
+controller.dqRef.recompute(1)
+
+print( "\nKp: %s" % (Kp,) )
+
+print( "\nq: %s" % (controller.q.value,) )
+print( "qDes: %s" % (controller.qDes.value,) )
+print( "dqDes: %s" % (controller.dqDes.value,) )
+
+print( "\ndqRef: %s" % (controller.dqRef.value,) )
+
+#print( "\nInputs: %f, %f" % (ex.firstAddend.value, ex.secondAddend.value) )
+#print( "Output: %f" % ex.sum.value )
+