diff --git a/src/Oncilla.cpp b/src/Oncilla.cpp
index e81ef04..3aca1bf 100644
--- a/src/Oncilla.cpp
+++ b/src/Oncilla.cpp
@@ -1,93 +1,115 @@
 /*
  * Oncilla.cpp
  *
  *  Created on: 15 dec. 2011
  *      Author: Alexandre Tuleu, Arne Nordmann
  */
 #include "Oncilla.h"
 #include "OncillaTrunk.h"
 #include "OncillaL01.h"
 #include "OncillaL2.h"
 #include "OncillaL3.h"
 #include "OncillaL4.h"
 
 namespace rci {
 namespace oncilla {
 
+Oncilla::LegNames Oncilla::s_legNames;
+
+
+
+const std::string & Oncilla::nameOfLeg(Leg l){
+	if(s_legNames.empty()){
+		s_legNames.push_back("Left Fore");
+		s_legNames.push_back("Right Fore");
+		s_legNames.push_back("Left Hind");
+		s_legNames.push_back("Right Hind");
+		s_legNames.push_back("Undefined");
+	}
+
+	if(l >= NUM_LEGS){
+		return s_legNames.back();
+	}
+
+	return s_legNames[l];
+}
+
+
+
 Oncilla::Oncilla(OncillaSynchronizerPtr synchronizer)
 	:d_synchronizer(synchronizer){
 	init();
 }
 
 Oncilla::~Oncilla() {
 }
 
 #define CREATE_NODES(LegName)do{\
 	d_L0s.push_back(OncillaL0Ptr(new OncillaL0(LegName " Oncilla L0" )));\
 	d_L1s.push_back(OncillaL1Ptr(new OncillaL1(*d_synchronizer,\
 	                                           LegName " Oncilla L1")));\
 	d_L2s.push_back(OncillaL2Ptr(new OncillaL2(*d_synchronizer,\
 	                                           LegName " Oncilla L2")));\
 	d_L3s.push_back(OncillaL3Ptr(new OncillaL3(LegName " Oncilla L3")));\
 	d_L4s.push_back(OncillaL4Ptr(new OncillaL4(LegName " Oncilla L4")));\
 }while(0)
 
 #define REGISTER_NODES(Type)do{\
 	for(std::vector <  Oncilla ## Type ## Ptr >::const_iterator n = d_ ## Type ## s.begin();\
 	    n != d_ ## Type ## s.end();\
 	    ++n){\
 		Oncilla::Leg l = static_cast<Oncilla::Leg>(n - d_ ## Type ## s.begin());\
 		d_synchronizer->register ## Type ## Node(l,*n);\
 	}\
 }while(0)
 
 
 void Oncilla::init() {
 	d_L0s.reserve(4);
 	d_L1s.reserve(4);
 	d_L2s.reserve(4);
 	d_L3s.reserve(4);
 	d_L4s.reserve(4);
 
 	CREATE_NODES("Left Fore");
 	CREATE_NODES("Right Fore");
 	CREATE_NODES("Left Hind");
 	CREATE_NODES("Right Hind");
 
 	d_trunk = OncillaTrunkPtr(new OncillaTrunk("Oncilla Trunk"));
 
 	REGISTER_NODES(L0);
 	REGISTER_NODES(L1);
 	REGISTER_NODES(L2);
 	REGISTER_NODES(L3);
 	REGISTER_NODES(L4);
 	d_synchronizer->registerTrunkNode(d_trunk);
 
 }
 
 OncillaL0Ptr Oncilla::getL0(Leg l) const {
 	return this->d_L0s[l];
 }
 
 OncillaL1Ptr Oncilla::getL1(Leg l) const {
 	return this->d_L1s[l];
 }
 
 OncillaL2Ptr Oncilla::getL2(Leg l) const {
 	return this->d_L2s[l];
 }
 
 OncillaL3Ptr Oncilla::getL3(Leg l) const {
 	return this->d_L3s[l];
 }
 
 OncillaL4Ptr Oncilla::getL4(Leg l) const {
 	return this->d_L4s[l];
 }
 
 OncillaTrunkPtr Oncilla::getTrunk() const {
 	return this->d_trunk;
 }
 
 }
 }
