Merge branch 'devel' of gepgitlab.laas.fr:loco-3d/sot-talos-balance into devel

CMakeLists.txt

@@ -163,6 +163,7 @@ IF(BUILD_PYTHON_INTERFACE)
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_checkfeedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_checkfeedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_ffSubscriber.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_zmpEstimator.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComZmpControl.py
@@ -170,6 +171,8 @@ IF(BUILD_PYTHON_INTERFACE)
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint_velocity_based.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint_velocity_based.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.hh

@@ -62,7 +62,7 @@ namespace dynamicgraph {
 
         void init(const double & dt, const unsigned & n);
 
-        void setPosition(const dynamicgraph::Vector &);
+        void setPosition(const dynamicgraph::Vector & position);
 
         /* --- SIGNALS --- */
         DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
@@ -82,6 +82,7 @@ namespace dynamicgraph {
           getLogger().sendMsg("[AdmittanceController-"+name+"] "+msg, t, file, line);
         }
 
+        bool m_useState;
       protected:
         int m_n;
         bool m_initSucceeded;    /// true if the entity has been successfully initialized

python/sot_talos_balance/test/appli_admittance_single_joint_velocity_based.py

+from sot_talos_balance.create_entities_utils import create_joint_admittance_controller
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from sot_talos_balance.meta_task_joint import MetaTaskKineJoint
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+
+N_JOINTS = 32
+N_CONFIG = N_JOINTS + 6
+
+dt = robot.timeStep
+
+JOINT = 25
+QJOINT = JOINT + 6
+
+target = -10.0
+
+# --- Joint
+robot.taskJoint = MetaTaskKineJoint(robot.dynamic,QJOINT)
+robot.taskJoint.featureDes.errorIN.value = [0.0]
+robot.taskJoint.task.controlGain.value = 0
+robot.taskJoint.task.setWithDerivative(True)
+
+# --- Admittance controller
+Kp = [0.1]
+robot.admittance_control = create_joint_admittance_controller(JOINT,Kp,dt,robot)
+plug(robot.admittance_control.qRef,robot.taskJoint.featureDes.errorIN)
+plug(robot.admittance_control.dqRef,robot.taskJoint.featureDes.errordotIN)
+
+# --- 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)
+

python/sot_talos_balance/test/appli_ffSubscriber.py

+from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+
+from dynamic_graph.ros import RosSubscribe
+
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+
+dt = robot.timeStep;
+robot.comTrajGen = create_com_trajectory_generator(dt,robot);
+
+robot.subscriber = RosSubscribe("base_subscriber")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 10
+
+# --- 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)
+plug(robot.comTrajGen.x,    robot.taskCom.featureDes.errorIN)
+
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+robot.sot.push(robot.taskCom.task.name)
+robot.device.control.recompute(0)
+
+# --- TRACER
+robot.tracer = TracerRealTime("zmp_tracer")
+robot.tracer.setBufferSize(80*(2**20))
+robot.tracer.open('/tmp','dg_','.dat')
+robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+robot.device.after.addSignal('{0}.position'.format(robot.subscriber.name)) # force recalculation
+robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
+
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.subscriber, 'position')
+addTrace(robot.tracer, robot.subscriber, 'velocity')
+
+robot.tracer.start()
+

python/sot_talos_balance/test/test_admittance_single_joint_velocity_based.py

+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from time import sleep
+
+run_test('appli_admittance_single_joint_velocity_based.py')
+
+sleep(1.0)
+runCommandClient('robot.admittance_control.tauDes.value = [target]')
+runCommandClient('robot.sot.push(robot.taskJoint.task.name)')
+
+sleep(5.0)
+des_tau = evalCommandClient('target')
+tau = evalCommandClient('robot.device.ptorque.value[JOINT]')
+
+print("Desired torque: %f" % des_tau)
+print("Current torque: %f" % tau)
+

python/sot_talos_balance/test/test_ffSubscriber.py

-from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
-from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
-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 sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.utils.gazebo_utils import GazeboLinkStatePublisher
 from time import sleep
-import os
 
-from dynamic_graph.ros import RosSubscribe
-
-from dynamic_graph.tracer_real_time import TracerRealTime
-from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
 import matplotlib.pyplot as plt
 import numpy as np
 
