# --- Parameter server ---
print("--- Parameter server ---")
param_server = create_parameter_server(param_server_conf, dt)
# --- DCM controller
Kp_dcm = [0.0, 0.0, 0.0]
Ki_dcm = [0.0, 0.0, 0.0]
gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl")
dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = m
dcm_controller.omega.value = omega
dcm_controller.com.value = comDes
dcm_controller.dcm.value = comDes
dcm_controller.zmpDes.value = zmpDes
dcm_controller.dcmDes.value = dcmDes
dcm_controller.init(dt)
dcm_controller.wrenchRef.recompute(0)
print("reference wrench: %s" % str(dcm_controller.wrenchRef.value))
assertApprox(wrench, dcm_controller.wrenchRef.value, 3)
from numpy.testing import assert_almost_equal as assertApprox
from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
dummy = DummyDcmEstimator('dummy')
dummy.mass.value = 1.0
dummy.omega.value = 1.0
dummy.com.value = [1.] * 3
dummy.momenta.value = [2.] * 3
dummy.init()
dummy.dcm.recompute(0)
print(dummy.dcm.value)
assertApprox(dummy.dcm.value, [3.] * 3)
dummy.momenta.value = [3.] * 6
dummy.dcm.recompute(1)
print(dummy.dcm.value)
assertApprox(dummy.dcm.value, [4.] * 3)
print("--- Conversion ---")
e2q = EulerToQuat('e2q')
plug(stf.q, e2q.euler)
e2q.quaternion.recompute(0)
print(e2q.quaternion.value)
print(len(e2q.quaternion.value))
q_est = np.matrix(e2q.quaternion.value).T
# temp
if q_est[6] < 0 and q[6] > 0:
print("Warning: quaternions have different signs")
q_est[3:7] = -q_est[3:7]
print(q_est.T)
assertApprox(q, q_est, 3)
# --- Raw q difference ---
print("--- Raw q difference ---")
q_diff = q_est - q
print(q_diff.flatten().tolist()[0])
# --- DCM estimator ---
print("--- DCM estimator ---")
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.init(dt, robot_name)
plug(e2q.quaternion, dcm_estimator.q)
plug(base_estimator.v, dcm_estimator.v)
dcm_estimator.c.recompute(0)
print(dcm_estimator.c.value)
print("wrenchLeft: %s" % (estimator.wrenchLeft.value, ))
print("wrenchRight: %s" % (estimator.wrenchRight.value, ))
print("poseLeft:\n%s" % (np.matrix(estimator.poseLeft.value), ))
print("poseRight:\n%s" % (np.matrix(estimator.poseRight.value), ))
estimator.init()
estimator.zmp.recompute(0)
copLeft = (1.0, 0.0, 0.0)
copRight = (0.0, 1.0, 0.0)
zmp = (0.5, 0.5, 0.0)
print()
print("copLeft: %s" % (estimator.copLeft.value, ))
assertApprox(estimator.copLeft.value, copLeft)
print("copRight: %s" % (estimator.copRight.value, ))
assertApprox(estimator.copRight.value, copRight)
print("zmp: %s" % (estimator.zmp.value, ))
assertApprox(estimator.zmp.value, zmp)
# --- Test emergency stop
print()
print("--- Test emergency stop ---")
print()
estimator.emergencyStop.recompute(0)
stop = estimator.emergencyStop.value
print("emergencyStop: %d" % stop)
np.testing.assert_equal(stop, 0)
lfw = conf.lfw
ftc.setLeftFootWeight(lfw)
ftc.setRightFootWeight(rfw)
print('Weights of both feet set to {0}'.format(rfw))
print('The robot should be in the air.')
ftc.right_foot_force_in.value = [1, 1, 8, 1, 1, 1]
ftc.left_foot_force_in.value = [1, 1, 6, 1, 1, 1]
print("Let's calibrate the ft sensors...")
ftc.calibrateFeetSensor()
for i in range(2, 1003):
ftc.right_foot_force_in.value = np.random.randn(6) * 0.0000001 + [
1, 1, 8, 1, 1, 1
]
ftc.left_foot_force_in.value = np.random.randn(6) * 0.0000001 + [
1, 1, 6, 1, 1, 1
]
ftc.right_foot_force_out.recompute(i)
ftc.left_foot_force_out.recompute(i)
assertApprox(ftc.right_foot_force_out.value, np.array((0, 0, -rfw, 0, 0, 0)),
5)
assertApprox(ftc.left_foot_force_out.value, np.array((0, 0, -lfw, 0, 0, 0)), 5)
print("Ft sensors calibrated!")
print("Value outputed after calibration:")
print(ftc.right_foot_force_out.value)
print(ftc.left_foot_force_out.value)
print("expected global left wrench: %s" % str(wrenchLeft))
print("expected global right wrench: %s" % str(wrenchRight))
print("expected ankle left wrench: %s" % str(ankleWrenchLeft))
print("expected ankle right wrench: %s" % str(ankleWrenchRight))
copLeft = [float(com[0] - leftPos.translation[0]), 0., 0.]
copRight = [float(com[0] - rightPos.translation[0]), 0., 0.]
print("expected sole left CoP: %s" % str(copLeft))
print("expected sole right CoP: %s" % str(copRight))
print()
distribute.zmpRef.recompute(0)
print("resulting global wrench: %s" % str(distribute.wrenchRef.value))
assertApprox(wrench, distribute.wrenchRef.value, 2)
print("resulting global left wrench: %s" % str(distribute.wrenchLeft.value))
assertApprox(wrenchLeft, distribute.wrenchLeft.value, 3)
print("resulting global right wrench: %s" % str(distribute.wrenchRight.value))
assertApprox(wrenchRight, distribute.wrenchRight.value, 3)
distribute.ankleWrenchLeft.recompute(0)
distribute.ankleWrenchRight.recompute(0)
print("resulting ankle left wrench: %s" %
str(distribute.ankleWrenchLeft.value))
assertApprox(ankleWrenchLeft, distribute.ankleWrenchLeft.value, 3)
print("resulting ankle right wrench: %s" %
str(distribute.ankleWrenchRight.value))
assertApprox(ankleWrenchRight, distribute.ankleWrenchRight.value, 3)
dcm_controller.omega.value = omega
dcm_controller.com.value = comDes
dcm_controller.dcm.value = comDes
dcm_controller.zmpDes.value = zmpDes
dcm_controller.dcmDes.value = dcmDes
dcm_controller.init(dt)
# --- Wrench distribution ---
print("--- Wrench distribution ---")
distribute = create_distribute_wrench(distribute_conf)
distribute.q.value = halfSitting
plug(dcm_controller.wrenchRef, distribute.wrenchDes)
distribute.init(robot_name)
distribute.zmpRef.recompute(0)
print("reference wrench: %s" % str(dcm_controller.wrenchRef.value))
assertApprox(wrench, dcm_controller.wrenchRef.value, 3)
print("resulting wrench: %s" % str(distribute.wrenchRef.value))
assertApprox(wrench, distribute.wrenchRef.value, 2)
print("resulting left wrench: %s" % str(distribute.wrenchLeft.value))
assertApprox(wrenchLeft, distribute.wrenchLeft.value, 3)
print("resulting right wrench: %s" % str(distribute.wrenchRight.value))
assertApprox(wrenchRight, distribute.wrenchRight.value, 3)
controller.vLeft.recompute(0)
controller.gainRight.recompute(0)
controller.gainLeft.recompute(0)
# 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))
print()
assertApprox(vRight, controller.vRight.value)
assertApprox(vLeft, controller.vLeft.value)
print("gainRight: %s" % str(controller.gainRight.value))
print("gainLeft: %s" % str(controller.gainLeft.value))
print()
assertApprox(gainDouble, controller.gainRight.value)
assertApprox(gainDouble, controller.gainLeft.value)
print("---- Left support ----")
controller.phase.value = 1
controller.vRight.recompute(1)
controller.vLeft.recompute(1)
controller.gainRight.recompute(1)
data2 = model.createData()
pin.framesForwardKinematics(model, data2, q_est)
print(data2.oMf[rightId])
print(data2.oMf[leftId])
# --- DCM estimator ---
print("--- DCM estimator ---")
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.init(dt, robot_name)
plug(e2q.quaternion, dcm_estimator.q)
plug(base_estimator.v, dcm_estimator.v)
dcm_estimator.c.recompute(0)
print(dcm_estimator.c.value)
# --- Direct CoM ---
print("--- Direct CoM ---")
print(com.flatten().tolist()[0])
# --- Raw CoM difference ---
print("--- Raw CoM difference ---")
com_rawdiff = np.matrix(dcm_estimator.c.value).T - com
print(com_rawdiff.flatten().tolist()[0])
# --- Relative CoM difference ---
print("--- Relative CoM difference ---")
com_reldiff = np.matrix(dcm_estimator.c.value).T - comRel
print(com_reldiff.flatten().tolist()[0])
assertApprox(np.matrix(dcm_estimator.c.value).T, comRel, 3)
from sot_talos_balance.euler_to_quat import EulerToQuat
from sot_talos_balance.pose_quaternion_to_matrix_homo import PoseQuaternionToMatrixHomo
from sot_talos_balance.quat_to_euler import QuatToEuler
# --- Euler to quat ---
print("--- Euler to quat ---")
signal_in = [0.0, 0.0, 0.5, 0.0, 0.0, np.pi, 0.2, 0.6]
e2q = EulerToQuat('e2q')
e2q.euler.value = signal_in
print(e2q.euler.value)
e2q.quaternion.recompute(0)
print(e2q.quaternion.value)
assertApprox(e2q.quaternion.value,
[0.0, 0.0, 0.5, 0.0, 0.0, 1.0, 0.0, 0.2, 0.6], 6)
# --- Quat to Euler ---
print("--- Quat to Euler ---")
signal_in = [0.0, 0.0, 0.5, 0.0, 0.0, 1.0, 0.0, 0.2, 0.6]
q2e = QuatToEuler('q2e')
q2e.quaternion.value = signal_in
print(q2e.quaternion.value)
q2e.euler.recompute(0)
print(q2e.euler.value)
assertApprox(q2e.euler.value, [0.0, 0.0, 0.5, 0.0, 0.0, np.pi, 0.2, 0.6], 6)
# --- Quat to homogeneous ---
print("--- Quat to homogeneous ---")
model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer())
model.lowerPositionLimit = np.vstack((np.matrix([-1.] * 7).T, model.lowerPositionLimit[7:]))
model.upperPositionLimit = np.vstack((np.matrix([-1.] * 7).T, model.upperPositionLimit[7:]))
data = model.createData()
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
pin.centerOfMass(model, data, q, v)
print("Expected:")
print("CoM position value: {0}".format(tuple(data.com[0].flat)))
print("CoM velocity value: {0}".format(tuple(data.vcom[0].flat)))
conf.param_server = parameter_server_conf
param_server = ParameterServer("param_server")
param_server.init(dt, conf.param_server.urdfFileName, robot_name)
param_server.setJointsUrdfToSot(conf.param_server.urdftosot)
param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ)
param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ)
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.q.value = list(q.flat)
dcm_estimator.v.value = list(v.flat)
dcm_estimator.init(dt, robot_name)
dcm_estimator.c.recompute(1)
dcm_estimator.dc.recompute(1)
print("Computed:")
print("CoM position value: {0}".format(dcm_estimator.c.value))
assertApprox(data.com[0], np.matrix(dcm_estimator.c.value).T, 3)
print("CoM velocity value: {0}".format(dcm_estimator.dc.value))
assertApprox(data.vcom[0], np.matrix(dcm_estimator.dc.value).T, 3)
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
plug(rf.referenceFrame, wp.referenceFrame)
wp.omega.value = omega
wp.footLeft.value = leftPos.homogeneous.tolist()
wp.footRight.value = rightPos.homogeneous.tolist()
wp.com.value = com
wp.vcom.value = vcom
wp.acom.value = acom
wp.comDes.recompute(0)
wp.dcmDes.recompute(0)
wp.zmpDes.recompute(0)
# --- Output
print("Desired CoM:")
print(wp.comDes.value)
assertApprox(comDes, wp.comDes.value, 3)
print("Desired CoM velocity:")
print(wp.vcomDes.value)
assertApprox(vcomDes, wp.vcomDes.value, 3)
print("Desired CoM acceleration:")
print(wp.acomDes.value)
assertApprox(acomDes, wp.acomDes.value, 3)
print("Desired DCM:")
print(wp.dcmDes.value)
assertApprox(dcmDes, wp.dcmDes.value, 3)
print("Desired ZMP:")
print(wp.zmpDes.value)
assertApprox(zmpDes, wp.zmpDes.value, 3)
import eigenpy
eigenpy.switchToNumpyMatrix()
dt = 1e-3
initstate = [0.] * 9
integrator = SimpleStateIntegrator("integrator")
integrator.init(dt)
integrator.setState(initstate)
integrator.setVelocity([0.] * 9)
t = 0
integrator.control.value = [0.] * 9
integrator.state.recompute(0)
assertApprox(integrator.state.value, initstate)
t += 1
integrator.control.value = [1., 0., 0.] + [0.] * 6
integrator.state.recompute(t)
assertApprox(integrator.state.value, [dt, 0., 0.] + [0.] * 6)
t += 1
integrator.control.value = [0., 1., 0.] + [0.] * 6
integrator.state.recompute(t)
assertApprox(integrator.state.value, [dt, dt, 0.] + [0.] * 6)
t += 1
integrator.control.value = [0., 0., 1.] + [0.] * 6
integrator.state.recompute(t)
assertApprox(integrator.state.value, [dt, dt, dt] + [0.] * 6)
controller.Kp.value = Kp
controller.state.value = state
controller.tauDes.value = tauDes
controller.tau.value = tau
print("\nKp: %s" % (controller.Kp.value, ))
print("\nq: %s" % (controller.state.value, ))
print("tauDes: %s" % (controller.tauDes.value, ))
print("tau: %s" % (controller.tau.value, ))
q = tuple(N_JOINTS * [1.0])
dt = 1
controller.init(dt, N_JOINTS)
controller.setPosition(q)
controller.qRef.recompute(0)
print("\nqRef: %s" % (controller.qRef.value, ))
assertApprox(controller.qRef.value, q)
tauDes = tuple(N_JOINTS * [1.0])
controller.tauDes.value = tauDes
controller.qRef.recompute(1)
print("\nqRef: %s" % (controller.qRef.value, ))
qRef = tuple([q[i] + Kp[i] * (tauDes[i] - tau[i]) * dt for i in range(len(q))])
assertApprox(controller.qRef.value, qRef)