diff --git a/src/Oncilla.h b/src/Oncilla.h
index 8b6f73f..f5d5e23 100644
--- a/src/Oncilla.h
+++ b/src/Oncilla.h
@@ -1,77 +1,84 @@
 /*
  * Oncilla.h
  *
  *  Created on: 15 dec. 2011
  *      Author: Alexandre Tuleu, Arne Nordmann
  */
 #pragma once
 
 #include <vector>
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 /*
  *
  */
 /*
  *
  */
 namespace rci {
 namespace oncilla {
 
 class Oncilla;
-typedef boost::shared_ptr<Oncilla> OncillaPtr;
+typedef std::tr1::shared_ptr<Oncilla> OncillaPtr;
 
 class OncillaSynchronizer;
-typedef boost::shared_ptr<OncillaSynchronizer> OncillaSynchronizerPtr;
+typedef std::tr1::shared_ptr<OncillaSynchronizer> OncillaSynchronizerPtr;
 
 class OncillaTrunk;
-typedef boost::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
+typedef std::tr1::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
 class OncillaL0;
-typedef boost::shared_ptr<OncillaL0> OncillaL0Ptr;
+typedef std::tr1::shared_ptr<OncillaL0> OncillaL0Ptr;
 class OncillaL1;
-typedef boost::shared_ptr<OncillaL1> OncillaL1Ptr;
+typedef std::tr1::shared_ptr<OncillaL1> OncillaL1Ptr;
 class OncillaL2;
-typedef boost::shared_ptr<OncillaL2> OncillaL2Ptr;
+typedef std::tr1::shared_ptr<OncillaL2> OncillaL2Ptr;
 class OncillaL3;
-typedef boost::shared_ptr<OncillaL3> OncillaL3Ptr;
+typedef std::tr1::shared_ptr<OncillaL3> OncillaL3Ptr;
 class OncillaL4;
-typedef boost::shared_ptr<OncillaL4> OncillaL4Ptr;
+typedef std::tr1::shared_ptr<OncillaL4> OncillaL4Ptr;
 
 class Oncilla {
 public:
+
 	enum Leg {
 		LEFT_FORE = 0,
 		RIGHT_FORE = 1,
 		LEFT_HIND = 2,
 		RIGHT_HIND = 3,
 		NUM_LEGS = 4
 	};
 
+	static const std::string & nameOfLeg(Leg l);
+
 	Oncilla(OncillaSynchronizerPtr synchronizer);
 	virtual ~Oncilla();
 
 	OncillaL0Ptr getL0(Leg l) const;
 	OncillaL1Ptr getL1(Leg l) const;
 	OncillaL2Ptr getL2(Leg l) const;
 	OncillaL3Ptr getL3(Leg l) const;
 	OncillaL4Ptr getL4(Leg l) const;
 	OncillaTrunkPtr getTrunk() const;
 
 
 private:
+	typedef std::vector<std::string> LegNames;
+
+	static LegNames s_legNames;
+
 	void init();
 
 	OncillaSynchronizerPtr d_synchronizer;
 
 	std::vector<OncillaL0Ptr> d_L0s;
 	std::vector<OncillaL1Ptr> d_L1s;
 	std::vector<OncillaL2Ptr> d_L2s;
 	std::vector<OncillaL3Ptr> d_L3s;
 	std::vector<OncillaL4Ptr> d_L4s;
 
 	OncillaTrunkPtr d_trunk;
 };
 
 }
 }
