Unverified Commit b852b82a authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #15 from nim65s/devel

[Tests] Activate test_ctrl_manager again
parents 53b00e9c 63b52e50
Pipeline #12186 passed with stage
in 35 minutes and 31 seconds
......@@ -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)
......
......@@ -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
......
......@@ -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("*****************")
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