diff --git a/src/liboncilla-hw/synchronization/SBCPQueue.cpp b/src/liboncilla-hw/synchronization/SBCPQueue.cpp
index 37d5a93..4594cfb 100644
--- a/src/liboncilla-hw/synchronization/SBCPQueue.cpp
+++ b/src/liboncilla-hw/synchronization/SBCPQueue.cpp
@@ -1,314 +1,292 @@
 /***
  * \file SBCPQueue.cpp
  *
  *  \date Jun 27, 2013
  *  \author Alexandre Tuleu
  */
 
 #include "SBCPQueue.h"
 
 #include <biorob-cpp/log/Logger.h>
 
 #include <libsbcp/utils/Config.h>
 
 #include <liboncilla-hw/config/Config.h>
 
 namespace liboncilla {
 namespace hw {
 
 SBCPQueue::SBCPQueue(const Config & config) 
 	: Queue(config.Main().SBCPPriority(), true)
 	, d_bus() {
 	sbcp::Config sbcpConfig;
 	sbcpConfig.LoadAllFiles();
 
 	// create links to the 4 motorboards
 	unsigned int requiredFrameSize = 4;   // 4 consecutive packets send at oncefrom roboard to master communication board
 	unsigned int requiredPacketSize = 32; // Size of the biggest packet you will send, the actual maximum might be higher
 
 	d_bus = sbcpConfig.OpenDefaultBusWithFrame(requiredFrameSize,
 	                                           requiredPacketSize);
 
 	if (!d_bus) {
 		throw std::runtime_error("SBCPQueue::SBCPQueue() Can't load bus due to missing configuration.");
 	} else {
 		log(debug, "Bus Workflow: ", d_bus->CurrentWorkflow());
 	}
 
 	const MotorDriverGroup & devices = config.Motors().Devices();
 	const BrushlessParameterGroup & motorConfig = config.Motors().Params();
 	int16_t timestep = config.Main().Timestep();
 
 	d_motordrivers[rci::oncilla::LEFT_FORE] = 
 		OpenAndConfigureMotorDriver(devices.LeftFore(), motorConfig, timestep);
 	d_motordrivers[rci::oncilla::RIGHT_FORE] = 
 		OpenAndConfigureMotorDriver(devices.RightFore(), motorConfig, timestep);
 	d_motordrivers[rci::oncilla::LEFT_HIND] = 
 		OpenAndConfigureMotorDriver(devices.LeftHind(), motorConfig, timestep);
 	d_motordrivers[rci::oncilla::RIGHT_HIND] = 
 		OpenAndConfigureMotorDriver(devices.RightHind(), motorConfig, timestep);
 
 }
 
 SBCPQueue::~SBCPQueue() {
 }
 
 void SBCPQueue::DownstreamData() {
 	for (MotorAndEncoderByL1L2::const_iterator motAndEnc = d_motAndEncByL1L2.begin(); 
 	     motAndEnc != d_motAndEncByL1L2.end();
 	     ++motAndEnc) {
 		motAndEnc->second.motor->GoalPosition().Set(motAndEnc->first->nodeToQueueJointPosition());
 	}
 }
 
 void SBCPQueue::UpstreamData() {
 	for (MotorAndEncoderByL1L2::const_iterator motAndEnc = d_motAndEncByL1L2.begin(); 
 	     motAndEnc != d_motAndEncByL1L2.end();
 	     ++motAndEnc) {
 		uint16_t pos(motAndEnc->second.encoder->PositionAndStatus().Get() & 0x3fff);
 		uint16_t status((motAndEnc->second.encoder->PositionAndStatus().Get() & 0xc000) >> 14);
 		int16_t motPos(motAndEnc->second.motor->PresentPosition().Get());
 		motAndEnc->first->queueToNodeJointPosition(pos, status, motPos);
 	}
 
 	for (MotorDriverByL0::const_iterator mdv = d_mdvByL0.begin();
 	     mdv != d_mdvByL0.end();
 	     ++mdv) {
 		if (mdv->second == NULL) {
 			// TODO: Throw? std::cout << "SBCPQueue::InitializeIO: Ignoring motor driver "<<md->second<<std::endl;
 		} else {
 			mdv->first->queueToNodeForces(mdv->second->Force(0).Get(),
 			                              mdv->second->Force(1).Get(), 
 			                              mdv->second->Force(2).Get());
 		}
 	}
 
 	for (MagneticEncoderByL3::const_iterator enc = d_encByL3.begin();
 	     enc != d_encByL3.end(); 
 	     ++enc) {
 		if (enc->second == NULL) {
 			// TODO: Throw? std::cout << "SBCPQueue::InitializeIO: Ignoring motor driver "<<md->second<<std::endl;
 		} else {
 			enc->first->queueToNodeJointPosition(enc->second->PositionAndStatus().Get() & 0x3fff,
 			                                     (enc->second->PositionAndStatus().Get() & 0xc000) >> 14);
 		}
 	}
 }
 
 void SBCPQueue::PerformIO() {
 	sbcp::ScheduledWorkflow & w = this->d_bus->Scheduled();
 
 	// Start transfer
 	w.StartTransfers();
 
 	// Wait for transfer to complete
 	w.WaitForTransfersCompletion();
 }
 
 void SBCPQueue::InitializeIO() {
 	// Enable scheduled woirkflow
 	sbcp::ScheduledWorkflow & w = this->d_bus->Scheduled();
 
 	// Append devices to transfer
 	for (MotordriverByLeg::iterator md = d_motordrivers.begin();
 	     md != d_motordrivers.end(); 
 	     ++md) {
 
 		if (md->second == NULL) {
 			log(debug, "SBCPQueue::InitializeIO: Ignoring motor driver ",
 			    LegPrefix(md->first) );
 		} else {
 			log( debug,  "Appending scheduled device " , LegPrefix(md->first) );
 			w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device>(md->second));
 		}
 	}
 }
 
 void SBCPQueue::DeinitializeIO() {
 	// Disable scheduled woirkflow
 	this->d_bus->Lazy();
 }
 
 void SBCPQueue::RegisterL0(rci::oncilla::Leg l, const L0::Ptr & node) {
 	// check if this map contains this leg
 	MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
 	if (fi == d_motordrivers.end()) {
 		throw(std::runtime_error("Failed to open motordriver, upon initialization of this Queue, for motordriver "
 		                         + LegPrefix(l)));
 	}
 
 	d_mdvByL0[node] = fi->second;
 }
 
 void SBCPQueue::RegisterL1(rci::oncilla::Leg l, const L1L2::Ptr & node) {
 	bool isLeftLeg =
 			((l == rci::oncilla::LEFT_FORE) || (l == rci::oncilla::LEFT_HIND)) ?
 					true : false;
 	bool isHip = true;
 
 	//check if this map contains this leg
 	MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
-	if (fi == d_motordrivers.end()) {
+	if (fi == d_motordrivers.end() || !fi->second) {
 		throw(std::runtime_error("Failed to open motordriver earlier, upon initialization of this Queue, for motordriver "
 		                         + LegPrefix(l)));
 	}
 
-	if (fi->second == NULL) {
-		// TODO: Throw?
-		log(debug, "SBCPQueue::RegisterL1: Ignoring L1 of leg ",
-		    LegPrefix(l),". Is it connected?");
-		return;
-	}
-
 	node->initialize(isLeftLeg, isHip,
 	                 fi->second->Motor1().PositionLimit().Get() & 0x7fff,
 	                 fi->second->Motor1().PresentPosition().Get());
 	
 	/// \todo hardcoded mapping, could be configurable
 	MotorAndEncoder m1;
 	m1.motor = &(fi->second->Motor1());
 	m1.encoder = &(fi->second->Q1());
 
 	d_motAndEncByL1L2[node] = m1;
 }
 
 void SBCPQueue::RegisterL2(rci::oncilla::Leg l, const L1L2::Ptr & node) {
 	bool isRightLeg = ((l == rci::oncilla::RIGHT_FORE) || (l == rci::oncilla::RIGHT_HIND)) ? true 
 	                                                                                       : false;
 	bool isHip = false;
 
 	//check if this map contains this leg
 	MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
-	if (fi == d_motordrivers.end()) {
+	if (fi == d_motordrivers.end() || !fi->second) {
 		throw(std::runtime_error("Failed to open motordriver earlier, upon initialization of this Queue, for motordriver "
 		                         + LegPrefix(l)));
 	}
 
-	if (fi->second == NULL) {
-		// TODO: Throw?
-		log(debug, "SBCPQueue::RegisterL2: Ignoring L2 of leg ", LegPrefix(l), ". Is it connected?");
-		return;
-	}
 
 	node->initialize(isRightLeg, 
 	                 isHip,
 	                 fi->second->Motor2().PositionLimit().Get() & 0x7fff,
 	                 fi->second->Motor2().PresentPosition().Get());
 
 	/// \todo hardcoded mapping, could be configurable
 	MotorAndEncoder m2;
 	m2.motor = &(fi->second->Motor2());
 	m2.encoder = &(fi->second->Q2());
 
 	d_motAndEncByL1L2[node] = m2;
 }
 
 void SBCPQueue::RegisterL3(rci::oncilla::Leg l, const L3::Ptr & node) {
 	//check if this map contains this leg
 	MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
-	if (fi == d_motordrivers.end()) {
+	if (fi == d_motordrivers.end() || !fi->second) {
 		throw(std::runtime_error("Failed to open motordriver, upon initialization of this Queue, for motordriver "
 		                         + LegPrefix(l)));
 	}
 
-	if (fi->second == NULL) {
-		// TODO: Throw?
-		log(debug, "SBCPQueue::RegisterL3: Ignoring L3 of leg ",
-		    LegPrefix(l), ". Is it connected?");
-		return;
-	}
-
 	d_encByL3[node] = &(fi->second->Q3());
 }
 
 sbcp::amarsi::MotorDriver::Ptr 
 SBCPQueue::OpenAndConfigureMotorDriver(const MotorDriverSection & def, 
                                        const BrushlessParameterGroup & params,
                                        int16_t expectedTsInMs) {
 	log(debug, "SBCPQueue::OpenAndConfigureMotorDriver()");
-
-    this->d_bus->Lazy();
+	
 
 	if (!d_bus) {
-		throw std::runtime_error("SBCPQueue::OpenAndConfigureMotorDriver() Can't"
-		                         " load bus due to missing configuration.");
-	} else {
-		log(debug, "SBCPQueue::OpenAndConfigureMotorDriver() Bus Workflow: ",
-		    d_bus->CurrentWorkflow());
+		throw std::logic_error("SBCPQueue::d_bus private member should be initialized in"
+		                       "constructor. Please report this internal bug");
 	}
 
+    this->d_bus->Lazy();
+
 	sbcp::amarsi::MotorDriver::Ptr res = d_bus->OpenDevice< sbcp::amarsi::MotorDriver > (def.BoardID());
 
 	if (!res) {
 		// board is just not here, buit we will throw later if it is required.
-		log(debug, "SBCPQueue::OpenAndConfigureMotorDriver() Ignoring board, ",
-		    "doesn't seem to be connected.");
+		log(debug, "Could not find an AMARSi motordriver board with ID ", (int)def.BoardID(), "on the bus");
 		return res;
 	}
 
     bool m1Calibrated(GetCalibrationStatus(res->Motor1(),1));
 	bool m2Calibrated(GetCalibrationStatus(res->Motor2(),2));
 	if(! (m1Calibrated || m2Calibrated)){
 		res->CalibrateMotors();
 	} else {
 		log(debug, "Motor already calibrated");
 	}
 
 	while(!(m1Calibrated && m2Calibrated)){
 		usleep(1e5);
 		m1Calibrated = GetCalibrationStatus(res->Motor1(), 1);
 		m2Calibrated = GetCalibrationStatus(res->Motor2(), 2);
 	}
-	lohg(debug, "Motor calibrated");
+	log(debug, "Motor calibrated");
 
 	SetMotorParameters(params, def.M1Params(), expectedTsInMs, res->Motor1());
 	SetMotorParameters(params, def.M2Params(), expectedTsInMs, res->Motor2());
 
 
 	res->Motor1().MaxTorque().Set(300); // 300
 	res->Motor1().MaxSpeed().Set(20000); // 32000
 	res->Motor1().MaxAcceleration().Set(400); // 800
 
 	res->Motor2().MaxTorque().Set(300);
 	res->Motor2().MaxSpeed().Set(20000);
 	res->Motor2().MaxAcceleration().Set(400);
 
 	return res;
 }
 
 void SBCPQueue::SetMotorParameters(const BrushlessParameterGroup & paramGroup,
                                    const std::string & paramName, int16_t expectedTsInMs,
                                    sbcp::amarsi::MotorDriver::Motor & motor) {
 	log(debug, "SBCPQueue::SetMotorParameters()" );
 	//due to a bad design of biorob-cpp dynamic section we should do this
 	std::tr1::shared_ptr<BrushlessParameterSection> params =
 		const_cast<BrushlessParameterGroup &>(paramGroup).SubSection(paramName);
 	
 	if (!params) {
 		throw std::runtime_error("Unknown motor parameter group '" + paramName + "'");
 	}
 
 	motor.MotorControlMode().Set(sbcp::amarsi::MotorDriver::Motor::SMOOTH_POSITION);
 
 	motor.PGain().Set(params->PGain());
 	motor.IGain().Set(params->IGain());
 	motor.DGain().Set(params->DGain());
 
 	motor.Stiffness().Set(params->Stiffness());
 	motor.Damping().Set(params->Damping());
 	motor.Preload().Set(params->PreCompression());
 
 	motor.MaxTorque().Set(params->MaxTorque());
 	motor.MaxSpeed().Set(params->MaxSpeed());
 	motor.MaxAcceleration().Set(params->MaxAcceleration());
 
 	motor.SmoothPositionUpdate().Set(expectedTsInMs);
 }
 
 bool SBCPQueue::GetCalibrationStatus(sbcp::amarsi::MotorDriver::Motor & m, int number) {
 	uint16_t val(m.MotorControlMode().Get());
 	return val & (1 << 9);
 }
 
 } /* namespace hw */
 } /* namespace liboncilla */
diff --git a/src/liboncilla-hw/synchronization/Synchronizer.cpp b/src/liboncilla-hw/synchronization/Synchronizer.cpp
index d64e276..cd081c0 100644
--- a/src/liboncilla-hw/synchronization/Synchronizer.cpp
+++ b/src/liboncilla-hw/synchronization/Synchronizer.cpp
@@ -1,219 +1,217 @@
 #include "Synchronizer.h"
 #include <sys/mman.h>
 #include <signal.h>
 #include <errno.h>
 
 #include <liboncilla-hw/xenomai-utils/Utils.h>
 
 #include <liboncilla-hw/config/Config.h>
 
 namespace liboncilla {
 namespace hw {
 
 Synchronizer::Synchronizer(const Config & config)
 	: rci::oncilla::Synchronizer("Oncilla HW Synchronizer")
 	, d_timestep(config.Main().Timestep() * 1.0e-3)
 	, d_priority(config.Main().MainPriority())
 	, d_sbcpQueue(config)
 	, d_rbioQueue(config.Main().RBIOPriority()) {
 
 	CheckMainConfig(config.Main());
 
 	Init();
 
 	//TODO bad bad pratice new queue should be added automatically to this list.
 	d_queues.push_back(&d_sbcpQueue);
 	d_queues.push_back(&d_rbioQueue);
 }
 
 void Synchronizer::Init() {
 
 	InitRT();
 	InitModules();
 }
 
 void Synchronizer::InitRT() {
 
 	xeno_call(rt_task_shadow, NULL, NULL, d_priority, T_FPU);
 }
 
 void Synchronizer::CheckPriority(unsigned int p, const std::string & name) {
 	if (p > 99) {
 		std::ostringstream os;
 		os << "Unsupported " << name << " of " << p
 		   << " only value between 0 and 99 are supported";
 		throw std::runtime_error(os.str());
 	}
 }
 
 void Synchronizer::CheckMainConfig(const MainSection& config) {
 	std::ostringstream os;
 	if (config.Timestep() < 2) {
 		os << "Unsupported timestep of " << config.Timestep()
 		   << " ms. It should be >= 2ms";
 		throw std::runtime_error(os.str());
 	}
 
 	CheckPriority(config.MainPriority(), "main priority");
 	CheckPriority(config.SBCPPriority(), "sbcp priority");
 	CheckPriority(config.RBIOPriority(), "rbio priority");
 }
 
 void Synchronizer::InitModules() {
 
 }
 
 Synchronizer::~Synchronizer() {
 }
 
 void Synchronizer::calibrateIfNeeded() {
 
 }
 
 bool Synchronizer::start() {
 	d_firstStepped = false;
 
 	RT_EVENT * e = new RT_EVENT();
 	xeno_call(rt_event_create, e, Queue::EventName, 0, EV_PRIO);
 	d_event = NativeHolder<RT_EVENT>(e);
 
 	for (ListOfQueue::const_iterator q = d_queues.begin();
 	     q != d_queues.end();
 	     ++q) {
 		(*q)->StartTask();
 	}
 	d_idleQueueMask = Queue::AllQueueMask();
 
 	bool res = rci::oncilla::Synchronizer::start();
 
 	xeno_call(rt_task_set_periodic, NULL, TM_NOW, d_timestep * 1e9);
 
 	return res;
 }
 
 bool Synchronizer::stop() {
 	for (ListOfQueue::const_iterator q = d_queues.begin(); 
 	     q != d_queues.end();
 	     ++q) {
 		(*q)->StopTask();
 	}
 	d_event = NativeHolder<RT_EVENT>();
 
 	return rci::Synchronizer::stop();
 }
 
 double Synchronizer::lastProcessTimeStep() const {
 	if (!d_firstStepped) {
 		return 0.0;
 	}
 	return d_timestep * (d_overruns + 1);
 }
 
 void Synchronizer::ProcessAsyncPrimpl() {
 	for (ListOfQueue::const_iterator q = d_queues.begin();
 	     q != d_queues.end();
 	     ++q) {
 		if (IsFinished(**q)) {
 			(*q)->DownstreamData();
 		}
 	}
 	WakeIdleQueues();
 	d_firstStepped = true;
 }
 
 void Synchronizer::WaitForProcessAsyncPrimpl() {
 	int res;
 
 	unsigned int maxTrials(20);
 
 	while (true) {
 		res = rt_task_wait_period(&d_overruns);
 		if (res != 0 && res != -ETIMEDOUT) {
 			xeno_throw_error_from(rt_task_wait_period, res);
 		}
 
 		FetchIdleQueues();
 		//we ensure that all sbcp communication are done !
 		if (IsFinished(d_sbcpQueue)) {
 			break;
 		}
 
 		--maxTrials;
 		if (maxTrials == 0) {
 			std::ostringstream os;
 			os << "SBCP communication did not finished after " << maxTrials
 			   << " timesteps. We abort everything";
 			throw std::runtime_error(os.str());
 		}
 	}
 
 	for (ListOfQueue::const_iterator q = d_queues.begin();
 	     q != d_queues.end();
 	     ++q) {
 		if (IsFinished(**q)) {
 			(*q)->UpstreamData();
 		}
 	}
 
 }
 
 void Synchronizer::RegisterTrunk(const Trunk::Ptr& node) {
 	/// \todo implement
 }
 
 void Synchronizer::RegisterL0(rci::oncilla::Leg l, const L0::Ptr& node) {
 	d_rbioQueue.RegisterL0(l, node);
 	d_sbcpQueue.RegisterL0(l, node);
 }
 
 void Synchronizer::RegisterL1(rci::oncilla::Leg l, const L1L2::Ptr& node) {
-	if (node == NULL) {
-		// TODO: Logging/Warning
-		std::cout << "Ignoring L1 node of leg " << l << ". Is it connected?"
-		          << std::endl;
-		return; // Node is missing - board might be disconnected
+	if (!node) {
+		std::ostringstream os;
+		os << "Ignoring L1 node of leg " << l << ". Is it connected?";
+		throw std::runtime_error(os.str());
 	}
 	d_sbcpQueue.RegisterL1(l, node);
 }
 
 void Synchronizer::RegisterL2(rci::oncilla::Leg l, const L1L2::Ptr& node) {
-	if (node == NULL) {
-		// TODO: Logging/Warning
-		std::cout << "Ignoring L2 node of leg " << l << ". Is it connected?"
-		          << std::endl;
-		return; // Node is missing - board might be disconnected
+	if (!node) {
+		
+		std::ostringstream os;
+		os << "Ignoring L2 node of leg " << l << ". Is it connected?";
+		throw std::runtime_error(os.str());
 	}
 	d_sbcpQueue.RegisterL2(l, node);
 }
 
 void Synchronizer::RegisterL3(rci::oncilla::Leg l, const L3::Ptr& node) {
-	if (node == NULL) {
-		// TODO: Logging/Warning
-		std::cout << "Ignoring L3 node of leg " << l << ". Is it connected?"
-		          << std::endl;
-		return; // Node is missing - board might be disconnected
+	if (!node) {
+		std::ostringstream os;
+		os << "Ignoring L3 node of leg " << l << ". Is it connected?";
+		throw std::runtime_error(os.str());
 	}
 	d_sbcpQueue.RegisterL3(l, node);
 }
 
 void Synchronizer::WakeIdleQueues() {
 	xeno_call(rt_event_signal, d_event.get(), d_idleQueueMask);
 }
 
 void Synchronizer::FetchIdleQueues() {
 	xeno_call(rt_event_wait,
 	          d_event.get(), 
 	          0,
 	          &d_idleQueueMask, 
 	          EV_ANY, 
 	          TM_NONBLOCK);
 	d_idleQueueMask ^= Queue::AllQueueMask();
 }
 
 bool Synchronizer::IsFinished(const Queue& q) {
 	return q.Mask() & d_idleQueueMask;
 }
 
 } //namespace hw
 } //namespace liboncilla