Admittance control single joint

CMakeLists.txt

@@ -144,6 +144,7 @@ IF(BUILD_PYTHON_INTERFACE)
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/run_test_utils.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py

include/sot/talos_balance/admittance-controller-single-joint.hh

@@ -60,15 +60,16 @@ namespace dynamicgraph {
         /* --- CONSTRUCTOR ---- */
         AdmittanceControllerSingleJoint( const std::string & name );
 
-        void init(const unsigned & n);
+        void init(const double & dt, const unsigned & n);
+
+        void setPosition(const dynamicgraph::Vector &);
 
         /* --- 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);
+        DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(tauDes, dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(qRef, dynamicgraph::Vector);
 
         /* --- COMMANDS --- */
         /* --- ENTITY INHERITANCE --- */
@@ -83,6 +84,8 @@ namespace dynamicgraph {
         int m_n;
         bool m_initSucceeded;    /// true if the entity has been successfully initialized
         dynamicgraph::Vector m_Kp;
+        dynamicgraph::Vector m_q; // internal state
+        double m_dt;
 
       }; // class AdmittanceControllerSingleJoint
 

python/sot_talos_balance/create_entities_utils.py

+from dynamic_graph.sot.core.operator import Mix_of_vector
 from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
 from sot_talos_balance.joint_position_controller import JointPositionController
+from sot_talos_balance.admittance_controller_single_joint import AdmittanceControllerSingleJoint
 
 from dynamic_graph import plug
 
 N_JOINTS = 32;
 
+def create_extend_mix(n_in,n_out):
+    assert n_out>n_in
+    mix_of_vector = Mix_of_vector( "mix " + str(n_in) + "-" + str(n_out) )
+
+    mix_of_vector.setSignalNumber(3)
+
+    n_diff = n_out-n_in
+    mix_of_vector.addSelec(1,0,n_diff)
+    mix_of_vector.addSelec(2,n_diff,n_in)
+
+    mix_of_vector.default.value=[0.0]*n_out
+    mix_of_vector.signal("sin1").value = [0.0]*n_diff
+    mix_of_vector.signal("sin2").value = [2.0]*n_in
+
+    return mix_of_vector
+
 def create_joint_trajectory_generator(dt):
     jtg = NdTrajectoryGenerator("jtg");
     jtg.initial_value.value = tuple(N_JOINTS*[0.0]);
@@ -31,3 +49,19 @@ def create_joint_controller(Kp):
     controller = JointPositionController("posctrl")
     controller.Kp.value = Kp
     return controller
+
+def create_admittance_controller(Kp,dt,robot):
+    controller = AdmittanceControllerSingleJoint("admctrl")
+    controller.Kp.value = Kp
+    plug(robot.device.state,controller.state)
+
+    mix = create_extend_mix(N_JOINTS,N_JOINTS+6)
+    plug(robot.device.currents,mix.signal("sin2"))
+    plug(mix.sout,controller.tau)
+
+    # plug(robot.device.ptorque,controller.tau)
+
+    controller.tauDes.value = [0.0]*(N_JOINTS+6)
+    controller.init(dt, N_JOINTS+6)
+    controller.setPosition(robot.device.state.value)
+    return controller

python/sot_talos_balance/test/test_admittance_single_joint.py

+from sot_talos_balance.create_entities_utils import create_admittance_controller
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+from numpy import eye
+from time import sleep
+import os
+
+N_JOINTS = 32
+N_CONFIG = N_JOINTS + 6
+
+def main(robot):
+    dt = robot.timeStep;
+
+    JOINT = 25
+    QJOINT = JOINT + 6
+
+    # --- Joint
+    robot.taskJoint = MetaTaskKineConfig(robot.dynamic,[QJOINT])
+    # robot.dynamic.com.recompute(0)
+    robot.taskJoint.featureDes.errorIN.value = N_CONFIG*[0.0]
+    robot.taskJoint.task.controlGain.value = 100
+
+    # --- Admittance controller
+    Kp = [0.1]*N_CONFIG
+    robot.admittance_control = create_admittance_controller(Kp,dt,robot)
+    plug(robot.admittance_control.qRef,robot.taskJoint.featureDes.errorIN)
+
+    # --- CONTACTS
+    #define contactLF and contactRF
+    robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+    robot.contactLF.feature.frame('desired')
+    robot.contactLF.gain.setConstant(100)
+    robot.contactLF.keep()
+    locals()['contactLF'] = robot.contactLF
+
+    robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+    robot.contactRF.feature.frame('desired')
+    robot.contactRF.gain.setConstant(100)
+    robot.contactRF.keep()
+    locals()['contactRF'] = robot.contactRF
+
+    robot.sot = SOT('sot')
+    robot.sot.setSize(robot.dynamic.getDimension())
+    plug(robot.sot.control,robot.device.control)
+
+    robot.sot.push(robot.contactRF.task.name)
+    robot.sot.push(robot.contactLF.task.name)
+    robot.device.control.recompute(0)
+
+    sleep(1.0)
+    os.system('rosservice call /start_dynamic_graph')
+    sleep(1.0)
+
+    target = N_CONFIG*[0.0]
+    target[QJOINT] = -10.0
+    robot.admittance_control.tauDes.value = target
+    robot.admittance_control.setPosition(robot.device.state.value)
+    robot.sot.push(robot.taskJoint.task.name)
+
+    sleep(5)
+    print("Desired torque: %f" % target[QJOINT])
+    print("Current torque: %f" % robot.device.currents.value[JOINT])
+

src/admittance-controller-single-joint.cpp

@@ -33,12 +33,12 @@ namespace dynamicgraph
       using namespace dg;
       using namespace dg::command;
 
-//Size to be aligned                                              "-------------------------------------------------------"
-#define PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION "AdmittanceControllerSingleJoint: dqRef computation     "
+//Size to be aligned                                             "-------------------------------------------------------"
+#define PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_QREF_COMPUTATION "AdmittanceControllerSingleJoint: qRef computation      "
 
-#define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_qDesSIN << m_dqDesSIN
+#define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_tauSIN << m_tauDesSIN
 
-#define OUTPUT_SIGNALS m_dqRefSOUT
+#define OUTPUT_SIGNALS m_qRefSOUT
 
       /// Define EntityClassName here rather than in the header file
       /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
@@ -55,63 +55,72 @@ namespace dynamicgraph
                       : 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)
