diff --git a/src/OncillaL0.cpp b/src/OncillaL0.cpp
index e60c015..154f848 100644
--- a/src/OncillaL0.cpp
+++ b/src/OncillaL0.cpp
@@ -1,53 +1,54 @@
 #include "OncillaL0.h"
 
 using namespace std;
 
 namespace rci {
 namespace oncilla {
 
 OncillaL0::OncillaL0(const std::string &name)
 	: ResourceNode(name)
 	, Controlled()
 	, PositionControlled() {
 	// \todo add default value as a setting
+	_controlMode = modeJointPositionControl;
 	_lastCommandedPosition = JointAngles::create(1,0.0);
 }
 
 bool OncillaL0::isConverged() const {
 	return false;
 }
 
 void OncillaL0::unsafeSetCommand(double value){
 	this->_lastCommandedPosition->setValue(value);
 }
 
 double OncillaL0::unsafeGetCommand() const{
 	return this->_lastCommandedPosition->asDouble(0,false);
 }
 
 bool OncillaL0::setJointPosition(JointAnglesPtr position) {
 	// Successfull
 	this->_lastCommandedPosition = position;
 	return true;
 }
 
 JointAnglesPtr OncillaL0::getLastPositionCommand() const {
 	if (this->_lastCommandedPosition) {
 		return this->_lastCommandedPosition;
 	}
 
 	throw runtime_error("No position command received yet.");
 }
 
 OncillaL0::~OncillaL0() {
 }
 
 std::string OncillaL0::print() const {
 	ostringstream outstream(ostringstream::out);
 	outstream.precision(3); // Precision when printing double values
 	outstream << "<Oncilla Hip>" << endl;
 	return outstream.str();
 }
 
 }
 }
diff --git a/src/OncillaL1L2.cpp b/src/OncillaL1L2.cpp
index e705973..40b2bd0 100644
--- a/src/OncillaL1L2.cpp
+++ b/src/OncillaL1L2.cpp
@@ -1,108 +1,108 @@
 #include "OncillaL1L2.h"
 
 
 #include <stdexcept>
 
 using namespace std;
 
 namespace rci{
 namespace oncilla{
 OncillaL1L2::OncillaL1L2(OncillaSynchronizer &s, const std::string &name)
 	: ResourceNode(name)
 	, Controlled()
 	, Sensing()
 	, PositionControlled()
 	, PositionSensing()
 	, TorqueControlled()
 	, ImpedanceControlled()
 	, synchronizer(s) {
 	/// \todo add default value as setting
-
+	this->_controlMode = modeJointPositionControl;
 	this->_lastCommandedPosition = JointAngles::create(1,0.0);
 	this->_latestJointPosition = JointAngles::create(2,0.0);
 }
 
 bool OncillaL1L2::isConverged() const {
 	throw std::runtime_error("Not yet implemented.");
 	return false;
 }
 
 bool OncillaL1L2::setJointPosition(JointAnglesPtr position) {
 	// Successfull
 	this->_lastCommandedPosition = position;
 	return true;
 }
 
 JointAnglesPtr OncillaL1L2::getJointPosition() const {
 	return this->_latestJointPosition;
 }
 
 bool OncillaL1L2::setJointVelocity(JointVelocitiesPtr ) {
 	throw std::runtime_error("Not yet implemented.");
 }
 
 bool OncillaL1L2::setJointTorque(JointTorquesPtr ) {
 	throw std::runtime_error("Not yet implemented.");
 }
 
 bool OncillaL1L2::setJointImpedance(JointImpedancePtr ) {
 	if (this->synchronizer.tooBusy()) {
 		throw std::runtime_error(
 				"Synchronizer has to be switched off to change JointImpedance.");
 	} else {
 		throw std::runtime_error("Not yet implemented.");
 	}
 }
 
 JointAnglesPtr OncillaL1L2::getLastPositionCommand() const {
 	if (this->_lastCommandedPosition) {
 		return this->_lastCommandedPosition;
 	}
 
 	// If we don`t have a command yet, we return the latest sensor value
 	if (this->_latestJointPosition) {
 		// TODO: Log
 		return this->_latestJointPosition;
 	}
 
 	throw runtime_error("No position command received yet.");
 }
 
 OncillaL1L2::~OncillaL1L2() {
 }
 
 std::string OncillaL1L2::print() const {
 	ostringstream outstream(ostringstream::out);
 	outstream.precision(3); // Precision when printing double values
 	outstream << "<Oncilla Hip>" << endl;
 	return outstream.str();
 }
 
 
 void OncillaL1L2::unsafeSetCommand(double value){
 	this->_lastCommandedPosition->setValue(0,value);
 }
 
 double OncillaL1L2::unsafeGetCommand() const{
 	return this->_lastCommandedPosition->asDouble(0);
 }
 
 double OncillaL1L2::unsafeGetMotorAxisPosition() const{
 	return this->_latestJointPosition->asDouble(MotorAxisIndex);
 }
 
 double OncillaL1L2::unsafeGetMagneticEncoderPosition() const{
 	return this->_latestJointPosition->asDouble(MagneticEncoderIndex);
 }
 
 void OncillaL1L2::unsafeUpdateMotorAxisPosition(double value){
 	this->_latestJointPosition->setValue(MotorAxisIndex,value);
 }
 
 
 void OncillaL1L2::unsafeUpdateMagneticEncoderPosition(double value){
 	this->_latestJointPosition->setValue(MagneticEncoderIndex,value);
 }
 
 }
 }