def experiment(self, x0 = np.array([0,0]) , tf = 10 , dt = 0.05 , plot = False ):
""" Simulate (EULER) and animate robot """
n = int( ( tf + 0.0 ) / dt + 1 )
self.DS.Sim = RDDS.Simulation( self.DS , tf , n , 'euler' )
self.DS.Sim.x0 = x0
self.DS.Sim.compute()
if plot:
self.DS.PTS = np.zeros((2,2,n))
for i in range(n):
self.DS.PTS[:,:,i] = self.DS.fwd_kinematic( self.DS.Sim.x_sol_CL[i,0] ) # Forward kinematic
self.fig = plt.figure()
self.ax = self.fig.add_subplot(111, autoscale_on=False, xlim=(-2, 2), ylim=(-2, 2))
self.ax.grid()
self.DS.line, = self.ax.plot([], [], 'o-', lw=2)
self.DS.time_template = 'time = %.1fs'
self.DS.time_text = self.ax.text(0.05, 0.9, '', transform=self.ax.transAxes)
self.DS.ani = animation.FuncAnimation( self.fig, self.DS.__animateStop__, n, interval=25, blit=True, init_func=self.DS.__ani_init__ , repeat=False)
plt.show()
#Learning
for i in range(n):
self.Qlearn3( self.DS.Sim.x_sol_CL[i,:] , self.DS.Sim.u_sol_CL[i,:] )
def Rollout( self , x , t ):
self.sim_n = self.horizon / self.sim_dt + 1
self.Sim = DS.Simulation( self.R , t + self.horizon , self.sim_n , 'euler' )
self.Sim.J = 0
self.Sim.x0 = x
self.Sim.t0 = t
# Cost: torque quadratic cost
self.Sim.Q = np.zeros( ( self.R.n , self.R.n ) )
self.Sim.H = np.zeros( ( self.R.n , self.R.n ) )
self.Sim.R = np.diag( np.append( np.ones( self.R.dof ) , 0 ) )
self.Sim.domain_check = self.domain_check
self.Sim.domain_fail_cost = self.domain_fail_cost
#Compute
self.Sim.compute()
#self.Sim.plot_CL()
#raw_input('Press <ENTER> to continue')
return self.Sim.J
def plot_open_loop_solution(self):
""" """
self.OL_SIM = DS.Simulation(self.DS, self.time_to_goal,
self.solution_length)
self.OL_SIM.t = self.solution[2].T
self.OL_SIM.x_sol_CL = self.solution[0].T
self.OL_SIM.u_sol_CL = self.solution[1].T
self.OL_SIM.plot_CL('x')
self.OL_SIM.plot_CL('u')
def computeSim(self,
x0=np.array([0, 0, 0, 0]),
tf=10,
n=10001,
solver='ode'):
""" Simulate the robot """
self.Sim = RDDS.Simulation(self, tf, n, solver)
self.Sim.x0 = x0
# Integrate EoM
self.Sim.compute()
def animate3D_solution(self, time_scale=1):
"""
animate robots with open loop solution
--------------------------------------
only works if self.DS is a 3D mainpulator class
"""
self.OL_SIM = DS.Simulation(self.DS, self.time_to_goal,
self.solution_length)
self.OL_SIM.t = self.solution[2].T
self.OL_SIM.x_sol_CL = self.solution[0].T
self.OL_SIM.u_sol_CL = self.solution[1].T
self.DS.Sim = self.OL_SIM
self.DS.animate3DSim(time_scale)
R.Sim.plot_CL()
R.Sim.plots[0].set_yticks( [-4,0] )
R.Sim.plots[1].set_yticks( [-4,0, 4] )
R.Sim.plots[2].set_yticks( [-8,0,8] )
R.Sim.plots[3].set_ylim( 0,11 )
R.Sim.plots[3].set_yticks( [1,10] )
R.Sim.plots[3].set_xlim( 0,10 )
R.Sim.plots[3].set_xticks( t_ticks )
R.Sim.fig.set_size_inches(4,2.5)
R.Sim.fig.canvas.draw()
if save_fig:
R.Sim.fig.savefig( all_fig , format='pdf', bbox_inches='tight', pad_inches=0.05)
# phase plane 1
PP1 = DS.PhasePlot( R )
PP1.y1max = 1
PP1.y1min = -5
PP1.y2max = 4
PP1.y2min = -4
name = 'output/simpp1' + '.pdf'
PP1.CL = False
PP1.color_CL = 'b'
PP1.linewidth = 0.04
PP1.headlength = 3.5
PP1.fontsize = 9
PP1.dpi = 600
PP1.figsize = (3,2)
# Definre domain of interest R.x_ub = np.array([3 * np.pi, 2 * np.pi]) R.x_lb = np.array([0, -2 * np.pi]) # Set of params r = 1 # Gear ratio R.d1 = 0 # Load side damping R.Da = 0 # Actuator side damping R.I1 = 1 R.Ia = 1 R.ubar = np.array([0.0, r]) """ Ploting """ R.PP = DS.PhasePlot(R, 0, 1, False, True) # Figure no 1 name = 'output/pp1' + '.' + format_fig R.PP.color_CL = 'b' R.PP.linewidth = 0.04 R.PP.headlength = 4.5 R.PP.fontsize = 9 R.PP.dpi = 600 R.PP.figsize = (3, 2) R.PP.y1n = 11 R.PP.y2n = 11 R.PP.compute() R.PP.plot() if save_fig:
""" Created on Sat Mar 5 16:01:32 2016 @author: alex """ from AlexRobotics.dynamic import DynamicSystem as RDDS from AlexRobotics.control import linear as RCL import matplotlib.pyplot as plt import numpy as np """ Define system """ # Define dynamic system double_integrator = RDDS.DynamicSystem() # Define controller kp = 1 kd = 1 PD_controller = RCL.PD( kp , kd ) # Asign feedback law to the dynamic system double_integrator.ctl = PD_controller.u """ Simulation and plotting """ # Phase plot #PP = RDDS.PhasePlot( double_integrator ) #PP.compute() #PP.plot()