<commit_before>﻿#!/usr/bin/python
# -*- coding: utf-8 -*-
from __future__ import division
# Rendering Tests

import vtk
import os.path
from time import sleep
from numpy import sin, cos, pi
from numpy import array as narray
from scipy.integrate import ode

#---------------------------------------------------------------------
# global settings
#--------------------------------------------------------------------- 
dt = 0.01       # stepwidth
q0 = [0, 0.01, 0, 0]    # initial minimal state vector (r, dr, theta, dtheta)'

M = 0.05    # kg
R = 0.01    # m
J = 0.02    # kgm^2
Jb = 2e-6   # kgm^2
G = 9.81    # m/s^2
l = 0.5     # m
beam_width = 0.01
beam_depth = 0.03

scale = 1


#---------------------------------------------------------------------
# plotting 
#---------------------------------------------------------------------
table = vtk.vtkTable()
#Time:
arrTime = vtk.vtkFloatArray()
arrTime.SetName("time")
#Error: yd-r
arrE = vtk.vtkFloatArray()
arrE.SetName("epsilon")
#psi3
arrPsi3 = vtk.vtkFloatArray()
arrPsi3.SetName("psi3")
#theta
arrTheta = vtk.vtkFloatArray()
arrTheta.SetName("theta")
#tau
arrTau = vtk.vtkFloatArray()
arrTau.SetName("tau")
# add rows to table 
table.AddColumn(arrTime)
table.AddColumn(arrE)
table.AddColumn(arrTheta)
table.AddColumn(arrPsi3)
table.AddColumn(arrTau)

#---------------------------------------------------------------------
# trajectory generation
#---------------------------------------------------------------------
def calcTrajectory(t):
    '''
    Calculates desired trajectory for ball position
    '''

    #TODO
    A = 1
    #A = 2
    #A = 3
    #yd = A * cos(pi*t/5)
    yd = 0.1
    return yd

#---------------------------------------------------------------------
# model equations
#---------------------------------------------------------------------
def rhs(t, q):
    '''
    Calculations of system state changes
    '''
    #definitoric
    x1 = q[0]
    x2 = q[1]
    x3 = q[2]
    x4 = q[3]
    y= x1

    dx1 = x2
    B = M/(Jb/R**2+M)
    dx2 = B*(x1*x4**2 - G*sin(x3))
    dx3 = x4

    #choose controller
    yd = calcTrajectory(t)
#    tau = 0
    tau = p_controller(yd, q)
    print tau

    u = (tau - M* (2*x1*x2*x4 + G*x1*cos(x3))) / (M*x1**2 + J + Jb)
    dx4 = u

    #plotting data
    new_row = [t, yd-x1, x3, 0, tau] #TODO psi3
    for i in range( table.GetNumberOfColumns()):
        table.GetColumn(i).InsertNextValue(new_row[i])

    return [dx1, dx2, dx3, dx4]

#---------------------------------------------------------------------
# controller
#---------------------------------------------------------------------
def p_controller(yd, q):
    x1 = q[0]
    x4 = q[3]
    # Kp for yd = 0.1 and dr = 0.01
    Kp = 8
    if yd<0:
        yd = yd*(-1)
    if (x1>-0.05 and x1<0.05) or x4==0:
        print 'mitte'
        return 0
    # ball is on the right side
    if x1>0:
        print 'rechts'
        return Kp*(yd-x1)
    
    # ball is on the left side
    if x1<0:
        print 'links'
        return  Kp*(yd+x1)

#---------------------------------------------------------------------
# solver
#---------------------------------------------------------------------
solver = ode(rhs)
solver.set_initial_value(q0)
solver.set_integrator('vode', method='adams', rtol=1e-6, atol=1e-9)


#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def calcStep():
    '''
    Calcualte one step in simulation
    '''

    return solver.t, solver.integrate(solver.t+dt)


#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def calcPositions(q):
    '''
    Calculate stationary vectors and rot. matrices for bodies
    '''

    #beam
    r_beam = [0, -R/2 - beam_width, 0]
    T_beam = narray([[cos(q[2]), -sin(q[2]), 0], [sin(q[2]), cos(q[2]), 0], [0, 0, 1]])

    #ball
    r_ball = [cos(q[2])*q[0], sin(q[2])*q[0], 0]
    T_ball = narray([[1, 0, 0], [0, 1, 0], [0, 0, 1]])

    return r_beam, T_beam, r_ball, T_ball

    
#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def setBodyState(actor, r, T):
    poke = vtk.vtkMatrix4x4()
    
    for i in range(3):
        for n in range(3):
            poke.SetElement(i, n, T[i, n])
        poke.SetElement(i, 3, r[i])

    actor.PokeMatrix(poke)
    
#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def updateScene(*args):
    '''
    Calculate new states and update the scene
    '''

    t, q = calcStep()
    print t,'\t', q
    r_beam, T_beam, r_ball, T_ball = calcPositions(q)

    setBodyState(beamActor, r_beam, T_beam)
    setBodyState(ballActor, r_ball, T_ball)

    renWin.Render()

    if abs(q[0]) > 1.2*l/2 :
        print '\n###################################### \n#  Ball fell down -> exiting application.'
        quit()<commit_msg>calcTrajectory angepasst damit bis zu einer vorgegebenen Ordnung auch die Ableitungen brechnet werden können<commit_after>﻿#!/usr/bin/python
# -*- coding: utf-8 -*-
from __future__ import division
# Rendering Tests

import vtk
import os.path
from time import sleep
from numpy import sin, cos, pi
from numpy import array as narray
from scipy.integrate import ode