diff --git a/src/OncillaL01.h b/src/OncillaL01.h
index ea9ff9c..a21fabe 100644
--- a/src/OncillaL01.h
+++ b/src/OncillaL01.h
@@ -1,153 +1,153 @@
 #pragma once
 
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 #include <rci/ResourceNode.h>
 #include <rci/Controlled.h>
 #include <rci/Configurable.h>
 #include <rci/Sensing.h>
 
 #include <rci/dto/JointAngles.h>
 
 #include "OncillaSynchronizer.h"
 
 namespace rci {
 namespace oncilla {
 
 class OncillaL0;
-typedef boost::shared_ptr<OncillaL0> OncillaL0Ptr;
+typedef std::tr1::shared_ptr<OncillaL0> OncillaL0Ptr;
 
 /**
  * Node class, representing the hip node of the quadruped robot.
  * @todo In case of simulation, this node can also sense the power consumption.
  */
 class OncillaL0: public rci::ResourceNode,
                  public rci::Controlled,
                  public rci::PositionControlled {
 
 public:
 
 	/**
 	 * Special constructor to also link to webots
 	 */
 	OncillaL0(const std::string &name = "Oncilla Hip");
 
 	virtual ~OncillaL0();
 
 	/**
 	 * Returns, if controller is converged.
 	 * @return True, if controller is converged after last command.
 	 */
 	bool isConverged() const;
 
 	/**
 	 * Commanding a joint position.
 	 * @param position Position command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	bool setJointPosition(JointAnglesPtr position);
 
 	/**
 	 * Returns latest position command. Note, that this is the latest valid
 	 * commanded position. If the position command exceeds limits and is
 	 * therefore rejected, it ..
 	 */
 	virtual JointAnglesPtr getLastPositionCommand() const;
 
 	/**
 	 * Print
 	 */
 	std::string print() const;
 };
 
 class OncillaL1;
-typedef boost::shared_ptr<OncillaL1> OncillaL1Ptr;
+typedef std::tr1::shared_ptr<OncillaL1> OncillaL1Ptr;
 
 /**
  * Node class, representing the hip node of the quadruped robot.
  *
  * @todo In case of simulation, this node can also sense the power consumption.
  */
 class OncillaL1: public rci::ResourceNode,
                  public rci::Controlled,
                  public rci::Sensing,
                  public rci::PositionControlled,
                  public rci::PositionSensing, // Two encoder values
                  public rci::VelocityControlled,
                  public rci::TorqueControlled,
                  public rci::ImpedanceControlled {
 
 public:
 
 	/**
 	 * Special constructor to also link to webots
 	 */
 	OncillaL1(OncillaSynchronizer &s, const std::string &name = "Oncilla Hip");
 
 	virtual ~OncillaL1();
 
 	/**
 	 * Returns, if controller is converged.
 	 * @return True, if controller is converged after last command.
 	 */
 	bool isConverged() const;
 
 	/**
 	 * Commanding a joint position.
 	 * @param position Position command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	bool setJointPosition(JointAnglesPtr position);
 
 	/**
 	 * Returns current joint position.
 	 * @return Current joint position
 	 */
 	virtual JointAnglesPtr getJointPosition() const;
 
 	/**
 	 * Commanding a joint velocity.
 	 * @param position Position command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	virtual bool setJointVelocity(rci::JointVelocitiesPtr vel);
 
 	/**
 	 * Returns current joint torque.
 	 * @return Current joint torque
 	 */
 	virtual bool setJointTorque(JointTorquesPtr torque);
 
 	/**
 	 * Returns current joint torque.
 	 * @return Current joint torque
 	 */
 	virtual bool setJointImpedance(JointImpedancePtr imped);
 
 	/**
 	 * Returns latest position command. Note, that this is the latest valid
 	 * commanded position. If the position command exceeds limits and is
 	 * therefore rejected, it ..
 	 */
 	virtual JointAnglesPtr getLastPositionCommand() const;
 
 	/**
 	 * Print
 	 */
 	std::string print() const;
 
 private:
 	/**
 	 * Reference to OncillaSynchronizer
 	 * @todo This is a dangerous reference which will become invalid as soon as
 	 * OncillaSynchronizer gets deleted outside.
 	 */
 	OncillaSynchronizer &synchronizer;
 };
 
 }
 }
