for j in range(nq):
q_opt = qc_opt[j::14]
q_opt_30fps = []
for i in range(np.size(q_opt) - 1):
q_opt_30fps.append(
np.linspace(q_opt[i],
q_opt[i + 1],
frames / N + 1,
endpoint=False))
q_opt_30fps = np.concatenate(q_opt_30fps).tolist()
new_qc_opt.append(q_opt_30fps)
if Check.Rviz == True:
print('')
print('############## Rviz simulation ############## ')
ta.talker(new_qc_opt)
lbTe = []
ubTe = []
ubtq = []
lbtq = []
#Save the csv file
SaveCsv(folder, qc_opt, qcd_opt, F_opt, tau_LR, tau_RR, lbtq, ubtq,
box_pos, box_rpy, Tw, lbTe, ubTe)
print('Done')
Roll = np.arctan2(R1[2, 0], R1[2, 1])
Pitch = np.arccos(-R1[2, 2])
Yaw = -np.arctan2(R1[0, 2], R1[1, 2])
box_rpy.append([Roll, Pitch, Yaw])
box_pos.append(np.round((E1 + E2) / 2, 3))
k = k + 1
#Make a unique list
qc_opt = np.concatenate(qc_opt).tolist()
qcd_opt = np.concatenate(qcd_opt).tolist()
F_opt = np.concatenate(F_opt).tolist()
tau_LR = np.concatenate(np.concatenate(tau_LR).tolist())
tau_RR = np.concatenate(np.concatenate(tau_RR).tolist())
box_pos = np.concatenate(np.concatenate(box_pos))
box_rpy = np.concatenate(box_rpy)
if Check.Rviz == True:
print('')
print('############## Rviz simulation ############## ')
ta.talker(qc_opt)
#Save the csv file
SaveCsv(folder, qc_opt, qcd_opt, F_opt, tau_LR, tau_RR, lbt, ubt, box_pos,
box_rpy)
print('Done')
Tw = np.concatenate(Tw)
frames = 30 * T
new_qc_opt = []
for j in range(nq):
q_opt = qc_opt[j::14]
q_opt_30fps = []
for i in range(np.size(q_opt)-1):
q_opt_30fps.append(np.linspace(q_opt[i],q_opt[i+1],frames/N+1,endpoint=False))
q_opt_30fps = np.concatenate(q_opt_30fps).tolist()
new_qc_opt.append(q_opt_30fps)
if Check.Rviz == True:
print('')
print('############## Rviz simulation ############## ')
ta.talker(np.round(new_qc_opt,4))
lbTe = [];
ubTe = [];
ubtq = [];
lbtq = [];
#Save the csv file
SaveCsv(folder,qc_opt,qcd_opt,F_opt_glob,F_opt_loc,tau_LR,tau_RR,lbtq,ubtq,box_pos,box_rpy,Tw,lbTe,ubTe)
print('Done')