elif t >= time_offset / 2.0 * conf.dt and t < time_offset * conf.dt:
sampleCom = TSID.TrajectorySample(3)
sampleCom.pos(np.array([0.6, 0, 0.8]))
tsid.comTask.setReference(sampleCom)
elif t >= time_offset * conf.dt:
if cs > 40:
print (tsid.robot.framePosition(tsid.formulation.data(), tsid.LF))
print (aa)
if Walk_phases.getContactType(cs) == 0 or Walk_phases.getContactType(cs) == 2: # DSP
if sequence_change and Walk_phases.getContactType(cs) == 0:
if Walk_phases.getContactType(cs+1) == 'Rf':
tsid.remove_contact_LF(Walk_phases.getFinalTime(cs)- (t-time_offset * conf.dt))
else:
tsid.remove_contact_RF(Walk_phases.getFinalTime(cs) - (t-time_offset * conf.dt) )
sequence_change = False
elif sequence_change and Walk_phases.getContactType(cs) == 2:
if Walk_phases.getContactType(cs-1) == 'Rf':
tsid.add_contact_LF2(Walk_phases.p[cs].oMf_Lf)
else:
tsid.add_contact_RF2(Walk_phases.p[cs].oMf_Rf)
sequence_change = False
sampleCom = TSID.TrajectorySample(3)
sampleCom.pos(CurveSet.com_traj[i-time_offset])
sampleCom.vel(CurveSet.com_dot_traj[i-time_offset])
tsid.comTask.setReference(sampleCom)
sampleAM = TSID.TrajectorySample(3)
sampleAM.pos(CurveSet.L_traj[i-time_offset])
t = -conf.T_pre
q, v = tsid.q, tsid.v
for i in range(-N_pre, N + N_post):
time_start = time.time()
if i == 0:
print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF()
elif i > 0 and i < N - 1:
if contact_phase[i] != contact_phase[i - 1]:
print("Time %.3f Changing contact phase from %s to %s" %
(t, contact_phase[i - 1], contact_phase[i]))
if contact_phase[i] == 'left':
tsid.add_contact_LF()
tsid.remove_contact_RF()
else:
tsid.add_contact_RF()
tsid.remove_contact_LF()
if i < 0:
tsid.set_com_ref(com_pos_ref[:, 0], 0 * com_vel_ref[:, 0],
0 * com_acc_ref[:, 0])
elif i < N:
tsid.set_com_ref(com_pos_ref[:, i], com_vel_ref[:, i], com_acc_ref[:,
i])
tsid.set_LF_3d_ref(x_LF_ref[:, i], dx_LF_ref[:, i], ddx_LF_ref[:, i])
tsid.set_RF_3d_ref(x_RF_ref[:, i], dx_RF_ref[:, i], ddx_RF_ref[:, i])
HQPData = tsid.formulation.computeProblemData(t, q, v)