diff --git a/src/OncillaL2.h b/src/OncillaL2.h
index 32c0c31..472e651 100644
--- a/src/OncillaL2.h
+++ b/src/OncillaL2.h
@@ -1,100 +1,100 @@
 #pragma once
 
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 #include <rci/ResourceNode.h>
 #include <rci/Controlled.h>
 #include <rci/Configurable.h>
 #include <rci/Sensing.h>
 
 #include <rci/dto/JointAngles.h>
 
 #include "OncillaSynchronizer.h"
 
 namespace rci {
 namespace oncilla {
 
 class OncillaL2;
-typedef boost::shared_ptr<OncillaL2> OncillaL2Ptr;
+typedef std::tr1::shared_ptr<OncillaL2> OncillaL2Ptr;
 
 /**
  * Node class, representing the knee node of the quadruped robot.
  * @todo In case of simulation, this node can also sense the power consumption.
  */
 class OncillaL2: public rci::ResourceNode,
                  public rci::Controlled,
                  public rci::Sensing,
                  public rci::PositionControlled,
                  public rci::PositionSensing,
                  public rci::VelocityControlled,
                  public rci::TorqueControlled,
                  public rci::ImpedanceControlled {
 
 public:
 
 	OncillaL2(OncillaSynchronizer &s, const std::string &name = "Oncilla Knee");
 	virtual ~OncillaL2();
 
 	/**
 	 * Returns, if controller is converged.
 	 * @return True, if controller is converged after last command.
 	 */
 	bool isConverged() const;
 
 	/**
 	 * Commanding a joint position.
 	 * @param position Position command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	bool setJointPosition(JointAnglesPtr position);
 
 	/**
 	 * Returns current joint position.
 	 * @return Current joint position
 	 */
 	virtual JointAnglesPtr getJointPosition() const;
 
 	/**
 	 * Commanding a joint velocity.
 	 * @param position Velocity command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	bool setJointVelocity(JointVelocitiesPtr vel);
 
 	/**
 	 * Commanding a joint velocity.
 	 * @param position Velocity command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	bool setJointImpedance(JointImpedancePtr imped);
 
 	/**
 	 * Commanding a joint torque.
 	 * @param position Torque command
 	 * @return Return, if successfull. (e.g. not exceeding joint limits)
 	 * @todo Check for correct control mode
 	 */
 	virtual bool setJointTorque(rci::JointTorquesPtr torques);
 
 	/**
 	 * Returns latest position command. Note, that this is the latest valid
 	 * commanded position. If the position command exceeds limits and is
 	 * therefore rejected, it ..
 	 */
 	virtual JointAnglesPtr getLastPositionCommand() const;
 
 	/**
 	 * Print
 	 */
 	std::string print() const;
 
 private:
 	OncillaSynchronizer &synchronizer;
 };
 
 }
 }
diff --git a/src/OncillaL3.h b/src/OncillaL3.h
index be10b07..8a65886 100644
--- a/src/OncillaL3.h
+++ b/src/OncillaL3.h
@@ -1,45 +1,45 @@
 #pragma once
 
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 #include <rci/ResourceNode.h>
 #include <rci/Controlled.h>
 #include <rci/Configurable.h>
 #include <rci/Sensing.h>
 
 #include <rci/dto/JointAngles.h>
 
 namespace rci {
 namespace oncilla {
 
 class OncillaL3;
-typedef boost::shared_ptr<OncillaL3> OncillaL3Ptr;
+typedef std::tr1::shared_ptr<OncillaL3> OncillaL3Ptr;
 
 /**
  * Node class, representing the foot/ankle of the quadruped robot. Not actuated,
  * but force sensing.
  */
 class OncillaL3: public rci::ResourceNode,
                  public rci::Sensing,
                  public rci::PositionSensing {
 
 public:
 
 	OncillaL3(const std::string & name = "Oncilla Ankle");
 
 	virtual ~OncillaL3();
 
 	/**
 	 * Returns contact forces
 	 */
 	JointAnglesPtr getJointPosition() const;
 
 	/**
 	 * Print
 	 */
 	std::string print() const;
 };
 
 }
 }
