# Source code for Goulib.motion

```#!/usr/bin/env python
# coding: utf8
"""
motion simulation (kinematics)
"""

__author__ = "Philippe Guglielmetti"
__credits__= ["http://osterone.bobstgroup.com/wiki/index.php?title=UtlCam"]

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 = []

[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)

""" 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
except:
try: # solve v1.t-a.t^2/2 == 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,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

```