#!/usr/bin/env python
# coding: utf8
"""
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.any((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