diff --git a/include/darwin_joint_motion.h b/include/darwin_joint_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..90d491efea34895c58637c068afb4e539ca432cd
--- /dev/null
+++ b/include/darwin_joint_motion.h
@@ -0,0 +1,24 @@
+#ifndef _DARWIN_JOINT_MOTION_H
+#define _DARWIN_JOINT_MOTION_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+
+class CDarwinJointMotion : public CDarwinRobotBase
+{
+  public:
+    CDarwinJointMotion(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false);
+    void load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
+    std::vector<unsigned char> get_group_servos(joints_grp_t group);
+    std::vector<double> get_group_angles(joints_grp_t group);
+    std::vector<double> get_group_speeds(joints_grp_t group);
+    std::vector<double> get_group_accels(joints_grp_t group);
+    void start(joints_grp_t group);
+    void stop(joints_grp_t group);
+    bool are_moving(joints_grp_t group);
+    ~CDarwinJointMotion();
+};
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a1426e99afe2fa5a23a8310cdc7687ee2c8e33e3..d0520e18707b5aa18fd63efb4f508f4eeecf6ece 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo)
 INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
 
 # driver source files
-SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp)
+SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp)
 # application header files
-SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h)
+SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h)
 
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)
diff --git a/src/darwin_joint_motion.cpp b/src/darwin_joint_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..222ebf86d74995e5bfb4cd81a3f1e4c2c3d14245
--- /dev/null
+++ b/src/darwin_joint_motion.cpp
@@ -0,0 +1,78 @@
+#include "darwin_joint_motion.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinJointMotion::CDarwinJointMotion(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinRobotBase(name,bus_id,bus_speed,id,sim)
+{
+}
+
+void CDarwinJointMotion::load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
+{
+  unsigned int group_servos=0,i;
+  unsigned short int angle;
+  unsigned char speed,accel;
+
+  this->is_valid();
+  if(servos.size()!=angles.size() || servos.size()!=speeds.size() || servos.size()!=accels.size())
+    throw CDarwinRobotException(_HERE_,"Invalid input vector sizes");
+  for(i=0;i<servos.size();i++)
+  {
+    group_servos|=(0x00000001<<servos[i]);
+    angle=(unsigned short int)(angles[i]*128.0);
+    this->robot_device->write_word_register(JOINT_MOTION_GROUP_ANGLES+servos[i]*2,angle);
+    speed=(unsigned char)(speeds[i]);
+    this->robot_device->write_byte_register(JOINT_MOTION_GROUP_SPEEDS+servos[i],speed);
+    accel=(unsigned char)(accels[i]);
+    this->robot_device->write_byte_register(JOINT_MOTION_GROUP_ACCELS+servos[i],accel);
+  }
+  this->robot_device->write_registers(JOINT_MOTION_GROUP_SERVOS+((unsigned short int)group)*4,(unsigned char *)&group_servos,4);
+}
+
+std::vector<unsigned char> CDarwinJointMotion::get_group_servos(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_angles(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_speeds(joints_grp_t group)
+{
+
+}
+
+std::vector<double> CDarwinJointMotion::get_group_accels(joints_grp_t group)
+{
+
+}
+
+
+void CDarwinJointMotion::start(joints_grp_t group)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group),JOINT_MOTION_START);
+}
+
+void CDarwinJointMotion::stop(joints_grp_t group)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group),JOINT_MOTION_STOP);
+}
+
+bool CDarwinJointMotion::are_moving(joints_grp_t group)
+{
+  unsigned char value;
+
+  this->is_valid();
+  this->robot_device->read_byte_register(JOINT_MOTION_GROUP_CONTROL+((unsigned short int)group*5),&value);
+  if(value&JOINT_MOTION_RUNNING)
+    return true;
+  else
+    return false;
+}
+
+CDarwinJointMotion::~CDarwinJointMotion()
+{
+}
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a259c69e5439db167c3228e3ec52ac83dc5222f9..c6e4e9048991d5c6cf5e8fd7a10f109c4cbf762f 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -34,9 +34,9 @@ TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
 #TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
 
 # create an example application
