diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2f99a969ea560724d139a1bb81d46b405981cc3d..a9d5c834a76b8c037b820257a143991ea4f25c08 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/sot/talos_balance/admittance-controller.hh b/include/sot/talos_balance/admittance-controller.hh
index 1699238fae707f8b4079cc0be0bebfffea451947..c3f97a48fa32f172bd915137c8c26d024787bfd7 100644
--- a/include/sot/talos_balance/admittance-controller.hh
+++ b/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
diff --git a/python/sot_talos_balance/test/appli_admittance_single_joint_velocity_based.py b/python/sot_talos_balance/test/appli_admittance_single_joint_velocity_based.py
new file mode 100644
index 0000000000000000000000000000000000000000..9d2f2c543612d25172e212f32a63cb98c47d9919
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_admittance_single_joint_velocity_based.py
@@ -0,0 +1,50 @@
+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)
+
diff --git a/python/sot_talos_balance/test/appli_ffSubscriber.py b/python/sot_talos_balance/test/appli_ffSubscriber.py
new file mode 100644
index 0000000000000000000000000000000000000000..24eada78a5524e6316e0d3b97230a5a1f7770ff9
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_ffSubscriber.py
@@ -0,0 +1,62 @@
+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()
+
diff --git a/python/sot_talos_balance/test/test_admittance_single_joint_velocity_based.py b/python/sot_talos_balance/test/test_admittance_single_joint_velocity_based.py
new file mode 100644
index 0000000000000000000000000000000000000000..ad68a4216b8b6c5339a9d9f2c7bc1c34310985d0
--- /dev/null
+++ b/python/sot_talos_balance/test/test_admittance_single_joint_velocity_based.py
@@ -0,0 +1,16 @@
+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)
+
diff --git a/python/sot_talos_balance/test/test_ffSubscriber.py b/python/sot_talos_balance/test/test_ffSubscriber.py
index 8b038cd5325f6eb016d8e78e620bc80d52f10664..40acd0a429fc7be4e221abdf79ac3e6d62e76129 100644
--- a/python/sot_talos_balance/test/test_ffSubscriber.py
+++ b/python/sot_talos_balance/test/test_ffSubscriber.py
@@ -1,121 +1,70 @@
-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")
diff --git a/src/admittance-controller.cpp b/src/admittance-controller.cpp
index f38322ea1c39585d83e34241bdf42f7f5c05c955..d144c2fd99bbae7b36a9eb639b04720c3d3636eb 100644
--- a/src/admittance-controller.cpp
+++ b/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;
}