Документ взят из кэша поисковой машины. Адрес оригинального документа : http://www.apo.nmsu.edu/Telescopes/TCC/html/full_slew_8py_source.html
Дата изменения: Tue Sep 15 02:25:37 2015
Дата индексирования: Sun Apr 10 01:34:19 2016
Кодировка:

Поисковые слова: п п п п п п п п п п п п п п п п п п п п п п п п п
lsst.tcc: python/tcc/mov/fullSlew.py Source File
lsst.tcc  1.2.2-3-g89ecb63
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
fullSlew.py
Go to the documentation of this file.
1 from __future__ import division, absolute_import
2 
3 import sys
4 
5 from coordConv import PVT
6 
7 from .trapSlew import trapSlew
8 
9 def fullSlew(pA, vA, pB, vB, tJerk, vMax, aMax):
10  """!Compute a jerk-limited trapezoidal slew
11 
12  Inputs:
13  @param[in] pA dp position of starting point at time t = 0 (deg)
14  @param[in] vA dp velocity of starting point at time t = 0 (deg/sec)
15  @param[in] pB dp position of ending point at time t = 0 (deg)
16  @param[in] vB dp velocity of ending point at time t = 0 (deg/sec)
17  @param[in] tJerk dp sets maximum allowed jerk and minimum slew duration (deg/sec^3)
18  (see Details for more information)
19  @param[in] vMax dp maximum allowed |velocity| (deg/sec)
20  @param[in] aMax dp maximum allowed |acceleration| (deg/sec^2)
21 
22  @return pvtList: a list of approximately 8 PVTs; the time of the first one is 0
23 
24  @throw RuntimeError if cannot compute a slew
25 
26  @warning: |vA| must be <= vMax, |vB| must be some margin less than vMax.
27  See tcc.mov.trapSlew for details.
28 
29  Details:
30  tJerk sets the maximum jerk and minimum duration of slew as follows:
31  - jmax = tJerk/aMax
32  - minimum duration of slew = tJerk
33 
34  Here is how the subroutine works:
35 
36  First a trapezoidal slew is computed, with several special characterstics:
37  - it begins a short time ("tJerk / 2") after 0
38  - the duration of its constant-velocity segment is at least "tJerk"
39  - the maximum possible acceleration ("aMax") is used for both constant
40  acceleration segments.
41 
42  The resulting trapezoidal slew is then "rounded" to give jerk limiting,
43  as follows:
44  - Rounding extends for a time (tJerk / 2) (or less if required)
45  symmetrically to both sides of each trapezoidal slew node.
46  - The rounded segments are paths of constant jerk.
47  This process does not affect the total area under the v vs. t curve,
48  hence the total distance travelled remains unchanged.
49 
50  Trapezoidal slews, and possibly jerk-limiting, are discussed further
51  in my orange notebook.
52 
53  Note: most of the equations used below are less algebraically
54  straightforward than is possible. They have been processed to
55  reduce accumulated error and limit the number of intermediate variables.
56  For example, expressions such as j = a / dt are used to eliminate
57  explicit reference to jerk.
58 
59  History:
60  2013-12-06 ROwen Converted from mov_fullSlew.for
61  """
62  def reportBug(msgStr):
63  """!Write a detailed message to stderr and raise RuntimeError"""
64  sys.stderr.write(msgStr + "\n")
65  sys.stderr.write("fullSlew(pA=%s; pB=%s; vA=%s; vB=%s, tJerk=%s, vMax=%s; aMax=%s)\n" % \
66  (pA, pB, vA, vB, tJerk, vMax, aMax))
67  raise RuntimeError(msgStr)
68 
69  pvtList = []
70 
71  # compute a trapezoidal slew starting at time "tJerk / 2",
72  # with the minimum duration of the constant-velocity segment = "tJerk"
73  rBAi = (pB + vB * tJerk / 2) - (pA + vA * tJerk / 2)
74 
75  ts = trapSlew(rBAi=rBAi, vA=vA, vB=vB, dt2min=tJerk, vMax=vMax, aMax=aMax)
76 
77  # Compute the time and position of the end of the jerk-limited slew,
78  # and the beginning and end of the constant-velocity (middle) segment.
79  # Note: the constant-velocity segment may have zero duration.
80  t_end = tJerk + ts.dt1 + ts.dt2 + ts.dt3
81  p_end = pB + vB * t_end
82  if ts.dt2 == tJerk:
83  t_cv1 = tJerk + ts.dt1
84  t_cv2 = t_cv1
85  p_cv1 = pA + (tJerk + ts.dt1) * (vA + ts.vP) / 2
86  p_cv2 = p_cv1
87  elif ts.dt2 > tJerk:
88  t_cv1 = tJerk + ts.dt1
89  t_cv2 = t_end - (tJerk + ts.dt3)
90  p_cv1 = pA + (tJerk + ts.dt1) * (vA + ts.vP) / 2
91  p_cv2 = p_end - (tJerk + ts.dt3) * (ts.vP + vB) / 2
92  else:
93  reportBug("Bug! Trapezoidal constant-velocity segement too brief: ts.dt2=%s, tJerk=%s" % (ts.dt2, tJerk))
94 
95  # Compute time, velocity, and position for each node
96  # of the jerk-limited slew, and fill the appropriate arrays.
97  # Also count up the total number of nodes.
98 
99  # Beginning of the first constant jerk segment.
100  pvtList.append(PVT(pA, vA, 0))
101 
102  # End of the first constant-jerk segment and beginning of the second.
103  # These bracket a constant-acceleration segment; if that has zero duration,
104  # then there is one less node, and the duration of the two constant-jerk
105  # segments is shorter than tJerk/2.
106  if ts.dt1 <= tJerk:
107  pvtList.append(PVT(
108  ((pA + p_cv1) / 2) + ((tJerk + ts.dt1) * (vA - ts.vP) / 6),
109  (vA + ts.vP) / 2,
110  t_cv1 / 2,
111  ))
112  else:
113  pvtList.append(PVT(
114  pA + tJerk * (vA + tJerk * (ts.a1 / 6)),
115  vA + (tJerk / 2) * ts.a1,
116  tJerk,
117  ))
118  pvtList.append(PVT(
119  p_cv1 - tJerk * (ts.vP - tJerk * (ts.a1 / 6)),
120  ts.vP - (tJerk / 2) * ts.a1,
121  t_cv1 - tJerk,
122  ))
123 
124  # End of the second constant-jerk segment and beginning of the third.
125  # These bracket a constant-velocity segment; if that has zero duration,
126  # then there is one less node (but no change in the duration of the two
127  # constant-jerk segments).
128  if ts.dt2 == tJerk:
129  pvtList.append(PVT(
130  p_cv1,
131  ts.vP,
132  t_cv1,
133  ))
134  else:
135  pvtList.append(PVT(
136  p_cv1,
137  ts.vP,
138  t_cv1,
139  ))
140  pvtList.append(PVT(
141  p_cv2,
142  ts.vP,
143  t_cv2,
144  ))
145 
146  # End of the third constant-jerk segment and beginning of the fourth.
147  # These bracket a constant-acceleration segment; if that has zero duration,
148  # then there is one less node, and the duration of the two constant-jerk
149  # segments is shorter than tJerk/2.
150  if ts.dt3 <= tJerk:
151  pvtList.append(PVT(
152  ((p_cv2 + p_end) / 2) + ((tJerk + ts.dt3) * (ts.vP - vB) / 6),
153  (ts.vP + vB) / 2,
154  (t_cv2 + t_end) / 2,
155  ))
156  else:
157  pvtList.append(PVT(
158  p_cv2 + tJerk * (ts.vP + tJerk * (ts.a3 / 6)),
159  ts.vP + (tJerk / 2) * ts.a3,
160  t_cv2 + tJerk,
161  ))
162  pvtList.append(PVT(
163  p_end - tJerk * (vB - tJerk * (ts.a3 / 6)),
164  vB - (tJerk / 2) * ts.a3,
165  t_end - tJerk,
166  ))
167 
168  # End of the fourth (last) constant jerk segment.
169  pvtList.append(PVT(
170  p_end,
171  vB,
172  t_end,
173  ))
174 
175  return pvtList
def trapSlew
Compute a trapezoidal slew.
Definition: trapSlew.py:48
def fullSlew
Compute a jerk-limited trapezoidal slew.
Definition: fullSlew.py:9