-def main(robot):
-    dt = robot.timeStep;
-    robot.comTrajGen = create_com_trajectory_generator(dt,robot);
-
-    # --- COM
-    robot.taskCom = MetaTaskKineCom(robot.dynamic)
-    robot.dynamic.com.recompute(0)
-    robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
-    robot.taskCom.task.controlGain.value = 10
-
-    # --- 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.subscriber = RosSubscribe("base_subscriber")
-    robot.subscriber.add("vector","position","/sot/base_link/position")
-    robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
-
-    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.taskCom.task.name)
-    robot.sot.push(robot.contactLF.task.name)
-    robot.device.control.recompute(0)
-
-    # --- TRACER
-    robot.tracer = TracerRealTime("zmp_tracer")
-    robot.tracer.setBufferSize(80*(2**20))
-    robot.tracer.open('/tmp','dg_','.dat')
-    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
-    robot.device.after.addSignal('{0}.position'.format(robot.subscriber.name)) # force recalculation
-    robot.device.after.addSignal('{0}.velocity'.format(robot.subscriber.name)) # force recalculation
-
-    addTrace(robot.tracer, robot.dynamic, 'com')
-    addTrace(robot.tracer, robot.subscriber, 'position')
-    addTrace(robot.tracer, robot.subscriber, 'velocity')
-
-    plug(robot.comTrajGen.x,    robot.taskCom.featureDes.errorIN);
-
-    robot.tracer.start()
-
-    sleep(1.0);
-    os.system("rosservice call \start_dynamic_graph")
-    sleep(2.0);
-    robot.comTrajGen.move(1,-0.025,4.0);
-    sleep(20.0);
-    robot.comTrajGen.startSinusoid(1,0.05,8.0);
-    sleep(5.0);
-
-    dump_tracer(robot.tracer)
-
-	  # --- DISPLAY
-    com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
-    pos_data = np.loadtxt('/tmp/dg_'+robot.subscriber.name+'-position.dat')
-    vel_data = np.loadtxt('/tmp/dg_'+robot.subscriber.name+'-velocity.dat')
-
-    plt.figure()
-    plt.plot(com_data[:,1],'b-')
-    plt.plot(com_data[:,2],'r-')
-    plt.plot(com_data[:,3],'g-')
-    plt.title('COM')
-    plt.legend(['x','y','z'])
-
-    plt.figure()
-    plt.plot(pos_data[:,1],'b-')
-    plt.plot(pos_data[:,2],'r-')
-    plt.plot(pos_data[:,3],'g-')
-    plt.title('Position measure')
-    plt.legend(['x','y','z'])
-
-    plt.figure()
-    plt.plot(pos_data[:,4],'b-')
-    plt.plot(pos_data[:,5],'r-')
-    plt.plot(pos_data[:,6],'g-')
-    plt.title('Orientation measure')
-    plt.legend(['yaw','pitch','roll'])
-
-    plt.figure()
-    plt.plot(vel_data[:,1],'b-')
-    plt.plot(vel_data[:,2],'r-')
-    plt.plot(vel_data[:,3],'g-')
-    plt.title('Linear velocity measure')
-    plt.legend(['x','y','z'])
-
-    plt.figure()
-    plt.plot(vel_data[:,4],'b-')
-    plt.plot(vel_data[:,5],'r-')
-    plt.plot(vel_data[:,6],'g-')
-    plt.title('Angular velocity measure')
-    plt.legend(['x','y','z'])
-
-    plt.show()
+pub = GazeboLinkStatePublisher('base_link',1000)
+print("Starting Gazebo link state publisher...")
+pub.start()
+print("Gazebo link state publisher started")
+raw_input("Wait before running the test")
+
+run_test('appli_ffSubscriber.py')
+
+sleep(2.0)
+runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+sleep(5.0)
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+sleep(5.0)
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
+pos_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.subscriber.name') + '-position.dat')
+vel_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.subscriber.name') + '-velocity.dat')
+
+plt.ion()
+
+plt.figure()
+plt.plot(com_data[:,1],'b-')
+plt.plot(com_data[:,2],'r-')
+plt.plot(com_data[:,3],'g-')
+plt.title('COM')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(pos_data[:,1],'b-')
+plt.plot(pos_data[:,2],'r-')
+plt.plot(pos_data[:,3],'g-')
+plt.title('Position measure')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(pos_data[:,4],'b-')
+plt.plot(pos_data[:,5],'r-')
+plt.plot(pos_data[:,6],'g-')
+plt.title('Orientation measure')
+plt.legend(['yaw','pitch','roll'])
+
+plt.figure()
+plt.plot(vel_data[:,1],'b-')
+plt.plot(vel_data[:,2],'r-')
+plt.plot(vel_data[:,3],'g-')
+plt.title('Linear velocity measure')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(vel_data[:,4],'b-')
+plt.plot(vel_data[:,5],'r-')
+plt.plot(vel_data[:,6],'g-')
+plt.title('Angular velocity measure')
+plt.legend(['x','y','z'])
+
+raw_input("Wait before leaving the simulation")
+print("Stopping Gazebo link state publisher...")
+pub.stop()
+sleep(0.1)
+print("Gazebo link state publisher stopped")
 

src/admittance-controller.cpp

@@ -34,6 +34,9 @@ namespace dynamicgraph
       using namespace dg::command;
 
 //Size to be aligned                                   "-------------------------------------------------------"
+
+#define PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION  "AdmittanceController: qRef computation                 "
+
 #define PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "AdmittanceController: dqRef computation                "
 
 #define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_tauSIN << m_tauDesSIN
@@ -66,6 +69,8 @@ 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("isUsingExternalState", makeDirectGetter(*this,&m_useState, docDirectGetter("use external state","bool")));
       }
 
       void AdmittanceController::init(const double & dt, const unsigned & n)
@@ -74,8 +79,6 @@ namespace dynamicgraph
           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 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())
@@ -87,9 +90,10 @@ namespace dynamicgraph
         m_initSucceeded = true;
       }
 
-      void AdmittanceController::setPosition(const dynamicgraph::Vector & state)
+      void AdmittanceController::setPosition(const dynamicgraph::Vector & position)
+
       {
-        m_q = state;
+        m_q = position;
       }
 
       /* ------------------------------------------------------------------- */
@@ -110,7 +114,6 @@ namespace dynamicgraph
         const Vector & tau = m_tauSIN(iter);
         const Vector & Kp = m_KpSIN(iter);
 
-        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");
@@ -132,14 +135,30 @@ namespace dynamicgraph
           return s;
         }
 
+        getProfiler().start(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+
         const Vector & dqRef = m_dqRefSOUT(iter);
 
         assert(dqRef.size()==m_n  && "Unexpected size of signal dqRef");
 
+        if(m_useState)
+        {
+          if(!m_stateSIN.isPlugged())
+          {
+            SEND_MSG("Signal state is requested, but is not plugged", MSG_TYPE_ERROR);
+            return s;
+          }
+          const Vector & state = m_stateSIN(iter);
+          assert(state.size()==m_n  && "Unexpected size of signal state");
+          m_q = state;
+        }
+
         m_q += dqRef*m_dt;
 
         s = m_q;
 
+        getProfiler().stop(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+
         return s;
       }