[zmp estimator] Add get and set force threshold

include/sot/talos_balance/admittance-controller.hh

@@ -69,8 +69,8 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(tauDes, dynamicgraph::Vector);
 
-      DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
-      DECLARE_SIGNAL_OUT(qRef, dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(qRef, dynamicgraph::Vector);
 
         /* --- COMMANDS --- */
         /* --- ENTITY INHERITANCE --- */

include/sot/talos_balance/simple-zmp-estimator.hh

@@ -75,6 +75,8 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(emergencyStop, bool);
 
+        double m_eps;
+
         /* --- COMMANDS --- */
         /* --- ENTITY INHERITANCE --- */
         virtual void display( std::ostream& os ) const;
@@ -84,7 +86,6 @@ namespace dynamicgraph {
       protected:
         bool m_initSucceeded;    /// true if the entity has been successfully initialized
         bool m_emergency_stop_triggered;
-        double m_eps;
 
       }; // class SimpleZmpEstimator
 

src/admittance-controller.cpp

@@ -69,7 +69,7 @@ namespace dynamicgraph
         /* Commands. */
         addCommand("init", makeCommandVoid2(*this, &AdmittanceController::init, docCommandVoid2("Initialize the entity.","time step","Number of elements")));
         addCommand("setPosition", makeCommandVoid1(*this, &AdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
-        addCommand("useExternalState", makeDirectSetter(*this,&m_useState, docDirectGetter("use external state","bool")));
+        addCommand("useExternalState", makeDirectSetter(*this,&m_useState, docDirectSetter("use external state","bool")));
         addCommand("isUsingExternalState", makeDirectGetter(*this,&m_useState, docDirectGetter("use external state","bool")));
       }
 

src/simple-zmp-estimator.cpp

@@ -63,13 +63,15 @@ namespace dynamicgraph
                       , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSIN << m_poseRightSIN)
                       , CONSTRUCT_SIGNAL_OUT(zmp, dynamicgraph::Vector, m_wrenchLeftSIN << m_wrenchRightSIN << m_copLeftSOUT << m_copRightSOUT)
                       , CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpSOUT)
+                      , m_eps(eps)
                       , m_initSucceeded(false)
       {
         Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
 
         /* Commands. */
         addCommand("init", makeCommandVoid0(*this, &SimpleZmpEstimator::init, docCommandVoid0("Initialize the entity.")));
-        m_eps = eps;
+        addCommand("getForceThreshold", makeDirectGetter(*this,&m_eps, docDirectGetter("Get force threshold","double")));
+        addCommand("setForceThreshold", makeDirectSetter(*this,&m_eps, docDirectSetter("Set force threshold","double")));
       }
 
       void SimpleZmpEstimator::init()