frame.py 56 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575
  1. from sympy import (diff, expand, sin, cos, sympify, eye, zeros,
  2. ImmutableMatrix as Matrix, MatrixBase)
  3. from sympy.core.symbol import Symbol
  4. from sympy.simplify.trigsimp import trigsimp
  5. from sympy.physics.vector.vector import Vector, _check_vector
  6. from sympy.utilities.misc import translate
  7. from warnings import warn
  8. __all__ = ['CoordinateSym', 'ReferenceFrame']
  9. class CoordinateSym(Symbol):
  10. """
  11. A coordinate symbol/base scalar associated wrt a Reference Frame.
  12. Ideally, users should not instantiate this class. Instances of
  13. this class must only be accessed through the corresponding frame
  14. as 'frame[index]'.
  15. CoordinateSyms having the same frame and index parameters are equal
  16. (even though they may be instantiated separately).
  17. Parameters
  18. ==========
  19. name : string
  20. The display name of the CoordinateSym
  21. frame : ReferenceFrame
  22. The reference frame this base scalar belongs to
  23. index : 0, 1 or 2
  24. The index of the dimension denoted by this coordinate variable
  25. Examples
  26. ========
  27. >>> from sympy.physics.vector import ReferenceFrame, CoordinateSym
  28. >>> A = ReferenceFrame('A')
  29. >>> A[1]
  30. A_y
  31. >>> type(A[0])
  32. <class 'sympy.physics.vector.frame.CoordinateSym'>
  33. >>> a_y = CoordinateSym('a_y', A, 1)
  34. >>> a_y == A[1]
  35. True
  36. """
  37. def __new__(cls, name, frame, index):
  38. # We can't use the cached Symbol.__new__ because this class depends on
  39. # frame and index, which are not passed to Symbol.__xnew__.
  40. assumptions = {}
  41. super()._sanitize(assumptions, cls)
  42. obj = super().__xnew__(cls, name, **assumptions)
  43. _check_frame(frame)
  44. if index not in range(0, 3):
  45. raise ValueError("Invalid index specified")
  46. obj._id = (frame, index)
  47. return obj
  48. def __getnewargs_ex__(self):
  49. return (self.name, *self._id), {}
  50. @property
  51. def frame(self):
  52. return self._id[0]
  53. def __eq__(self, other):
  54. # Check if the other object is a CoordinateSym of the same frame and
  55. # same index
  56. if isinstance(other, CoordinateSym):
  57. if other._id == self._id:
  58. return True
  59. return False
  60. def __ne__(self, other):
  61. return not self == other
  62. def __hash__(self):
  63. return (self._id[0].__hash__(), self._id[1]).__hash__()
  64. class ReferenceFrame:
  65. """A reference frame in classical mechanics.
  66. ReferenceFrame is a class used to represent a reference frame in classical
  67. mechanics. It has a standard basis of three unit vectors in the frame's
  68. x, y, and z directions.
  69. It also can have a rotation relative to a parent frame; this rotation is
  70. defined by a direction cosine matrix relating this frame's basis vectors to
  71. the parent frame's basis vectors. It can also have an angular velocity
  72. vector, defined in another frame.
  73. """
  74. _count = 0
  75. def __init__(self, name, indices=None, latexs=None, variables=None):
  76. """ReferenceFrame initialization method.
  77. A ReferenceFrame has a set of orthonormal basis vectors, along with
  78. orientations relative to other ReferenceFrames and angular velocities
  79. relative to other ReferenceFrames.
  80. Parameters
  81. ==========
  82. indices : tuple of str
  83. Enables the reference frame's basis unit vectors to be accessed by
  84. Python's square bracket indexing notation using the provided three
  85. indice strings and alters the printing of the unit vectors to
  86. reflect this choice.
  87. latexs : tuple of str
  88. Alters the LaTeX printing of the reference frame's basis unit
  89. vectors to the provided three valid LaTeX strings.
  90. Examples
  91. ========
  92. >>> from sympy.physics.vector import ReferenceFrame, vlatex
  93. >>> N = ReferenceFrame('N')
  94. >>> N.x
  95. N.x
  96. >>> O = ReferenceFrame('O', indices=('1', '2', '3'))
  97. >>> O.x
  98. O['1']
  99. >>> O['1']
  100. O['1']
  101. >>> P = ReferenceFrame('P', latexs=('A1', 'A2', 'A3'))
  102. >>> vlatex(P.x)
  103. 'A1'
  104. ``symbols()`` can be used to create multiple Reference Frames in one
  105. step, for example:
  106. >>> from sympy.physics.vector import ReferenceFrame
  107. >>> from sympy import symbols
  108. >>> A, B, C = symbols('A B C', cls=ReferenceFrame)
  109. >>> D, E = symbols('D E', cls=ReferenceFrame, indices=('1', '2', '3'))
  110. >>> A[0]
  111. A_x
  112. >>> D.x
  113. D['1']
  114. >>> E.y
  115. E['2']
  116. >>> type(A) == type(D)
  117. True
  118. Unit dyads for the ReferenceFrame can be accessed through the attributes ``xx``, ``xy``, etc. For example:
  119. >>> from sympy.physics.vector import ReferenceFrame
  120. >>> N = ReferenceFrame('N')
  121. >>> N.yz
  122. (N.y|N.z)
  123. >>> N.zx
  124. (N.z|N.x)
  125. >>> P = ReferenceFrame('P', indices=['1', '2', '3'])
  126. >>> P.xx
  127. (P['1']|P['1'])
  128. >>> P.zy
  129. (P['3']|P['2'])
  130. Unit dyadic is also accessible via the ``u`` attribute:
  131. >>> from sympy.physics.vector import ReferenceFrame
  132. >>> N = ReferenceFrame('N')
  133. >>> N.u
  134. (N.x|N.x) + (N.y|N.y) + (N.z|N.z)
  135. >>> P = ReferenceFrame('P', indices=['1', '2', '3'])
  136. >>> P.u
  137. (P['1']|P['1']) + (P['2']|P['2']) + (P['3']|P['3'])
  138. """
  139. if not isinstance(name, str):
  140. raise TypeError('Need to supply a valid name')
  141. # The if statements below are for custom printing of basis-vectors for
  142. # each frame.
  143. # First case, when custom indices are supplied
  144. if indices is not None:
  145. if not isinstance(indices, (tuple, list)):
  146. raise TypeError('Supply the indices as a list')
  147. if len(indices) != 3:
  148. raise ValueError('Supply 3 indices')
  149. for i in indices:
  150. if not isinstance(i, str):
  151. raise TypeError('Indices must be strings')
  152. self.str_vecs = [(name + '[\'' + indices[0] + '\']'),
  153. (name + '[\'' + indices[1] + '\']'),
  154. (name + '[\'' + indices[2] + '\']')]
  155. self.pretty_vecs = [(name.lower() + "_" + indices[0]),
  156. (name.lower() + "_" + indices[1]),
  157. (name.lower() + "_" + indices[2])]
  158. self.latex_vecs = [(r"\mathbf{\hat{%s}_{%s}}" % (name.lower(),
  159. indices[0])),
  160. (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(),
  161. indices[1])),
  162. (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(),
  163. indices[2]))]
  164. self.indices = indices
  165. # Second case, when no custom indices are supplied
  166. else:
  167. self.str_vecs = [(name + '.x'), (name + '.y'), (name + '.z')]
  168. self.pretty_vecs = [name.lower() + "_x",
  169. name.lower() + "_y",
  170. name.lower() + "_z"]
  171. self.latex_vecs = [(r"\mathbf{\hat{%s}_x}" % name.lower()),
  172. (r"\mathbf{\hat{%s}_y}" % name.lower()),
  173. (r"\mathbf{\hat{%s}_z}" % name.lower())]
  174. self.indices = ['x', 'y', 'z']
  175. # Different step, for custom latex basis vectors
  176. if latexs is not None:
  177. if not isinstance(latexs, (tuple, list)):
  178. raise TypeError('Supply the indices as a list')
  179. if len(latexs) != 3:
  180. raise ValueError('Supply 3 indices')
  181. for i in latexs:
  182. if not isinstance(i, str):
  183. raise TypeError('Latex entries must be strings')
  184. self.latex_vecs = latexs
  185. self.name = name
  186. self._var_dict = {}
  187. # The _dcm_dict dictionary will only store the dcms of adjacent
  188. # parent-child relationships. The _dcm_cache dictionary will store
  189. # calculated dcm along with all content of _dcm_dict for faster
  190. # retrieval of dcms.
  191. self._dcm_dict = {}
  192. self._dcm_cache = {}
  193. self._ang_vel_dict = {}
  194. self._ang_acc_dict = {}
  195. self._dlist = [self._dcm_dict, self._ang_vel_dict, self._ang_acc_dict]
  196. self._cur = 0
  197. self._x = Vector([(Matrix([1, 0, 0]), self)])
  198. self._y = Vector([(Matrix([0, 1, 0]), self)])
  199. self._z = Vector([(Matrix([0, 0, 1]), self)])
  200. # Associate coordinate symbols wrt this frame
  201. if variables is not None:
  202. if not isinstance(variables, (tuple, list)):
  203. raise TypeError('Supply the variable names as a list/tuple')
  204. if len(variables) != 3:
  205. raise ValueError('Supply 3 variable names')
  206. for i in variables:
  207. if not isinstance(i, str):
  208. raise TypeError('Variable names must be strings')
  209. else:
  210. variables = [name + '_x', name + '_y', name + '_z']
  211. self.varlist = (CoordinateSym(variables[0], self, 0),
  212. CoordinateSym(variables[1], self, 1),
  213. CoordinateSym(variables[2], self, 2))
  214. ReferenceFrame._count += 1
  215. self.index = ReferenceFrame._count
  216. def __getitem__(self, ind):
  217. """
  218. Returns basis vector for the provided index, if the index is a string.
  219. If the index is a number, returns the coordinate variable correspon-
  220. -ding to that index.
  221. """
  222. if not isinstance(ind, str):
  223. if ind < 3:
  224. return self.varlist[ind]
  225. else:
  226. raise ValueError("Invalid index provided")
  227. if self.indices[0] == ind:
  228. return self.x
  229. if self.indices[1] == ind:
  230. return self.y
  231. if self.indices[2] == ind:
  232. return self.z
  233. else:
  234. raise ValueError('Not a defined index')
  235. def __iter__(self):
  236. return iter([self.x, self.y, self.z])
  237. def __str__(self):
  238. """Returns the name of the frame. """
  239. return self.name
  240. __repr__ = __str__
  241. def _dict_list(self, other, num):
  242. """Returns an inclusive list of reference frames that connect this
  243. reference frame to the provided reference frame.
  244. Parameters
  245. ==========
  246. other : ReferenceFrame
  247. The other reference frame to look for a connecting relationship to.
  248. num : integer
  249. ``0``, ``1``, and ``2`` will look for orientation, angular
  250. velocity, and angular acceleration relationships between the two
  251. frames, respectively.
  252. Returns
  253. =======
  254. list
  255. Inclusive list of reference frames that connect this reference
  256. frame to the other reference frame.
  257. Examples
  258. ========
  259. >>> from sympy.physics.vector import ReferenceFrame
  260. >>> A = ReferenceFrame('A')
  261. >>> B = ReferenceFrame('B')
  262. >>> C = ReferenceFrame('C')
  263. >>> D = ReferenceFrame('D')
  264. >>> B.orient_axis(A, A.x, 1.0)
  265. >>> C.orient_axis(B, B.x, 1.0)
  266. >>> D.orient_axis(C, C.x, 1.0)
  267. >>> D._dict_list(A, 0)
  268. [D, C, B, A]
  269. Raises
  270. ======
  271. ValueError
  272. When no path is found between the two reference frames or ``num``
  273. is an incorrect value.
  274. """
  275. connect_type = {0: 'orientation',
  276. 1: 'angular velocity',
  277. 2: 'angular acceleration'}
  278. if num not in connect_type.keys():
  279. raise ValueError('Valid values for num are 0, 1, or 2.')
  280. possible_connecting_paths = [[self]]
  281. oldlist = [[]]
  282. while possible_connecting_paths != oldlist:
  283. oldlist = possible_connecting_paths.copy()
  284. for frame_list in possible_connecting_paths:
  285. frames_adjacent_to_last = frame_list[-1]._dlist[num].keys()
  286. for adjacent_frame in frames_adjacent_to_last:
  287. if adjacent_frame not in frame_list:
  288. connecting_path = frame_list + [adjacent_frame]
  289. if connecting_path not in possible_connecting_paths:
  290. possible_connecting_paths.append(connecting_path)
  291. for connecting_path in oldlist:
  292. if connecting_path[-1] != other:
  293. possible_connecting_paths.remove(connecting_path)
  294. possible_connecting_paths.sort(key=len)
  295. if len(possible_connecting_paths) != 0:
  296. return possible_connecting_paths[0] # selects the shortest path
  297. msg = 'No connecting {} path found between {} and {}.'
  298. raise ValueError(msg.format(connect_type[num], self.name, other.name))
  299. def _w_diff_dcm(self, otherframe):
  300. """Angular velocity from time differentiating the DCM. """
  301. from sympy.physics.vector.functions import dynamicsymbols
  302. dcm2diff = otherframe.dcm(self)
  303. diffed = dcm2diff.diff(dynamicsymbols._t)
  304. angvelmat = diffed * dcm2diff.T
  305. w1 = trigsimp(expand(angvelmat[7]), recursive=True)
  306. w2 = trigsimp(expand(angvelmat[2]), recursive=True)
  307. w3 = trigsimp(expand(angvelmat[3]), recursive=True)
  308. return Vector([(Matrix([w1, w2, w3]), otherframe)])
  309. def variable_map(self, otherframe):
  310. """
  311. Returns a dictionary which expresses the coordinate variables
  312. of this frame in terms of the variables of otherframe.
  313. If Vector.simp is True, returns a simplified version of the mapped
  314. values. Else, returns them without simplification.
  315. Simplification of the expressions may take time.
  316. Parameters
  317. ==========
  318. otherframe : ReferenceFrame
  319. The other frame to map the variables to
  320. Examples
  321. ========
  322. >>> from sympy.physics.vector import ReferenceFrame, dynamicsymbols
  323. >>> A = ReferenceFrame('A')
  324. >>> q = dynamicsymbols('q')
  325. >>> B = A.orientnew('B', 'Axis', [q, A.z])
  326. >>> A.variable_map(B)
  327. {A_x: B_x*cos(q(t)) - B_y*sin(q(t)), A_y: B_x*sin(q(t)) + B_y*cos(q(t)), A_z: B_z}
  328. """
  329. _check_frame(otherframe)
  330. if (otherframe, Vector.simp) in self._var_dict:
  331. return self._var_dict[(otherframe, Vector.simp)]
  332. else:
  333. vars_matrix = self.dcm(otherframe) * Matrix(otherframe.varlist)
  334. mapping = {}
  335. for i, x in enumerate(self):
  336. if Vector.simp:
  337. mapping[self.varlist[i]] = trigsimp(vars_matrix[i],
  338. method='fu')
  339. else:
  340. mapping[self.varlist[i]] = vars_matrix[i]
  341. self._var_dict[(otherframe, Vector.simp)] = mapping
  342. return mapping
  343. def ang_acc_in(self, otherframe):
  344. """Returns the angular acceleration Vector of the ReferenceFrame.
  345. Effectively returns the Vector:
  346. ``N_alpha_B``
  347. which represent the angular acceleration of B in N, where B is self,
  348. and N is otherframe.
  349. Parameters
  350. ==========
  351. otherframe : ReferenceFrame
  352. The ReferenceFrame which the angular acceleration is returned in.
  353. Examples
  354. ========
  355. >>> from sympy.physics.vector import ReferenceFrame
  356. >>> N = ReferenceFrame('N')
  357. >>> A = ReferenceFrame('A')
  358. >>> V = 10 * N.x
  359. >>> A.set_ang_acc(N, V)
  360. >>> A.ang_acc_in(N)
  361. 10*N.x
  362. """
  363. _check_frame(otherframe)
  364. if otherframe in self._ang_acc_dict:
  365. return self._ang_acc_dict[otherframe]
  366. else:
  367. return self.ang_vel_in(otherframe).dt(otherframe)
  368. def ang_vel_in(self, otherframe):
  369. """Returns the angular velocity Vector of the ReferenceFrame.
  370. Effectively returns the Vector:
  371. ^N omega ^B
  372. which represent the angular velocity of B in N, where B is self, and
  373. N is otherframe.
  374. Parameters
  375. ==========
  376. otherframe : ReferenceFrame
  377. The ReferenceFrame which the angular velocity is returned in.
  378. Examples
  379. ========
  380. >>> from sympy.physics.vector import ReferenceFrame
  381. >>> N = ReferenceFrame('N')
  382. >>> A = ReferenceFrame('A')
  383. >>> V = 10 * N.x
  384. >>> A.set_ang_vel(N, V)
  385. >>> A.ang_vel_in(N)
  386. 10*N.x
  387. """
  388. _check_frame(otherframe)
  389. flist = self._dict_list(otherframe, 1)
  390. outvec = Vector(0)
  391. for i in range(len(flist) - 1):
  392. outvec += flist[i]._ang_vel_dict[flist[i + 1]]
  393. return outvec
  394. def dcm(self, otherframe):
  395. r"""Returns the direction cosine matrix of this reference frame
  396. relative to the provided reference frame.
  397. The returned matrix can be used to express the orthogonal unit vectors
  398. of this frame in terms of the orthogonal unit vectors of
  399. ``otherframe``.
  400. Parameters
  401. ==========
  402. otherframe : ReferenceFrame
  403. The reference frame which the direction cosine matrix of this frame
  404. is formed relative to.
  405. Examples
  406. ========
  407. The following example rotates the reference frame A relative to N by a
  408. simple rotation and then calculates the direction cosine matrix of N
  409. relative to A.
  410. >>> from sympy import symbols, sin, cos
  411. >>> from sympy.physics.vector import ReferenceFrame
  412. >>> q1 = symbols('q1')
  413. >>> N = ReferenceFrame('N')
  414. >>> A = ReferenceFrame('A')
  415. >>> A.orient_axis(N, q1, N.x)
  416. >>> N.dcm(A)
  417. Matrix([
  418. [1, 0, 0],
  419. [0, cos(q1), -sin(q1)],
  420. [0, sin(q1), cos(q1)]])
  421. The second row of the above direction cosine matrix represents the
  422. ``N.y`` unit vector in N expressed in A. Like so:
  423. >>> Ny = 0*A.x + cos(q1)*A.y - sin(q1)*A.z
  424. Thus, expressing ``N.y`` in A should return the same result:
  425. >>> N.y.express(A)
  426. cos(q1)*A.y - sin(q1)*A.z
  427. Notes
  428. =====
  429. It is important to know what form of the direction cosine matrix is
  430. returned. If ``B.dcm(A)`` is called, it means the "direction cosine
  431. matrix of B rotated relative to A". This is the matrix
  432. :math:`{}^B\mathbf{C}^A` shown in the following relationship:
  433. .. math::
  434. \begin{bmatrix}
  435. \hat{\mathbf{b}}_1 \\
  436. \hat{\mathbf{b}}_2 \\
  437. \hat{\mathbf{b}}_3
  438. \end{bmatrix}
  439. =
  440. {}^B\mathbf{C}^A
  441. \begin{bmatrix}
  442. \hat{\mathbf{a}}_1 \\
  443. \hat{\mathbf{a}}_2 \\
  444. \hat{\mathbf{a}}_3
  445. \end{bmatrix}.
  446. :math:`{}^B\mathbf{C}^A` is the matrix that expresses the B unit
  447. vectors in terms of the A unit vectors.
  448. """
  449. _check_frame(otherframe)
  450. # Check if the dcm wrt that frame has already been calculated
  451. if otherframe in self._dcm_cache:
  452. return self._dcm_cache[otherframe]
  453. flist = self._dict_list(otherframe, 0)
  454. outdcm = eye(3)
  455. for i in range(len(flist) - 1):
  456. outdcm = outdcm * flist[i]._dcm_dict[flist[i + 1]]
  457. # After calculation, store the dcm in dcm cache for faster future
  458. # retrieval
  459. self._dcm_cache[otherframe] = outdcm
  460. otherframe._dcm_cache[self] = outdcm.T
  461. return outdcm
  462. def _dcm(self, parent, parent_orient):
  463. # If parent.oreint(self) is already defined,then
  464. # update the _dcm_dict of parent while over write
  465. # all content of self._dcm_dict and self._dcm_cache
  466. # with new dcm relation.
  467. # Else update _dcm_cache and _dcm_dict of both
  468. # self and parent.
  469. frames = self._dcm_cache.keys()
  470. dcm_dict_del = []
  471. dcm_cache_del = []
  472. if parent in frames:
  473. for frame in frames:
  474. if frame in self._dcm_dict:
  475. dcm_dict_del += [frame]
  476. dcm_cache_del += [frame]
  477. # Reset the _dcm_cache of this frame, and remove it from the
  478. # _dcm_caches of the frames it is linked to. Also remove it from
  479. # the _dcm_dict of its parent
  480. for frame in dcm_dict_del:
  481. del frame._dcm_dict[self]
  482. for frame in dcm_cache_del:
  483. del frame._dcm_cache[self]
  484. # Reset the _dcm_dict
  485. self._dcm_dict = self._dlist[0] = {}
  486. # Reset the _dcm_cache
  487. self._dcm_cache = {}
  488. else:
  489. # Check for loops and raise warning accordingly.
  490. visited = []
  491. queue = list(frames)
  492. cont = True # Flag to control queue loop.
  493. while queue and cont:
  494. node = queue.pop(0)
  495. if node not in visited:
  496. visited.append(node)
  497. neighbors = node._dcm_dict.keys()
  498. for neighbor in neighbors:
  499. if neighbor == parent:
  500. warn('Loops are defined among the orientation of '
  501. 'frames. This is likely not desired and may '
  502. 'cause errors in your calculations.')
  503. cont = False
  504. break
  505. queue.append(neighbor)
  506. # Add the dcm relationship to _dcm_dict
  507. self._dcm_dict.update({parent: parent_orient.T})
  508. parent._dcm_dict.update({self: parent_orient})
  509. # Update the dcm cache
  510. self._dcm_cache.update({parent: parent_orient.T})
  511. parent._dcm_cache.update({self: parent_orient})
  512. def orient_axis(self, parent, axis, angle):
  513. """Sets the orientation of this reference frame with respect to a
  514. parent reference frame by rotating through an angle about an axis fixed
  515. in the parent reference frame.
  516. Parameters
  517. ==========
  518. parent : ReferenceFrame
  519. Reference frame that this reference frame will be rotated relative
  520. to.
  521. axis : Vector
  522. Vector fixed in the parent frame about about which this frame is
  523. rotated. It need not be a unit vector and the rotation follows the
  524. right hand rule.
  525. angle : sympifiable
  526. Angle in radians by which it the frame is to be rotated.
  527. Warns
  528. ======
  529. UserWarning
  530. If the orientation creates a kinematic loop.
  531. Examples
  532. ========
  533. Setup variables for the examples:
  534. >>> from sympy import symbols
  535. >>> from sympy.physics.vector import ReferenceFrame
  536. >>> q1 = symbols('q1')
  537. >>> N = ReferenceFrame('N')
  538. >>> B = ReferenceFrame('B')
  539. >>> B.orient_axis(N, N.x, q1)
  540. The ``orient_axis()`` method generates a direction cosine matrix and
  541. its transpose which defines the orientation of B relative to N and vice
  542. versa. Once orient is called, ``dcm()`` outputs the appropriate
  543. direction cosine matrix:
  544. >>> B.dcm(N)
  545. Matrix([
  546. [1, 0, 0],
  547. [0, cos(q1), sin(q1)],
  548. [0, -sin(q1), cos(q1)]])
  549. >>> N.dcm(B)
  550. Matrix([
  551. [1, 0, 0],
  552. [0, cos(q1), -sin(q1)],
  553. [0, sin(q1), cos(q1)]])
  554. The following two lines show that the sense of the rotation can be
  555. defined by negating the vector direction or the angle. Both lines
  556. produce the same result.
  557. >>> B.orient_axis(N, -N.x, q1)
  558. >>> B.orient_axis(N, N.x, -q1)
  559. """
  560. from sympy.physics.vector.functions import dynamicsymbols
  561. _check_frame(parent)
  562. if not isinstance(axis, Vector) and isinstance(angle, Vector):
  563. axis, angle = angle, axis
  564. axis = _check_vector(axis)
  565. theta = sympify(angle)
  566. if not axis.dt(parent) == 0:
  567. raise ValueError('Axis cannot be time-varying.')
  568. unit_axis = axis.express(parent).normalize()
  569. unit_col = unit_axis.args[0][0]
  570. parent_orient_axis = (
  571. (eye(3) - unit_col * unit_col.T) * cos(theta) +
  572. Matrix([[0, -unit_col[2], unit_col[1]],
  573. [unit_col[2], 0, -unit_col[0]],
  574. [-unit_col[1], unit_col[0], 0]]) *
  575. sin(theta) + unit_col * unit_col.T)
  576. self._dcm(parent, parent_orient_axis)
  577. thetad = (theta).diff(dynamicsymbols._t)
  578. wvec = thetad*axis.express(parent).normalize()
  579. self._ang_vel_dict.update({parent: wvec})
  580. parent._ang_vel_dict.update({self: -wvec})
  581. self._var_dict = {}
  582. def orient_explicit(self, parent, dcm):
  583. """Sets the orientation of this reference frame relative to another (parent) reference frame
  584. using a direction cosine matrix that describes the rotation from the parent to the child.
  585. Parameters
  586. ==========
  587. parent : ReferenceFrame
  588. Reference frame that this reference frame will be rotated relative
  589. to.
  590. dcm : Matrix, shape(3, 3)
  591. Direction cosine matrix that specifies the relative rotation
  592. between the two reference frames.
  593. Warns
  594. ======
  595. UserWarning
  596. If the orientation creates a kinematic loop.
  597. Examples
  598. ========
  599. Setup variables for the examples:
  600. >>> from sympy import symbols, Matrix, sin, cos
  601. >>> from sympy.physics.vector import ReferenceFrame
  602. >>> q1 = symbols('q1')
  603. >>> A = ReferenceFrame('A')
  604. >>> B = ReferenceFrame('B')
  605. >>> N = ReferenceFrame('N')
  606. A simple rotation of ``A`` relative to ``N`` about ``N.x`` is defined
  607. by the following direction cosine matrix:
  608. >>> dcm = Matrix([[1, 0, 0],
  609. ... [0, cos(q1), -sin(q1)],
  610. ... [0, sin(q1), cos(q1)]])
  611. >>> A.orient_explicit(N, dcm)
  612. >>> A.dcm(N)
  613. Matrix([
  614. [1, 0, 0],
  615. [0, cos(q1), sin(q1)],
  616. [0, -sin(q1), cos(q1)]])
  617. This is equivalent to using ``orient_axis()``:
  618. >>> B.orient_axis(N, N.x, q1)
  619. >>> B.dcm(N)
  620. Matrix([
  621. [1, 0, 0],
  622. [0, cos(q1), sin(q1)],
  623. [0, -sin(q1), cos(q1)]])
  624. **Note carefully that** ``N.dcm(B)`` **(the transpose) would be passed
  625. into** ``orient_explicit()`` **for** ``A.dcm(N)`` **to match**
  626. ``B.dcm(N)``:
  627. >>> A.orient_explicit(N, N.dcm(B))
  628. >>> A.dcm(N)
  629. Matrix([
  630. [1, 0, 0],
  631. [0, cos(q1), sin(q1)],
  632. [0, -sin(q1), cos(q1)]])
  633. """
  634. _check_frame(parent)
  635. # amounts must be a Matrix type object
  636. # (e.g. sympy.matrices.dense.MutableDenseMatrix).
  637. if not isinstance(dcm, MatrixBase):
  638. raise TypeError("Amounts must be a SymPy Matrix type object.")
  639. self.orient_dcm(parent, dcm.T)
  640. def orient_dcm(self, parent, dcm):
  641. """Sets the orientation of this reference frame relative to another (parent) reference frame
  642. using a direction cosine matrix that describes the rotation from the child to the parent.
  643. Parameters
  644. ==========
  645. parent : ReferenceFrame
  646. Reference frame that this reference frame will be rotated relative
  647. to.
  648. dcm : Matrix, shape(3, 3)
  649. Direction cosine matrix that specifies the relative rotation
  650. between the two reference frames.
  651. Warns
  652. ======
  653. UserWarning
  654. If the orientation creates a kinematic loop.
  655. Examples
  656. ========
  657. Setup variables for the examples:
  658. >>> from sympy import symbols, Matrix, sin, cos
  659. >>> from sympy.physics.vector import ReferenceFrame
  660. >>> q1 = symbols('q1')
  661. >>> A = ReferenceFrame('A')
  662. >>> B = ReferenceFrame('B')
  663. >>> N = ReferenceFrame('N')
  664. A simple rotation of ``A`` relative to ``N`` about ``N.x`` is defined
  665. by the following direction cosine matrix:
  666. >>> dcm = Matrix([[1, 0, 0],
  667. ... [0, cos(q1), sin(q1)],
  668. ... [0, -sin(q1), cos(q1)]])
  669. >>> A.orient_dcm(N, dcm)
  670. >>> A.dcm(N)
  671. Matrix([
  672. [1, 0, 0],
  673. [0, cos(q1), sin(q1)],
  674. [0, -sin(q1), cos(q1)]])
  675. This is equivalent to using ``orient_axis()``:
  676. >>> B.orient_axis(N, N.x, q1)
  677. >>> B.dcm(N)
  678. Matrix([
  679. [1, 0, 0],
  680. [0, cos(q1), sin(q1)],
  681. [0, -sin(q1), cos(q1)]])
  682. """
  683. _check_frame(parent)
  684. # amounts must be a Matrix type object
  685. # (e.g. sympy.matrices.dense.MutableDenseMatrix).
  686. if not isinstance(dcm, MatrixBase):
  687. raise TypeError("Amounts must be a SymPy Matrix type object.")
  688. self._dcm(parent, dcm.T)
  689. wvec = self._w_diff_dcm(parent)
  690. self._ang_vel_dict.update({parent: wvec})
  691. parent._ang_vel_dict.update({self: -wvec})
  692. self._var_dict = {}
  693. def _rot(self, axis, angle):
  694. """DCM for simple axis 1,2,or 3 rotations."""
  695. if axis == 1:
  696. return Matrix([[1, 0, 0],
  697. [0, cos(angle), -sin(angle)],
  698. [0, sin(angle), cos(angle)]])
  699. elif axis == 2:
  700. return Matrix([[cos(angle), 0, sin(angle)],
  701. [0, 1, 0],
  702. [-sin(angle), 0, cos(angle)]])
  703. elif axis == 3:
  704. return Matrix([[cos(angle), -sin(angle), 0],
  705. [sin(angle), cos(angle), 0],
  706. [0, 0, 1]])
  707. def _parse_consecutive_rotations(self, angles, rotation_order):
  708. """Helper for orient_body_fixed and orient_space_fixed.
  709. Parameters
  710. ==========
  711. angles : 3-tuple of sympifiable
  712. Three angles in radians used for the successive rotations.
  713. rotation_order : 3 character string or 3 digit integer
  714. Order of the rotations. The order can be specified by the strings
  715. ``'XZX'``, ``'131'``, or the integer ``131``. There are 12 unique
  716. valid rotation orders.
  717. Returns
  718. =======
  719. amounts : list
  720. List of sympifiables corresponding to the rotation angles.
  721. rot_order : list
  722. List of integers corresponding to the axis of rotation.
  723. rot_matrices : list
  724. List of DCM around the given axis with corresponding magnitude.
  725. """
  726. amounts = list(angles)
  727. for i, v in enumerate(amounts):
  728. if not isinstance(v, Vector):
  729. amounts[i] = sympify(v)
  730. approved_orders = ('123', '231', '312', '132', '213', '321', '121',
  731. '131', '212', '232', '313', '323', '')
  732. # make sure XYZ => 123
  733. rot_order = translate(str(rotation_order), 'XYZxyz', '123123')
  734. if rot_order not in approved_orders:
  735. raise TypeError('The rotation order is not a valid order.')
  736. rot_order = [int(r) for r in rot_order]
  737. if not (len(amounts) == 3 & len(rot_order) == 3):
  738. raise TypeError('Body orientation takes 3 values & 3 orders')
  739. rot_matrices = [self._rot(order, amount)
  740. for (order, amount) in zip(rot_order, amounts)]
  741. return amounts, rot_order, rot_matrices
  742. def orient_body_fixed(self, parent, angles, rotation_order):
  743. """Rotates this reference frame relative to the parent reference frame
  744. by right hand rotating through three successive body fixed simple axis
  745. rotations. Each subsequent axis of rotation is about the "body fixed"
  746. unit vectors of a new intermediate reference frame. This type of
  747. rotation is also referred to rotating through the `Euler and Tait-Bryan
  748. Angles`_.
  749. .. _Euler and Tait-Bryan Angles: https://en.wikipedia.org/wiki/Euler_angles
  750. The computed angular velocity in this method is by default expressed in
  751. the child's frame, so it is most preferable to use ``u1 * child.x + u2 *
  752. child.y + u3 * child.z`` as generalized speeds.
  753. Parameters
  754. ==========
  755. parent : ReferenceFrame
  756. Reference frame that this reference frame will be rotated relative
  757. to.
  758. angles : 3-tuple of sympifiable
  759. Three angles in radians used for the successive rotations.
  760. rotation_order : 3 character string or 3 digit integer
  761. Order of the rotations about each intermediate reference frames'
  762. unit vectors. The Euler rotation about the X, Z', X'' axes can be
  763. specified by the strings ``'XZX'``, ``'131'``, or the integer
  764. ``131``. There are 12 unique valid rotation orders (6 Euler and 6
  765. Tait-Bryan): zxz, xyx, yzy, zyz, xzx, yxy, xyz, yzx, zxy, xzy, zyx,
  766. and yxz.
  767. Warns
  768. ======
  769. UserWarning
  770. If the orientation creates a kinematic loop.
  771. Examples
  772. ========
  773. Setup variables for the examples:
  774. >>> from sympy import symbols
  775. >>> from sympy.physics.vector import ReferenceFrame
  776. >>> q1, q2, q3 = symbols('q1, q2, q3')
  777. >>> N = ReferenceFrame('N')
  778. >>> B = ReferenceFrame('B')
  779. >>> B1 = ReferenceFrame('B1')
  780. >>> B2 = ReferenceFrame('B2')
  781. >>> B3 = ReferenceFrame('B3')
  782. For example, a classic Euler Angle rotation can be done by:
  783. >>> B.orient_body_fixed(N, (q1, q2, q3), 'XYX')
  784. >>> B.dcm(N)
  785. Matrix([
  786. [ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
  787. [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
  788. [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
  789. This rotates reference frame B relative to reference frame N through
  790. ``q1`` about ``N.x``, then rotates B again through ``q2`` about
  791. ``B.y``, and finally through ``q3`` about ``B.x``. It is equivalent to
  792. three successive ``orient_axis()`` calls:
  793. >>> B1.orient_axis(N, N.x, q1)
  794. >>> B2.orient_axis(B1, B1.y, q2)
  795. >>> B3.orient_axis(B2, B2.x, q3)
  796. >>> B3.dcm(N)
  797. Matrix([
  798. [ cos(q2), sin(q1)*sin(q2), -sin(q2)*cos(q1)],
  799. [sin(q2)*sin(q3), -sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3), sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)],
  800. [sin(q2)*cos(q3), -sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1), -sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3)]])
  801. Acceptable rotation orders are of length 3, expressed in as a string
  802. ``'XYZ'`` or ``'123'`` or integer ``123``. Rotations about an axis
  803. twice in a row are prohibited.
  804. >>> B.orient_body_fixed(N, (q1, q2, 0), 'ZXZ')
  805. >>> B.orient_body_fixed(N, (q1, q2, 0), '121')
  806. >>> B.orient_body_fixed(N, (q1, q2, q3), 123)
  807. """
  808. from sympy.physics.vector.functions import dynamicsymbols
  809. _check_frame(parent)
  810. amounts, rot_order, rot_matrices = self._parse_consecutive_rotations(
  811. angles, rotation_order)
  812. self._dcm(parent, rot_matrices[0] * rot_matrices[1] * rot_matrices[2])
  813. rot_vecs = [zeros(3, 1) for _ in range(3)]
  814. for i, order in enumerate(rot_order):
  815. rot_vecs[i][order - 1] = amounts[i].diff(dynamicsymbols._t)
  816. u1, u2, u3 = rot_vecs[2] + rot_matrices[2].T * (
  817. rot_vecs[1] + rot_matrices[1].T * rot_vecs[0])
  818. wvec = u1 * self.x + u2 * self.y + u3 * self.z # There is a double -
  819. self._ang_vel_dict.update({parent: wvec})
  820. parent._ang_vel_dict.update({self: -wvec})
  821. self._var_dict = {}
  822. def orient_space_fixed(self, parent, angles, rotation_order):
  823. """Rotates this reference frame relative to the parent reference frame
  824. by right hand rotating through three successive space fixed simple axis
  825. rotations. Each subsequent axis of rotation is about the "space fixed"
  826. unit vectors of the parent reference frame.
  827. The computed angular velocity in this method is by default expressed in
  828. the child's frame, so it is most preferable to use ``u1 * child.x + u2 *
  829. child.y + u3 * child.z`` as generalized speeds.
  830. Parameters
  831. ==========
  832. parent : ReferenceFrame
  833. Reference frame that this reference frame will be rotated relative
  834. to.
  835. angles : 3-tuple of sympifiable
  836. Three angles in radians used for the successive rotations.
  837. rotation_order : 3 character string or 3 digit integer
  838. Order of the rotations about the parent reference frame's unit
  839. vectors. The order can be specified by the strings ``'XZX'``,
  840. ``'131'``, or the integer ``131``. There are 12 unique valid
  841. rotation orders.
  842. Warns
  843. ======
  844. UserWarning
  845. If the orientation creates a kinematic loop.
  846. Examples
  847. ========
  848. Setup variables for the examples:
  849. >>> from sympy import symbols
  850. >>> from sympy.physics.vector import ReferenceFrame
  851. >>> q1, q2, q3 = symbols('q1, q2, q3')
  852. >>> N = ReferenceFrame('N')
  853. >>> B = ReferenceFrame('B')
  854. >>> B1 = ReferenceFrame('B1')
  855. >>> B2 = ReferenceFrame('B2')
  856. >>> B3 = ReferenceFrame('B3')
  857. >>> B.orient_space_fixed(N, (q1, q2, q3), '312')
  858. >>> B.dcm(N)
  859. Matrix([
  860. [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
  861. [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
  862. [ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
  863. is equivalent to:
  864. >>> B1.orient_axis(N, N.z, q1)
  865. >>> B2.orient_axis(B1, N.x, q2)
  866. >>> B3.orient_axis(B2, N.y, q3)
  867. >>> B3.dcm(N).simplify()
  868. Matrix([
  869. [ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)],
  870. [-sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)],
  871. [ sin(q3)*cos(q2), -sin(q2), cos(q2)*cos(q3)]])
  872. It is worth noting that space-fixed and body-fixed rotations are
  873. related by the order of the rotations, i.e. the reverse order of body
  874. fixed will give space fixed and vice versa.
  875. >>> B.orient_space_fixed(N, (q1, q2, q3), '231')
  876. >>> B.dcm(N)
  877. Matrix([
  878. [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
  879. [ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
  880. [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
  881. >>> B.orient_body_fixed(N, (q3, q2, q1), '132')
  882. >>> B.dcm(N)
  883. Matrix([
  884. [cos(q1)*cos(q2), sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), -sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)],
  885. [ -sin(q2), cos(q2)*cos(q3), sin(q3)*cos(q2)],
  886. [sin(q1)*cos(q2), sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3)]])
  887. """
  888. from sympy.physics.vector.functions import dynamicsymbols
  889. _check_frame(parent)
  890. amounts, rot_order, rot_matrices = self._parse_consecutive_rotations(
  891. angles, rotation_order)
  892. self._dcm(parent, rot_matrices[2] * rot_matrices[1] * rot_matrices[0])
  893. rot_vecs = [zeros(3, 1) for _ in range(3)]
  894. for i, order in enumerate(rot_order):
  895. rot_vecs[i][order - 1] = amounts[i].diff(dynamicsymbols._t)
  896. u1, u2, u3 = rot_vecs[0] + rot_matrices[0].T * (
  897. rot_vecs[1] + rot_matrices[1].T * rot_vecs[2])
  898. wvec = u1 * self.x + u2 * self.y + u3 * self.z # There is a double -
  899. self._ang_vel_dict.update({parent: wvec})
  900. parent._ang_vel_dict.update({self: -wvec})
  901. self._var_dict = {}
  902. def orient_quaternion(self, parent, numbers):
  903. """Sets the orientation of this reference frame relative to a parent
  904. reference frame via an orientation quaternion. An orientation
  905. quaternion is defined as a finite rotation a unit vector, ``(lambda_x,
  906. lambda_y, lambda_z)``, by an angle ``theta``. The orientation
  907. quaternion is described by four parameters:
  908. - ``q0 = cos(theta/2)``
  909. - ``q1 = lambda_x*sin(theta/2)``
  910. - ``q2 = lambda_y*sin(theta/2)``
  911. - ``q3 = lambda_z*sin(theta/2)``
  912. See `Quaternions and Spatial Rotation
  913. <https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>`_ on
  914. Wikipedia for more information.
  915. Parameters
  916. ==========
  917. parent : ReferenceFrame
  918. Reference frame that this reference frame will be rotated relative
  919. to.
  920. numbers : 4-tuple of sympifiable
  921. The four quaternion scalar numbers as defined above: ``q0``,
  922. ``q1``, ``q2``, ``q3``.
  923. Warns
  924. ======
  925. UserWarning
  926. If the orientation creates a kinematic loop.
  927. Examples
  928. ========
  929. Setup variables for the examples:
  930. >>> from sympy import symbols
  931. >>> from sympy.physics.vector import ReferenceFrame
  932. >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
  933. >>> N = ReferenceFrame('N')
  934. >>> B = ReferenceFrame('B')
  935. Set the orientation:
  936. >>> B.orient_quaternion(N, (q0, q1, q2, q3))
  937. >>> B.dcm(N)
  938. Matrix([
  939. [q0**2 + q1**2 - q2**2 - q3**2, 2*q0*q3 + 2*q1*q2, -2*q0*q2 + 2*q1*q3],
  940. [ -2*q0*q3 + 2*q1*q2, q0**2 - q1**2 + q2**2 - q3**2, 2*q0*q1 + 2*q2*q3],
  941. [ 2*q0*q2 + 2*q1*q3, -2*q0*q1 + 2*q2*q3, q0**2 - q1**2 - q2**2 + q3**2]])
  942. """
  943. from sympy.physics.vector.functions import dynamicsymbols
  944. _check_frame(parent)
  945. numbers = list(numbers)
  946. for i, v in enumerate(numbers):
  947. if not isinstance(v, Vector):
  948. numbers[i] = sympify(v)
  949. if not (isinstance(numbers, (list, tuple)) & (len(numbers) == 4)):
  950. raise TypeError('Amounts are a list or tuple of length 4')
  951. q0, q1, q2, q3 = numbers
  952. parent_orient_quaternion = (
  953. Matrix([[q0**2 + q1**2 - q2**2 - q3**2,
  954. 2 * (q1 * q2 - q0 * q3),
  955. 2 * (q0 * q2 + q1 * q3)],
  956. [2 * (q1 * q2 + q0 * q3),
  957. q0**2 - q1**2 + q2**2 - q3**2,
  958. 2 * (q2 * q3 - q0 * q1)],
  959. [2 * (q1 * q3 - q0 * q2),
  960. 2 * (q0 * q1 + q2 * q3),
  961. q0**2 - q1**2 - q2**2 + q3**2]]))
  962. self._dcm(parent, parent_orient_quaternion)
  963. t = dynamicsymbols._t
  964. q0, q1, q2, q3 = numbers
  965. q0d = diff(q0, t)
  966. q1d = diff(q1, t)
  967. q2d = diff(q2, t)
  968. q3d = diff(q3, t)
  969. w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1)
  970. w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2)
  971. w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3)
  972. wvec = Vector([(Matrix([w1, w2, w3]), self)])
  973. self._ang_vel_dict.update({parent: wvec})
  974. parent._ang_vel_dict.update({self: -wvec})
  975. self._var_dict = {}
  976. def orient(self, parent, rot_type, amounts, rot_order=''):
  977. """Sets the orientation of this reference frame relative to another
  978. (parent) reference frame.
  979. .. note:: It is now recommended to use the ``.orient_axis,
  980. .orient_body_fixed, .orient_space_fixed, .orient_quaternion``
  981. methods for the different rotation types.
  982. Parameters
  983. ==========
  984. parent : ReferenceFrame
  985. Reference frame that this reference frame will be rotated relative
  986. to.
  987. rot_type : str
  988. The method used to generate the direction cosine matrix. Supported
  989. methods are:
  990. - ``'Axis'``: simple rotations about a single common axis
  991. - ``'DCM'``: for setting the direction cosine matrix directly
  992. - ``'Body'``: three successive rotations about new intermediate
  993. axes, also called "Euler and Tait-Bryan angles"
  994. - ``'Space'``: three successive rotations about the parent
  995. frames' unit vectors
  996. - ``'Quaternion'``: rotations defined by four parameters which
  997. result in a singularity free direction cosine matrix
  998. amounts :
  999. Expressions defining the rotation angles or direction cosine
  1000. matrix. These must match the ``rot_type``. See examples below for
  1001. details. The input types are:
  1002. - ``'Axis'``: 2-tuple (expr/sym/func, Vector)
  1003. - ``'DCM'``: Matrix, shape(3,3)
  1004. - ``'Body'``: 3-tuple of expressions, symbols, or functions
  1005. - ``'Space'``: 3-tuple of expressions, symbols, or functions
  1006. - ``'Quaternion'``: 4-tuple of expressions, symbols, or
  1007. functions
  1008. rot_order : str or int, optional
  1009. If applicable, the order of the successive of rotations. The string
  1010. ``'123'`` and integer ``123`` are equivalent, for example. Required
  1011. for ``'Body'`` and ``'Space'``.
  1012. Warns
  1013. ======
  1014. UserWarning
  1015. If the orientation creates a kinematic loop.
  1016. """
  1017. _check_frame(parent)
  1018. approved_orders = ('123', '231', '312', '132', '213', '321', '121',
  1019. '131', '212', '232', '313', '323', '')
  1020. rot_order = translate(str(rot_order), 'XYZxyz', '123123')
  1021. rot_type = rot_type.upper()
  1022. if rot_order not in approved_orders:
  1023. raise TypeError('The supplied order is not an approved type')
  1024. if rot_type == 'AXIS':
  1025. self.orient_axis(parent, amounts[1], amounts[0])
  1026. elif rot_type == 'DCM':
  1027. self.orient_explicit(parent, amounts)
  1028. elif rot_type == 'BODY':
  1029. self.orient_body_fixed(parent, amounts, rot_order)
  1030. elif rot_type == 'SPACE':
  1031. self.orient_space_fixed(parent, amounts, rot_order)
  1032. elif rot_type == 'QUATERNION':
  1033. self.orient_quaternion(parent, amounts)
  1034. else:
  1035. raise NotImplementedError('That is not an implemented rotation')
  1036. def orientnew(self, newname, rot_type, amounts, rot_order='',
  1037. variables=None, indices=None, latexs=None):
  1038. r"""Returns a new reference frame oriented with respect to this
  1039. reference frame.
  1040. See ``ReferenceFrame.orient()`` for detailed examples of how to orient
  1041. reference frames.
  1042. Parameters
  1043. ==========
  1044. newname : str
  1045. Name for the new reference frame.
  1046. rot_type : str
  1047. The method used to generate the direction cosine matrix. Supported
  1048. methods are:
  1049. - ``'Axis'``: simple rotations about a single common axis
  1050. - ``'DCM'``: for setting the direction cosine matrix directly
  1051. - ``'Body'``: three successive rotations about new intermediate
  1052. axes, also called "Euler and Tait-Bryan angles"
  1053. - ``'Space'``: three successive rotations about the parent
  1054. frames' unit vectors
  1055. - ``'Quaternion'``: rotations defined by four parameters which
  1056. result in a singularity free direction cosine matrix
  1057. amounts :
  1058. Expressions defining the rotation angles or direction cosine
  1059. matrix. These must match the ``rot_type``. See examples below for
  1060. details. The input types are:
  1061. - ``'Axis'``: 2-tuple (expr/sym/func, Vector)
  1062. - ``'DCM'``: Matrix, shape(3,3)
  1063. - ``'Body'``: 3-tuple of expressions, symbols, or functions
  1064. - ``'Space'``: 3-tuple of expressions, symbols, or functions
  1065. - ``'Quaternion'``: 4-tuple of expressions, symbols, or
  1066. functions
  1067. rot_order : str or int, optional
  1068. If applicable, the order of the successive of rotations. The string
  1069. ``'123'`` and integer ``123`` are equivalent, for example. Required
  1070. for ``'Body'`` and ``'Space'``.
  1071. indices : tuple of str
  1072. Enables the reference frame's basis unit vectors to be accessed by
  1073. Python's square bracket indexing notation using the provided three
  1074. indice strings and alters the printing of the unit vectors to
  1075. reflect this choice.
  1076. latexs : tuple of str
  1077. Alters the LaTeX printing of the reference frame's basis unit
  1078. vectors to the provided three valid LaTeX strings.
  1079. Examples
  1080. ========
  1081. >>> from sympy import symbols
  1082. >>> from sympy.physics.vector import ReferenceFrame, vlatex
  1083. >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
  1084. >>> N = ReferenceFrame('N')
  1085. Create a new reference frame A rotated relative to N through a simple
  1086. rotation.
  1087. >>> A = N.orientnew('A', 'Axis', (q0, N.x))
  1088. Create a new reference frame B rotated relative to N through body-fixed
  1089. rotations.
  1090. >>> B = N.orientnew('B', 'Body', (q1, q2, q3), '123')
  1091. Create a new reference frame C rotated relative to N through a simple
  1092. rotation with unique indices and LaTeX printing.
  1093. >>> C = N.orientnew('C', 'Axis', (q0, N.x), indices=('1', '2', '3'),
  1094. ... latexs=(r'\hat{\mathbf{c}}_1',r'\hat{\mathbf{c}}_2',
  1095. ... r'\hat{\mathbf{c}}_3'))
  1096. >>> C['1']
  1097. C['1']
  1098. >>> print(vlatex(C['1']))
  1099. \hat{\mathbf{c}}_1
  1100. """
  1101. newframe = self.__class__(newname, variables=variables,
  1102. indices=indices, latexs=latexs)
  1103. approved_orders = ('123', '231', '312', '132', '213', '321', '121',
  1104. '131', '212', '232', '313', '323', '')
  1105. rot_order = translate(str(rot_order), 'XYZxyz', '123123')
  1106. rot_type = rot_type.upper()
  1107. if rot_order not in approved_orders:
  1108. raise TypeError('The supplied order is not an approved type')
  1109. if rot_type == 'AXIS':
  1110. newframe.orient_axis(self, amounts[1], amounts[0])
  1111. elif rot_type == 'DCM':
  1112. newframe.orient_explicit(self, amounts)
  1113. elif rot_type == 'BODY':
  1114. newframe.orient_body_fixed(self, amounts, rot_order)
  1115. elif rot_type == 'SPACE':
  1116. newframe.orient_space_fixed(self, amounts, rot_order)
  1117. elif rot_type == 'QUATERNION':
  1118. newframe.orient_quaternion(self, amounts)
  1119. else:
  1120. raise NotImplementedError('That is not an implemented rotation')
  1121. return newframe
  1122. def set_ang_acc(self, otherframe, value):
  1123. """Define the angular acceleration Vector in a ReferenceFrame.
  1124. Defines the angular acceleration of this ReferenceFrame, in another.
  1125. Angular acceleration can be defined with respect to multiple different
  1126. ReferenceFrames. Care must be taken to not create loops which are
  1127. inconsistent.
  1128. Parameters
  1129. ==========
  1130. otherframe : ReferenceFrame
  1131. A ReferenceFrame to define the angular acceleration in
  1132. value : Vector
  1133. The Vector representing angular acceleration
  1134. Examples
  1135. ========
  1136. >>> from sympy.physics.vector import ReferenceFrame
  1137. >>> N = ReferenceFrame('N')
  1138. >>> A = ReferenceFrame('A')
  1139. >>> V = 10 * N.x
  1140. >>> A.set_ang_acc(N, V)
  1141. >>> A.ang_acc_in(N)
  1142. 10*N.x
  1143. """
  1144. if value == 0:
  1145. value = Vector(0)
  1146. value = _check_vector(value)
  1147. _check_frame(otherframe)
  1148. self._ang_acc_dict.update({otherframe: value})
  1149. otherframe._ang_acc_dict.update({self: -value})
  1150. def set_ang_vel(self, otherframe, value):
  1151. """Define the angular velocity vector in a ReferenceFrame.
  1152. Defines the angular velocity of this ReferenceFrame, in another.
  1153. Angular velocity can be defined with respect to multiple different
  1154. ReferenceFrames. Care must be taken to not create loops which are
  1155. inconsistent.
  1156. Parameters
  1157. ==========
  1158. otherframe : ReferenceFrame
  1159. A ReferenceFrame to define the angular velocity in
  1160. value : Vector
  1161. The Vector representing angular velocity
  1162. Examples
  1163. ========
  1164. >>> from sympy.physics.vector import ReferenceFrame
  1165. >>> N = ReferenceFrame('N')
  1166. >>> A = ReferenceFrame('A')
  1167. >>> V = 10 * N.x
  1168. >>> A.set_ang_vel(N, V)
  1169. >>> A.ang_vel_in(N)
  1170. 10*N.x
  1171. """
  1172. if value == 0:
  1173. value = Vector(0)
  1174. value = _check_vector(value)
  1175. _check_frame(otherframe)
  1176. self._ang_vel_dict.update({otherframe: value})
  1177. otherframe._ang_vel_dict.update({self: -value})
  1178. @property
  1179. def x(self):
  1180. """The basis Vector for the ReferenceFrame, in the x direction. """
  1181. return self._x
  1182. @property
  1183. def y(self):
  1184. """The basis Vector for the ReferenceFrame, in the y direction. """
  1185. return self._y
  1186. @property
  1187. def z(self):
  1188. """The basis Vector for the ReferenceFrame, in the z direction. """
  1189. return self._z
  1190. @property
  1191. def xx(self):
  1192. """Unit dyad of basis Vectors x and x for the ReferenceFrame."""
  1193. return Vector.outer(self.x, self.x)
  1194. @property
  1195. def xy(self):
  1196. """Unit dyad of basis Vectors x and y for the ReferenceFrame."""
  1197. return Vector.outer(self.x, self.y)
  1198. @property
  1199. def xz(self):
  1200. """Unit dyad of basis Vectors x and z for the ReferenceFrame."""
  1201. return Vector.outer(self.x, self.z)
  1202. @property
  1203. def yx(self):
  1204. """Unit dyad of basis Vectors y and x for the ReferenceFrame."""
  1205. return Vector.outer(self.y, self.x)
  1206. @property
  1207. def yy(self):
  1208. """Unit dyad of basis Vectors y and y for the ReferenceFrame."""
  1209. return Vector.outer(self.y, self.y)
  1210. @property
  1211. def yz(self):
  1212. """Unit dyad of basis Vectors y and z for the ReferenceFrame."""
  1213. return Vector.outer(self.y, self.z)
  1214. @property
  1215. def zx(self):
  1216. """Unit dyad of basis Vectors z and x for the ReferenceFrame."""
  1217. return Vector.outer(self.z, self.x)
  1218. @property
  1219. def zy(self):
  1220. """Unit dyad of basis Vectors z and y for the ReferenceFrame."""
  1221. return Vector.outer(self.z, self.y)
  1222. @property
  1223. def zz(self):
  1224. """Unit dyad of basis Vectors z and z for the ReferenceFrame."""
  1225. return Vector.outer(self.z, self.z)
  1226. @property
  1227. def u(self):
  1228. """Unit dyadic for the ReferenceFrame."""
  1229. return self.xx + self.yy + self.zz
  1230. def partial_velocity(self, frame, *gen_speeds):
  1231. """Returns the partial angular velocities of this frame in the given
  1232. frame with respect to one or more provided generalized speeds.
  1233. Parameters
  1234. ==========
  1235. frame : ReferenceFrame
  1236. The frame with which the angular velocity is defined in.
  1237. gen_speeds : functions of time
  1238. The generalized speeds.
  1239. Returns
  1240. =======
  1241. partial_velocities : tuple of Vector
  1242. The partial angular velocity vectors corresponding to the provided
  1243. generalized speeds.
  1244. Examples
  1245. ========
  1246. >>> from sympy.physics.vector import ReferenceFrame, dynamicsymbols
  1247. >>> N = ReferenceFrame('N')
  1248. >>> A = ReferenceFrame('A')
  1249. >>> u1, u2 = dynamicsymbols('u1, u2')
  1250. >>> A.set_ang_vel(N, u1 * A.x + u2 * N.y)
  1251. >>> A.partial_velocity(N, u1)
  1252. A.x
  1253. >>> A.partial_velocity(N, u1, u2)
  1254. (A.x, N.y)
  1255. """
  1256. from sympy.physics.vector.functions import partial_velocity
  1257. vel = self.ang_vel_in(frame)
  1258. partials = partial_velocity([vel], gen_speeds, frame)[0]
  1259. if len(partials) == 1:
  1260. return partials[0]
  1261. else:
  1262. return tuple(partials)
  1263. def _check_frame(other):
  1264. from .vector import VectorTypeError
  1265. if not isinstance(other, ReferenceFrame):
  1266. raise VectorTypeError(other, ReferenceFrame('A'))