Esempio n. 1
0
    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
Esempio n. 3
0
    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')
Esempio n. 4
0
    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()
Esempio n. 5
0
    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)
Esempio n. 6
0
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)
Esempio n. 7
0
# 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()