|
| 1 | +---@class HybridAStar.JpsMotionPrimitives : HybridAStar.SimpleMotionPrimitives |
| 2 | +HybridAStar.JpsMotionPrimitives = CpObject(HybridAStar.SimpleMotionPrimitives) |
| 3 | +function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaThetaGoal, constraints, goal) |
| 4 | + -- similar to the A*, the possible motion primitives (the neighbors) are in all 8 directions. |
| 5 | + AStar.SimpleMotionPrimitives.init(self, gridSize) |
| 6 | + for i = 1, #self.primitives do |
| 7 | + -- if we moved to the current node using primitive[i], then the primitive we would need to go back to |
| 8 | + -- the parent is primitive[i].parent |
| 9 | + self.primitives[i].parent = self.primitives[i + 4] or self.primitives[i - 4] |
| 10 | + if self.primitives[i].dy == 0 then |
| 11 | + -- horizontal move |
| 12 | + self.primitives[i].neighbors = { |
| 13 | + -- unconditionally include natural neighbors. |
| 14 | + -- always need this neighbor to the left or right of the current node |
| 15 | + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, |
| 16 | + -- if there is an obstacle at 'check' if we got to the node through this primitive, then |
| 17 | + -- create a forced neighbors at 'forcedNeighbor' |
| 18 | + { check = { dx = 0, dy = self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = self.gridSize } }, |
| 19 | + { check = { dx = 0, dy = -self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = -self.gridSize } } |
| 20 | + } |
| 21 | + elseif self.primitives[i].dx == 0 then |
| 22 | + -- vertical move |
| 23 | + self.primitives[i].neighbors = { |
| 24 | + -- unconditionally include natural neighbors. |
| 25 | + -- always need this neighbor to the left or right of the current node |
| 26 | + { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, |
| 27 | + -- forced neighbors are included only if the checked neighbors are invalid |
| 28 | + { check = { dx = self.gridSize, dy = 0 }, neighbor = { dx = self.gridSize, dy = self.primitives[i].dy } }, |
| 29 | + { check = { dx = -self.gridSize, dy = 0 }, neighbor = { dx = -self.gridSize, dy = self.primitives[i].dy } } |
| 30 | + } |
| 31 | + else |
| 32 | + -- diagonal move |
| 33 | + self.primitives[i].neighbors = { |
| 34 | + -- unconditionally include natural neighbors. |
| 35 | + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction |
| 36 | + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, |
| 37 | + { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, |
| 38 | + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = self.primitives[i].dy } }, |
| 39 | + -- forced neighbors are included only if the checked neighbors are invalid |
| 40 | + { check = { dx = -self.primitives[i].dx, dy = 0 }, |
| 41 | + neighbor = { dx = -self.primitives[i].dx, dy = self.primitives[i].dy } }, |
| 42 | + { check = { dx = 0, dy = -self.primitives[i].dy }, |
| 43 | + neighbor = { dx = self.primitives[i].dx, dy = -self.primitives[i].dy } }, |
| 44 | + } |
| 45 | + |
| 46 | + end |
| 47 | + end |
| 48 | + self.deltaPosGoal = deltaPosGoal |
| 49 | + self.deltaThetaGoal = deltaThetaGoal |
| 50 | + self.constraints = constraints |
| 51 | + self.goal = goal |
| 52 | +end |
| 53 | + |
| 54 | +function HybridAStar.JpsMotionPrimitives:getGridSize() |
| 55 | + return self.gridSize |
| 56 | +end |
| 57 | + |
| 58 | +function HybridAStar.JpsMotionPrimitives:isValidNode(x, y, t, constraints) |
| 59 | + local node = { x = x, y = y, t = t } |
| 60 | + return constraints:isValidNode(node, true, true) |
| 61 | +end |
| 62 | + |
| 63 | +-- Get the possible neighbors when coming from the predecessor node. |
| 64 | +-- While the other HybridAStar derived algorithms use a real motion primitive, meaning it gives the relative |
| 65 | +-- x, y and theta values which need to be added to the predecessor, JPS supplies the actual coordinates of |
| 66 | +-- the successors here instead. |
| 67 | +-- This is not the most elegant solution and the only reason we do this is to be able to reuse the whole |
| 68 | +-- framework in HybridAStar.lua with JPS. |
| 69 | +function HybridAStar.JpsMotionPrimitives:getPrimitives(node) |
| 70 | + local primitives = {} |
| 71 | + if node.pred then |
| 72 | + local x, y, t = node.x, node.y, node.t |
| 73 | + -- Node has a parent, we will prune some neighbours |
| 74 | + -- Gets the direction of move |
| 75 | + local dx = self.gridSize * (x - node.pred.x) / math.max(1, math.abs(x - node.pred.x)) |
| 76 | + local dy = self.gridSize * (y - node.pred.y) / math.max(1, math.abs(y - node.pred.y)) |
| 77 | + local dDiag = math.sqrt(dx * dx + dy * dy) |
| 78 | + local xOk, yOk = false, false |
| 79 | + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then |
| 80 | + -- diagonal move |
| 81 | + if self:isValidNode(x, y + dy, t, self.constraints) then |
| 82 | + table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) |
| 83 | + yOk = true |
| 84 | + end |
| 85 | + if self:isValidNode(x + dx, y, t, self.constraints) then |
| 86 | + table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) |
| 87 | + xOk = true |
| 88 | + end |
| 89 | + if xOk or yOk then |
| 90 | + table.insert(primitives, { x = x + dx, y = y + dy, t = math.atan2(dy, dx), d = dDiag }) |
| 91 | + end |
| 92 | + -- Forced neighbors |
| 93 | + if not self:isValidNode(x - dx, y, t, self.constraints) and yOk then |
| 94 | + table.insert(primitives, { x = x - dx, y = y + dy, t = math.atan2(dy, -dx), d = dDiag }) |
| 95 | + table.insert(JumpPointSearch.markers, { label = 'forced x - y +', x = x - dx, y = y + dy }) |
| 96 | + node.forced = true |
| 97 | + end |
| 98 | + if not self:isValidNode(x, y - dy, t, self.constraints) and xOk then |
| 99 | + table.insert(primitives, { x = x + dx, y = y - dy, t = math.atan2(-dy, dx), d = dDiag }) |
| 100 | + table.insert(JumpPointSearch.markers, { label = 'forced x + y -', x = x + dx, y = y - dy }) |
| 101 | + node.forced = true |
| 102 | + end |
| 103 | + else |
| 104 | + if math.abs(dx) < 0.1 then |
| 105 | + -- move along the y axis |
| 106 | + if self:isValidNode(x, y + dy, t, self.constraints) then |
| 107 | + table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) |
| 108 | + end |
| 109 | + -- Forced neighbors |
| 110 | + dDiag = math.sqrt(dy * dy + self.gridSize * self.gridSize) |
| 111 | + if not self:isValidNode(x + self.gridSize, y, t, self.constraints) then |
| 112 | + table.insert(primitives, { x = x + self.gridSize, y = y + dy, |
| 113 | + t = math.atan2(dy, self.gridSize), d = dDiag }) |
| 114 | + table.insert(JumpPointSearch.markers, { label = 'forced x +', x = x + self.gridSize, y = y + dy }) |
| 115 | + node.forced = true |
| 116 | + end |
| 117 | + if not self:isValidNode(x - self.gridSize, y, t, self.constraints) then |
| 118 | + table.insert(primitives, { x = x - self.gridSize, y = y + dy, |
| 119 | + t = math.atan2(dy, -self.gridSize), d = dDiag }) |
| 120 | + table.insert(JumpPointSearch.markers, { label = 'forced x -', x = x - self.gridSize, y = y + dy }) |
| 121 | + node.forced = true |
| 122 | + end |
| 123 | + else |
| 124 | + -- move along the x axis |
| 125 | + if self:isValidNode(x + dx, y, t, self.constraints) then |
| 126 | + table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) |
| 127 | + end |
| 128 | + -- Forced neighbors |
| 129 | + dDiag = math.sqrt(dx * dx + self.gridSize * self.gridSize) |
| 130 | + if not self:isValidNode(x, y + self.gridSize, t, self.constraints) then |
| 131 | + table.insert(primitives, { x = x + dx, y = y + self.gridSize, |
| 132 | + t = math.atan2(self.gridSize, dx), d = dDiag }) |
| 133 | + table.insert(JumpPointSearch.markers, { label = 'forced y +', x = x + dx, y = y + self.gridSize }) |
| 134 | + node.forced = true |
| 135 | + end |
| 136 | + if not self:isValidNode(x, y - self.gridSize, t, self.constraints) then |
| 137 | + table.insert(primitives, { x = x + dx, y = y - self.gridSize, |
| 138 | + t = math.atan2(-self.gridSize, dx), d = dDiag }) |
| 139 | + table.insert(JumpPointSearch.markers, { label = 'forced y -', x = x + dx, y = y - self.gridSize }) |
| 140 | + node.forced = true |
| 141 | + end |
| 142 | + end |
| 143 | + end |
| 144 | + else |
| 145 | + -- no parent, this is the start node |
| 146 | + for _, p in pairs(self.primitives) do |
| 147 | + -- JPS does not really use motion primitives, what we call primitives are actually the |
| 148 | + -- successors, with their real coordinates, not just a delta. |
| 149 | + table.insert(primitives, { x = node.x + p.dx, y = node.y + p.dy, t = p.dt, d = p.d }) |
| 150 | + end |
| 151 | + end |
| 152 | + return primitives |
| 153 | +end |
| 154 | + |
| 155 | +function HybridAStar.JpsMotionPrimitives:jump(node, pred, recursionCounter) |
| 156 | + if recursionCounter and recursionCounter > 2 then |
| 157 | + return node, recursionCounter |
| 158 | + end |
| 159 | + recursionCounter = recursionCounter and recursionCounter + 1 or 1 |
| 160 | + local x, y, t = node.x, node.y, node.t |
| 161 | + if not self:isValidNode(x, y, t, self.constraints) then |
| 162 | + return nil |
| 163 | + end |
| 164 | + if node:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then |
| 165 | + return node |
| 166 | + end |
| 167 | + local dx = x - pred.x |
| 168 | + local dy = y - pred.y |
| 169 | + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then |
| 170 | + -- diagonal move |
| 171 | + if (self:isValidNode(x - dx, y + dy, t, self.constraints) and not self:isValidNode(x - dx, y, t, self.constraints)) or |
| 172 | + (self:isValidNode(x + dx, y - dy, t, self.constraints) and not self:isValidNode(x, y - dy, t, self.constraints)) then |
| 173 | + -- Current node is a jump point if one of its left or right neighbors ahead is forced |
| 174 | + return node |
| 175 | + end |
| 176 | + else |
| 177 | + if math.abs(dx) > 0.1 then |
| 178 | + -- move along the x axis |
| 179 | + if (self:isValidNode(x + dx, y + self.gridSize, t, self.constraints) and not self:isValidNode(x, y + self.gridSize, t, self.constraints)) or |
| 180 | + (self:isValidNode(x + dx, y - self.gridSize, t, self.constraints) and not self:isValidNode(x, y - self.gridSize, t, self.constraints)) then |
| 181 | + -- Current node is a jump point if one of its left or right neighbors ahead is forced |
| 182 | + return node |
| 183 | + end |
| 184 | + else |
| 185 | + -- move along the y axis |
| 186 | + if (self:isValidNode(x + self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x + self.gridSize, y, t, self.constraints)) or |
| 187 | + (self:isValidNode(x - self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x - self.gridSize, y, t, self.constraints)) then |
| 188 | + -- Current node is a jump point if one of its left or right neighbors ahead is forced |
| 189 | + return node |
| 190 | + end |
| 191 | + end |
| 192 | + end |
| 193 | + -- Recursive horizontal/vertical search |
| 194 | + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then |
| 195 | + local nextNode = State3D.copy(node) |
| 196 | + nextNode.x = nextNode.x + dx |
| 197 | + nextNode.g = nextNode.g + dx |
| 198 | + if self:jump(nextNode, node, recursionCounter) then |
| 199 | + return node |
| 200 | + end |
| 201 | + nextNode = State3D.copy(node) |
| 202 | + nextNode.y = nextNode.y + dy |
| 203 | + nextNode.g = nextNode.g + dy |
| 204 | + if self:jump(nextNode, node, recursionCounter) then |
| 205 | + return node |
| 206 | + end |
| 207 | + end |
| 208 | + -- Recursive diagonal search |
| 209 | + if self:isValidNode(x + dx, y, t, self.constraints) or self:isValidNode(x, y + dy, t, self.constraints) then |
| 210 | + local nextNode = State3D.copy(node) |
| 211 | + nextNode.x = nextNode.x + dx |
| 212 | + nextNode.y = nextNode.y + dy |
| 213 | + |
| 214 | + nextNode.g = nextNode.g + dy |
| 215 | + return self:jump(nextNode, node, recursionCounter) |
| 216 | + end |
| 217 | +end |
| 218 | + |
| 219 | +function HybridAStar.JpsMotionPrimitives:createSuccessor(node, primitive, hitchLength) |
| 220 | + local neighbor = State3D(primitive.x, primitive.y, primitive.t) |
| 221 | + local jumpNode, jumps = self:jump(neighbor, node) |
| 222 | + primitive.d = jumps and jumps * primitive.d or primitive.d |
| 223 | + if jumpNode then |
| 224 | + return State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, |
| 225 | + node:getNextTrailerHeading(self.gridSize, hitchLength)) |
| 226 | + end |
| 227 | +end |
| 228 | +--- A Jump Point Search |
| 229 | +---@class JumpPointSearch : AStar |
| 230 | +JumpPointSearch = CpObject(AStar) |
| 231 | +JumpPointSearch.markers = {} |
| 232 | + |
| 233 | +function JumpPointSearch:init(vehicle, yieldAfter, maxIterations) |
| 234 | + AStar.init(self, vehicle, yieldAfter, maxIterations) |
| 235 | + JumpPointSearch.markers = {} |
| 236 | +end |
| 237 | + |
| 238 | +function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) |
| 239 | + self.motionPrimitives = HybridAStar.JpsMotionPrimitives(self.deltaPos, self.deltaPosGoal, self.deltaThetaGoal, constraints, goal) |
| 240 | + return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) |
| 241 | +end |
| 242 | + |
| 243 | +function JumpPointSearch:getMotionPrimitives(turnRadius, allowReverse) |
| 244 | + return self.motionPrimitives |
| 245 | +end |
| 246 | + |
| 247 | +---@class HybridAStarWithJpsInTheMiddle : HybridAStarWithAStarInTheMiddle |
| 248 | +HybridAStarWithJpsInTheMiddle = CpObject(HybridAStarWithAStarInTheMiddle) |
| 249 | + |
| 250 | +function HybridAStarWithJpsInTheMiddle:init(hybridRange, yieldAfter, maxIterations, mustBeAccurate) |
| 251 | + HybridAStarWithAStarInTheMiddle.init(self, hybridRange, yieldAfter, maxIterations, mustBeAccurate) |
| 252 | +end |
| 253 | + |
| 254 | +function HybridAStarWithJpsInTheMiddle:getAStar() |
| 255 | + return JumpPointSearch(self.yieldAfter) |
| 256 | +end |
0 commit comments