From c9d7ef7cc9793e751d3e973fb6f2f4e58945d27b Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Mon, 10 Dec 2018 15:38:28 +0100
Subject: [PATCH] Admittance controller sketch
---
.../admittance-controller-single-joint.hh | 95 ++++++++++++
src/CMakeLists.txt | 1 +
src/admittance-controller-single-joint.cpp | 138 ++++++++++++++++++
3 files changed, 234 insertions(+)
create mode 100644 include/sot/talos_balance/admittance-controller-single-joint.hh
create mode 100644 src/admittance-controller-single-joint.cpp
diff --git a/include/sot/talos_balance/admittance-controller-single-joint.hh b/include/sot/talos_balance/admittance-controller-single-joint.hh
new file mode 100644
index 0000000..7b45c97
--- /dev/null
+++ b/include/sot/talos_balance/admittance-controller-single-joint.hh
@@ -0,0 +1,95 @@
+/*
+ * 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__
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b5bb1ed..d7ef057 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
+ admittance-controller-single-joint
joint-position-controller
nd-trajectory-generator
)
diff --git a/src/admittance-controller-single-joint.cpp b/src/admittance-controller-single-joint.cpp
new file mode 100644
index 0000000..1e7a083
--- /dev/null
+++ b/src/admittance-controller-single-joint.cpp
@@ -0,0 +1,138 @@
+/*
+ * 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
+
--
GitLab