symbolic.py

   1 #!/usr/bin/python3
   2
   3 from sympy import *
   4 import itertools
   5
   6 import sys, codecs
   7 from optparse import OptionParser
   8
   9 opt_parser = OptionParser()
  10 opt_parser.add_option('-q',dest='quiet',action='store_true',
  11                       default=False,help='suppress diagnostic output')
  12 opt_parser.add_option('-7',dest='ascii',action='store_true',
  13                       default=False,help='use 7-bit output only')
  14 opt_parser.add_option('-w',dest='width',action='store',type='int',
  15                       default=80,help='width for printing')
  16 (options, args) = opt_parser.parse_args()
  17 assert(not len(args))
  18
  19 if (not options.ascii) and sys.stdout.encoding is None:
  20   sys.stdout = codecs.open("/dev/stdout", "w", 'utf-8')
  21
  22 init_printing(use_unicode=(not options.ascii), num_columns=options.width)
  23
  24 r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa')
  25
  26 # start      original formulation
  27 # rightvars  replaces
  28
  29 p_start = Matrix([
  30   r * (1 - cos(theta)),
  31   r * sin(theta),
  32   mu * s,
  33 ])
  34
  35 p_rightvars = p_start.subs( theta, s/r ).subs( r, 1/la )
  36
  37 def dbg(*args):
  38   for vn in args:
  39     print('\n    ' + vn + '\n')
  40     pprint(eval(vn))
  41     print('\n          =\n')
  42     pprint(cse(eval(vn)))
  43
  44 dbg('p_rightvars')
  45
  46 p_dirn_rightvars = diff(p_rightvars, s)
  47
  48 dbg('p_dirn_rightvars')
  49
  50 zeta = Wild('zeta')
  51
  52 p_nosing = (p_rightvars
  53             .replace( 1-cos(zeta)  ,   2*sin(zeta/2)**2          )
  54             .replace( sin(zeta)**2 ,   zeta*sinc(zeta)*sin(zeta) )
  55             )
  56 p_nosing[1] = (p_nosing[1]
  57             .replace( sin(zeta) , zeta * sinc(zeta)                )
  58                )
  59
  60 dbg('p_nosing')
  61
  62 t = symbols('t')
  63
  64 q_owncoords = p_nosing.replace(s,t).replace(la,-la)
  65 q_dirn_owncoords = p_dirn_rightvars.replace(s,t).replace(la,-la)
  66
  67 dbg('q_owncoords','q_dirn_owncoords')
  68 dbg('q_owncoords.replace(t,0)','q_dirn_owncoords.replace(t,0)')
  69
  70 p2q_translate = p_nosing
  71 #p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
  72
  73 #p2q_rotate = eye(3)
  74 #p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
  75 #p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
  76
  77 p2q_rotate = Matrix([[  cos(theta), sin(theta), 0 ],
  78                      [ -sin(theta), cos(theta), 0 ],
  79                      [  0         , 0,          1 ]]).subs(theta,la*s)
  80 #p2q_rotate.add_col([0,0])
  81 #p2q_rotate.add_row([0,0,1])
  82
  83 dbg('p2q_rotate')
  84
  85 q_dirn_maincoords = p2q_rotate * q_dirn_owncoords;
  86 q_maincoords = p2q_rotate * q_owncoords + p2q_translate
  87
  88 dbg('diff(p_dirn_rightvars,s)')
  89 dbg('diff(q_dirn_maincoords,t)')
  90 dbg('diff(q_dirn_maincoords,t).replace(t,0)')
  91
  92 assert(Eq(p2q_rotate * Matrix([0,1,mu]), p_dirn_rightvars))
  93
  94 #for v in 's','t','la','mu':
  95 #  dbg('diff(q_maincoords,%s)' % v)
  96
  97 #print('\n eye3 subs etc.\n')
  98 #dbg('''Eq(eye(3) * Matrix([1,0,mu]),
  99 #     p_dirn_rightvars .cross(Matrix([0,0,1]) .subs(s,0)))''')
 100
 101 #dbg('''Eq(p2q_rotate * Matrix([1,0,mu]),
 102 #          p_dirn_rightvars .cross(Matrix([0,0,1])))''')
 103
 104 #eq = Eq(qmat * q_dirn_owncoords_0, p_dirn_rightvars)
 105 #print
 106 #pprint(eq)
 107 #solve(eq, Q)
 108
 109 dbg('q_maincoords.replace(t,0)','q_dirn_maincoords.replace(t,0)')
 110
 111 dbg('q_maincoords','q_dirn_maincoords')
 112
 113 sinof_mu = sin(atan(mu))
 114 cosof_mu = cos(atan(mu))
 115
 116 dbg('cosof_mu','sinof_mu')
 117
 118 o2p_rotate1 = Matrix([[ 1,  0,         0        ],
 119                       [ 0,  cosof_mu, +sinof_mu ],
 120                       [ 0, -sinof_mu,  cosof_mu ]])
 121
 122 check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0)
 123 check_dirn_p_s0.simplify()
 124 dbg('check_dirn_p_s0')
 125
 126 o2p_rotate2 = Matrix([[  cos(kappa), 0, -sin(kappa) ],
 127                       [  0,          1,  0          ],
 128                       [ +sin(kappa), 0,  cos(kappa) ]])
 129
 130 p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars
 131
 132 check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0)
 133 check_dirn_p_s0.simplify()
 134 dbg('check_dirn_p_s0')
 135
 136 check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0)
 137 check_accel_p_s0.simplify()
 138 dbg('check_accel_p_s0')
 139
 140 q_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * q_dirn_maincoords;
 141 q_orgcoords = o2p_rotate2 * o2p_rotate1 * q_maincoords;
 142 dbg('q_orgcoords','q_dirn_orgcoords')
 143
 144 sh, th = symbols('alpha beta')
 145
 146 q_dirn_sqparm = q_dirn_orgcoords.replace(s, sh**2).replace(t, th**2)
 147 q_sqparm      = q_orgcoords     .replace(s, sh**2).replace(t, th**2)
 148
 149 print('----------------------------------------')
 150 dbg('q_sqparm', 'q_dirn_sqparm')
 151 print('----------------------------------------')
 152 for v in 'sh','th','la','mu':
 153   dbg('diff(q_sqparm,%s)' % v)
 154   dbg('diff(q_dirn_sqparm,%s)' % v)
 155 print('----------------------------------------')
 156
 157 gamma = symbols('gamma')
 158
 159 q_dirn_dirnscaled = q_dirn_sqparm * gamma
 160
 161 result_dirnscaled = q_sqparm.col_join(q_dirn_dirnscaled)
 162 dbg('result_dirnscaled')
 163
 164 params = ('sh','th','la','mu','gamma','kappa')
 165
 166 def cprint(s):
 167   print(s)
 168
 169 def cse_prep_cprint(v, tmp_prefix):
 170   # => v, but also having cprint'd the common subexpression assignments
 171   sym_iter = map((lambda i: symbols('%s%d' % (tmp_prefix,i))),
 172                  itertools.count())
 173   (defs, vs) = cse(v, symbols=sym_iter)
 174   for defname, defval in defs:
 175     cprint(ccode(defval, assign_to=defname))
 176   return vs[0]
 177
 178 def cassign(v, assign_to, tmp_prefix):
 179   v = cse_prep_cprint(v, tmp_prefix)
 180   cprint(ccode(v, assign_to=assign_to))
 181
 182 def gen_diff(current, smalls):
 183   global j
 184   if not smalls:
 185     j = zeros(len(params),0)
 186     for param in params:
 187       global d
 188       paramv = eval(param)
 189       d = diff(current, paramv)
 190       dbg('d')
 191       j = j.row_join(d)
 192     dbg('j')
 193     j = cse_prep_cprint(j, 'jtmp')
 194     for ix in range(0, j.cols):
 195       cprint(ccode(j.col(ix), 'J(%d)' % ix))
 196       cprint('J_END_COL(%d)' % ix)
 197   else:
 198     small = smalls[0]
 199     smalls = smalls[1:]
 200     cprint('if (!IS_SMALL(' + ccode(small) + ')) {')
 201     gen_diff(current, smalls)
 202     cprint('} else { /* %s small */' % small)
 203     gen_diff(current.replace(
 204       sinc(small),
 205       1 - small*small/factorial(3) - small**4/factorial(5),
 206       ),
 207       smalls
 208     )
 209     print('} /* %s small */' % small)
 210
 211 gen_diff(result_dirnscaled, (sh*sh*la, th*th*la))
 212
 213 #bad = q_orgcoords[0]
 214 #badd = diff(bad, la)
 215
 216 #dbg('bad','badd')