[WIP] unit test of Joint_trajectory_generator entity

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)
 

python/sot_talos_balance/create_entities_utils.py

+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;

unittest/python/appli_jointTrajGen.py

-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)

unittest/python/test_jointTrajGen.py

 #!/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');