diff --git a/CMakeLists.txt b/CMakeLists.txt
index eb46c512fc5389693ebe16a51cdb4e8efa1d3299..d3e59b1dc74df3dfb22b21a084e5485d4066e0c5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
 IF(BUILD_PYTHON_INTERFACE)
   INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
                 python/${SOTTALOSBALANCE_PYNAME}/main.py
+                python/${SOTTALOSBALANCE_PYNAME}/create_entities_utils.py
                 DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME})
 ENDIF(BUILD_PYTHON_INTERFACE)
 
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..b3d22dc13929d610d086305e837df24beb27b96c
--- /dev/null
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -0,0 +1,14 @@
+from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
+from sot_talos_balance.joint_position_controller import JointPositionController
+from dynamic_graph import plug
+
+def create_joint_trajectory_generator(device, dt=0.001, robot_name="robot"):
+    jtg = JointTrajectoryGenerator("jtg");
+    plug(device.robotState,      jtg.base6d_encoders);
+    jtg.init(dt, robot_name);
+    return jtg;
+
+def create_joint_position_controller(robot, dt=0.001, robot_name="robot"):
+    jpc = JointPositionController("jpc");
+    plug(robot.device.robotState,      jpc.q);
+    return jpc;
diff --git a/unittest/python/appli_jointTrajGen.py b/unittest/python/appli_jointTrajGen.py
deleted file mode 100644
index 6d18fc55045405571f88b9f72b1192201f576a6e..0000000000000000000000000000000000000000
--- a/unittest/python/appli_jointTrajGen.py
+++ /dev/null
@@ -1,13 +0,0 @@
-from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
-from dynamic_graph import plug
-
-dt = robot.timeStep;
-
-robot.traj_gen = JointTrajectoryGenerator("jtg");
-plug(robot.device.robotState, robot.traj_gen.base6d_encoders);
-robot.traj_gen.init(dt, "robot");
-plug(robot.traj_gen.dq, robot.device.control);
-
-
-
-robot.device.control.recompute(0)
diff --git a/unittest/python/test_jointTrajGen.py b/unittest/python/test_jointTrajGen.py
index b8c2021fb61d932a78957e2f44c1ab4d81102682..e502f369d233f05839fe18c0435f880ad6b64d6f 100644
--- a/unittest/python/test_jointTrajGen.py
+++ b/unittest/python/test_jointTrajGen.py
@@ -1,58 +1,22 @@
 #!/usr/bin/python
 # -*- coding: utf-8 -*-1
-
-import sys
-import rospy
+from sot_talos_balance.create_entities_utils import create_joint_trajectory_generator
+from sot_talos_balance.create_entities_utils import create_joint_position_controller
 from dynamic_graph import plug
-from std_srvs.srv import *
-from dynamic_graph_bridge.srv import *
-from dynamic_graph_bridge_msgs.srv import *
-
-def launchScript(code,title,description = ""):
-    raw_input(title+':   '+description)
-    rospy.loginfo(title)
-    rospy.loginfo(code)
-    for line in code:
-        if line != '' and line[0] != '#':
-            print line
-            answer = runCommandClient(str(line))
-            rospy.logdebug(answer)
-            print answer
-    rospy.loginfo("...done with "+title)
 
 
 
 # Waiting for services
 try:
-    rospy.loginfo("Waiting for run_command")
-    rospy.wait_for_service('/run_command')
-    rospy.loginfo("...ok")
-
-    rospy.loginfo("Waiting for start_dynamic_graph")
-    rospy.wait_for_service('/start_dynamic_graph')
-    rospy.loginfo("...ok")
-
-    runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
-    runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
-
-    initCode = open( "appli_jointTrajGen.py", "r").read().split("\n")
-    
-    rospy.loginfo("Stack of Tasks launched")
-
-    # runCommandClient("from dynamic_graph import plug")
-    # runCommandClient("from dynamic_graph.sot.core import SOT")
-    # runCommandClient("sot = SOT('sot')")
-    # runCommandClient("sot.setSize(robot.dynamic.getDimension())")
-    # runCommandClient("plug(sot.control,robot.device.control)")
+    dt = robot.timeStep;
 
-    launchScript(initCode,'initialize SoT')
-    raw_input("Wait before starting the dynamic graph")
-    runCommandStartDynamicGraph()
-    #raw_input("Wait before moving the hand")
-    #runCommandClient("target = (0.5,-0.2,1.0)")
-    #runCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))")
-    #runCommandClient("sot.push(taskRH.task.name)")
+    robot.traj_gen = create_joint_trajectory_generator(robot.device,dt)
+    robot.joint_pos_controller =create_joint_position_controller(robot,dt);
+    plug(robot.traj_gen.q,                       robot.joint_pos_controller.qDes);
+    plug(robot.traj_gen.dq,                       robot.joint_pos_controller.dqDes);
+    plug(robot.joint_pos_controller.dqRef,     robot.device.control);
+    robot.joint_pos_controller.init(tuple([1.0]*32));
 
-except rospy.ServiceException, e:
-    rospy.logerr("Service call failed: %s" % e)
 
+    sleep(1.0)
+    os.system('rosservice call /start_dynamic_graph');