public DubinState(DubinNode node) { this.pos = node.pos; this.v = node.vel; this.theta = node.theta; this.omega = node.omega; }
void addNode(List <DubinNode> nodes, DubinNode node) { nodes.Add(node); kdTree.insert(new double[2] { node.pos.x, node.pos.y }, node); }
public static void backtrackPath(List <DubinNode> nodes, DubinNode startNode, DubinNode current, List <DubinNode> path) { if (current.parent != null) { backtrackPath(nodes, startNode, current.parent, path); } path.Add(current); }
public void SetParent(DubinNode p) { if (this.parent != null) { this.parent.children.Remove(this); } this.parent = p; }
public DubinNode(Vector2 pos, float vel, float theta, float omega, float time) { this.pos = pos; this.vel = vel; this.theta = theta; this.omega = omega; this.time = time; this.children = new List <DubinNode>(); this.parent = null; }
public DubinNode() { this.pos = Vector2.zero; this.vel = 0F; this.theta = 0F; this.omega = 0F; this.time = 0F; this.children = new List <DubinNode>(); this.parent = null; }
public static void TopoSort(DubinNode root, List <Pair <DubinNode, float> > sorted, float dt) { foreach (var child in root.children) { TopoSort(child, sorted, child.time - root.time); } sorted.Add(new Pair <DubinNode, float>(root, dt)); if (root.parent == null) { sorted.Reverse(); } }
/* * dubins transition function, multiple uses * 1. if collision param is true: check for collision, will return bool * 2. else: put interpolated nodes into list */ public static bool dubinsTransition(bool collision, List <Node> list, DubinNode from, DubinNode to, DubinInput input) { DubinNode prev = from; double tx = from.pos.x, ty = from.pos.y, ttheta = from.theta; // HACK: to improve precision foreach (DubinInput.Steer steer in input.steer) { float sumT = 0; double v = steer.v; while (sumT < steer.time) { var dt = Mathf.Min(DELTA_T, steer.time); tx = tx + dt * v * Math.Cos(prev.theta); ty = ty + dt * v * Math.Sin(prev.theta); var pos = new Vector2((float)tx, (float)ty); v += dt * steer.a; ttheta = ttheta + dt * steer.omega; while (ttheta > 2 * Mathf.PI) { ttheta -= 2 * Mathf.PI; } while (ttheta < 0) { ttheta += 2 * Mathf.PI; } if (collision && !Cheetah.instance.IsValid(pos)) { return(false); } DubinNode nextPoint = new DubinNode(pos, steer.v, (float)ttheta, steer.omega, prev.time + dt); if (!collision) { list.Add(nextPoint); } // if ((nextPoint.pos - prev.pos).magnitude > 0.5) // TODO: debug // { // Debug.Log("WTF"); // dubinInput(from, to); // return false; // } prev = nextPoint; sumT += dt; } } // // TODO: debug // if ((prev.pos - to.pos).magnitude > 0.5 || Mathf.Abs(prev.theta - to.theta) > 0.3) // { // Debug.Log("WTF"); // dubinInput(from, to); // return false; // } return(true); }
bool pathFeasible(DubinNode from, DubinNode to, DubinInput input) { // TODO: check for max condition foreach (DubinInput.Steer steer in input.steer) { if (Mathf.Abs(steer.a) > MAX_ACCEL) { return(false); } } var dubinsCollision = DDrive.dubinsTransition(true, null, from, to, input); return(dubinsCollision); }
Pair <DubinNode, float> findCheapestPoint(List <DubinNode> nodes, DubinNode target) { float minCost = Mathf.Infinity; DubinNode cheapest = new DubinNode(); //var knn = kdTree.nearest(new double[2] {target.pos.x, target.pos.y}, Math.Min(nodes.Count, 100)); var knn = nodes; foreach (var nn in knn) { DubinNode p = (DubinNode)nn; var input = dubinInput(p, target); if (input.time + p.time < minCost && pathFeasible(p, target, input)) { minCost = p.time + input.time; cheapest = p; } } return(new Pair <DubinNode, float>(cheapest, minCost)); }
void reparent(DubinNode root, DubinNode parent, DubinNode goal) { var nodes = new List <Pair <DubinNode, float> >(); Utils.TopoSort(root, nodes, 0F); for (int i = 0; i < nodes.Count; i++) { DubinNode p = nodes[i].first; float dt = nodes[i].second; var input = dubinInput(parent, p); if (p.parent != null && p.parent.time + dt < p.time) { p.time = p.parent.time + dt; } if (input.time + parent.time < p.time && pathFeasible(parent, p, input)) { p.time = input.time + parent.time; p.SetParent(parent); reparentCnt++; } } }
List <Node> GetFilledPath(List <TreeNode> path) { List <Node> filledPath = new List <Node>(); for (var i = 0; i < path.Count - 1; i++) { if (path[i + 1].isDubins) { DubinNode from = nodeToDubinNode(path[i]); DubinNode to = nodeToDubinNode(path[i + 1]); var input = dubinInput(from, to); filledPath.Add(from); DDrive.dubinsTransition(false, filledPath, from, to, input); } else { var input = dynamicPointInput(path[i], path[i + 1]); //Debug.Log(path[i + 1].vel.magnitude + " " + (path[i].vel + input.first * input.second).magnitude); float sumT = 0; var prev = path[i]; filledPath.Add(prev); while (sumT < input.time) { var dt = Mathf.Min(DELTA_T, input.time); var pos = prev.pos + prev.vel * dt + 0.5F * input.accel * dt * dt; var vel = prev.vel + input.accel * dt; TreeNode nextPoint = new TreeNode(pos, vel, prev.time + dt); filledPath.Add((Node)nextPoint); prev = nextPoint; sumT += dt; } } } return(filledPath); }
private DubinInput dubinInput(DubinNode near, DubinNode target) { return(DubinUtils.DubinSP(new DubinState(near), new DubinState(target), true)); }
List <Node> PlanRrtStar() { // RRT* startNode = new DubinNode(new Vector2(startPos[0], startPos[1]), new Vector2(startVel[0], startVel[1]).magnitude, startTheta, MAX_OMEGA, 0); addNode(nodes, startNode); goalNode = new DubinNode(new Vector2(goalPos[0], goalPos[1]), new Vector2(goalVel[0], goalVel[1]).magnitude, goalTheta, MAX_OMEGA, 0); int cnt = iter, i = 0; float r = Mathf.Infinity; // TODO: Dynamic compute this r var solnCnt = 0; while (i++ < cnt) { DubinNode target = i == cnt ? goalNode : randomNode(); var cheapest = findCheapestPoint(nodes, target); var cheapestNode = cheapest.first; var cheapestCost = cheapest.second; if (cheapestCost == Mathf.Infinity) { continue; } // TODO: Change the magic number if ((target.pos - goalNode.pos).magnitude <= 0.5) { Debug.Log("found a soln"); solnCnt++; informedLength = Mathf.Min(informedLength, cheapestCost * MAX_SPEED); curBestTime = Mathf.Min(curBestTime, cheapestCost); Debug.Log(target.pos); Debug.Log(cheapestCost); } if (cheapestCost + (goalNode.pos - target.pos).magnitude / MAX_SPEED > curBestTime + 0.5) { usefulCnt++; // i--; // continue; } cheapestNode.children.Add(target); target.SetParent(cheapestNode); target.time = cheapestCost; addNode(nodes, target); reparent(startNode, target, goalNode); } Debug.Log("reparent cnt = " + reparentCnt); Debug.Log("useful count = " + usefulCnt); Debug.Log("goalNode parent count " + (goalNode.parent != null)); List <DubinNode> path = new List <DubinNode>(); Utils.backtrackPath(nodes, startNode, goalNode, path); for (i = 0; i < path.Count - 1; i++) { var f = path[i]; Debug.Log("vel " + i); Debug.Log(f.vel); Debug.Log("pos"); Debug.Log(f.pos); } var filledPath = GetFilledPath(path); Utils.SaveFilledPath(filledPath); Debug.Log("path " + path.Count); Debug.Log("path[cnt] " + path[path.Count - 1].pos.x + " " + path[path.Count - 1].pos.y + " " + path[path.Count - 1].time); return(filledPath); }