test joint control

CMakeLists.txt

@@ -143,6 +143,7 @@ IF(BUILD_PYTHON_INTERFACE)
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py
                   DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/test)
   ENDIF(BUILD_TEST)
 ENDIF(BUILD_PYTHON_INTERFACE)

include/sot/talos_balance/joint-position-controller.hh

@@ -60,14 +60,15 @@ namespace dynamicgraph {
         /* --- CONSTRUCTOR ---- */
         JointPositionController( const std::string & name );
 
-        void init(const Eigen::VectorXd & Kp);
+        void init(const unsigned & n);
 
         /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(q, Eigen::VectorXd);
-        DECLARE_SIGNAL_IN(qDes, Eigen::VectorXd);
-        DECLARE_SIGNAL_IN(dqDes, Eigen::VectorXd);
+        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, Eigen::VectorXd);
+        DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
 
         /* --- COMMANDS --- */
         /* --- ENTITY INHERITANCE --- */
@@ -79,8 +80,9 @@ namespace dynamicgraph {
         }
 
       protected:
+        int m_n;
         bool m_initSucceeded;    /// true if the entity has been successfully initialized
-        Eigen::VectorXd m_Kp;
+        dynamicgraph::Vector m_Kp;
 
       }; // class JointPositionController
 

python/sot_talos_balance/create_entities_utils.py

 from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
+from sot_talos_balance.joint_position_controller import JointPositionController
+
 from dynamic_graph import plug
 
 N_JOINTS = 32;
 
 def create_joint_trajectory_generator(dt):
     jtg = NdTrajectoryGenerator("jtg");
-    jtg.initial_value.value = tuple(32*[0.0]);
+    jtg.initial_value.value = tuple(N_JOINTS*[0.0]);
     jtg.trigger.value = 1.0;
     jtg.init(dt, N_JOINTS);
     return jtg;
+
+def create_joint_controller(Kp):
+    controller = JointPositionController("posctrl")
+    controller.Kp.value = Kp
+    return controller

python/sot_talos_balance/test/test_jointControl.py

+#!/usr/bin/python
+# -*- coding: utf-8 -*-1
+from sot_talos_balance.create_entities_utils import create_joint_trajectory_generator
+from sot_talos_balance.create_entities_utils import create_joint_controller
+from dynamic_graph import plug
+from time import sleep
+import os
+
+def main(robot,gain):
+    N_JOINTS = 32
+    Kp = N_JOINTS*[gain]
+    dt = robot.timeStep
+
+    robot.traj_gen   = create_joint_trajectory_generator(dt)
+    robot.device.control.value = N_JOINTS*[0.0]
+
+    robot.controller = create_joint_controller(Kp)
+
+    plug(robot.traj_gen.x,    robot.controller.qDes)
+    plug(robot.traj_gen.dx,   robot.controller.dqDes)
+
+    plug(robot.device.state,    robot.controller.state)
+
+    plug(robot.controller.dqRef,    robot.device.control)
+
+    robot.controller.init(N_JOINTS)
+
+    sleep(1.0)
+    os.system('rosservice call /start_dynamic_graph')
+    sleep(1.0)
+    robot.traj_gen.move(31,-1.0,1.0)
+    sleep(1.1)
+    robot.traj_gen.startSinusoid(31,3.0,2.0)
+    sleep(10.0)
+    robot.traj_gen.stop(31)

src/joint-position-controller.cpp

@@ -36,7 +36,7 @@ namespace dynamicgraph
 //Size to be aligned                                      "-------------------------------------------------------"
 #define PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION "JointPositionController: dqRef computation             "
 
-#define INPUT_SIGNALS     m_qSIN << m_qDesSIN << m_dqDesSIN
+#define INPUT_SIGNALS     m_KpSIN << m_stateSIN << m_qDesSIN << m_dqDesSIN
 
 #define OUTPUT_SIGNALS m_dqRefSOUT
 
