osnspb = LCP()
s = TimeStepping(impactingBar, t,OSI,osnspb)
k =0
N = int((T-t0)/h)
dataPlot = np.zeros((N+1, 5))
q = bar.q()
v = bar.velocity()
p = bar.p(1)
lambda_ = inter.lambda_(1)
# time loop
while s.hasNextEvent():
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
print('time=', dataPlot[k, 0])
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]/h
dataPlot[k, 4] = lambda_[0]
k += 1
s.nextStep()
dataPlot.resize(k,5)
import matplotlib.pyplot as plt
lambda_ = inter.lambda_(1)
#
# initial data
#
dataPlot[0, 0] = t0
dataPlot[0, 1] = q[0]
dataPlot[0, 2] = v[0]
dataPlot[0, 3] = p[0]
dataPlot[0, 4] = lambda_[0]
k = 1
# time loop
while s.hasNextEvent():
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]
dataPlot[k, 4] = lambda_[0]
k += 1
s.nextStep()
#
# comparison with the reference file
#
from siconos.kernel import SimpleMatrix, getMatrix
s.insertNonSmoothProblem(osnspb)
s.setComputeResiduY(True)
s.setComputeResiduR(True)
s.setNewtonMaxIteration(30)
s.setNewtonTolerance(1e-14)
# matrix to save data
dataPlot = empty((N + 1, 5))
control = empty((N + 1, ))
dataPlot[0, 0] = t0
dataPlot[0, 1:3] = process.x()
dataPlot[0, 3] = myProcessInteraction.lambda_(0)[0]
dataPlot[0, 4] = myProcessInteraction.lambda_(0)[1]
# time loop
k = 1
while (s.hasNextEvent()):
s.advanceToEvent()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = process.x()[0]
dataPlot[k, 2] = process.x()[1]
dataPlot[k, 3] = myProcessInteraction.lambda_(0)[0]
dataPlot[k, 4] = myProcessInteraction.lambda_(0)[1]
control[k] = process.r()[1]
k += 1
s.nextStep()
#print s.nextTime()
# save to disk
np.savetxt('ZI_Twisting.txt', dataPlot)
np.savetxt('ZI_Twisting_u.txt', control)
# plot interesting stuff
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()):
processSimulation.computeOneStep()
dataPlot[k, 0] = processSimulation.nextTime()
dataPlot[k, 1] = processDS.x()[0]
dataPlot[k, 2] = processDS.x()[1]
dataPlot[k, 3] = processDS.z()[0]
dataPlot[k, 4] = processDS.z()[1]
k += 1
print processSimulation.nextTime()
processSimulation.nextStep()
# Resize matrix
dataPlot.resize(k, outputSize)
# Save to disk
savetxt('SMCExampleImplicitOT2-py.dat', dataPlot)
# Plot interesting data
subplot(411)
def test_serialization4():
from siconos.kernel import LagrangianLinearTIDS, NewtonImpactNSL, \
LagrangianLinearTIR, Interaction, Model, MoreauJeanOSI, TimeDiscretisation, LCP, TimeStepping
from numpy import array, eye, empty
t0 = 0 # start time
T = 10 # end time
h = 0.005 # time step
r = 0.1 # ball radius
g = 9.81 # gravity
m = 1 # ball mass
e = 0.9 # restitution coeficient
theta = 0.5 # theta scheme
#
# dynamical system
#
x = array([1, 0, 0]) # initial position
v = array([0, 0, 0]) # initial velocity
mass = eye(3) # mass matrix
mass[2, 2] = 3./5 * r * r
# the dynamical system
ball = LagrangianLinearTIDS(x, v, mass)
# set external forces
weight = array([-m * g, 0, 0])
ball.setFExtPtr(weight)
#
# Interactions
#
# ball-floor
H = array([[1, 0, 0]])
nslaw = NewtonImpactNSL(e)
relation = LagrangianLinearTIR(H)
inter = Interaction(1, nslaw, relation)
#
# Model
#
first_bouncingBall = Model(t0, T)
# add the dynamical system to the non smooth dynamical system
first_bouncingBall.nonSmoothDynamicalSystem().insertDynamicalSystem(ball)
# link the interaction and the dynamical system
first_bouncingBall.nonSmoothDynamicalSystem().link(inter, ball)
#
# Simulation
#
# (1) OneStepIntegrators
OSI = MoreauJeanOSI(theta)
# (2) Time discretisation --
t = TimeDiscretisation(t0, h)
# (3) one step non smooth problem
osnspb = LCP()
# (4) Simulation setup with (1) (2) (3)
s = TimeStepping(t)
s.insertIntegrator(OSI)
s.insertNonSmoothProblem(osnspb)
# end of model definition
#
# computation
#
# simulation initialization
first_bouncingBall.setSimulation(s)
first_bouncingBall.initialize()
#
# save and load data from xml and .dat
#
from siconos.io.io_base import save, load
save(first_bouncingBall, "bouncingBall.xml")
bouncingBall = load("bouncingBall.xml")
# the number of time steps
N = (T-t0)/h+1
# Get the values to be plotted
# ->saved in a matrix dataPlot
dataPlot = empty((N, 5))
#
# numpy pointers on dense Siconos vectors
#
q = ball.q()
v = ball.velocity()
p = ball.p(1)
lambda_ = inter.lambda_(1)
#
# initial data
#
dataPlot[0, 0] = t0
dataPlot[0, 1] = q[0]
dataPlot[0, 2] = v[0]
dataPlot[0, 3] = p[0]
dataPlot[0, 4] = lambda_[0]
k = 1
# time loop
while(s.hasNextEvent()):
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]
dataPlot[k, 4] = lambda_[0]
k += 1
print(s.nextTime())
s.nextStep()
#
# comparison with the reference file
#
from siconos.kernel import SimpleMatrix, getMatrix
from numpy.linalg import norm
ref = getMatrix(SimpleMatrix(os.path.join(working_dir,
"data/result.ref")))
assert (norm(dataPlot - ref) < 1e-12)
def test_bouncing_ball1():
from siconos.kernel import LagrangianLinearTIDS, NewtonImpactNSL, \
LagrangianLinearTIR, Interaction, Model, MoreauJeanOSI, TimeDiscretisation, LCP, TimeStepping
from numpy import array, eye, empty
t0 = 0 # start time
T = 10 # end time
h = 0.005 # time step
r = 0.1 # ball radius
g = 9.81 # gravity
m = 1 # ball mass
e = 0.9 # restitution coeficient
theta = 0.5 # theta scheme
#
# dynamical system
#
x = array([1, 0, 0]) # initial position
v = array([0, 0, 0]) # initial velocity
mass = eye(3) # mass matrix
mass[2, 2] = 3./5 * r * r
# the dynamical system
ball = LagrangianLinearTIDS(x, v, mass)
# set external forces
weight = array([-m * g, 0, 0])
ball.setFExtPtr(weight)
#
# Interactions
#
# ball-floor
H = array([[1, 0, 0]])
nslaw = NewtonImpactNSL(e)
relation = LagrangianLinearTIR(H)
inter = Interaction(1, nslaw, relation)
#
# Model
#
bouncingBall = Model(t0, T)
# add the dynamical system to the non smooth dynamical system
bouncingBall.nonSmoothDynamicalSystem().insertDynamicalSystem(ball)
# link the interaction and the dynamical system
bouncingBall.nonSmoothDynamicalSystem().link(inter, ball)
#
# Simulation
#
# (1) OneStepIntegrators
OSI = MoreauJeanOSI(theta)
OSI.insertDynamicalSystem(ball)
# (2) Time discretisation --
t = TimeDiscretisation(t0, h)
# (3) one step non smooth problem
osnspb = LCP()
# (4) Simulation setup with (1) (2) (3)
s = TimeStepping(t)
s.insertIntegrator(OSI)
s.insertNonSmoothProblem(osnspb)
# end of model definition
#
# computation
#
# simulation initialization
bouncingBall.initialize(s)
#
# save and load data from xml and .dat
#
try:
from siconos.io import save
save(bouncingBall, "bouncingBall.xml")
save(bouncingBall, "bouncingBall.bin")
except:
print("Warning : could not import save from siconos.io")
# the number of time steps
N = (T-t0)/h+1
# Get the values to be plotted
# ->saved in a matrix dataPlot
dataPlot = empty((N, 5))
#
# numpy pointers on dense Siconos vectors
#
q = ball.q()
v = ball.velocity()
p = ball.p(1)
lambda_ = inter.lambda_(1)
#
# initial data
#
dataPlot[0, 0] = t0
dataPlot[0, 1] = q[0]
dataPlot[0, 2] = v[0]
dataPlot[0, 3] = p[0]
dataPlot[0, 4] = lambda_[0]
k = 1
# time loop
while(s.hasNextEvent()):
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]
dataPlot[k, 4] = lambda_[0]
k += 1
#print(s.nextTime())
s.nextStep()
#
# comparison with the reference file
#
from siconos.kernel import SimpleMatrix, getMatrix
from numpy.linalg import norm
ref = getMatrix(SimpleMatrix(os.path.join(working_dir, "data/result.ref")))
assert (norm(dataPlot - ref) < 1e-12)
# numpy pointers on dense Siconos vectors
#
q = body.q()
v = body.velocity()
#
# initial data
#
dataPlot[0, 0] = t0
dataPlot[0, 1] = q[2]
dataPlot[0, 2] = v[2]
k = 1
# time loop
while (simulation.hasNextEvent()):
# Add a second box dynamically to the simulation
if k == 100:
ds = makeBox(pos=3.0, vel=0.0)
bouncingBox.nonSmoothDynamicalSystem().insertDynamicalSystem(ds)
simulation.prepareIntegratorForDS(osi, ds, bouncingBox,
simulation.nextTime())
simulation.computeOneStep()
dataPlot[k, 0] = simulation.nextTime()
dataPlot[k, 1] = q[2]
dataPlot[k, 2] = v[2]
#if (broadphase.collisionWorld().getDispatcher().getNumManifolds() > 0):
s.insertNonSmoothProblem(osnspb)
s.setComputeResiduY(True)
s.setComputeResiduR(True)
filippov.initialize(s);
# matrix to save data
dataPlot = empty((N+1,5))
control = empty((N+1,))
dataPlot[0, 0] = t0
dataPlot[0, 1:3] = process.x()
dataPlot[0, 3] = myProcessInteraction.lambda_(0)[0]
dataPlot[0, 4] = myProcessInteraction.lambda_(0)[1]
# time loop
k = 1
while(s.hasNextEvent()):
s.newtonSolve(1e-14, 30)
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = process.x()[0]
dataPlot[k, 2] = process.x()[1]
dataPlot[k, 3] = myProcessInteraction.lambda_(0)[0]
dataPlot[k, 4] = myProcessInteraction.lambda_(0)[1]
control[k] = process.r()[1]
k += 1
s.nextStep()
#print s.nextTime()
# save to disk
np.savetxt('output.txt', dataPlot)
# plot interesting stuff
plt.subplot(411)
dataPlot[k, 19] = rotationVector.getValue(1)
dataPlot[k, 20] = rotationVector.getValue(2)
dataPlot[k, 22] = h* omega[0]
dataPlot[k, 23] = h* omega[1]
dataPlot[k, 24] = h* omega[2]
dataPlot[k, 25] = np.linalg.norm(h*omega)
k = 1
# time loop
while(s.hasNextEvent() and k < N):
# print(' ' )
# print (
# '------- k = ',
# k,
# '-----------------------------------------')
# print(' ' )
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = q[1]
dataPlot[k, 3] = q[2]
dataPlot[k, 4] = q[3]
dataPlot[k, 5] = q[4]
dataPlot[k, 6] = q[5]
dataPlot[k, 7] = q[6]
dataPlot[0, 4] = lambda_[0]
dataPlot[0, 5] = math.acos(q[3])
dataPlot[0, 6] = linalg.norm(relation.contactForce())
dataPlot[0, 7] = q[0]
dataPlot[0, 8] = q[1]
dataPlot[0, 9] = q[2]
dataPlot[0, 10] = q[3]
dataPlot[0, 11] = q[4]
dataPlot[0, 12] = q[5]
dataPlot[0, 13] = q[6]
dataPlot[0, 14] = v[1]
dataPlot[0, 15] = v[2]
k = 1
# time loop
while(s.hasNextEvent() and k < 2000):
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]
dataPlot[k, 4] = lambda_[0]
dataPlot[k, 5] = math.acos(q[3])
dataPlot[k, 6] = linalg.norm(relation.contactForce())
dataPlot[k, 7] = q[0]
dataPlot[k, 8] = q[1]
dataPlot[k, 9] = q[2]
dataPlot[k, 10] = q[3]
dataPlot[k, 11] = q[4]
dataPlot[k, 12] = q[5]
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)
dataPlot[0, 4] = lambda_[0]
dataPlot[0, 5] = math.acos(q[3])
dataPlot[0, 6] = linalg.norm(relation.contactForce())
dataPlot[0, 7] = q[0]
dataPlot[0, 8] = q[1]
dataPlot[0, 9] = q[2]
dataPlot[0, 10] = q[3]
dataPlot[0, 11] = q[4]
dataPlot[0, 12] = q[5]
dataPlot[0, 13] = q[6]
dataPlot[0, 14] = v[1]
dataPlot[0, 15] = v[2]
k = 1
# time loop
while (s.hasNextEvent() and k < 2000):
s.computeOneStep()
dataPlot[k, 0] = s.nextTime()
dataPlot[k, 1] = q[0]
dataPlot[k, 2] = v[0]
dataPlot[k, 3] = p[0]
dataPlot[k, 4] = lambda_[0]
dataPlot[k, 5] = math.acos(q[3])
dataPlot[k, 6] = linalg.norm(relation.contactForce())
dataPlot[k, 7] = q[0]
dataPlot[k, 8] = q[1]
dataPlot[k, 9] = q[2]
dataPlot[k, 10] = q[3]
dataPlot[k, 11] = q[4]
dataPlot[k, 12] = q[5]
