Commit ecd630be authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[scripts] Implements new organization in python modules

Additionnally fix wrong use of assert.
The tests are more robust.
parent 4cbb7f44
......@@ -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,
......
......@@ -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)
......
......@@ -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)')
......@@ -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)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment