Commit df698dfc authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[test_foot_force_difference_controller] Improve test

parent 948460c9
......@@ -5,18 +5,21 @@ import numpy as np
import pinocchio as pin
from numpy.testing import assert_almost_equal as assertApprox
# --- General ---
print("--- General ---")
controller = FootForceDifferenceController("footController")
controller.init()
controller.dfzAdmittance.value = 0.
controller.dfzAdmittance.value = 1.
controller.wrenchRight.value = [0.]*2 + [500.] + [0.]*3
controller.wrenchLeft.value = [0.]*2 + [300.] + [0.]*3
controller.wrenchRightDes.value = [0.]*2 + [400.] + [0.]*3
controller.wrenchLeftDes.value = [0.]*2 + [400.] + [0.]*3
controller.wrenchRight.value = [0.]*6
controller.wrenchLeft.value = [0.]*6
controller.wrenchRightDes.value = [0.]*6
controller.wrenchLeftDes.value = [0.]*6
print("wrenchRight: %s" % str(controller.wrenchRight.value))
print("wrenchLeft: %s" % str(controller.wrenchLeft.value))
print("wrenchRightDes: %s" % str(controller.wrenchRightDes.value))
print("wrenchLeftDes: %s" % str(controller.wrenchLeftDes.value))
print()
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
......@@ -29,8 +32,16 @@ controller.posLeft.value = np.eye(4).tolist()
controller.vRight.recompute(0)
controller.vLeft.recompute(0)
print(controller.vRight.value)
assertApprox([0.0]*6,controller.vRight.value)
print(controller.vLeft.value)
assertApprox([0.0]*6,controller.vLeft.value)
# There is more pressure on the right foot.
# Therefore, the right foot must go up to reduce it
vRight = [0.]*2 + [ 100.] + [0.]*3
vLeft = [0.]*2 + [-100.] + [0.]*3
print("Expected vRight: %s" % str(vRight))
print("Actual vRight: %s" % str(controller.vRight.value))
print("Expected vLeft: %s" % str(vLeft))
print("Actual vLeft: %s" % str(controller.vLeft.value))
assertApprox(vRight,controller.vRight.value)
assertApprox(vLeft,controller.vLeft.value)
Supports Markdown
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