p2q_translate = p_nosing
#p2q_rotate_2d = Matrix([ p_dirn_rightvars[0:2],
-p2q_rotate = eye(3)
-p2q_rotate[0:2, 0] = Matrix([ -p_dirn_rightvars[1], p_dirn_rightvars[0] ])
-p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
+#p2q_rotate = eye(3)
+#p2q_rotate[0:2, 0] = Matrix([ p_dirn_rightvars[1], -p_dirn_rightvars[0] ])
+#p2q_rotate[0:2, 1] = p_dirn_rightvars[0:2]
+
+p2q_rotate = Matrix([[ cos(theta), sin(theta), 0 ],
+ [ -sin(theta), cos(theta), 0 ],
+ [ 0 , 0, 1 ]]).subs(theta,la*s)
#p2q_rotate.add_col([0,0])
#p2q_rotate.add_row([0,0,1])