"""
motion simulation (kinematics)
"""
__author__ = "Philippe Guglielmetti"
__copyright__ = "Copyright 2013, Philippe Guglielmetti"
__credits__ = ["http://osterone.bobstgroup.com/wiki/index.php?title=UtlCam"]
__license__ = "LGPL"
from . import plot, polynomial, itertools2, math2
[docs]class PVA(plot.Plot): # TODO: make it an Expr
"""represents a function of time returning position, velocity, and acceleration
"""
[docs] def __init__(self, funcs):
self.funcs = funcs
[docs] def __call__(self, t, t0=0):
return tuple(f(t - t0) for f in self.funcs)
[docs]class Segment(PVA):
""" a PVA defined between 2 times, null elsewhere
"""
[docs] def __init__(self, t0, t1, funcs):
super(Segment, self).__init__(funcs)
self.t0 = t0
self.t1 = t1
self.ticks = []
[docs] def dt(self):
return self.t1 - self.t0
[docs] def start(self):
return super(Segment, self).__call__(self.t0, self.t0)
[docs] def startPos(self):
return self.start()[0]
[docs] def startSpeed(self):
return self.start()[1]
[docs] def startAcc(self):
return self.start()[2]
[docs] def startJerk(self):
return self.start()[3]
[docs] def startTime(self):
return self.t0
[docs] def end(self):
return super(Segment, self).__call__(self.t1, self.t0)
[docs] def endPos(self):
return self.end()[0]
[docs] def endSpeed(self):
return self.end()[1]
[docs] def endAcc(self):
return self.end()[2]
[docs] def endJerk(self):
return self.end()[3]
[docs] def endTime(self):
return (self.t1)
[docs] def timeWhenPosBiggerThan(self, pos, resolution=0.010):
""" search the first time when the position is bigger than pos
:params pos: the pos that must at least be reached
:params resolution: the time resolution in sec"""
# brute force and stupid!!!
for t in itertools2.arange(self.t0, self.t1, resolution):
if self(t)[0] >= pos:
break
return t
[docs] def __call__(self, t):
if t >= self.t0 and t < self.t1:
return super(Segment, self).__call__(t, self.t0)
else:
return (0,) * len(self.funcs)
def _plot(self, ax, t0=None, t1=None, ylim=None, **kwargs):
"""
:params ticks: a list of (time,state) to add ticks
"""
if t0 is None: t0 = self.t0
if t1 is None: t1 = self.t1
step = (t1 - t0) / 500.
x = [t for t in itertools2.arange(t0, t1, step)]
y = [self(t) for t in x]
y = map(list, zip(*y)) # transpose because of
labels = ['pos', 'vel', 'acc', 'jrk']
for y_arr, label in zip(y, labels):
ax.plot(x, y_arr, label=label)
ax.legend(loc='best')
# for tick in self.ticks:
# ax.axvline(x=tick[0],color='0.5')
return ax
[docs]class Segments(Segment):
[docs] def __init__(self, segments=[], label='Segments'):
"""
can be initialized with a list of segment (that of course can also be a Segments)
:param label: a label can be given
"""
self.label = label
self.t0 = -float('inf')
self.t1 = -float('inf')
self.segments = []
self.add(segments)
[docs] def __str__(self):
return self.label
[docs] def html(self):
t = self.label + '<br/>'
for s in self.segments:
t += 't=%f (%f,%f,%f,%f) --> t=%f (%f,%f,%f,%f)<br/>' % (
s.t0, s.start()[0], s.start()[1], s.start()[2], s.start()[3],
s.t1, s.endPos(), s.endSpeed(), s.endAcc(), s.end()[3])
return t
[docs] def update(self):
""" yet only calculates t0 and t1 """
for s in self.segments:
if s.t0 < self.t0:
self.t0 = s.t0
if s.t1 > self.t1:
self.t1 = s.t1
[docs] def insert(self, segment, autoJoin=True):
""" insert a segment into Segments
:param segment: the segment to add. must be in a range that is not already defined or it will rise a value error exception
:param autoJoin: if True and the added segment has the same starting position as the last segment's end
and both velocity are 0 then a segment of (pos,v=0,a=0) is automatically added.
this help discribing movements only where there is curently a movement
"""
t0 = segment.t0
t1 = segment.t1
if self.segments == []:
self.segments = [segment]
self.t0 = segment.t0
self.t1 = segment.t1
return
if t0 >= self.segments[-1].t1:
previous = self.segments[-1]
previousP = previous.endPos()
previousT = previous.endTime()
previousV = previous.endSpeed()
segmentP = segment.start()[0]
segmentV = segment.start()[1]
if autoJoin and t0 > previousT and math2.allclose([previousP, previousV, segmentV], [segmentP, 0, 0],
abs_tol=0.001):
self.segments.append(SegmentPoly(previous.t1, t0, [previous.endPos()]))
self.segments.append(segment)
return
if t1 <= self.segments[0].t0:
self.segments.insert(0, segment)
return
for i in range(0, len(self.segments) - 1):
if self.segments[i].t1 <= t0 and self.segments[i + 1].t0 >= t1:
self.segments.insert(i + 1, segment)
return
l = ''
for s in self.segments:
l += '\n' + str(s.t0) + '-->' + str(s.t1)
raise ValueError('impossible to add the segment t0=' + str(segment.t0) + ' t1=' + str(
segment.t1) + ' to already existing segments' + l)
[docs] def add(self, segments, autoJoin=True):
""" add a segment or a list of segment to the segments """
if type(segments) is not list:
self.insert(segments, autoJoin)
else:
for s in segments:
self.insert(s, autoJoin)
self.update()
self.label = 'Segments starts=' + str(self.t0) + ' ends=' + str(self.t1)
[docs] def start(self):
if self.segments != []:
return self.segments[0].start()
else:
return (0, 0, 0, 0)
[docs] def end(self):
if self.segments != []:
return self.segments[-1].end()
else:
return (0, 0, 0, 0)
[docs] def __call__(self, t):
for s in self.segments:
if t >= s.t0 and t < s.t1:
return s(t)
return (0, 0, 0, 0) # oversimplified: assuming PVAJ; should check that all segments are of the same nature
[docs]class SegmentPoly(Segment):
""" a segment defined by a polynomial position law
"""
[docs] def __init__(self, t0, t1, p):
p = polynomial.Polynomial(p)
v = p.derivative()
a = v.derivative()
j = a.derivative()
super(SegmentPoly, self).__init__(t0, t1, (p, v, a, j))
def _latex(self):
""":return: string LaTex formula"""
return 'pos(t)=%s' % self.funcs[0]._latex(x='t')
def _repr_latex_(self):
return '$%s$' % self._latex()
def _pva(val):
try:
p = val[0]
except:
p = val
try:
v = val[1]
except:
v = None
try:
a = val[2]
except:
a = None
return p, v, a
def _delta(x0, x1):
try:
return float(x1 - x0)
except:
return None
[docs]def ramp(dp, v0, v1, a):
"""
:param dp: float delta position or None if unknown
:param v0: float initial velocity or None if unknown
:param v1: float final velocity or None if unknown
:param a: float acceleration
:return: float shortest time to accelerate between constraints
"""
dt = []
dv = _delta(v0, v1)
if dv:
dt.append(dv / a) # time to accelerate
try: # solve a.t^2/2+v0.t == dp
dt.extend(list(math2.quad(a / 2., v0, -dp)))
except:
try: # solve v1.t-a.t^2/2 == dp
dt.extend(list(math2.quad(-a / 2., v1, -dp)))
except:
pass
return min(t for t in dt if t > 0) # return smallest positive
[docs]def trapeze(dp, vmax, a, v0=0, v2=0):
"""
:param dp: float delta position
:param vmax: float maximal velocity
:param a: float acceleration
:param v0: float initial velocity, 0 by default
:param v2: float final velocity, 0 by default
:return: tuple of 6 values:
* time at end of acceleration
* position at end of acceleration
* velocity at end of acceleration
* time at begin of deceleration
* position at begin of deceleration
* total time
"""
t1 = ramp(dp / 2., v0, vmax, a) # acceleration time
v1 = v0 + a * t1 # speed reached
p1 = t1 * (v0 + v1) / 2. # position at end of acceleration
t3 = ramp(dp / 2., v1, v2, -a) # deceleration time
p2 = t3 * (v1 + v2) / 2. # distance to decelerate
t2 = float(dp - p1 - p2) / v1 # time at constant velocity
return t1, p1, v1, t1 + t2, dp - p2, t1 + t2 + t3
[docs]def Segment2ndDegree(t0, t1, start, end=(None)):
"""calculates a constant acceleration Segment between start and end
:param t0,t1: float start,end time. one of both may be None for undefined
:param start: (position, velocity, acceleration) float tuple. some values may be None for undefined
:param end: (position, velocity, acceleration) float tuple. some values may be None for undefined
:return: :class:`SegmentPoly`
the function can cope with almost any combination of defined/undefined parameters,
among others (see tests):
* Segment2ndDegree(t0,t1,(p0,v0),p1) # time interval and start + end positions + initial speed
* Segment2ndDegree(t0,t1,(p0,v0,a)) # time interval and start with acceleration
* Segment2ndDegree(t0,t1,None,(p1,v1,a)) # time interval and end pva
* Segment2ndDegree(t0,None,(p0,v0),(p1,v1)) # start + end positions + velocities
* Segment2ndDegree(t0,None,(p0,v0,a),(None,v1)) # start pva + end velocity
* Segment2ndDegree(None,t1,p0,(p1,v1,a)) # end pva + start position
the function also accepts some combinations of overconstraining parameters:
* Segment2ndDegree(t0,t1,(p0,v0,a),p1) # time interval, start pva, end position => adjust t1
* Segment2ndDegree(t0,t1,(p0,v0,a),(None,v1)) # time interval, start pva, v1=max vel => adjust t1
:raise ValueError: when not enough parameters are specified to define the Segment univoquely
"""
p0, v0, a0 = _pva(start)
p1, v1, a1 = _pva(end)
if a0 is None: a0 = a1
# to handle the many possible cases, we evaluate missing information in a loop
for _retries in range(2): # two loops are enough to solve all cases , according to tests
dt = _delta(t0, t1)
dp = _delta(p0, p1)
dv = _delta(v0, v1)
if not itertools2.anyf((dt, p0, v0, a0), lambda x: x is None): # we have all required data
res = SegmentPoly(t0, t1, [p0, v0, a0 / 2.])
end = res.end()
if p1 is not None and not math2.isclose(end[0], p1): # consider p1 as max position
res2 = Segment2ndDegree(t0, None, (p0, v0, a0), p1)
if res2.dt() < res.dt(): # this case arises earlier
res = res2
if v1 is not None and not math2.isclose(end[1], v1): # consider v1 as max velocity
res2 = Segment2ndDegree(t0, None, (p0, v0, a0), (None, v1))
if res2.dt() < res.dt(): # this case arises earlier
res = res2
return res
if dt is None: # try to determine it from available params
if a0:
dt = ramp(dp, v0, v1, a0)
else:
try:
dt = 2. * dp / (v0 + v1) # time to reach the position
except:
pass
if t0 is None:
try:
t0 = t1 - dt
except:
pass
if t1 is None:
try:
t1 = t0 + dt
except:
pass
if a0 is None:
try:
a0 = float(dv) / dt
except:
try:
a0 = 2. * (dp - v0 * dt) / dt * dt
except:
pass
if v0 is None:
try:
v0 = v1 - a0 * dt
except:
pass
if p0 is None:
try:
p0 = p1 - dt * float(v1 + v0) / 2.
except:
pass
raise ValueError
[docs]def Segment4thDegree(t0, t1, start, end):
"""smooth trajectory from an initial position and initial speed (p0,v0) to a final position and speed (p1,v1)
* if t1<=t0, t1 is calculated
"""
p0, v0, _a0 = _pva(start)
p1, v1, _a1 = _pva(end)
if t1 <= t0:
dt = float(p1 - p0) / ((v1 - v0) / 2. + v0) # truediv
t1 = t0 + dt
else:
dt = t1 - t0
return SegmentPoly(t0, t1, [p0, v0, 0, float(v1 - v0) / (dt * dt), -float(v1 - v0) / (2 * dt * dt * dt)]) # truediv
[docs]def SegmentsTrapezoidalSpeed(t0, p0, p3, a, T=0, vmax=float('inf'), v0=0, v3=0):
"""
:param t0: float start time
:param p0: float start position
:param p3: float end position
:param a: float specified acceleration. if =0, use specified time
:param T: float specified time. if =0 (default), use specified acceleration
:param vmax: float max speed. default is infinity (i.e. triangular speed)
:param v0: initial speed
:param v3: final speed if T <> 0 then v3 = v0
v1 +-------+
/ \
/ + v3
v0 +
| | | |
t0 t1 t2 t3
"""
dp = p3 - p0
if T != 0:
assert t0 == 0.0, 'must fix this bug'
assert v3 == v0, 'if T is the constraint, v0 must equal v3'
t1 = T / 2
v1 = dp / t1 - v0 # (v0+v1)/2 *t1 = dp/2 ==> v0*t1 + v1*t1 =dp
if v1 > vmax:
RuntimeError('vmax not yet implemented: must be infinite')
else:
a = (v1 - v0) / t1
(t1, p1, v1, t2, p2, t3) = trapeze(dp, vmax, a, v0, v3)
label = 'Trapeze start={0}, end={1}'.format(t0, t0 + t3)
acc = Segment2ndDegree(t0, t1 + t0, (p0, v0, a))
cst = SegmentPoly(t1 + t0, t2 + t0, [p1 + p0, v1])
dec = Segment2ndDegree(t2 + t0, t3 + t0, (p2 + p0, v1, -a))
trap = Segments([acc, cst, dec], label=label)
return trap