diff --git a/src/OncillaL4.cpp b/src/OncillaL4.cpp
index 1a32e50..c02f0e5 100644
--- a/src/OncillaL4.cpp
+++ b/src/OncillaL4.cpp
@@ -1,29 +1,28 @@
 #include "OncillaL4.h"
 
 using namespace std;
 
 namespace rci {
 namespace oncilla {
 
 OncillaL4::OncillaL4(const std::string & name)
 	:  rci::ResourceNode(name)
 	, rci::Sensing()
 	, rci::ForceSensing() {
 }
 
 ForcesPtr OncillaL4::getForces() const {
-	throw std::runtime_error("Not yet implemented.");
 }
 
 OncillaL4::~OncillaL4() {
 }
 
 std::string OncillaL4::print() const {
 	ostringstream outstream(ostringstream::out);
 	outstream.precision(3); // Precision when printing double values
 	outstream << "<Oncilla Foot>" << endl;
 	return outstream.str();
 }
 
 }
 }
diff --git a/src/OncillaL4.h b/src/OncillaL4.h
index 3cb016b..d1ad167 100644
--- a/src/OncillaL4.h
+++ b/src/OncillaL4.h
@@ -1,45 +1,45 @@
 #pragma once
 
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 #include <rci/ResourceNode.h>
 #include <rci/Controlled.h>
 #include <rci/Configurable.h>
 #include <rci/Sensing.h>
 
 #include <rci/dto/JointAngles.h>
 
 namespace rci {
 namespace oncilla {
 
 class OncillaL4;
-typedef boost::shared_ptr<OncillaL4> OncillaL4Ptr;
+typedef std::tr1::shared_ptr<OncillaL4> OncillaL4Ptr;
 
 /**
  * Node class, representing the foot of the quadruped robot. Not actuated,
  * but force sensing.
  */
 class OncillaL4: public rci::ResourceNode,
                  public rci::Sensing,
                  public rci::ForceSensing {
 
 public:
 
 	OncillaL4(const std::string & name = "Oncilla Foot");
 
 	virtual ~OncillaL4();
 
 	/**
 	 * Returns contact forces
 	 */
 	ForcesPtr getForces() const;
 
 	/**
 	 * Print
 	 */
 	std::string print() const;
 };
 
 }
 }
