[scripts] Implements new organization in python modules

Additionnally fix wrong use of assert.
The tests are more robust.

scripts/start_sot_online_walking.py

@@ -26,13 +26,13 @@ class TestSoTTalos(unittest.TestCase):
         f.write("x:"+str(gzGetModelPropResp.pose.position.x)+"\n")
         f.write("y:"+str(gzGetModelPropResp.pose.position.y)+"\n")
         f.write("z:"+str(gzGetModelPropResp.pose.position.z)+"\n")
-        dx=gzGetModelPropResp.pose.position.x-2.8331
-        dy=gzGetModelPropResp.pose.position.y-0.0405
-        dz=gzGetModelPropResp.pose.position.z-1.0019
+        dx=gzGetModelPropResp.pose.position.x-2.1045
+        dy=gzGetModelPropResp.pose.position.y-0.0038
+        dz=gzGetModelPropResp.pose.position.z-1.00152
         ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
         f.write("dist:"+str(ldistance))
         f.close()
-        if ldistance<0.009:
+        if ldistance<0.05:
             self.assertTrue(True,msg="Converged to the desired position")
         else:
             self.assertFalse(True,

scripts/test_kine.py

@@ -41,14 +41,20 @@ def runTest():
 
         rospy.loginfo("Stack of Tasks launched")
 
+        handleRunCommandClient('from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d')
+        handleRunCommandClient('robot.taskRH  = MetaTaskKine6d(\'rh\',robot.dynamic,\'rh\',robot.OperationalPointsMap[\'right-wrist\'])')
+        handleRunCommandClient('from dynamic_graph.sot.core.sot import SOT')
+        handleRunCommandClient('robot.sot = SOT(\'sot\')')
+    
         handleRunCommandClient('from talos_integration_tests.appli import init_appli')
-        handleRunCommandClient('init_sot_talos_balance(robot)')
-
-        launchScript(initCode,'initialize SoT')
-        handleRunCommandStartDynamicGraph()
+        
+        handleRunCommandClient('init_appli(robot)')
+        
+        handleRunCommandClient('from dynamic_graph.sot.core.meta_tasks_kine import gotoNd')
+        runCommandStartDynamicGraph()
         handleRunCommandClient("target = (0.5,-0.2,1.0)")
-        handleRunCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))")
-        handleRunCommandClient("sot.push(taskRH.task.name)")
+        handleRunCommandClient("gotoNd(robot.taskRH,target,'111',(4.9,0.9,0.01,0.9))")
+        handleRunCommandClient("robot.sot.push(robot.taskRH.task.name)")
 
         time.sleep(10)
 

scripts/test_online_walking.py

@@ -98,5 +98,6 @@ handleRunCommandClient('robot.pg.velocitydes.value=(0.3,0.0,0.0)')
 time.sleep(9)
 handleRunCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)')
 
-time.sleep(5)
+time.sleep(9)
+handleRunCommandClient('from sot_talos_balance.create_entities_utils import dump_tracer')
 handleRunCommandClient('dump_tracer(robot.tracer)')

scripts/test_sot_talos_balance.py

@@ -45,6 +45,7 @@ runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph',
                                                  Empty)
 
 runCommandStartDynamicGraph()
+time.sleep(5)
 # Connect ZMP reference and reset controllers
 print('Connect ZMP reference')
 handleRunCommandClient('from dynamic_graph import plug')
@@ -58,7 +59,6 @@ handleRunCommandClient('Ki_dcm = [1.0, 1.0, 1.0]')  # this value is employed lat
 handleRunCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
 
 print('Executing the trajectory')
-#time.sleep(1)
+time.sleep(1)
 handleRunCommandClient('robot.triggerTrajGen.sin.value = 1')
-time.sleep(2500)
-#handleRunCommandClient('dump_tracer(robot.tracer)')
+time.sleep(25)