tActuator = TimeDiscretisation(t0, h) s = TimeStepping(t, 0) s.insertIntegrator(OSI) # Actuator, Sensor & ControlManager control = ControlManager(s) sens = LinearSensor(doubleIntegrator, C) control.addSensorPtr(sens, tSensor) act = PID(sens) act.setB(B) control.addActuatorPtr(act, tActuator) # Initialization process.setSimulation(s) process.initialize() control.initialize(process) act.setRef(xFinal) act.setK(K) act.setDeltaT(h) # This is not working right now #eventsManager = s.eventsManager() # Matrix for data storage dataPlot = empty((3*N+3, outputSize)) dataPlot[0, 0] = process.t0() dataPlot[0, 1] = doubleIntegrator.x()[0] dataPlot[0, 2] = doubleIntegrator.x()[1] dataPlot[0, 3] = doubleIntegrator.b()[0] dataPlot[0, 4] = doubleIntegrator.b()[1] # Main loop
tActuator = sk.TimeDiscretisation(t0, h)
s = sk.TimeStepping(t, 0)
s.insertIntegrator(OSI)
# Actuator, Sensor & ControlManager
control = ControlManager(s)
sens = LinearSensor(doubleIntegrator, C)
control.addSensorPtr(sens, tSensor)
act = PID(sens)
act.setB(B)
control.addActuatorPtr(act, tActuator)
# Initialization
process.setSimulation(s)
process.initialize()
control.initialize(process)
act.setRef(xFinal)
act.setK(K)
act.setDeltaT(h)
# This is not working right now
#eventsManager = s.eventsManager()
# Matrix for data storage
dataPlot = zeros((3 * N + 3, outputSize))
dataPlot[0, 0] = process.t0()
dataPlot[0, 1] = doubleIntegrator.x()[0]
dataPlot[0, 2] = doubleIntegrator.x()[1]
if doubleIntegrator.b() is not None:
dataPlot[0, 3] = doubleIntegrator.b()[0]
dataPlot[0, 4] = doubleIntegrator.b()[1]
processSimulation.setName("plant simulation")
# Declaration of the integrator
processIntegrator = ZeroOrderHoldOSI(processDS)
processSimulation.insertIntegrator(processIntegrator)
# Actuator, Sensor & ControlManager
control = ControlManager(process)
sens = LinearSensor(tSensor, processDS, sensorC)
control.addSensorPtr(sens)
act = LinearSMCOT2(tActuator, processDS)
act.setCsurfacePtr(Csurface)
act.addSensorPtr(sens)
control.addActuatorPtr(act)
# Initialization
process.initialize(processSimulation)
control.initialize()
# This is not working right now
#eventsManager = s.eventsManager()
# Matrix for data storage
dataPlot = empty((N+1, outputSize))
#dataPlot[0, 0] = processDS.t0()
dataPlot[0, 0] = t0
dataPlot[0, 1] = processDS.x()[0]
dataPlot[0, 2] = processDS.x()[1]
dataPlot[0, 3] = processDS.z()[0]
dataPlot[0, 4] = processDS.z()[1]
# Main loop
k = 1
while(processSimulation.hasNextEvent()):
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)