+                      , CONSTRUCT_SIGNAL_IN(tau, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_IN(tauDes, dynamicgraph::Vector)
+                      , CONSTRUCT_SIGNAL_OUT(qRef, 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")));
+        addCommand("init", makeCommandVoid2(*this, &AdmittanceControllerSingleJoint::init, docCommandVoid2("Initialize the entity.","Control gains","time step")));
+        addCommand("setPosition", makeCommandVoid1(*this, &AdmittanceControllerSingleJoint::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
       }
 
-      void AdmittanceControllerSingleJoint::init(const unsigned & n)
+      void AdmittanceControllerSingleJoint::init(const double & dt, 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);
+          return SEND_MSG("Init failed: signal state is not plugged", MSG_TYPE_ERROR);
+        if(!m_tauSIN.isPlugged())
+          return SEND_MSG("Init failed: signal tau is not plugged", MSG_TYPE_ERROR);
+        if(!m_tauDesSIN.isPlugged())
+          return SEND_MSG("Init failed: signal tauDes is not plugged", MSG_TYPE_ERROR);
 
         m_n = n;
+        m_dt = dt;
+        m_q.setZero(n);
         m_initSucceeded = true;
       }
 
+      void AdmittanceControllerSingleJoint::setPosition(const dynamicgraph::Vector & state)
+      {
+        m_q = state;
+      }
+
       /* ------------------------------------------------------------------- */
       /* --- SIGNALS ------------------------------------------------------- */
       /* ------------------------------------------------------------------- */
 
-      DEFINE_SIGNAL_OUT_FUNCTION(dqRef, dynamicgraph::Vector)
+      DEFINE_SIGNAL_OUT_FUNCTION(qRef, dynamicgraph::Vector)
       {
         if(!m_initSucceeded)
         {
-          SEND_WARNING_STREAM_MSG("Cannot compute signal dqRef before initialization!");
+          SEND_WARNING_STREAM_MSG("Cannot compute signal qRef before initialization!");
           return s;
         }
 
-        getProfiler().start(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
+        getProfiler().start(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_QREF_COMPUTATION);
 
-        const Vector & state = m_stateSIN(iter);
-        const Vector & qDes = m_qDesSIN(iter);
-        const Vector & dqDes = m_dqDesSIN(iter);
+        const Vector & tauDes = m_tauDesSIN(iter);
+        const Vector & tau = m_tauSIN(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");
+        assert(state.size()==m_n  && "Unexpected size of signal state");
+        assert(tau.size()==m_n    && "Unexpected size of signal tau");
+        assert(tauDes.size()==m_n && "Unexpected size of signal tauDes");
+        assert(Kp.size()==m_n     && "Unexpected size of signal Kp");
+
+        const Vector & dqRef = Kp.cwiseProduct(tauDes-tau);
 
-        const Vector & q = state.tail(m_n);
+        m_q += dqRef*m_dt;
 
-        s = dqDes + Kp.cwiseProduct(qDes-q);
+        s = m_q;
 
-        getProfiler().stop(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
+        getProfiler().stop(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_QREF_COMPUTATION);
 
         return s;
       }

unittest/python/test_admittance.py

+from __future__ import print_function
+from sot_talos_balance.admittance_controller_single_joint import AdmittanceControllerSingleJoint
+
+controller = AdmittanceControllerSingleJoint("ciao")
+
+print("Commands:")
+print(controller.commands())
+
+print("\nSignals (at creation):")
+controller.displaySignals()
+
+N_JOINTS = 2
+
+controller.Kp.value = tuple(N_JOINTS*[10.0])
+controller.state.value = tuple([0.0]*6 + N_JOINTS*[0.0])
+controller.tauDes.value = tuple(N_JOINTS*[0.0])
+controller.tau.value = tuple(N_JOINTS*[0.0])
+
+print( "\nKp:   %s" % (controller.Kp.value,) )
+print( "\nq:    %s" % (controller.state.value,) )
+print( "tauDes: %s" % (controller.tauDes.value,) )
+print( "tau:    %s" % (controller.tau.value,) )
+
+q = N_JOINTS*[1.0]
+dt = 1
+
+controller.init(dt,N_JOINTS)
+
+controller.setPosition(q)
+controller.qRef.recompute(0)
+
+print( "\nqRef: %s" % (controller.qRef.value,) )
+
+controller.tauDes.value = tuple(N_JOINTS*[1.0])
+
+controller.qRef.recompute(1)
+
+print( "\nqRef: %s" % (controller.qRef.value,) )