def w_from_fixed_axis(axis, q, frame, sys=None):
sys = sys or pynamics.get_system()
axis = sympy.Matrix(axis)
l_2 = axis.dot(axis)
axis = axis / (l_2**.5)
w = sys.derivative(q)
w_ = w * Vector({frame: axis})
return w_
def __init__(self,name = None):
super(Frame,self).__init__()
self.connections_R = {}
self.precomputed_R = {}
self.connections_w = {}
self.precomputed_w = {}
self.reps = {}
self.R_tree = TreeNode(self)
self.w_tree = TreeNode(self)
name = name or self.generate_name()
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name+'.x')
self.y_sym = sympy.Symbol(name+'.y')
self.z_sym = sympy.Symbol(name+'.z')
self.syms = sympy.Matrix([self.x_sym,self.y_sym,self.z_sym])
self.x.add_component(self,[1,0,0])
self.y.add_component(self,[0,1,0])
self.z.add_component(self,[0,0,1])
r = Rotation(self,self,sympy.Matrix.eye(3))
w = RotationalVelocity(self,self,sympy.Number(0)*self.x)
self.add_rotation(r)
self.add_w(w)
pynamics.addself(self,name)
def build_fixed_axis(axis, q, sys, frame):
axis = sympy.Matrix(axis)
l_2 = axis.dot(axis)
axis = axis / (l_2**.5)
e0 = axis[0]
e1 = axis[1]
e2 = axis[2]
c = axis * axis.T
a = (sympy.Matrix.eye(3) - c) * cos(q)
b = sympy.Matrix([[0, -e2, e1], [e2, 0, -e0], [-e1, e0, 0]]) * sin(q)
R = a + b + c
R = R.T
w = sys.derivative(q)
w_ = w * Vector({frame: axis})
return R, w_, axis
def __init__(self, name, system):
super(Frame, self).__init__()
self.connections = {}
self.connections['R'] = {}
self.connections['w'] = {}
self.precomputed = {}
self.precomputed['R'] = {}
self.precomputed['w'] = {}
self.tree = {}
self.tree['R'] = TreeNode(self)
self.tree['w'] = TreeNode(self)
self.reps = {}
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name + '.x')
self.y_sym = sympy.Symbol(name + '.y')
self.z_sym = sympy.Symbol(name + '.z')
self.syms = sympy.Matrix([self.x_sym, self.y_sym, self.z_sym])
self.x.add_component(self, [1, 0, 0])
self.y.add_component(self, [0, 1, 0])
self.z.add_component(self, [0, 0, 1])
r = Rotation(self, self, sympy.Matrix.eye(3), Quaternion(0, 0, 0, 0))
w = RotationalVelocity(self, self,
sympy.Number(0) * self.x,
Quaternion(0, 0, 0, 0))
self.add_generic(r, 'R')
self.add_generic(w, 'w')
self.system = system
self.system.add_frame(self)
def __init__(self,name = None):
super(Frame,self).__init__()
self.connections = {}
self.precomputed = {}
self.reps = {}
if name==None:
name=Name.frame()
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name+'.x')
self.y_sym = sympy.Symbol(name+'.y')
self.z_sym = sympy.Symbol(name+'.z')
self.syms = sympy.Matrix([self.x_sym,self.y_sym,self.z_sym])
self.x.add_component(self,[1,0,0])
self.y.add_component(self,[0,1,0])
self.z.add_component(self,[0,0,1])
self.add_rotation(Rotation(self,self,sympy.Matrix.eye(3),sympy.Number(0)*self.x))
pynamics.addself(self,name)
class Frame(NameGenerator):
def __init__(self, name, system):
super(Frame, self).__init__()
self.connections = {}
self.connections['R'] = {}
self.connections['w'] = {}
self.precomputed = {}
self.precomputed['R'] = {}
self.precomputed['w'] = {}
self.tree = {}
self.tree['R'] = TreeNode(self)
self.tree['w'] = TreeNode(self)
self.reps = {}
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name + '.x')
self.y_sym = sympy.Symbol(name + '.y')
self.z_sym = sympy.Symbol(name + '.z')
self.syms = sympy.Matrix([self.x_sym, self.y_sym, self.z_sym])
self.x.add_component(self, [1, 0, 0])
self.y.add_component(self, [0, 1, 0])
self.z.add_component(self, [0, 0, 1])
r = Rotation(self, self, sympy.Matrix.eye(3), Quaternion(0, 0, 0, 0))
w = RotationalVelocity(self, self,
sympy.Number(0) * self.x,
Quaternion(0, 0, 0, 0))
self.add_generic(r, 'R')
self.add_generic(w, 'w')
self.system = system
self.system.add_frame(self)
def add_generic(self, rotation, my_type):
self.connections[my_type][rotation.other(self)] = rotation
def add_precomputed_generic(self, rotation, my_type):
self.precomputed[my_type][rotation.other(self)] = rotation
@property
def principal_axes(self):
return [self.x, self.y, self.z]
def __str__(self):
return self.name
def __repr__(self):
return str(self)
def get_generic(self, other, my_type):
if other in self.connections[my_type]:
return self.connections[my_type][other]
elif other in self.precomputed[my_type]:
return self.precomputed[my_type][other]
else:
path = self.tree['R'].path_to(other.tree['R'])
path = [item.myclass for item in path]
from_frames = path[:-1]
to_frames = path[1:]
if my_type == 'R':
items = [
from_frame.connections[my_type][to_frame].get_r_to(
to_frame)
for from_frame, to_frame in zip(from_frames, to_frames)
]
q_items = [
from_frame.connections[my_type][to_frame].get_rq_to(
to_frame)
for from_frame, to_frame in zip(from_frames, to_frames)
]
elif my_type == 'w':
items = [
from_frame.connections[my_type][to_frame].get_w_to(
to_frame)
for from_frame, to_frame in zip(from_frames, to_frames)
]
item_final = items.pop(0)
if my_type == 'R':
q_item_final = q_items.pop(0)
for item, to_frame in zip(items, to_frames[1:]):
item_final = item * item_final
for q_item, to_frame in zip(q_items, to_frames[1:]):
q_item_final = q_item * q_item_final
result = Rotation(self, to_frame, item_final, q_item_final)
elif my_type == 'w':
for item, to_frame in zip(items, to_frames[1:]):
item_final += item
result = RotationalVelocity(self, to_frame, item_final,
Quaternion(0, 0, 0, 0))
self.add_precomputed_generic(result, my_type)
to_frame.add_precomputed_generic(result, my_type)
return result
def get_r_to(self, other):
return self.get_generic(other, 'R').get_r_to(other)
def get_r_from(self, other):
return self.get_generic(other, 'R').get_r_from(other)
def get_rq_to(self, other):
return self.get_generic(other, 'R').get_rq_to(other)
def get_rq_from(self, other):
return self.get_generic(other, 'R').get_rq_from(other)
def get_w_from(self, other):
return self.get_generic(other, 'w').get_w_from(other)
def get_w_to(self, other):
return self.get_generic(other, 'w').get_w_to(other)
def set_generic(self, other, item, my_type):
if my_type == 'R':
result = Rotation(self, other, item, Quaternion(0, 0, 0, 0))
elif my_type == 'w':
result = RotationalVelocity(self, other, item,
Quaternion(0, 0, 0, 0))
self.add_generic(result, my_type)
other.add_generic(result, my_type)
def set_parent_generic(self, parent, item, my_type):
self.set_generic(parent, item, my_type)
parent.tree[my_type].add_branch(self.tree[my_type])
def set_child_generic(self, child, item, my_type):
self.set_generic(child, item, my_type)
self.tree[my_type].add_branch(child.tree[my_type])
def set_w(self, other, w):
self.set_child_generic(other, w, 'w')
def rotate_fixed_axis(self, fromframe, axis, q, system):
import pynamics.misc_tools
if not all([pynamics.misc_tools.is_literal(item) for item in axis]):
raise (Exception('not all axis variables are constant'))
rotation = Rotation.build_fixed_axis(fromframe, self, axis, q, system)
rotational_velocity = RotationalVelocity.build_fixed_axis(
fromframe, self, axis, q, system)
self.set_parent_generic(fromframe, rotation, 'R')
self.set_parent_generic(fromframe, rotational_velocity, 'w')
self.add_generic(rotation, 'R')
self.add_generic(rotational_velocity, 'w')
fromframe.add_generic(rotation, 'R')
fromframe.add_generic(rotational_velocity, 'w')
fromframe.tree['R'].add_branch(self.tree['R'])
fromframe.tree['w'].add_branch(self.tree['w'])
class Frame(NameGenerator):
def __init__(self,name = None):
super(Frame,self).__init__()
self.connections_R = {}
self.precomputed_R = {}
self.connections_w = {}
self.precomputed_w = {}
self.reps = {}
self.R_tree = TreeNode(self)
self.w_tree = TreeNode(self)
name = name or self.generate_name()
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name+'.x')
self.y_sym = sympy.Symbol(name+'.y')
self.z_sym = sympy.Symbol(name+'.z')
self.syms = sympy.Matrix([self.x_sym,self.y_sym,self.z_sym])
self.x.add_component(self,[1,0,0])
self.y.add_component(self,[0,1,0])
self.z.add_component(self,[0,0,1])
r = Rotation(self,self,sympy.Matrix.eye(3))
w = RotationalVelocity(self,self,sympy.Number(0)*self.x)
self.add_rotation(r)
self.add_w(w)
pynamics.addself(self,name)
def add_rotation(self,rotation):
self.connections_R[rotation.other(self)] = rotation
def add_precomputed_rotation(self,rotation):
self.precomputed_R[rotation.other(self)] = rotation
def add_w(self,w):
self.connections_w[w.other(self)] = w
def add_precomputed_w(self,w):
self.precomputed_w[w.other(self)] = w
@property
def principal_axes(self):
return [self.x,self.y,self.z]
def __str__(self):
return self.name
def __repr__(self):
return str(self)
def calc_R(self,other):
if other in self.connections_R:
return self.connections_R[other]
elif other in self.precomputed_R:
return self.precomputed_R[other]
else:
path = self.R_tree.path_to(other.R_tree)
path = [item.myclass for item in path]
from_frames = path[:-1]
to_frames = path[1:]
Rs = [from_frame.connections_R[to_frame].to_other(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
# w_s = [from_frame.connections[to_frame].w__from(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
R_final = Rs.pop(0)
# w_final = w_s.pop(0)
for R,to_frame in zip(Rs,to_frames[1:]):
R_final = R*R_final
# w_final += w_
rotation = Rotation(self,to_frame,R_final)
# rotation.set_w(w_final)
self.add_precomputed_rotation(rotation)
to_frame.add_precomputed_rotation(rotation)
# rotation = Rotation(self,to_frame,R_final,w_final)
return rotation
def calc_w(self,other):
if other in self.connections_w:
return self.connections_w[other]
elif other in self.precomputed_w:
return self.precomputed_w[other]
else:
path = self.w_tree.path_to(other.w_tree)
path = [item.myclass for item in path]
from_frames = path[:-1]
to_frames = path[1:]
# Rs = [from_frame.connections[to_frame].to_other(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
w_s = [from_frame.connections_w[to_frame].w__from(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
# R_final = Rs.pop(0)
w_final = w_s.pop(0)
for w_,to_frame in zip(w_s,to_frames[1:]):
# R_final = R*R_final
w_final += w_
rotational_velocity = RotationalVelocity(self,to_frame,w_final)
# rotation.set_w(w_final)
self.add_precomputed_w(rotational_velocity)
to_frame.add_precomputed_w(rotational_velocity)
# rotation = Rotation(self,to_frame,R_final,w_final)
return rotational_velocity
def getR(self,other):
return self.calc_R(other).to_other(self)
def getw_(self,other):
return self.calc_w(other).w__from(self)
def set_w(self,other,w):
rotational_velocity = RotationalVelocity(self, other, w)
self.add_w(rotational_velocity)
other.add_w(rotational_velocity)
self.w_tree.add_branch(other.w_tree)
def rotate_fixed_axis(self,fromframe,axis,q,system = None):
system = system or pynamics.get_system()
rotation = Rotation.build_fixed_axis(fromframe,self,axis,q,system)
rotational_velocity = RotationalVelocity.build_fixed_axis(fromframe,self,axis,q,system)
self.add_rotation(rotation)
self.add_w(rotational_velocity)
fromframe.add_rotation(rotation)
fromframe.add_w(rotational_velocity)
fromframe.R_tree.add_branch(self.R_tree)
fromframe.w_tree.add_branch(self.w_tree)
def rotate_fixed_axis_directed(self,fromframe,axis,q,system=None):
self.rotate_fixed_axis(fromframe,axis,q,system)
def efficient_rep(self,other,functionname):
key = (other,functionname)
if key in self.reps:
return self.reps[key]
else:
path = self.R_tree.path_to(other.R_tree)
dot = {}
for mysym,myvec in zip(self.syms,[self.x,self.y,self.z]):
for othersym,othervec in zip(other.syms,[other.x,other.y,other.z]):
min_dot_len = 0
for frame in path:
frame = frame.myclass
v1 = myvec.express(frame).components[frame]
v2 = othervec.express(frame).components[frame]
function = getattr(v1,functionname)
dot_rep = function(v2)
dot_len = len(str(dot_rep))
if min_dot_len==0 or dot_len<min_dot_len:
min_dot_len=dot_len
min_dot_frame = frame
elif dot_len==min_dot_len:
if min_dot_frame in frame.decendents:
min_dot_frame = frame
dot[frozenset((mysym,othersym))] = min_dot_frame
self.reps[key] = dot
return dot
class Frame(TreeNode):
def __init__(self,name = None):
super(Frame,self).__init__()
self.connections = {}
self.precomputed = {}
self.reps = {}
if name==None:
name=Name.frame()
self.name = name
self.x = Vector()
self.y = Vector()
self.z = Vector()
self.x_sym = sympy.Symbol(name+'.x')
self.y_sym = sympy.Symbol(name+'.y')
self.z_sym = sympy.Symbol(name+'.z')
self.syms = sympy.Matrix([self.x_sym,self.y_sym,self.z_sym])
self.x.add_component(self,[1,0,0])
self.y.add_component(self,[0,1,0])
self.z.add_component(self,[0,0,1])
self.add_rotation(Rotation(self,self,sympy.Matrix.eye(3),sympy.Number(0)*self.x))
pynamics.addself(self,name)
def add_rotation(self,rotation):
self.connections[rotation.other(self)] = rotation
def add_precomputed(self,rotation):
self.precomputed[rotation.other(self)] = rotation
def __str__(self):
return self.name
def __repr__(self):
return str(self)
def calc(self,other):
if other in self.connections:
return self.connections[other]
elif other in self.precomputed:
return self.precomputed[other]
else:
path = self.path_to(other)
from_frames = path[:-1]
to_frames = path[1:]
Rs = [from_frame.connections[to_frame].to_other(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
w_s = [from_frame.connections[to_frame].w__from(from_frame) for from_frame,to_frame in zip(from_frames,to_frames)]
R_final = Rs.pop(0)
w_final = w_s.pop(0)
for R,w_,to_frame in zip(Rs,w_s,to_frames):
R_final = R*R_final
w_final += w_
rotation = Rotation(self,to_frame,R_final,w_final)
# self.add_precomputed(rotation)
# to_frame.add_precomputed(rotation)
return rotation
def getR(self,other):
return self.calc(other).to_other(self)
def getw_(self,other):
return self.calc(other).w__from(self)
def rotate_fixed_axis(self,fromframe,axis,q,sys):
rotation = FixedAxisRotation(fromframe,self,axis,q,sys)
self.add_rotation(rotation)
fromframe.add_rotation(rotation)
def rotate_fixed_axis_directed(self,fromframe,axis,q,sys):
self.rotate_fixed_axis(fromframe,axis,q,sys)
fromframe.add_branch(self)
def efficient_rep(self,other,functionname):
key = (other,functionname)
if key in self.reps:
return self.reps[key]
else:
path = self.path_to(other)
dot = {}
for mysym,myvec in zip(self.syms,[self.x,self.y,self.z]):
for othersym,othervec in zip(other.syms,[other.x,other.y,other.z]):
min_dot_len = 0
for frame in path:
v1 = myvec.express(frame).components[frame]
v2 = othervec.express(frame).components[frame]
function = getattr(v1,functionname)
dot_rep = function(v2)
dot_len = len(str(dot_rep))
if min_dot_len==0 or dot_len<min_dot_len:
min_dot_len=dot_len
min_dot_frame = frame
elif dot_len==min_dot_len:
if min_dot_frame in frame.decendents:
min_dot_frame = frame
dot[frozenset((mysym,othersym))] = min_dot_frame
self.reps[key] = dot
return dot