示例#1
0
 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)
示例#2
0
    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')