#---------------------------------------------------------------------
# global settings
#--------------------------------------------------------------------- 
dt = 0.01       # stepwidth
q0 = [0, 0.01, 0, 0]    # initial minimal state vector (r, dr, theta, dtheta)'

M = 0.05    # kg
R = 0.01    # m
J = 0.02    # kgm^2
Jb = 2e-6   # kgm^2
G = 9.81    # m/s^2
l = 0.5     # m
beam_width = 0.01
beam_depth = 0.03

scale = 1


#---------------------------------------------------------------------
# plotting 
#---------------------------------------------------------------------
table = vtk.vtkTable()
#Time:
arrTime = vtk.vtkFloatArray()
arrTime.SetName("time")
#Error: yd-r
arrE = vtk.vtkFloatArray()
arrE.SetName("epsilon")
#psi3
arrPsi3 = vtk.vtkFloatArray()
arrPsi3.SetName("psi3")
#theta
arrTheta = vtk.vtkFloatArray()
arrTheta.SetName("theta")
#tau
arrTau = vtk.vtkFloatArray()
arrTau.SetName("tau")
# add rows to table 
table.AddColumn(arrTime)
table.AddColumn(arrE)
table.AddColumn(arrTheta)
table.AddColumn(arrPsi3)
table.AddColumn(arrTau)

#---------------------------------------------------------------------
# trajectory generation
#---------------------------------------------------------------------
def calcTrajectory(t,order):
    '''
    Calculates desired trajectory for ball position
    '''
    #TODO
    A = 1
    #A = 2
    #A = 3
    yd_0 = A * cos(pi*t/5)
    yd_1 = -A * (pi/5) * sin(pi*t/5)
    yd_2 = -A * (pi/5)**2 * cos(pi*t/5)
    yd_3 = A * (pi/5)**3 * sin(pi*t/5)
    yd_4 = A * (pi/5)**4 * cos(pi*t/5)
    yd_derivates = [yd_0 , yd_1 , yd_2 , yd_3 , yd_4]
    yd = []    
    
    for i in range(order+1):
        yd.append(yd_derivates[i])
         
    return yd

#---------------------------------------------------------------------
# model equations
#---------------------------------------------------------------------
def rhs(t, q):
    '''
    Calculations of system state changes
    '''
    #definitoric
    x1 = q[0]
    x2 = q[1]
    x3 = q[2]
    x4 = q[3]
    y= x1

    dx1 = x2
    B = M/(Jb/R**2+M)
    dx2 = B*(x1*x4**2 - G*sin(x3))
    dx3 = x4

    #choose controller
#   tau = 0
    tau = p_controller(t,q)
#    tau = f_controller()

    u = (tau - M* (2*x1*x2*x4 + G*x1*cos(x3))) / (M*x1**2 + J + Jb)
    dx4 = u

    #plotting data
    yd = calcTrajectory(t,0)
    new_row = [t, yd-x1, x3, 0, tau] #TODO psi3
    for i in range( table.GetNumberOfColumns()):
        table.GetColumn(i).InsertNextValue(new_row[i])

    return [dx1, dx2, dx3, dx4]

#---------------------------------------------------------------------
# controller
#---------------------------------------------------------------------
def p_controller(t,q):
    yd = calcTrajectory(t,0)
    x1 = q[0]
    x4 = q[3]
    # Kp for yd = 0.1 and dr = 0.01
    Kp = 8
    if yd<0:
        yd = yd*(-1)
    if (x1>-0.05 and x1<0.05) or x4==0:
        return 0
    # ball is on the right side
    if x1>0:
        return Kp*(yd-x1)
    
    # ball is on the left side
    if x1<0:
        return  Kp*(yd+x1)

    

#---------------------------------------------------------------------
# solver
#---------------------------------------------------------------------
solver = ode(rhs)
solver.set_initial_value(q0)
solver.set_integrator('vode', method='adams', rtol=1e-6, atol=1e-9)


#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def calcStep():
    '''
    Calcualte one step in simulation
    '''

    return solver.t, solver.integrate(solver.t+dt)


#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def calcPositions(q):
    '''
    Calculate stationary vectors and rot. matrices for bodies
    '''

    #beam
    r_beam = [0, -R/2 - beam_width, 0]
    T_beam = narray([[cos(q[2]), -sin(q[2]), 0], [sin(q[2]), cos(q[2]), 0], [0, 0, 1]])

    #ball
    r_ball = [cos(q[2])*q[0], sin(q[2])*q[0], 0]
    T_ball = narray([[1, 0, 0], [0, 1, 0], [0, 0, 1]])

    return r_beam, T_beam, r_ball, T_ball

    
#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def setBodyState(actor, r, T):
    poke = vtk.vtkMatrix4x4()
    
    for i in range(3):
        for n in range(3):
            poke.SetElement(i, n, T[i, n])
        poke.SetElement(i, 3, r[i])

    actor.PokeMatrix(poke)
    
#---------------------------------------------------------------------
#
#---------------------------------------------------------------------
def updateScene(*args):
    '''
    Calculate new states and update the scene
    '''

    t, q = calcStep()
#    print t,'\t', q
    r_beam, T_beam, r_ball, T_ball = calcPositions(q)

    setBodyState(beamActor, r_beam, T_beam)
    setBodyState(ballActor, r_ball, T_ball)

    renWin.Render()

    if abs(q[0]) > 1.2*l/2 :
        print '\n###################################### \n#  Ball fell down -> exiting application.'
        quit()
{'fraction_non_alphanumeric': 0.38657142857142857, 'fraction_numerical': 0.03142857142857143, 'mean_word_length': 1.1141534125226495, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2302111', 'n_tokens_mistral': 3532, 'n_tokens_neox': 3325, 'n_words': 800}