[admittance-ctrl] Add admittance control entity

CMakeLists.txt

@@ -88,6 +88,7 @@ SET(${LIBRARY_NAME}_HEADERS
   include/sot/torque_control/trace-player.hh
   include/sot/torque_control/torque-offset-estimator.hh
   include/sot/torque_control/imu_offset_compensation.hh
+  include/sot/torque_control/admittance-controller.hh
   include/sot/torque_control/utils/logger.hh
   include/sot/torque_control/utils/trajectory-generators.hh
   include/sot/torque_control/utils/lin-estimator.hh

include/sot/torque_control/admittance-controller.hh

+/*
+ * Copyright 2014, Andrea Del Prete, LAAS-CNRS
+ *
+ * This file is part of sot-torque-control.
+ * sot-torque-control 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-torque-control 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-torque-control.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_torque_control_admittance_controller_H__
+#define __sot_torque_control_admittance_controller_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (sot_admittance_controller_EXPORTS)
+#    define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
+#  else
+#    define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTADMITTANCECONTROLLER_EXPORT
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <sot/torque_control/signal-helper.hh>
+#include <sot/torque_control/utils/vector-conversions.hh>
+#include <sot/torque_control/utils/logger.hh>
+#include <sot/torque_control/common.hh>
+#include <map>
+#include <initializer_list>
+#include "boost/assign.hpp"
+
+#include <tsid/robots/robot-wrapper.hpp>
+#include <tsid/tasks/task-se3-equality.hpp>
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace torque_control {
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController
+	:public::dynamicgraph::Entity
+      {
+        typedef AdmittanceController EntityClassName;
+        DYNAMIC_GRAPH_ENTITY_DECL();
+        
+      public: 
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /* --- CONSTRUCTOR ---- */
+        AdmittanceController( const std::string & name );
+
+        void init(const double& dt, const std::string& robotRef);
+
+        /* --- SIGNALS --- */
+        DECLARE_SIGNAL_IN(encoders,         dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(kp_force,         dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(ki_force,         dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(kp_vel,           dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(ki_vel,           dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(fRightFootRef,    dynamicgraph::Vector); /// 6d reference force
+        DECLARE_SIGNAL_IN(fLeftFootRef,     dynamicgraph::Vector); /// 6d reference force
+        DECLARE_SIGNAL_IN(fRightFoot,       dynamicgraph::Vector); /// 6d estimated force
+        DECLARE_SIGNAL_IN(fLeftFoot,        dynamicgraph::Vector); /// 6d estimated force
+        DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
+        DECLARE_SIGNAL_IN(damping,          dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
+
+//        DECLARE_SIGNAL_IN(fRightHandRef,    dynamicgraph::Vector); /// 6d reference force
+//        DECLARE_SIGNAL_IN(fLeftHandRef,     dynamicgraph::Vector); /// 6d reference force
+//        DECLARE_SIGNAL_IN(fRightHand,       dynamicgraph::Vector); /// 6d estimated force
+//        DECLARE_SIGNAL_IN(fLeftHand,        dynamicgraph::Vector); /// 6d estimated force
+
+        DECLARE_SIGNAL_OUT(u,               dynamicgraph::Vector);  /// control
+        // DEBUG SIGNALS
+        DECLARE_SIGNAL_OUT(dqDes,             dynamicgraph::Vector);  /// dqDes = J^+ * Kf * (fRef-f)
+        DECLARE_SIGNAL_OUT(fRightFootError,   dynamicgraph::Vector);  /// fRef-f
+        DECLARE_SIGNAL_OUT(fLeftFootError,    dynamicgraph::Vector);  /// fRef-f
+//        DECLARE_SIGNAL_OUT(fRightHandError,   dynamicgraph::Vector);  /// fRef-f
+//        DECLARE_SIGNAL_OUT(fLeftHandError,    dynamicgraph::Vector);  /// fRef-f
+
+
+        /* --- COMMANDS --- */
+        /* --- ENTITY INHERITANCE --- */
+        virtual void display( std::ostream& os ) const;
+        virtual void commandLine(const std::string& cmdLine,
+                                 std::istringstream& cmdArgs,
+                                 std::ostream& os);
+
+        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
+        {
+          getLogger().sendMsg("["+name+"] "+msg, t, file, line);
+        }
+        
+      protected:
+        Eigen::VectorXd   m_u;                /// control (i.e. motor currents)
+        bool              m_firstIter;
+        bool              m_initSucceeded;    /// true if the entity has been successfully initialized
+        bool              m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
+        double            m_dt;               /// control loop time period
+        int               m_nj;
+
+        /// robot geometric/inertial data
+        int m_frame_id_rf;  /// frame id of right foot
+        int m_frame_id_lf;  /// frame id of left foot
+
+        /// tsid
+        tsid::robots::RobotWrapper *        m_robot;
+        se3::Data*                          m_data;
+
+        tsid::math::Vector6 m_f_RF;                /// desired 6d wrench right foot
+        tsid::math::Vector6 m_f_LF;                /// desired 6d wrench left foot
+        tsid::math::Vector  m_q_urdf;
+        tsid::math::Vector  m_v_urdf;
+
+        tsid::math::Vector  m_dq_des_urdf;
+        tsid::math::Vector  m_dqErrIntegral;    /// integral of the velocity error
+//        tsid::math::Vector  m_dqDesIntegral;    /// integral of the desired joint velocities
+        tsid::math::Vector  m_dq_fd;            /// joint velocities computed with finite differences
+        tsid::math::Vector  m_qPrev;            /// previous value of encoders
+
+        typedef se3::Data::Matrix6x Matrix6x;
+        Matrix6x m_J_RF;
+        Matrix6x m_J_LF;
+        Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
+        Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
+        tsid::math::Vector6 m_v_RF_int;
+        tsid::math::Vector6 m_v_LF_int;
+
+        RobotUtil * m_robot_util;
+
+//        tsid::math::Vector3 m_zmp_des_LF;          /// 3d desired zmp left foot
+//        tsid::math::Vector3 m_zmp_des_RF;          /// 3d desired zmp left foot
+//        tsid::math::Vector3 m_zmp_des_LF_local;    /// 3d desired zmp left foot expressed in local frame
+//        tsid::math::Vector3 m_zmp_des_RF_local;    /// 3d desired zmp left foot expressed in local frame
+//        tsid::math::Vector3 m_zmp_des;             /// 3d desired global zmp
+//        tsid::math::Vector3 m_zmp_LF;              /// 3d zmp left foot
+//        tsid::math::Vector3 m_zmp_RF;              /// 3d zmp left foot
+//        tsid::math::Vector3 m_zmp;                 /// 3d global zmp
+      }; // class AdmittanceController
+      
+    }    // namespace torque_control
+  }      // namespace sot
+}        // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_torque_control_admittance_controller_H__

src/admittance-controller.cpp

+/*
+ * Copyright 2015, Andrea Del Prete, LAAS-CNRS
+ *
+ * This file is part of sot-torque-control.
+ * sot-torque-control 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-torque-control 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-torque-control.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot/torque_control/admittance-controller.hh>
+#include <sot/core/debug.hh>
+#include <dynamic-graph/factory.h>
+
+#include <sot/torque_control/commands-helper.hh>
+#include <sot/torque_control/utils/metapod-helper.hh>
+#include <tsid/utils/stop-watch.hpp>
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace torque_control
+    {
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+      using namespace dg::command;
+      using namespace std;
+      using namespace Eigen;
+      using namespace tsid;
+      using namespace tsid::math;
+      using namespace tsid::tasks;
+
+#define PROFILE_DQ_DES_COMPUTATION "Admittance control computation"
+
+#define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN
+//                          m_fRightHandRefSIN << m_fLeftHandRefSIN
+#define FORCE_SIGNALS     m_fRightFootSIN << m_fLeftFootSIN
+//                          m_fRightHandSIN << m_fLeftHandSIN
+#define GAIN_SIGNALS      m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN << m_force_integral_saturationSIN
+#define STATE_SIGNALS     m_encodersSIN << m_jointsVelocitiesSIN
+
+#define INPUT_SIGNALS     STATE_SIGNALS << REF_FORCE_SIGNALS << \
+                          FORCE_SIGNALS << GAIN_SIGNALS << m_controlledJointsSIN << m_dampingSIN
+
+#define FORCE_ERROR_SIGNALS m_fRightFootErrorSOUT << m_fLeftFootErrorSOUT //<< m_fRightHandErrorSOUT << m_fLeftHandErrorSOUT
+#define OUTPUT_SIGNALS      m_uSOUT << m_dqDesSOUT << FORCE_ERROR_SIGNALS
+
+      /// Define EntityClassName here rather than in the header file
+      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+      typedef AdmittanceController EntityClassName;
+
+      typedef Eigen::Matrix<double,3,1>                            Vector3;
+      typedef Eigen::Matrix<double,6,1>                            Vector6;
+      /* --- DG FACTORY ---------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController,
+                                         "AdmittanceController");
+
+      /* ------------------------------------------------------------------- */
+      /* --- CONSTRUCTION -------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+      AdmittanceController::AdmittanceController(const std::string& name)
+            : Entity(name)
+            ,CONSTRUCT_SIGNAL_IN(encoders,            dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(jointsVelocities,    dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(kp_force,            dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(ki_force,            dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(kp_vel,              dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(ki_vel,              dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(fRightFootRef,       dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(fLeftFootRef,        dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(fRightFoot,          dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(fLeftFoot,           dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(controlledJoints,    dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(damping,             dynamicgraph::Vector)
+//            ,CONSTRUCT_SIGNAL_IN(fRightHandRef,       dynamicgraph::Vector)
+//            ,CONSTRUCT_SIGNAL_IN(fLeftHandRef,        dynamicgraph::Vector)
+//            ,CONSTRUCT_SIGNAL_IN(fRightHand,          dynamicgraph::Vector)
+//            ,CONSTRUCT_SIGNAL_IN(fLeftHand,           dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_OUT(u,                  dynamicgraph::Vector, STATE_SIGNALS<<
+                                                                FORCE_SIGNALS<<
+                                                                REF_FORCE_SIGNALS<<
+                                                                GAIN_SIGNALS<<
+                                                                m_controlledJointsSIN)
+            ,CONSTRUCT_SIGNAL_OUT(dqDes,            dynamicgraph::Vector, STATE_SIGNALS <<
+                                                                GAIN_SIGNALS <<
+                                                                FORCE_ERROR_SIGNALS <<
+                                                                m_dampingSIN)
+            ,CONSTRUCT_SIGNAL_OUT(fRightFootError,  dynamicgraph::Vector, m_fRightFootSIN <<
+                                                                m_fRightFootRefSIN)
+            ,CONSTRUCT_SIGNAL_OUT(fLeftFootError,   dynamicgraph::Vector, m_fLeftFootSIN <<
+                                                                m_fLeftFootRefSIN)
+//            ,CONSTRUCT_SIGNAL_OUT(fRightHandError,  dynamicgraph::Vector, m_fRightHandSIN <<
+//                                                                m_fRightHandRefSIN)
+//            ,CONSTRUCT_SIGNAL_OUT(fLeftHandError,   dynamicgraph::Vector, m_fLeftHandSIN <<
+//                                                                m_fLeftHandRefSIN)
+            ,m_initSucceeded(false)
+            ,m_useJacobianTranspose(false)
+            ,m_firstIter(true)
+      {
+        Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
+
+        m_v_RF_int.setZero();
+        m_v_LF_int.setZero();
+
+        /* Commands. */
+        addCommand("getUseJacobianTranspose",
+                   makeDirectGetter(*this,&m_useJacobianTranspose,
+                                    docDirectGetter("If true it uses the Jacobian transpose, otherwise the pseudoinverse","bool")));
+        addCommand("setUseJacobianTranspose",
+                   makeDirectSetter(*this, &m_useJacobianTranspose,
+                                    docDirectSetter("If true it uses the Jacobian transpose, otherwise the pseudoinverse",
+                                                    "bool")));
+        addCommand("init",
+                   makeCommandVoid2(*this, &AdmittanceController::init,
+                                    docCommandVoid2("Initialize the entity.",
+                                                    "Time period in seconds (double)",
+                                                    "Robot name (string)")));
+      }
+
+      void AdmittanceController::init(const double& dt, const std::string& robotRef)
+      {
+        if(dt<=0.0)
+          return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+        if(!m_encodersSIN.isPlugged())
+          return SEND_MSG("Init failed: signal encoders is not plugged", MSG_TYPE_ERROR);
+        if(!m_jointsVelocitiesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
+        if(!m_controlledJointsSIN.isPlugged())
+          return SEND_MSG("Init failed: signal controlledJoints is not plugged", MSG_TYPE_ERROR);
+
+        m_dt = dt;
+        m_initSucceeded = true;
+
+        /* Retrieve m_robot_util  informations */
+        std::string localName(robotRef);
+        if (isNameInRobotUtil(localName))
+          m_robot_util = getRobotUtil(localName);
+        else
+          return SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
+
+        try
+        {
+          vector<string> package_dirs;
+          m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename,
+                                             package_dirs,
+                                             se3::JointModelFreeFlyer());
+          m_data = new se3::Data(m_robot->model());
+
+          assert(m_robot->nv()>=6);
+          m_robot_util->m_nbJoints = m_robot->nv()-6;
+          m_nj = m_robot->nv()-6;
+          m_u.setZero(m_nj);
+          m_dq_des_urdf.setZero(m_nj);
+          m_dqErrIntegral.setZero(m_nj);
+          //m_dqDesIntegral.setZero(m_nj);
+
+          m_q_urdf.setZero(m_robot->nq());
+          m_q_urdf(6) = 1.0;
+          m_v_urdf.setZero(m_robot->nv());
+          m_J_RF.setZero(6, m_robot->nv());
+          m_J_LF.setZero(6, m_robot->nv());
+
+          m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
+          m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
+
+        }
+        catch (const std::exception& e)
+        {
+          std::cout << e.what();
+          return SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
+        }
+      }
+
+
+      /* ------------------------------------------------------------------- */
+      /* --- SIGNALS ------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
+          return s;
+        }
+
+        const Vector& dqDes  = m_dqDesSOUT(iter); // n
+        const Vector& q      = m_encodersSIN(iter);  // n
+        const Vector& dq     = m_jointsVelocitiesSIN(iter); // n
+        const Vector& kp     = m_kp_velSIN(iter);
+        const Vector& ki     = m_ki_velSIN(iter);
+
+        if(m_firstIter)
+        {
+          m_qPrev = q;
+          m_firstIter = false;
+        }
+
+        // estimate joint velocities using finite differences
+        m_dq_fd = (q-m_qPrev)/m_dt;
+        m_qPrev = q;
+
+        m_dqErrIntegral += m_dt * ki.cwiseProduct(dqDes - m_dq_fd);
+        s = kp.cwiseProduct(dqDes-dq) + m_dqErrIntegral;
+
+        return s;
+      }
+
+
+      DEFINE_SIGNAL_OUT_FUNCTION(dqDes,dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_WARNING_STREAM_MSG("Cannot compute signal dqDes before initialization!");
+          return s;
+        }
+
+        getProfiler().start(PROFILE_DQ_DES_COMPUTATION);
+        {
+          const Vector6& e_f_RF          = m_fRightFootErrorSOUT(iter); // 6
+          const Vector6& e_f_LF          = m_fLeftFootErrorSOUT(iter);  // 6
+          const Vector6d& kp             = m_kp_forceSIN(iter);
+          const Vector6d& ki             = m_ki_forceSIN(iter);
+          const Vector6d& f_sat          = m_force_integral_saturationSIN(iter);
+          const Vector& q_sot            = m_encodersSIN(iter);  // n
+//          const Vector& dq_sot           = m_jointsVelocitiesSIN(iter); // n
+          //const Vector& qMask            = m_controlledJointsSIN(iter); // n
+          //const Eigen::Vector4d& damping = m_dampingSIN(iter);          // 4
+
+          assert(q.size()==m_nj     && "Unexpected size of signal encoder");
+          assert(dq.size()==m_nj    && "Unexpected size of signal dq");
+          assert(qMask.size()==m_nj && "Unexpected size of signal controlledJoints");
+
+          Eigen::Vector6d v_des_RF = -kp.cwiseProduct(e_f_RF);
+          Eigen::Vector6d v_des_LF = -kp.cwiseProduct(e_f_LF);
+
+          m_v_RF_int -= m_dt*ki.cwiseProduct(e_f_RF);
+          m_v_LF_int -= m_dt*ki.cwiseProduct(e_f_LF);
+
+          // saturate
+          bool saturating_LF = false;
+          bool saturating_RF = false;
+          for(int i=0; i<6; i++)
+          {
+            if(m_v_RF_int(i) > f_sat(i))
+            {
+              saturating_RF = true;
+              m_v_RF_int(i) = f_sat(i);
+            }
+            else if(m_v_RF_int(i) < -f_sat(i))
+            {
+              saturating_RF = true;
+              m_v_RF_int(i) = -f_sat(i);
+            }
+
+            if(m_v_LF_int(i) > f_sat(i))
+            {
+              saturating_LF = true;
+              m_v_LF_int(i) = f_sat(i);
+            }
+            else if(m_v_LF_int(i) < -f_sat(i))
+            {
+              saturating_LF = true;
+              m_v_LF_int(i) = -f_sat(i);
+            }
+          }
+          if(saturating_LF)
+            SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: "+toString(m_v_LF_int.transpose()));
+          if(saturating_RF)
+            SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: "+toString(m_v_RF_int.transpose()));
+
+
+          /// *** Compute all Jacobians ***
+          m_robot_util->joints_sot_to_urdf(q_sot, m_q_urdf.tail(m_nj));
+          m_robot->computeAllTerms(*m_data, m_q_urdf, m_v_urdf);
+          m_robot->frameJacobianLocal(*m_data, m_frame_id_rf, m_J_RF);
+          m_robot->frameJacobianLocal(*m_data, m_frame_id_lf, m_J_LF);
+
+          //SEND_INFO_STREAM_MSG("RFoot Jacobian :" + toString(m_J_RF.rightCols(m_nj)));
+          // set to zero columns of Jacobian corresponding to unactive joints
+//          for(int i=0; i<m_nj; i++)
+//            if(qMask(i)==0)
+//              m_J_all.col(6+i).setZero();
+
+          /// Compute admittance control law
+          if(m_useJacobianTranspose)
+          {
+            m_dq_des_urdf  = m_J_RF.rightCols(m_nj).transpose()*(v_des_RF+m_v_RF_int);
+            m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose()*(v_des_LF+m_v_LF_int);
+          }
+          else
+          {
+            m_J_RF_QR.compute(m_J_RF.rightCols(m_nj));
+            m_J_LF_QR.compute(m_J_LF.rightCols(m_nj));
+
+            m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF+m_v_RF_int);
+//            SEND_INFO_STREAM_MSG("v_des_RF+m_v_RF_int:" + toString((v_des_RF+m_v_RF_int).transpose()));
+//            SEND_INFO_STREAM_MSG("dq_des_urdf (RF only):" + toString(m_dq_des_urdf.transpose()));
+            m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF+m_v_LF_int);
+//            SEND_INFO_STREAM_MSG("dq_des_urdf (RF+LF):" + toString(m_dq_des_urdf.transpose()));
+          }
+
+          if(s.size()!=m_nj)
+            s.resize(m_nj);
+
+          m_robot_util->joints_urdf_to_sot(m_dq_des_urdf, s);
+        }
+        getProfiler().stop(PROFILE_DQ_DES_COMPUTATION);
+
+        return s;
+      }
+
+      DEFINE_SIGNAL_OUT_FUNCTION(fRightFootError,dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_MSG("Cannot compute signal fRightFootError before initialization!", MSG_TYPE_WARNING_STREAM);
+          return s;
+        }
+        const Vector6& f =           m_fRightFootSIN(iter);      // 6
+        const Vector6& fRef =        m_fRightFootRefSIN(iter);   // 6
+        if(s.size()!=6)
+          s.resize(6);
+        s = fRef - f;
+        return s;
+      }
+
+      DEFINE_SIGNAL_OUT_FUNCTION(fLeftFootError,dynamicgraph::Vector)
+      {
+        if(!m_initSucceeded)
+        {
+          SEND_MSG("Cannot compute signal fLeftFootError before initialization!", MSG_TYPE_WARNING_STREAM);
+          return s;
+        }
+        const Vector6& f =           m_fLeftFootSIN(iter);      // 6
+        const Vector6& fRef =        m_fLeftFootRefSIN(iter);   // 6
+        if(s.size()!=6)
+          s.resize(6);
+        s = fRef - f;
+        return s;
+      }
+
+//      DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector)
+//      {
+//        if(!m_initSucceeded)
+//        {
+//          SEND_MSG("Cannot compute signal fRightHandError before initialization!", MSG_TYPE_WARNING_STREAM);
+//          return s;
+//        }
+
+//        const Eigen::Matrix<double,24,1>& Kf =        m_KfSIN(iter); // 6*4
+//        const Vector6& f =          m_fRightHandSIN(iter);      // 6
+//        const Vector6& fRef =       m_fRightHandRefSIN(iter);   // 6
+//        assert(f.size()==6     && "Unexpected size of signal fRightHand");
+//        assert(fRef.size()==6  && "Unexpected size of signal fRightHandRef");
+
+//        if(s.size()!=6)
+//          s.resize(6);
+//        s.tail<3>() = Kf.segment<3>(12).cwiseProduct(fRef.head<3>() - f.head<3>() );
+//        s.head<3>() = Kf.segment<3>(15).cwiseProduct(fRef.tail<3>() - f.tail<3>());
+//        return s;
+//      }
+
+//      DEFINE_SIGNAL_OUT_FUNCTION(fLeftHandError,dynamicgraph::Vector)
+//      {
+//        if(!m_initSucceeded)
+//        {
+//          SEND_MSG("Cannot compute signal fLeftHandError before initialization!", MSG_TYPE_WARNING_STREAM);
+//          return s;
+//        }
+
+//        const Eigen::Matrix<double,24,1>&  Kf =         m_KfSIN(iter); // 6*4
+//        const Vector6& f =           m_fLeftHandSIN(iter);      // 6
+//        const Vector6& fRef =        m_fLeftHandRefSIN(iter);   // 6
+//        assert(f.size()==6     && "Unexpected size of signal fLeftHand");
+//        assert(fRef.size()==6  && "Unexpected size of signal fLeftHandRef");
+
+//        if(s.size()!=6)
+//          s.resize(6);
+//	s.tail<3>() = Kf.segment<3>(18).cwiseProduct(fRef.head<3>() - f.head<3>() );
+//	s.head<3>() = Kf.segment<3>(21).cwiseProduct(fRef.tail<3>() - f.tail<3>());
+
+//        return s;
+//      }
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      /* ------------------------------------------------------------------- */
+      /* --- ENTITY -------------------------------------------------------- */
+      /* ------------------------------------------------------------------- */
+
+      void AdmittanceController::display(std::ostream& os) const
+      {
+        os << "AdmittanceController "<<getName();
+        try
+        {
+          getProfiler().report_all(3, os);
+        }
+        catch (ExceptionSignal e) {}
+      }
+
+
+      void AdmittanceController::commandLine(const std::string& cmdLine,
+                                            std::istringstream& cmdArgs,
+                                            std::ostream& os )
+      {
+        if( cmdLine == "help" )
+        {
+          os << "sotAdmittanceController:\n"
+              << "\t -." << std::endl;
+          Entity::commandLine(cmdLine, cmdArgs, os);
+        }
+        else
+        {
+          Entity::commandLine(cmdLine,cmdArgs,os);
+        }
+      }
+
+      //**************************************************************************************************
+      VectorXd svdSolveWithDamping(const JacobiSVD<MatrixXd>& A, const VectorXd &b, double damping)
+      {
+          eigen_assert(A.computeU() && A.computeV() && "svdSolveWithDamping() requires both unitaries U and V to be computed (thin unitaries suffice).");
+          assert(A.rows()==b.size());
+
+          //    cout<<"b = "<<toString(b,1)<<endl;