Merge pull request #15 from nim65s/devel

[Tests] Activate test_ctrl_manager again

CMakeLists.txt

@@ -40,7 +40,6 @@ IF(INITIALIZE_WITH_NAN)
 ENDIF(INITIALIZE_WITH_NAN)
 
 # Project dependencies
-#SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex)
 IF(BUILD_TESTING)
   FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
 ENDIF(BUILD_TESTING)

tests/python/CMakeLists.txt

@@ -6,7 +6,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
   test_com_admittance
   test_com_estimation
   test_control
-  #test_ctrl_manager TODO: ctrl_
+  test_ctrl_manager
   test_dcm_controller
   test_dcm_controller_talos
   test_dcm_distribute

tests/python/test_ctrl_manager.py

@@ -24,8 +24,10 @@ for key in conf.param_server.mapJointNameToID:
     param_server.setNameToId(key, conf.param_server.mapJointNameToID[key])
 
 cm = TalosControlManager("TalosControlManager")
+cm.add_commands()
 cm.init(dt, robot_name)
 print("***Control manager initialized***")
+cm.add_signals()
 cm.u_max.value = np.array(N_JOINTS * (u_max, ))
 print("u_max is set at {0} for all joints".format(u_max, ))
 
@@ -35,11 +37,12 @@ control_value = 0.5
 u = np.array(N_JOINTS * [control_value])
 cm.addCtrlMode('input')
 cm.setCtrlMode('all', 'input')
+cm.add_signals()
 cm.ctrl_input.value = u
 print("Control input is set at {0} for all joints".format(control_value))
 cm.u.recompute(1)
 cm.u_safe.recompute(1)
-assert cm.u_safe.value == (control_value, ) * N_JOINTS
+assert (cm.u_safe.value == (control_value, ) * N_JOINTS).all()
 print("Safe control = control input")
 print("*****************")
 # Control is too big
@@ -49,15 +52,17 @@ cm.ctrl_input.value = u
 print("Control input is set at {0} for all joints".format(control_value))
 cm.u.recompute(2)
 cm.u_safe.recompute(2)
-assert cm.u_safe.value == (0., ) * N_JOINTS
+assert (cm.u_safe.value == (0., ) * N_JOINTS).all()
 print("Safe control = zero")
 print("Control set to 0 forever")
 print("*****************")
 
 cm2 = TalosControlManager("TalosControlManager")
+cm2.add_commands()
 cm2.init(dt, robot_name)
 print("***New Control manager initialized***")
-cm.u_max.value = N_JOINTS * (u_max, )
+cm2.add_signals()
+cm.u_max.value = np.array(N_JOINTS * (u_max, ))
 print("u_max is set at {0} for all joints".format(u_max))
 
 # Control is Ok
@@ -70,7 +75,7 @@ cm2.ctrl_input.value = u
 print("Control input is set at {0} for all joints".format(control_value))
 cm2.u.recompute(3)
 cm2.u_safe.recompute(3)
-assert cm2.u_safe.value == (control_value, ) * N_JOINTS
+assert (cm2.u_safe.value == (control_value, ) * N_JOINTS).all()
 print("Safe control = control input")
 print("*****************")
 
@@ -78,17 +83,18 @@ print("*****************")
 emergency = 0
 print("EMERGENCY = 0")
 cm2.addEmergencyStopSIN('test')
+cm2.add_signals()
 cm2.emergencyStop_test.value = emergency
 cm2.u.recompute(4)
 cm2.u_safe.recompute(4)
-assert cm2.u_safe.value == (control_value, ) * N_JOINTS
+assert (cm2.u_safe.value == (control_value, ) * N_JOINTS).all()
 print("Safe control = control input")
 emergency = 1
 print('EMERGENCY = 1')
 cm2.emergencyStop_test.value = emergency
 cm2.u.recompute(5)
 cm2.u_safe.recompute(5)
-assert cm2.u_safe.value == (0., ) * N_JOINTS
+assert (cm2.u_safe.value == (0., ) * N_JOINTS).all()
 print("Safe control = zero")
 print("Control set to 0 forever")
 print("*****************")