# file_name = 'talos_contact_switch_R3SO3.cs' # INTTYPE = 'preint' INTTYPE = 'direct' # INTTYPE = 'directSE3' # INTTYPE = 'pinocchio' PATH = 'figs_'+INTTYPE+'/' cs = ContactSequence() print('Loadding cs file: ', examples_dir + file_name) cs.loadFromBinary(examples_dir + file_name) q_traj = cs.concatenateQtrajectories() dq_traj = cs.concatenateDQtrajectories() ddq_traj = cs.concatenateDDQtrajectories() c_traj = cs.concatenateCtrajectories() dc_traj = cs.concatenateDCtrajectories() Lc_traj = cs.concatenateLtrajectories() contact_frames = cs.getAllEffectorsInContact() f_traj_lst = [cs.concatenateContactForceTrajectories(l) for l in contact_frames] print(contact_frames) min_ts = q_traj.min() max_ts = q_traj.max() print('traj dur (s): ', max_ts - min_ts) dt = 1e-3 # discretization timespan # dt = 1e-2 # discretization timespan
def mcapi_playback(name_interface, filename): device = Solo12(name_interface,dt=DT) qc = QualisysClient(ip="140.93.16.160", body_id=0) logger = Logger(device, qualisys=qc) nb_motors = device.nb_motors q_viewer = np.array((7 + nb_motors) * [0.,]) # Load contactsequence from file: cs = ContactSequence(0) cs.loadFromBinary(filename) # extract (q, dq, tau) trajectories: q_t = cs.concatenateQtrajectories() # with the freeflyer configuration dq_t = cs.concatenateDQtrajectories() # with the freeflyer configuration ddq_t = cs.concatenateDDQtrajectories() # with the freeflyer configuration tau_t = cs.concatenateTauTrajectories() # joints torques # Get time interval from planning: t_min = q_t.min() t_max = q_t.max() print("## Complete duration of the motion loaded: ", t_max - t_min) # Sanity checks: assert t_min < t_max assert dq_t.min() == t_min assert ddq_t.min() == t_min assert tau_t.min() == t_min assert dq_t.max() == t_max assert ddq_t.max() == t_max assert tau_t.max() == t_max assert q_t.dim() == 19 assert dq_t.dim() == 18 assert ddq_t.dim() == 18 assert tau_t.dim() == 12 num_steps = ceil((t_max - t_min) / DT) + 1 q_mes_t = np.zeros([8, num_steps]) v_mes_t = np.zeros([8, num_steps]) q_des_t = np.zeros([8, num_steps]) v_des_t = np.zeros([8, num_steps]) tau_des_t = np.zeros([8, num_steps]) tau_send_t = np.zeros([8, num_steps]) tau_mesured_t = np.zeros([8, num_steps]) q_init = config_12_to_8(q_t(t_min)[7:]) device.Init(calibrateEncoders=True, q_init=q_init) t = t_min put_on_the_floor(device, q_init) #CONTROL LOOP *************************************************** t_id = 0 while ((not device.hardware.IsTimeout()) and (t < t_max)): # q_desired = config_12_to_8(q_t(t)[7:]) # remove freeflyer # dq_desired = config_12_to_8(dq_t(t)[6:]) # remove freeflyer # tau_desired = config_12_to_8(tau_t(t)) device.UpdateMeasurment() # tau = compute_pd(q_desired, dq_desired, tau_desired, device) # Parameters of the PD controller KP = 4. KD = 0.05 KT = 1. tau_max = 3. * np.ones(12) # Desired position and velocity for this loop and resulting torques q_desired, v_desired = test_solo12(t) pos_error = q_desired.ravel() - actuators_pos[:] vel_error = v_desired.ravel() - actuators_vel[:] tau = KP * pos_error + KD * vel_error tau = 0.0 * np.maximum(np.minimum(tau, tau_max), -tau_max) # Send desired torques to the robot device.SetDesiredJointTorque(tau) # store desired and mesured data for plotting q_des_t[:, t_id] = q_desired v_des_t[:, t_id] = dq_desired q_mes_t[:, t_id] = device.q_mes v_mes_t[:, t_id] = device.v_mes tau_des_t[:, t_id] = tau_desired tau_send_t[:, t_id] = tau tau_mesured_t[: , t_id] = device.torquesFromCurrentMeasurment # call logger # logger.sample(device, qualisys=qc) device.SendCommand(WaitEndOfCycle=True) if ((device.cpt % 100) == 0): device.Print() q_viewer[3:7] = device.baseOrientation # IMU Attitude q_viewer[7:] = device.q_mes # Encoders t += DT t_id += 1 #**************************************************************** # Whatever happened we send 0 torques to the motors. device.SetDesiredJointTorque([0]*nb_motors) device.SendCommand(WaitEndOfCycle=True) if device.hardware.IsTimeout(): print("Masterboard timeout detected.") print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.") device.hardware.Stop() # Shut down the interface between the computer and the master board # Save the logs of the Logger object # logger.saveAll() # Plot the results times = np.arange(t_min, t_max + DT, DT) plot_mes_des_curve(times, q_mes_t, q_des_t, "Joints positions", "joint position") plot_mes_des_curve(times, v_mes_t, v_des_t, "Joints velocities", "joint velocity") plot_mes_des_curve(times, tau_send_t, tau_des_t, "Joints torque", "Nm") current_t = np.zeros([8, num_steps]) for motor in range(device.nb_motors): current_t[device.motorToUrdf[motor], :] = tau_send_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor] plot_mes_des_curve(times, current_t, title="Motor current", y_label="A") tracking_pos_error = q_des_t - q_mes_t plot_mes_des_curve(times, tracking_pos_error, title="Tracking error") plot_mes_des_curve(times, tau_mesured_t, title="Torque mesured from current", y_label="nM") current_mesured_t = np.zeros([8, num_steps]) for motor in range(device.nb_motors): current_mesured_t[device.motorToUrdf[motor], :] = tau_mesured_t[device.motorToUrdf[motor], :] / device.jointKtSigned[motor] plot_mes_des_curve(times, current_mesured_t, title="Measured motor current", y_label="A") plt.show()