From 24a930be5fbaf34d81e0ac20e13b8b676afc6dd1 Mon Sep 17 00:00:00 2001 From: Ian Jackson Date: Fri, 17 Nov 2017 22:06:24 +0000 Subject: [PATCH] symbolic.py: wip o2p_rotate Signed-off-by: Ian Jackson --- symbolic.py | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/symbolic.py b/symbolic.py index 8b42ccf..6cc5c2b 100755 --- a/symbolic.py +++ b/symbolic.py @@ -10,7 +10,7 @@ if sys.stdout.encoding is None: init_printing(use_unicode=True) -r, theta, s, la, mu = symbols('r theta s lambda mu') +r, theta, s, la, mu, kappa = symbols('r theta s lambda mu kappa') # start original formulation # rightvars replaces @@ -104,4 +104,24 @@ cosof_mu = cos(atan(mu)) dbg('cosof_mu','sinof_mu') -#o2p_rotate1 = Matrix([ [ +o2p_rotate1 = Matrix([[ 1, 0, 0 ], + [ 0, cosof_mu, +sinof_mu ], + [ 0, -sinof_mu, cosof_mu ]]) + +check_dirn_p_s0 = o2p_rotate1 * p_dirn_rightvars.replace(s,0) +check_dirn_p_s0.simplify() +dbg('check_dirn_p_s0') + +o2p_rotate2 = Matrix([[ cos(kappa), 0, -sin(kappa) ], + [ 0, 1, 0 ], + [ +sin(kappa), 0, cos(kappa) ]]) + +p_dirn_orgcoords = o2p_rotate2 * o2p_rotate1 * p_dirn_rightvars + +check_dirn_p_s0 = p_dirn_orgcoords.replace(s,0) +check_dirn_p_s0.simplify() +dbg('check_dirn_p_s0') + +check_accel_p_s0 = diff(p_dirn_orgcoords,s).replace(s,0) +check_accel_p_s0.simplify() +dbg('check_accel_p_s0') -- 2.30.2