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 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 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(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'])
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 __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 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 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)