// Initializes and runs rrt
override protected void LocalStart()
{
// Setting state options
KinematicCarState.maxVel = maxVel;
KinematicCarState.maxPhi = maxPhi;
KinematicCarState.L = L;
KinematicCarState.lo = lowLimitRRT;
KinematicCarState.hi = highLimitRRT;
KinematicCarState.heuristic = heuristic;
// Set scale and rotate to right
transform.localScale = new Vector3(1, 1, L);
transform.rotation = Quaternion.Euler(0, 90, 0);
// Create states, rotation is right
startState = new KinematicCarState(
startPos.x, startPos.y, Vector3.right);
goalState = new KinematicCarState(
goalPos.x, goalPos.y, Vector3.right);
// Run rrt
rrt = new RRT <KinematicCarState>(
startState,
goalState,
KinematicCarState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Set moves and other data needed for base
moves = new Stack <Move>(Enumerable.Reverse(rrt.moves));
cost = rrt.cost;
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
// This part generates points for realistic path
GameObject tmp = new GameObject();
Transform tr = tmp.transform;
tr.position = transform.position;
tr.rotation = transform.rotation;
List <Move> tmpMoves = new List <Move>();
foreach (Move m in rrt.moves)
{
tmpMoves.Add(m.Copy());
}
float step = 1 / maxVel;
foreach (Move m in tmpMoves)
{
while (m.t > 0)
{
m.MoveMe(tr, step);
poss.Add(tr.position);
}
}
Destroy(tmp);
}
// Runs RRT and finds path
override protected void LocalStart()
{
// Setting state options
KinematicPointState.maxVel = maxVel;
KinematicPointState.lo = lowLimitRRT;
KinematicPointState.hi = highLimitRRT;
// Initialize and run RRT
startState = new KinematicPointState(startPos);
goalState = new KinematicPointState(goalPos);
rrt = new RRT <KinematicPointState>(
startState,
goalState,
KinematicPointState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Create a stack of moves, very important to use stack
moves = new Stack <Move>(Enumerable.Reverse(rrt.moves));
// Set cost and init time which will be printed to screen later
cost = rrt.cost;
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
}
// Use this for initialization
void Start()
{
rrt = GetComponent <RRT>();
segments = new List <GameObject>();
_main_camera = Camera.main;
mc_position = _main_camera.transform.position;
period = 5.0f;
flashTime = 0.5f;
userInteraction = false;
}
// Use this for initialization
void Start()
{
lidar = GetComponentInChildren <Lidar>();
tmp_gizmo_drawer = new List <Node>();
k = 1.0f + k;
gravity = Physics.gravity.y;
local_goal = transform.position;
rrt = new RRT(goal, 1.0f);
rrt.forward = transform.forward;
rrt.build_rrt(transform.position, 1000);
global_path = AStar(goal);
}
/* Generates the core star */
public void init(NodeInfo info, GameObject fab, float size)
{
rTree = new RRT();
rTree.XMAX = BOX_SIZE;
rTree.XMIN = -BOX_SIZE;
rTree.YMAX = BOX_SIZE;
rTree.YMIN = -BOX_SIZE;
rTree.ZMAX = BOX_SIZE;
rTree.ZMIN = -BOX_SIZE;
rTree.START_POS = this.transform.position;
rTree.nodeFab = fab;
rTree.connectionFab = connectionFab;
mainNode = rTree.init(fab, info).GetComponent <NormalNode>();
mainNode.size = size;
isInit = true;
}
// Initialize path
override protected void LocalStart()
{
// Setting state options
DynamicPointState.lo = lowLimitRRT;
DynamicPointState.hi = highLimitRRT;
DynamicPointState.maxAcc = maxAcceleration;
DynamicPointState.heuristic = heuristic;
DynamicPointState.hParam = hParam;
// Set states with zero initial velocity and run rrt
startState = new DynamicPointState(
startPos.x, startPos.y, Vector3.zero);
goalState = new DynamicPointState(
goalPos.x, goalPos.y, Vector3.zero);
DynamicPointState.goalState = goalState;
rrt = new RRT <DynamicPointState>(
startState,
goalState,
DynamicPointState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Remove the last move in the list and replace it with breaking
DynamicPointMove last =
rrt.moves[rrt.moves.Count - 1] as DynamicPointMove;
Vector3 lastVel = last.velocity;
float lastTime = lastVel.magnitude / maxAcceleration;
Move lastMove = new DynamicPointMove(lastVel,
-lastVel.normalized * maxAcceleration, lastTime);
moves = new Stack <Move>(
Enumerable.Concat(
new Move[] { lastMove },
Enumerable.Reverse(rrt.moves).Skip(1)
)
);
// Set times
cost = rrt.cost + lastTime - last.t;
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
}
public void createRRT()
{
float[] rrtStartConf = {0, 0};
rrt = new RRT(new StateConfig(rrtStartConf), twoDimLimits, RRTIncrement, RRTGoalBias, RRTGoalDistanceThreshold);
constructRRT();
}
// Initializes and runs rrt
override protected void LocalStart()
{
// Setting state options
DynamicCarState.maxAcc = maxAcc;
DynamicCarState.maxPhi = maxPhi;
DynamicCarState.L = L;
DynamicCarState.r = L / Mathf.Tan(maxPhi * Mathf.PI / 180);
DynamicCarState.lo = lowLimitRRT;
DynamicCarState.hi = highLimitRRT;
DynamicCarState.timeStep = time;
// Set scale and rotate to right
transform.localScale = new Vector3(1, 1, L);
transform.rotation = Quaternion.Euler(0, 90, 0);
// Create states, rotation is right
startState = new DynamicCarState(startPos.x, startPos.y, Vector3.right, 0);
goalState = new DynamicCarState(goalPos.x, goalPos.y, Vector3.right, 0);
// Run rrt
rrt = new RRT <DynamicCarState>(
startState,
goalState,
DynamicCarState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Remove the last move in the list and replace it with breaking
DynamicCarMove last =
rrt.moves[rrt.moves.Count - 1] as DynamicCarMove;
Vector3 lastVel = last.velocity;
float lastTime = last.speed / maxAcc;
Move lastMove = new DynamicPointMove(lastVel * last.speed,
-lastVel.normalized * maxAcc, lastTime);
moves = new Stack <Move>(
Enumerable.Concat(
new Move[] { lastMove },
Enumerable.Reverse(rrt.moves).Skip(1)
)
);
// Set moves and other data needed for base
cost = rrt.cost + lastTime - last.t;
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
// This part generates points for realistic path
GameObject tmp = new GameObject();
Transform tr = tmp.transform;
tr.position = transform.position;
tr.rotation = transform.rotation;
List <Move> tmpMoves = new List <Move>();
foreach (Move m in rrt.moves)
{
tmpMoves.Add(m.Copy());
}
foreach (Move m in tmpMoves)
{
while (m.t > 0)
{
m.MoveMe(tr, 0.1f);
poss.Add(tr.position);
}
}
Destroy(tmp);
}
private void ManipulatorProperties(Manipulator manipulator)
{
ImGui.Checkbox($"Show collider", ref manipulator.ShowCollider);
ImGui.InputFloat3("Goal", ref manipulator.Goal);
ImGui.Separator();
if (ImGui.BeginTabBar("ManipulatorTabs"))
{
if (ImGui.BeginTabItem("Solver"))
{
int currType = (int)manipulator.Controller.InverseKinematicsSolver.Type; // TODO: refactor?
int newType = currType;
ImGui.Combo("Type", ref newType, InverseKinematicsSolver.Types, InverseKinematicsSolver.Types.Length);
// change inverse kinematics solver if queried
if (newType != currType)
{
switch ((InverseKinematicsSolverType)newType)
{
case InverseKinematicsSolverType.JacobianTranspose:
manipulator.Controller.InverseKinematicsSolver = JacobianTranspose.Default();
break;
case InverseKinematicsSolverType.JacobianPseudoinverse:
manipulator.Controller.InverseKinematicsSolver = JacobianPseudoinverse.Default();
break;
case InverseKinematicsSolverType.DampedLeastSquares:
manipulator.Controller.InverseKinematicsSolver = DampedLeastSquares.Default();
break;
case InverseKinematicsSolverType.HillClimbing:
manipulator.Controller.InverseKinematicsSolver = HillClimbing.Default();
break;
}
}
ImGui.Separator();
// inverse kinematics solver properties
ImGui.InputFloat("Threshold", ref manipulator.Controller.InverseKinematicsSolver.Threshold);
ImGui.InputInt("Max iterations", ref manipulator.Controller.InverseKinematicsSolver.MaxIterations);
if (manipulator.Controller.InverseKinematicsSolver is JacobianTranspose jacobianTranspose)
{
ImGui.InputFloat("Base damping coefficient", ref jacobianTranspose.Damping);
}
else if (manipulator.Controller.InverseKinematicsSolver is JacobianPseudoinverse jacobianInverse)
{
// TODO: input something here?
}
else if (manipulator.Controller.InverseKinematicsSolver is DampedLeastSquares dampedLeastSquares)
{
ImGui.InputFloat("Damping coefficient", ref dampedLeastSquares.Damping);
}
else if (manipulator.Controller.InverseKinematicsSolver is HillClimbing hillClimbing)
{
ImGui.InputFloat("Max step size", ref hillClimbing.MaxStepSize);
}
ImGui.EndTabItem();
}
if (ImGui.BeginTabItem("Planner"))
{
int currType = (int)manipulator.Controller.PathPlanner.Type;
int newType = currType;
ImGui.Combo("Type", ref newType, PathPlanner.Types, PathPlanner.Types.Length);
// change path planner if queried
if (newType != currType)
{
switch ((PathPlannerType)newType)
{
case PathPlannerType.RRT:
manipulator.Controller.PathPlanner = RRT.Default();
break;
case PathPlannerType.ARRT:
manipulator.Controller.PathPlanner = ARRT.Default(manipulator);
break;
case PathPlannerType.GeneticAlgorithm:
manipulator.Controller.PathPlanner = GeneticAlgorithm.Default();
break;
}
}
ImGui.Separator();
// path planner properties
ImGui.Checkbox("Collision check", ref manipulator.Controller.PathPlanner.CollisionCheck);
ImGui.InputInt("Max iterations", ref manipulator.Controller.PathPlanner.MaxIterations);
ImGui.InputFloat("Threshold", ref manipulator.Controller.PathPlanner.Threshold);
if (manipulator.Controller.PathPlanner is RRT rrt)
{
ImGui.Checkbox($"Show tree", ref rrt.ShowTree);
ImGui.InputFloat("Step", ref rrt.Step);
ImGui.Checkbox("Enable trimming", ref rrt.EnableTrimming);
ImGui.InputInt("Trim period", ref rrt.TrimPeriod);
if (rrt is ARRT arrt)
{
ImGui.InputInt("Attractors count", ref arrt.AttractorsCount);
}
else
{
ImGui.InputInt("Goal bias period", ref rrt.GoalBiasPeriod);
}
}
else if (manipulator.Controller.PathPlanner is GeneticAlgorithm geneticAlgorithm)
{
// TODO: add genetic algorithm related properties
ImGui.InputInt("Offspring size", ref geneticAlgorithm.OffspringSize);
ImGui.InputInt("Survival size", ref geneticAlgorithm.SurvivalSize);
ImGui.InputInt("Bezier control points", ref geneticAlgorithm.BezierControlPointsCount);
ImGui.InputFloat("Bezier step", ref geneticAlgorithm.BezierStep);
}
ImGui.EndTabItem();
}
if (ImGui.BeginTabItem("Controller"))
{
// TODO: add motion control related properties
ImGui.EndTabItem();
}
ImGui.EndTabBar();
}
}
// Initializes and runs rrt
override protected void LocalStart()
{
// Setting state options
DynamicKinState.maxVel = 1;
DynamicKinState.maxPhi = maxPhi;
DynamicKinState.L = L;
DynamicKinState.lo = lowLimitRRT;
DynamicKinState.hi = highLimitRRT;
DynamicKinState.heuristic = heuristic;
// Set scale and rotate to right
transform.localScale = new Vector3(1, 1, L);
transform.rotation = Quaternion.Euler(0, 90, 0);
// Create states, rotation is right
startState = new DynamicKinState(
startPos.x, startPos.y, Vector3.right);
goalState = new DynamicKinState(
goalPos.x, goalPos.y, Vector3.right);
// Run rrt
rrt = new RRT <DynamicKinState>(
startState,
goalState,
DynamicKinState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Recompute all moves for dynamic car
float distance = rrt.cost; // full path
float accUntil = distance / 2; // braking
float r = L / Mathf.Tan(maxPhi * Mathf.PI / 180); // radius
float v = 0; // speed
cost = 0; // new cost
List <Move> newMoves = new List <Move>();
foreach (Move m in rrt.moves)
{
float d = m.t; // This move distance
KinematicCarMove tmpMove = m as KinematicCarMove;
if (distance > accUntil && distance - d < accUntil)
{
// Special case when in the middle move, has to start braking
// First needs to accelerate for some time
float d1 = distance - accUntil; // Accelerating distance
float time = (-v + Mathf.Sqrt(v * v + 2 * maxAcc * d1)) / maxAcc;
newMoves.Add(
new DynamicKinMove(tmpMove.velocity, v, maxAcc,
tmpMove.omega, r, time)
);
v += maxAcc * time; // update speed
cost += time; // update cost
// Then needs to deccelerate
// New direction of the car
Vector3 newVel = Quaternion.Euler(0, tmpMove.omega * d1, 0)
* tmpMove.velocity;
float d2 = d - d1; // Braking distance
time = (-v + Mathf.Sqrt(v * v - 2 * maxAcc * d2)) / (-maxAcc);
newMoves.Add(
new DynamicKinMove(newVel, v, -maxAcc,
tmpMove.omega, r, time)
);
v -= maxAcc * time; // update speed
cost += time; // update cost
}
else
{
// Other case when it needs only to accelerate or brake
float a = distance > accUntil ? maxAcc : -maxAcc;
a = Mathf.Max(a, -0.5f * v * v / d + 0.0001f); // For last brake
float time = (-v + Mathf.Sqrt(v * v + 2 * a * d)) / a;
//TODO there appeared to be a bug here
newMoves.Add(
new DynamicKinMove(tmpMove.velocity, v, a,
tmpMove.omega, r, time)
);
v += a * time; // update speed
cost += time; // update cost
}
// Reduce full distance by the distance of the last move
distance -= d;
}
// Set moves and other data needed for base
moves = new Stack <Move>(Enumerable.Reverse(newMoves));
// Update times
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
// This part generates points for realistic path
GameObject tmp = new GameObject();
Transform tr = tmp.transform;
tr.position = transform.position;
tr.rotation = transform.rotation;
List <Move> tmpMoves = new List <Move>();
foreach (Move m in rrt.moves)
{
tmpMoves.Add(m.Copy());
}
int count = 0;
foreach (Move m in tmpMoves)
{
while (m.t > 0)
{
m.MoveMe(tr, 0.1f);
if (count % 10 == 0) // Put every 10th point
{
poss.Add(tr.position);
}
count++;
}
}
Destroy(tmp);
}
