symbolic.py

   1 #!/usr/bin/python
   2
   3 from sympy import *
   4
   5 import sys
   6
   7 import sys, codecs
   8 if sys.stdout.encoding is None:
   9   sys.stdout = codecs.open("/dev/stdout", "w", 'utf-8')
  10
  11 init_printing(use_unicode=True)
  12
  13 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
  14
  15 # start      original formulation
  16 # rightvars  replaces
  17
  18 p_start = Matrix([
  19   r * (1 - cos(theta)),
  20   r * sin(theta),
  21   mu * s,
  22 ])
  23
  24 p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
  25
  26 def dbg(*args):
  27   for vn in args:
  28     print('\n    ' + vn + '\n')
  29     pprint(eval(vn))
  30 #    print('\n          =\n')
  31 #    pprint(cse(eval(vn)))
  32
  33 dbg('p_rightvars')
  34
  35 p_dirn_rightvars = diff(p_rightvars, s)
  36
  37 dbg('p_dirn_rightvars')
  38
  39 zeta = Wild('zeta')
  40
  41 p_nosing = (p_rightvars
  42             .replace( 1-cos(zeta)  ,   2*sin(zeta/2)**2          )
  43             .replace( sin(zeta)**2 ,   zeta*sinc(zeta)*sin(zeta) )
  44             )
  45 p_nosing[1] = (p_nosing[1]
  46             .replace( sin(zeta) , zeta * sinc(zeta)                )
  47                )
  48
  49 dbg('p_nosing')
  50
  51 t = symbols('t')
  52
  53 q_owncoords = p_nosing.replace(s,t).replace(la,-la)
  54 q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
  55
  56 dbg('q_owncoords','q_dirn_owncoords')
  57 dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
  58
  59 p2q_translate = p_nosing
  60 #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
  61
  62 #p2q_rotate = eye(3)
  63 #p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
  64 #p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
  65
  66 p2q_rotate = Matrix([[  cos(theta), sin(theta), 0 ],
  67                      [ -sin(theta), cos(theta), 0 ],
  68                      [  0         , 0,          1 ]]).subs(theta,la*s)
  69 #p2q_rotate.add_col([0,0])
  70 #p2q_rotate.add_row([0,0,1])
  71
  72 dbg('p2q_rotate')
  73
  74 q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
  75 q_maincoords = p2q_rotate * q_owncoords + p2q_translate
  76
  77 dbg('diff(p_dirn_rightvars,s)')
  78 dbg('diff(q_dirn_maincoords,t)')
  79 dbg('diff(q_dirn_maincoords,t).replace(t,0)')
  80
  81 assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
  82
  83 #for v in 's','t','la','mu':
  84 #  dbg('diff(q_maincoords,%s)' % v)
  85
  86 #print('\n eye3 subs etc.\n')
  87 #dbg('''Eq(eye(3) * Matrix([1,0,mu]),
  88 #     p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
  89
  90 #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
  91 #          p_dirn_rightvars .cross(Matrix([0,0,1])))''')
  92
  93 #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
  94 #print
  95 #pprint(eq)
  96 #solve(eq, Q)
  97
  98 dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
  99
 100 dbg('q_maincoords','q_dirn_maincoords')
 101
 102 sinof_mu = sin(atan(mu))
 103 cosof_mu = cos(atan(mu))
 104
 105 dbg('cosof_mu','sinof_mu')
 106
 107 o2p_rotate1 = Matrix([[ 1,  0,         0        ],
 108                       [ 0,  cosof_mu, +sinof_mu ],
 109                       [ 0, -sinof_mu,  cosof_mu ]])
 110
 111 check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
 112 check_dirn_p_s0.simplify()
 113 dbg('check_dirn_p_s0')
 114
 115 o2p_rotate2 = Matrix([[  cos(kappa), 0, -sin(kappa) ],
 116                       [  0,          1,  0          ],
 117                       [ +sin(kappa), 0,  cos(kappa) ]])
 118
 119 p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
 120
 121 check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
 122 check_dirn_p_s0.simplify()
 123 dbg('check_dirn_p_s0')
 124
 125 check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
 126 check_accel_p_s0.simplify()
 127 dbg('check_accel_p_s0')