From a30396af7bcbdb9db7341b51ad883195d106ca5d Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Fri, 7 Dec 2018 18:37:00 +0100
Subject: [PATCH] test joint control
---
CMakeLists.txt | 1 +
.../joint-position-controller.hh | 14 +++---
.../create_entities_utils.py | 9 +++-
.../test/test_jointControl.py | 35 +++++++++++++++
src/joint-position-controller.cpp | 43 +++++++++++++------
unittest/python/test_control.py | 20 ++++-----
6 files changed, 89 insertions(+), 33 deletions(-)
create mode 100644 python/sot_talos_balance/test/test_jointControl.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8b750c5..657a25e 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/include/sot/talos_balance/joint-position-controller.hh b/include/sot/talos_balance/joint-position-controller.hh
index 63113d5..52f1f9e 100644
--- a/include/sot/talos_balance/joint-position-controller.hh
+++ b/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
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index 2372170..079664f 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -1,11 +1,18 @@
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
diff --git a/python/sot_talos_balance/test/test_jointControl.py b/python/sot_talos_balance/test/test_jointControl.py
new file mode 100644
index 0000000..dd9f15c
--- /dev/null
+++ b/python/sot_talos_balance/test/test_jointControl.py
@@ -0,0 +1,35 @@
+#!/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)
diff --git a/src/joint-position-controller.cpp b/src/joint-position-controller.cpp
index 7202831..22b4e72 100644
--- a/src/joint-position-controller.cpp
+++ b/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);
diff --git a/unittest/python/test_control.py b/unittest/python/test_control.py
index 3a7af4a..4683767 100644
--- a/unittest/python/test_control.py
+++ b/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 )
-
--
GitLab