diff --git a/src/OncillaSynchronizer.h b/src/OncillaSynchronizer.h
index 4cdbf9e..b74180f 100644
--- a/src/OncillaSynchronizer.h
+++ b/src/OncillaSynchronizer.h
@@ -1,83 +1,83 @@
 #pragma once
 
 #include <vector>
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 
 #include <rci/Synchronizer.h>
 
 #include "Oncilla.h"
 
 namespace rci {
 namespace oncilla {
 
 class OncillaSynchronizer;
-typedef boost::shared_ptr<OncillaSynchronizer> OncillaSynchronizerPtr;
+typedef std::tr1::shared_ptr<OncillaSynchronizer> OncillaSynchronizerPtr;
 
 class OncillaTrunk;
-typedef boost::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
+typedef std::tr1::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
 class OncillaL0;
-typedef boost::shared_ptr<OncillaL0> OncillaL0Ptr;
+typedef std::tr1::shared_ptr<OncillaL0> OncillaL0Ptr;
 class OncillaL1;
-typedef boost::shared_ptr<OncillaL1> OncillaL1Ptr;
+typedef std::tr1::shared_ptr<OncillaL1> OncillaL1Ptr;
 class OncillaL2;
-typedef boost::shared_ptr<OncillaL2> OncillaL2Ptr;
+typedef std::tr1::shared_ptr<OncillaL2> OncillaL2Ptr;
 class OncillaL3;
-typedef boost::shared_ptr<OncillaL3> OncillaL3Ptr;
+typedef std::tr1::shared_ptr<OncillaL3> OncillaL3Ptr;
 class OncillaL4;
-typedef boost::shared_ptr<OncillaL4> OncillaL4Ptr;
+typedef std::tr1::shared_ptr<OncillaL4> OncillaL4Ptr;
 
 /**
  * OncillaSynchronizer
  * Different OncillaSynchronizers may be for example:
  * * Update with a given frequency
  * ** Send data every time
  * ** Send only if new data
  * * Update only if new data (event-based)
  * * Triggered from robot side (e.g. FRI)
  */
 class OncillaSynchronizer: public Synchronizer {
 public:
 	OncillaSynchronizer(const std::string &name);
 	virtual ~OncillaSynchronizer();
 
 	/**
 	 * Registers a trunk node.
 	 */
 	virtual void registerTrunkNode(OncillaTrunkPtr node) = 0;
 
 	/**
 	 * Registers an L0 node.
 	 */
 	virtual void registerL0Node(Oncilla::Leg l, OncillaL0Ptr node) = 0;
 
 	/**
 	 * Registers an L1 node.
 	 */
 	virtual void registerL1Node(Oncilla::Leg l, OncillaL1Ptr node) = 0;
 
 	/**
 	 * Registers an L2 node.
 	 */
 	virtual void registerL2Node(Oncilla::Leg l, OncillaL2Ptr node) = 0;
 
 	/**
 	 * Registers an L3 node.
 	 */
 	virtual void registerL3Node(Oncilla::Leg l, OncillaL3Ptr node) = 0;
 
 	/**
 	 * Registers an L4 node.
 	 */
 	virtual void registerL4Node(Oncilla::Leg l, OncillaL4Ptr node) = 0;
 
 	/**
 	 *
 	 */
 	friend std::ostream
 	& operator<<(std::ostream& os, const OncillaSynchronizer& val);
 
 };
 
 }
 }
diff --git a/src/OncillaTrunk.h b/src/OncillaTrunk.h
index f5e89a4..efe53f2 100644
--- a/src/OncillaTrunk.h
+++ b/src/OncillaTrunk.h
@@ -1,58 +1,58 @@
 #pragma once
 
 #include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <tr1/memory>
 
 #include <rci/ResourceNode.h>
 #include <rci/Controlled.h>
 #include <rci/Configurable.h>
 #include <rci/Sensing.h>
 
 namespace rci {
 namespace oncilla {
 
 class OncillaTrunk;
-typedef boost::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
+typedef std::tr1::shared_ptr<OncillaTrunk> OncillaTrunkPtr;
 
 /**
  * Node class, representing the quadruped trunk, including pose sensing.
  *
  * @todo Expose translational accelerations and rotational velocities
  */
 class OncillaTrunk: public rci::ResourceNode, public Sensing, public PoseSensing {
 
 public:
 
     OncillaTrunk(const std::string & name = "Oncilla Trunk");
     virtual ~OncillaTrunk();
 
     /**
      * Returns current pose.
      *
      * @return Current pose
      */
     virtual PosePtr getAbsolutePose() const;
 
     /**
      * Returns current displacement.
      *
      * @return Current pose
      */
     virtual PosePtr getDisplacement() const;
 
     /**
      * Returns current translational and rotational acceleration.
      *
      * @return Current pose
      */
     virtual PosePtr getAcceleration() const;
 
     /**
      * Print
      */
     std::string print() const;
 
 };
 
 }
 }