HQPData = invdyn.computeProblemData(t, q + noise_q, v + noise_v)
sol = solver.solve_local(HQPData, delayed_sol)
data.local_time = time.time() - local_start
tau = invdyn.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol)
else:
solver.compute_slack(HQPData, delayed_sol)
sol = delayed_sol
tau = delayed_data.tau_fullqp
dv = delayed_data.dv_fullqp
data.local_time = 0
data.slack = sol.slack
data.tau = tau
data.dv = dv
data.activeset = sol.activeSet
# data logging from noiseless measurements
HQPData = invdyn.computeProblemData(t, q, v)
data.com.pos = robot.com(invdyn.data())
data.com.pos_ref = sampleCom.pos()
data.left_foot.pos = left_foot.pos(t, q, v).translation
data.left_foot.pos_ref = left_foot.motion_ref.pos()[:3]
data.right_foot.pos = right_foot.pos(t, q, v).translation
data.right_foot.pos_ref = right_foot.motion_ref.pos()[:3]
# pinocchio
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = se3.integrate(robot.model(), q, dt * v_mean)
pbwrapper.reset_state(q, v)
