diff --git a/CMakeLists.txt b/CMakeLists.txt
index f1efc417b7f172a00e86dc542811a9f65aa45f78..ecefd38e4b754395dee53861f83fbd3d982a5e65 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/sot/talos_balance/admittance-controller-single-joint.hh b/include/sot/talos_balance/admittance-controller-single-joint.hh
index 7b45c9704252027fe2fd08ece4be7ad7f218fb4f..120d7472688a0283142c73a88e5cef9454e318ae 100644
--- a/include/sot/talos_balance/admittance-controller-single-joint.hh
+++ b/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
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index 3cf42991e0ff928e00cdb17436e79a3d9545e637..b8d6e3bd634a3026ad45c1f7c81cb515b32cb1cc 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -1,10 +1,28 @@
+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
diff --git a/python/sot_talos_balance/test/test_admittance_single_joint.py b/python/sot_talos_balance/test/test_admittance_single_joint.py
new file mode 100644
index 0000000000000000000000000000000000000000..990da50467ed9eacd6fc9978786e416ce6a8fe91
--- /dev/null
+++ b/python/sot_talos_balance/test/test_admittance_single_joint.py
@@ -0,0 +1,66 @@
+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])
+
diff --git a/src/admittance-controller-single-joint.cpp b/src/admittance-controller-single-joint.cpp
index 1e7a083a5155b105ea1893818f78904d51b58e63..16fcc4afcb20026da3b713432489f48a5ab4a18b 100644
--- a/src/admittance-controller-single-joint.cpp
+++ b/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;
}
diff --git a/unittest/python/test_admittance.py b/unittest/python/test_admittance.py
new file mode 100644
index 0000000000000000000000000000000000000000..f0438056b9dfaf87c725f28f5e254d254ed1d439
--- /dev/null
+++ b/unittest/python/test_admittance.py
@@ -0,0 +1,38 @@
+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,) )