@@ -53,10 +53,11 @@ namespace dynamicgraph
       /* ------------------------------------------------------------------- */
       JointPositionController::JointPositionController(const std::string& name)
                       : Entity(name)
-                      , CONSTRUCT_SIGNAL_IN(q, Eigen::VectorXd)
-                      , CONSTRUCT_SIGNAL_IN(qDes, Eigen::VectorXd)
-                      , CONSTRUCT_SIGNAL_IN(dqDes, Eigen::VectorXd)
-                      , CONSTRUCT_SIGNAL_OUT(dqRef, Eigen::VectorXd, INPUT_SIGNALS)
+                      , 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 );
@@ -65,14 +66,20 @@ namespace dynamicgraph
         addCommand("init", makeCommandVoid1(*this, &JointPositionController::init, docCommandVoid1("Initialize the entity.","Control gains")));
       }
 
-      void JointPositionController::init(const Eigen::VectorXd & Kp)
+      void JointPositionController::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 firstAddend is not plugged", MSG_TYPE_ERROR);
+          return SEND_MSG("Init failed: signal qDes is not plugged", MSG_TYPE_ERROR);
         if(!m_dqDesSIN.isPlugged())
-          return SEND_MSG("Init failed: signal secondAddend is not plugged", MSG_TYPE_ERROR);
+          return SEND_MSG("Init failed: signal dqDes is not plugged", MSG_TYPE_ERROR);
 
-        m_Kp = Kp;
+        m_n = n;
         m_initSucceeded = true;
       }
 
@@ -80,7 +87,7 @@ namespace dynamicgraph
       /* --- SIGNALS ------------------------------------------------------- */
       /* ------------------------------------------------------------------- */
 
-      DEFINE_SIGNAL_OUT_FUNCTION(dqRef, Eigen::VectorXd)
+      DEFINE_SIGNAL_OUT_FUNCTION(dqRef, dynamicgraph::Vector)
       {
         if(!m_initSucceeded)
         {
@@ -90,11 +97,19 @@ namespace dynamicgraph
 
         getProfiler().start(PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
 
-        Eigen::VectorXd q = m_qSIN(iter);
-        Eigen::VectorXd qDes = m_qDesSIN(iter);
-        Eigen::VectorXd dqDes = m_dqDesSIN(iter);
+        const Vector & state = m_stateSIN(iter);
+        const Vector & qDes = m_qDesSIN(iter);
+        const Vector & dqDes = m_dqDesSIN(iter);
+        const Vector & Kp = m_KpSIN(iter);
 
-        s = dqDes + m_Kp.cwiseProduct(qDes-q);
+        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_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
 

unittest/python/test_control.py

@@ -9,24 +9,20 @@ print(controller.commands())
 print("\nSignals (at creation):")
 controller.displaySignals()
 
-controller.q.value = (0.0,0.0)
-controller.qDes.value = (1.0,1.0)
-controller.dqDes.value = (0.0,0.0)
+N_JOINTS = 2
 
-Kp = (10.0,10.0)
+controller.Kp.value = tuple(N_JOINTS*[10.0])
+controller.state.value = tuple([0.0]*6 + N_JOINTS*[0.0])
+controller.qDes.value = tuple(N_JOINTS*[1.0])
+controller.dqDes.value = tuple(N_JOINTS*[0.0])
 
-controller.init(Kp)
+controller.init(N_JOINTS)
 
 controller.dqRef.recompute(1)
 
-print( "\nKp: %s" % (Kp,) )
-
-print( "\nq: %s" % (controller.q.value,) )
+print( "\nKp: %s" % (controller.Kp.value,) )
+print( "\nq: %s" % (controller.state.value,) )
 print( "qDes: %s" % (controller.qDes.value,) )
 print( "dqDes: %s" % (controller.dqDes.value,) )
 
 print( "\ndqRef: %s" % (controller.dqRef.value,) )
-
-#print( "\nInputs: %f, %f" % (ex.firstAddend.value, ex.secondAddend.value) )
-#print( "Output: %f" % ex.sum.value )
-