diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7bce72dafedb872729a74c272e5fb8f35c304c0a..8dcb45fd8a477579a0b88a0c04032b2025a8bcc2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -136,6 +136,7 @@ compile_plugin(ros_time)
 compile_plugin(ros_joint_state)
 
 target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dp-dynamic.so")
+target_link_libraries(ros_publish ros_bridge)
 
 #compile_plugin(robot_model)
 
@@ -173,7 +174,7 @@ target_link_libraries(geometric_simu  ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_D
 add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
 pkg_config_use_dependency(sot_loader dynamic-graph)
 pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge)
+target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf)
 install(TARGETS sot_loader DESTINATION lib)
 
 add_subdirectory(src)
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 60f151c27c409ec8caa1500e7831a07d3354d0de..1b9071c4e7a0229ef6f168a53eed2e8ac5bf7ec9 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -76,15 +76,20 @@ void SotLoader::startControlLoop()
 void SotLoader::initializeRosNode(int argc, char *argv[])
 {
   SotLoaderBasic::initializeRosNode(argc, argv);
-  angleEncoder_.resize(nbOfJoints_);
+  //Temporary fix. TODO: where should nbOfJoints_ be initialized from?                         
+  if (ros::param::has("/sot/state_vector_map")) {
+    angleEncoder_.resize(nbOfJoints_);
+  }
+
   startControlLoop();
 }
 
 void 
 SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
 {
-  
   // Update joint values.w
+  assert(angleControl_.size() == angleEncoder_.size());
+
   sensorsIn["joints"].setName("angle");
   for(unsigned int i=0;i<angleControl_.size();i++)
     angleEncoder_[i] = angleControl_[i];
@@ -97,7 +102,7 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
 
   
   // Update joint values.
-  angleControl_ = controlValues["joints"].getValues();
+  angleControl_ = controlValues["control"].getValues();
 
   //Debug
   std::map<std::string,dgs::ControlValues>::iterator it = controlValues.begin();