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')