def from_previousState(cls, previousState, accel, alpha, dt):
p = vec.vsum(previousState.p,vec.smul(dt, previousState.v))
v = vec.vsum(previousState.v,vec.smul(dt, accel))
th = vec.vsum(previousState.th,vec.smul(dt,vec.leftMat(vec.bMat(previousState.th),previousState.w)))
if th[0] > math.pi*2:
th[0] = th[0] - math.pi*2
if th[1] > math.pi*2:
th[1] = th[1] - math.pi*2
if th[2] > math.pi*2:
th[2] = th[2] - math.pi*2
w = vec.vsum(previousState.w,vec.smul(dt,alpha))
return cls(p,v,previousState.th,previousState.w)
def heuristic(self, state, spacecraft):
# print('placeholder4')
# DEFINE HEURISTIC HERE
# Don't refer to self.firstNode or self.firstHeuristic
# See notes file part 1
if not rotation:
amax = spacecraft.totalAccelMax
alpmax = spacecraft.totalAlphaMax
v0 = state.v
th0 = state.th
# worstTime = 0
# There is a critical speed along an axis such that if the spacecraft decelerates at its maximum
# rate it will stop exactly at the goal. The first contribution to the heuristic will be how
# different the spacecraft's actual velocity v0 is to the critical velocity, vc. The second
# contribution is the time it will take to decelerate to the goal if its speed were equal to the
# critical velocity
deltaP = []
deltaP.append(self.finalState.p[0] - state.p[0])
deltaP.append(self.finalState.p[1] - state.p[1])
deltaP.append(self.finalState.p[2] - state.p[2])
deltaTh = []
deltaTh.append(self.finalState.th[0] - state.th[0])
deltaTh.append(self.finalState.th[1] - state.th[1])
deltaTh.append(self.finalState.th[2] - state.th[2])
# h = []
# for i in range(3):
# if abs(deltaP[i]) < self.dp:
# h.append( v0[i]**4 / amax**4)
# # if h > worstTime:
# # worstTime = h
# # continue
# continue
# c1 = deltaP[i]**2 / (amax*amax)
# c2 = v0[i]*abs(v0[i]) / (amax*deltaP[i])
# h.append(c1*(c2**2 + 4*c2 + 8))
# # if h > worstTime:
# # worstTime = h
# # return worstTime
normDeltaP = vec.norm(deltaP)
normal = vec.smul(1/normDeltaP,deltaP)
normalV = vec.dot(v0,normal)
normV = vec.norm(v0)
tangentialV = math.sqrt(normV**2 - normalV**2)
vcrit = math.sqrt(2*amax*normDeltaP)
t1 = 0
if normalV < 0: #spacraft is heading the wrong way
tstop = -normalV/amax #time required to stop
t1 = tstop + self.ramp((1/2)*amax*(tstop**2) + normDeltaP,amax)
elif normalV > vcrit: #spacecraft is moving too fast and will overshoot
tstop = normalV/amax
t1 = tstop + self.ramp((1/2)*amax*(tstop**2) - normDeltaP,amax)
else: #spacecraft is heading the right way, slow enough to stop in time
negTStop = -normalV/amax #represents the time in the past the ramp would have started
t1 = negTStop + self.ramp((1/2)*amax*(negTStop**2) + normDeltaP,amax)
# t1 is the heuristic if there is no tangential velocity
# at the very least, the tangential velocity needs to be cancelled (taking extra time)
h = t1 + tangentialV/amax + math.sqrt((deltaTh[0]/alpmax)**2 + (deltaTh[1]/alpmax)**2 + (deltaTh[2]/alpmax)**2)
return h
raise NotImplementedError('Rotating Spacecraft Heuristic Not Implemented')