-#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
 # link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
 
 # create an example application
 #ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index e86d584d900a5e4331fa0aeb5d6955f1210e2cd2..2c63842e5812f1b4893ae540a561bc73996d3359 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -1,35 +1,49 @@
-#include "darwin_robot.h"
+#include "darwin_mmanager.h"
+#include "darwin_joint_motion.h"
 #include "darwin_robot_exceptions.h"
 
 #include <iostream>
 
-std::string robot_device="/dev/pts/20";
-//std::string robot_device="A603LOBS";
+std::string robot_device="/tmp/darwin_driver";
 
 int main(int argc, char *argv[])
 {
-  int num_servos,i,j;
+  int i=0,num_servos;
   unsigned int present_servos;
   std::vector<unsigned char> servos;
   std::vector<double> angles,speeds,accels;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
-    num_servos=darwin.mm_get_num_servos();
-    std::cout << "Found " << num_servos << " servos " << std::endl;
-    present_servos=darwin.mm_get_present_servos();
-    std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
-    darwin.mm_enable_power();
-    for(i=0;i<MAX_NUM_SERVOS;i++)
+    CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
+    CDarwinJointMotion joints("Darwin",robot_device,1000000,0x01,true);
+    std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
+    std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
+    std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
+    std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
+    darwin.start_scan();
+    while(darwin.is_scanning())
+    {
+      std::cout << "scanning ..." << std::endl;
+      usleep(100000);
+    }
+    num_servos=darwin.get_num_devices();
+    std::cout << "num. devices: " << num_servos << std::endl;
+    present_servos=darwin.get_present_devices();
+    std::cout << "present devices: " << std::hex << present_servos << std::endl;
+    present_servos=darwin.get_present_servos();
+    std::cout << "present servos: " << std::hex << present_servos << std::endl;
+    i=0;
+    while(i<num_servos)
     {
       if(present_servos&(0x00000001<<i))
       {
-        darwin.mm_enable_servo(i);
-        darwin.mm_assign_module(i,DARWIN_MM_JOINTS); 
+        darwin.enable_servo(i);
+        darwin.assign_module(i,DARWIN_MM_JOINTS);
       }
+      i++;
     }
-    darwin.mm_start();
-    for(j=0;j<4;j++)
+    darwin.start();
+    for(i=0;i<4;i++)
     { 
       servos.clear();
       angles.clear();
@@ -40,12 +54,12 @@ int main(int argc, char *argv[])
       speeds.push_back(50.0);
       accels.push_back(50.0);
       std::cout << "Start joint_motion ..." << std::endl;
-      darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
-      darwin.joints_start(JOINTS_GRP0);
-      while(darwin.joints_are_moving(JOINTS_GRP0))
+      joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
+      joints.start(JOINTS_GRP0);
+      while(joints.are_moving(JOINTS_GRP0))
       {
         std::cout << "moving joints ..." << std::endl;
-        std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
+        std::cout << "current angle: " << darwin.get_servo_angle(19) << std::endl;
         usleep(100000);
       }
       servos.clear();
@@ -56,17 +70,16 @@ int main(int argc, char *argv[])
       angles.push_back(-45.0);
       speeds.push_back(50.0);
       accels.push_back(50.0);
-      darwin.joints_load(JOINTS_GRP0,servos,angles,speeds,accels);
-      darwin.joints_start(JOINTS_GRP0);
-      while(darwin.joints_are_moving(JOINTS_GRP0))
+      joints.load(JOINTS_GRP0,servos,angles,speeds,accels);
+      joints.start(JOINTS_GRP0);
+      while(joints.are_moving(JOINTS_GRP0))
       {
         std::cout << "moving joints ..." << std::endl;
-        std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
+        std::cout << "current angle: " << darwin.get_servo_angle(19) << std::endl;
         usleep(100000);
       }
     }
-    darwin.mm_stop();
-    darwin.mm_disable_power();
+    darwin.stop();
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }