forceRight = [0.0, 0.0, fz]
lever = float(com[0] - rightPos.translation[0])
tauy = -fz * lever
wrenchLeft = forceLeft + [0.0, tauy, 0.0]
wrenchRight = forceRight + [0.0, tauy, 0.0]
# --- Parameter server ---
print("--- Parameter server ---")
param_server = create_parameter_server(param_server_conf, dt)
# --- Base estimator ---
print("--- Base estimator ---")
conf = base_estimator_conf
base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
base_estimator.joint_positions.value = halfSitting[7:]
base_estimator.forceLLEG.value = wrenchLeft
base_estimator.forceRLEG.value = wrenchRight
base_estimator.dforceLLEG.value = [0.0] * 6
base_estimator.dforceRLEG.value = [0.0] * 6
base_estimator.joint_velocities.value = [0.0] * (model.nv - 6)
base_estimator.imu_quaternion.value = [0.0] * 3 + [1.0]
base_estimator.gyroscope.value = [0.0] * 3
base_estimator.accelerometer.value = [0.0] * 3
base_estimator.K_fb_feet_poses.value = conf.K_fb_feet_poses
base_estimator.w_lf_in.value = conf.w_lf_in
base_estimator.w_rf_in.value = conf.w_rf_in
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
from sot_talos_balance.create_entities_utils import Bunch, Example,\
TalosBaseEstimator, create_parameter_server
dt = 0.001
conf = Bunch()
robot_name = 'robot'
param_server = create_parameter_server(param_server_conf, dt)
base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
example = Example('example')
example.firstAddend.value = 0.
example.secondAddend.value = 0.
example.init(robot_name)