def test_smc1(): from siconos.kernel import FirstOrderLinearDS, Model, TimeDiscretisation, \ TimeStepping, ZeroOrderHoldOSI, TD_EVENT from siconos.control.simulation import ControlManager from siconos.control.sensor import LinearSensor from siconos.control.controller import LinearSMCOT2 from numpy import eye, empty, zeros import numpy as np from math import ceil, sin # Derive our own version of FirstOrderLinearDS class MyFOLDS(FirstOrderLinearDS): def computeb(self, time): t = sin(50*time) # XXX fix this ! u = [t, -t] self.setb(u) # variable declaration ndof = 2 # Number of degrees of freedom of your system t0 = 0.0 # start time T = 1 # end time h = 1.0e-4 # time step for simulation hControl = 1.0e-2 # time step for control Xinit = 1.0 # initial position N = ceil((T-t0)/h + 10) # number of time steps outputSize = 4 # number of variable to store at each time step # Matrix declaration A = zeros((ndof, ndof)) x0 = [Xinit, -Xinit] Brel = np.array([[0], [1]]) sensorC = eye(ndof) sensorD = zeros((ndof, ndof)) Csurface = [[0, 1.0]] # Simple check if h > hControl: print("hControl must be bigger than h") exit(1) # Declaration of the Dynamical System processDS = MyFOLDS(x0, A) # XXX b is not automatically created ... # processDS.setb([0, 0]) # Model process = Model(t0, T) process.nonSmoothDynamicalSystem().insertDynamicalSystem(processDS) # time discretization processTD = TimeDiscretisation(t0, h) tSensor = TimeDiscretisation(t0, hControl) tActuator = TimeDiscretisation(t0, hControl) # Creation of the Simulation processSimulation = TimeStepping(processTD, 0) processSimulation.setName("plant simulation") # Declaration of the integrator processIntegrator = ZeroOrderHoldOSI() process.nonSmoothDynamicalSystem().setOSI(processDS, processIntegrator) processSimulation.insertIntegrator(processIntegrator) # Actuator, Sensor & ControlManager control = ControlManager(processSimulation) sens = LinearSensor(processDS, sensorC, sensorD) control.addSensorPtr(sens, tSensor) act = LinearSMCOT2(sens) act.setCsurface(Csurface) act.setB(Brel) control.addActuatorPtr(act, tActuator) # Initialization. process.initialize(processSimulation) control.initialize(process) # This is not working right now # eventsManager = s.eventsManager() # Matrix for data storage dataPlot = empty((3*(N+1), outputSize)) dataPlot[0, 0] = t0 dataPlot[0, 1] = processDS.x()[0] dataPlot[0, 2] = processDS.x()[1] dataPlot[0, 3] = act.u()[0] # Main loop k = 1 while processSimulation.hasNextEvent(): if processSimulation.eventsManager().nextEvent().getType() == TD_EVENT: processSimulation.computeOneStep() dataPlot[k, 0] = processSimulation.nextTime() dataPlot[k, 1] = processDS.x()[0] dataPlot[k, 2] = processDS.x()[1] dataPlot[k, 3] = act.u()[0] k += 1 processSimulation.nextStep() # print processSimulation.nextTime() # Resize matrix dataPlot.resize(k, outputSize)
print("hControl must be bigger than h") exit(1) # Declaration of the Dynamical System processDS = MyFOLDS(x0, A) # XXX b is not automatically created ... processDS.setbPtr([0, 0]) # Control Simulation sim = ControlZOHSimulation(t0, T, h) sim.setSaveOnlyMainSimulation(True) sim.addDynamicalSystem(processDS) # Actuator, Sensor & ControlManager sens = LinearSensor(processDS, sensorC) sim.addSensor(sens, hControl) act = LinearSMCOT2(sens) act.setCsurface(Csurface) act.setB(Brel) sim.addActuator(act, hControl) # Initialization sim.initialize() # Run simulation sim.run() # Get data dataPlot = sim.data() # Save to disk savetxt('SMCExampleImplicitOT2-noCplugin-sage-py.dat', dataPlot)
def test_smc2(): from siconos.kernel import FirstOrderLinearDS, getMatrix, SimpleMatrix from siconos.control.sensor import LinearSensor from siconos.control.controller import LinearSMCOT2 from siconos.control.simulation import ControlZOHSimulation from numpy import eye, zeros import numpy as np from math import sin from numpy.linalg import norm # Derive our own version of FirstOrderLinearDS class MyFOLDS(FirstOrderLinearDS): def computeb(self, time): t = sin(50*time) u = [t, -t] self.setb(u) # variable declaration ndof = 2 # Number of degrees of freedom of your system t0 = 0.0 # start time T = 1 # end time h = 1.0e-4 # time step for simulation hControl = 1.0e-2 # time step for control Xinit = 1.0 # initial position # Matrix declaration A = zeros((ndof, ndof)) x0 = [Xinit, -Xinit] sensorC = eye(ndof) sensorD = zeros((ndof, ndof)) Brel = np.array([[0], [1]]) Csurface = [[0, 1.0]] # Simple check if h > hControl: print("hControl must be bigger than h") exit(1) # Declaration of the Dynamical System processDS = MyFOLDS(x0, A) # XXX b is not automatically created ... processDS.setb([0, 0]) sim = ControlZOHSimulation(t0, T, h) sim.addDynamicalSystem(processDS) # time discretisation # Actuator, Sensor & ControlManager sens = LinearSensor(processDS, sensorC, sensorD) sim.addSensor(sens, hControl) act = LinearSMCOT2(sens) act.setCsurface(Csurface) act.setB(Brel) sim.addActuator(act, hControl) # Run the simulation sim.initialize() sim.run() # get the results tmpData = sim.data() dataPlot = tmpData # compare with the reference ref = np.loadtxt(os.path.join(working_dir, "data/smc_2.ref.gz")) np.savetxt("smc_2.dat", dataPlot) print("%e" % norm(dataPlot - ref)) if (norm(dataPlot - ref) > 5e-12): print(dataPlot - ref) print("ERROR: The result is rather different from the reference file.") assert norm(dataPlot - ref) < 5e-12