_file = "systems/urdf/sliding_block.urdf" cc_erm_control = np.loadtxt('data/slidingblock/erm_w_cc/control.txt') erm_control= np.loadtxt('data/slidingblock/erm/control.txt') cc_erm_control = cc_erm_control.reshape([5, 101]) erm_control = erm_control.reshape([5, 101]) reference_control = np.loadtxt('data/slidingblock/warm_start/u.txt') # u = u.reshape([1, 101]) h = 0.01 frictions = np.array([ 0.3, 0.43, 0.57, 0.7]) x0 = np.array([0, 0.5, 0, 0]) fig, axs = plt.subplots(3,1) for i in range(len(frictions)): plant = TimeSteppingMultibodyPlant(file= _file, terrain = FlatTerrain(friction = frictions[i])) plant.Finalize() control = reference_control t, x, f = plant.simulate(h, x0, u = control.reshape(1,101), N = 101) y_bar = np.zeros(t.shape) + 5 axs[0].plot(t, y_bar, 'k', linewidth =1) axs[0].plot(t, x[0, :], linewidth =3, label = '$\mu$ = {f}'.format(f = frictions[i])) axs[0].set_yticks([0, 2, 4, 6]) axs[0].set_ylim([0,6.1]) axs[0].set_xlim([0,1]) # Hide the right and top spines axs[0].spines['right'].set_visible(False) axs[0].spines['top'].set_visible(False) axs[0].yaxis.set_ticks_position('left') for i in range(len(frictions)): plant = TimeSteppingMultibodyPlant(file= _file, terrain = FlatTerrain(friction = frictions[i])) plant.Finalize() control = erm_control[4]
h = 0.01 frictions = np.array([0.3, 0.4, 0.5, 0.6, 0.7]) x0 = np.array([0, 0.5, 0, 0]) reference = np.ones([101, 1]) * 5 reference_final_position = np.zeros(frictions.shape) erm_final_position = np.zeros([5, len(frictions)]) cc_erm_final_position = np.zeros([5, len(frictions)]) final_position = np.zeros([11, len(frictions)]) # reference control for i in range(len(frictions)): plant = TimeSteppingMultibodyPlant( file=_file, terrain=FlatTerrain(friction=frictions[i])) plant.Finalize() t, x_ref, f = plant.simulate(h, x0, u=reference_control.reshape(1, 101), N=101) final_position[0, i] = x_ref[0, 100] for n in range(5): t, x_erm, f = plant.simulate(h, x0, u=erm_control[n, :].reshape(1, 101), N=101) t, x_erm_cc, f = plant.simulate(h, x0, u=cc_erm_control[n, :].reshape(1, 101), N=101) final_position[2 * n + 1, i] = x_erm[0, 100] final_position[2 * n + 2, i] = x_erm_cc[0, 100] # axs1.plot(t, x[0,:], linewidth=1.5, label = '$\mu$ = {f}'.format(f = frictions[i])) # axs1.set_title('ERM + CC Control $\sigma$ = 1')