public FootstepPlanningState(GridPlanningState gridState) { //float startTime = Time.realtimeSinceStartup; this.actionName = "GridAction"; //this.actionSpeed = 1.0f; //this.currentAcceleration = 1.0f; this.previousState = gridState.previousState as FootstepPlanningState; this.currentPosition = gridState.currentPosition; //this.currentRotation = gridState.previousState.currentRotation; //this.currentSpeed = 1.0f; //this.leftFoot = gridState.currentPosition; //this.leftHand = gridState.currentPosition; //this.obstaclePos = original.obstaclePos; //this.preconditions = original.preconditions; //this.rightFoot = gridState.currentPosition; //this.rightHand = gridState.currentPosition; //this.time = original.time; /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ }
//public static float timeSum = 0.0f; //public static int numCalls = 0; // Construction function for a state that comes from a previous state with a movement of mov public GridPlanningState(GridPlanningState prevState, Vector3 mov) { //float startTime = Time.realtimeSinceStartup; previousState = prevState; actionMov = mov; ComputeActualState(mov); /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); numCalls = 0; timeSum = 0; } */ }
//public static float timeSum = 0.0f; //public static int numCalls = 0; // Construction function for a state that comes from a previous state with a movement of mov public GridPlanningState(GridPlanningState prevState, Vector3 mov) { //float startTime = Time.realtimeSinceStartup; previousState = prevState; actionMov = mov; ComputeActualState(mov); /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ }
private float[] ComputeNodeDistances(DefaultAction[] plan) { float[] distances = new float[plan.Length]; int i = 0; GridPlanningAction action = null; GridPlanningState state = null; while (action == null && i < plan.Length) { action = plan[i] as GridPlanningAction; if (action != null) { state = action.state as GridPlanningState; } distances[i] = 0; i++; } GridPlanningState nextState; while (i + 1 < plan.Length) { nextState = (plan[i + 1] as GridPlanningAction).state as GridPlanningState; distances[i + 1] = (state.currentPosition - nextState.currentPosition).magnitude; state = nextState; i++; } return(distances); }
// Collision check for the lower resolutions model public bool CheckRootCollisions(GridPlanningState state) { Vector3 start = state.currentPosition - new Vector3(0,10*analyzer.GetHeight()/2,0); Vector3 end = start + new Vector3(0,10*analyzer.GetHeight()/2,0); float radius = analyzer.GetRadius(); if (Physics.CheckCapsule(start,end,radius,layer)) //if (Physics.CheckSphere(state.currentPosition,radius,layer)) return true; if (state.previousState != null) { int samples = analyzer.samples; Vector3 mov = state.actionMov; for (int i=1; i<samples-1; i+=2) // we sample less in this domain the collision check { start = (state.previousState as GridPlanningState).currentPosition + i/(samples-1)*mov; end = start + new Vector3(0,analyzer.GetHeight()/2,0); if (Physics.CheckCapsule(start,end,radius,layer)) //if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer)) return true; } } return false; }
// Collision check for the lower resolutions model public bool CheckRootCollisions(GridPlanningState state) { Vector3 start = state.currentPosition - new Vector3(0, 10 * analyzer.GetHeight() / 2, 0); Vector3 end = start + new Vector3(0, 10 * analyzer.GetHeight() / 2, 0); float radius = analyzer.GetRadius(); if (Physics.CheckCapsule(start, end, radius, layer)) { //if (Physics.CheckSphere(state.currentPosition,radius,layer)) return(true); } if (state.previousState != null) { int samples = analyzer.samples; Vector3 mov = state.actionMov; for (int i = 1; i < samples - 1; i += 2) // we sample less in this domain the collision check { start = (state.previousState as GridPlanningState).currentPosition + i / (samples - 1) * mov; end = start + new Vector3(0, analyzer.GetHeight() / 2, 0); if (Physics.CheckCapsule(start, end, radius, layer)) { //if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer)) return(true); } } } return(false); }
//This constructor creates a previous state given a current state and an action. //The dummyType is to differentiate it from the constructor used to create successors states. public GridPlanningState(GridPlanningState currentState, Vector3 mov, float dummyType) { previousState = currentState; actionMov = mov; ComputePreviousState(mov); }
private float ComputeEstimatedTime(GridPlanningState state, float velocity) { float distance = (state.currentPosition - startState.currentPosition).magnitude; float time = distance / velocity; return(time); }
//public static float timeSum = 0.0f; //public static int numCalls = 0; public override bool equals(DefaultState s1, DefaultState s2, bool isStart) { //float startTime = Time.realtimeSinceStartup; bool b = false; GridPlanningState state1 = s1 as GridPlanningState; GridPlanningState state2 = s2 as GridPlanningState; if (isStart) { if ((Mathf.Abs(state1.currentPosition.x - state2.currentPosition.x) < .5) && (Mathf.Abs(state1.currentPosition.y - state2.currentPosition.y) < .5) && (Mathf.Abs(state1.currentPosition.z - state2.currentPosition.z) < .5)) { b = true; //return true; } else { b = false; //return false; } } else { if ((Mathf.Abs(state1.currentPosition.x - state2.currentPosition.x) < .1) && (Mathf.Abs(state1.currentPosition.y - state2.currentPosition.y) < .1) && (Mathf.Abs(state1.currentPosition.z - state2.currentPosition.z) < .1)) { b = true; //return true; } else { b = false; // return false; } } /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ return(b); }
bool doTunnelTask() { Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> (); // convert tunnel for (int i = 0; i < path.Count; i++) { GridPlanningState gs = new GridPlanningState(path[i].getPosition()); GridPlanningAction g = new GridPlanningAction(gs, new Vector3()); convertedTunnel.Push(g); } GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(convertedTunnel, this.startState.getPosition(), startState._time, goalState._time, gridTimeDomain, 300, 2.0f, 1.0f, 1.0f, startState._speed); //new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F); //gridTunnelSearch.tryARAStarPlanner(spaceTimePath); //Debug.Log ("grid time plan using ara star planner " + spaceTimePath.Count); //return true; //// generating plan for now spaceTimePath.Clear(); /* * // this is the state plan * Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newStatePlan.Count); * while (gridTunnelSearch.newStatePlan.Count != 0) * { * GridTimeState state = gridTunnelSearch.newStatePlan.Pop() as GridTimeState; * //Debug.LogWarning ("space time state " + state.time); * spaceTimePath.Add(new State(state.currentPosition, state.time)); * } */ // this is the action plan Debug.Log("grid tunnel plan using best first planner " + gridTunnelSearch.newPlan.Count); while (gridTunnelSearch.newPlan.Count != 0) { DefaultAction action = gridTunnelSearch.newPlan.Pop(); GridTimeState state = action.state as GridTimeState; //Debug.LogWarning ("space time state " + state.time + " " + state.currentPosition); spaceTimePath.Add(new State(state.currentPosition, state.time, state.speed)); } bool planComputed = gridTunnelSearch.goalReached; //Debug.LogError("grid tunnel plan status " + planComputed); return(planComputed); }
override public float estimateTotalCost(ref DefaultState DcurrentState, ref DefaultState DidealGoalState, float currentg) { //float startTime = Time.realtimeSinceStartup; GridPlanningState currentState = DcurrentState as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (currentState == null) { FootstepPlanningState fsState = DcurrentState as FootstepPlanningState; currentState = new GridPlanningState(fsState); } //Debug.Log("Estimating cost"); Vector3 toGoal; if (idealGoalState != null) { // A state is a goal one if it's really close to the goal toGoal = idealGoalState.currentPosition - currentState.currentPosition; } else { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; toGoal = fsIdealGoalState.currentPosition - currentState.currentPosition; } float h = toGoal.magnitude / analyzer.meanStepSize; float f = currentg + W * h; //Debug.Log("Estimated cost = " + f); /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ return(f); }
override public bool CheckStateCollisions(DefaultState Dstate) { //float startTime = Time.realtimeSinceStartup; GridPlanningState state = Dstate as GridPlanningState; bool b = false; // If we are checking a state of this domain if (state != null) { b = CheckRootCollisions(state); //return CheckRootCollisions(state); } else // If not { // We need to transform the state of the coarser domain // to our low resolution domain FootstepPlanningState footstepState = Dstate as FootstepPlanningState; if (footstepState != null) { state = new GridPlanningState(footstepState); b = CheckRootCollisions(state); //return CheckRootCollisions(state); } else { b = false; //return false; } } /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ return(b); }
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List <DefaultAction> transitions) { GridPlanningState currentState = DcurrentState as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (currentState == null) { FootstepPlanningState fsState = DcurrentState as FootstepPlanningState; if (fsState != null) { currentState = new GridPlanningState(fsState); } else { currentState = new GridPlanningState(DcurrentState as GridTimeState); } } if (idealGoalState == null) { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; if (fsIdealGoalState != null) { idealGoalState = new GridPlanningState(fsIdealGoalState); } else { idealGoalState = new GridPlanningState(DidealGoalState as GridTimeState); } } foreach (Vector3 mov in movDirections) { GridPlanningAction newAction = new GridPlanningAction(currentState, mov); //if (!CheckTransitionCollisions(newAction.state,DcurrentState)) if (!CheckStateCollisions(newAction.state)) { //Debug.Log(Time.time + ": grid successor generated"); transitions.Add(newAction); } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
public override TaskStatus execute(float maxTime) { Stack <DefaultAction> convertedTunnel = new Stack <DefaultAction> (); // convert tunnel for (int i = tunnel.Count - 1; i >= 0; i--) { GridPlanningState gs = new GridPlanningState(tunnel[i].getPosition()); GridPlanningAction g = new GridPlanningAction(gs, new Vector3()); convertedTunnel.Push(g); } GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F); Debug.Log("grid tunnel plan " + gridTunnelSearch.newPlan.Count); //// generating plan for now spaceTimePath.Clear(); while (gridTunnelSearch.newPlan.Count != 0) { DefaultAction action = gridTunnelSearch.newPlan.Pop(); GridTimeState state = action.state as GridTimeState; spaceTimePath.Add(new State(state.currentPosition, state.time)); } bool planComputed = gridTunnelSearch.goalReached; // TODO : determine task status TaskStatus taskStatus = TaskStatus.Success; if (planComputed == true) { taskStatus = TaskStatus.Success; } else { taskStatus = TaskStatus.Failure; // TODO: I dont know, check } setTaskPriority(TaskPriority.Inactive); return(taskStatus); }
//Compute previous state based on the effect of the action over the current state //Meant to be used with backtracking algorithms (ADA*) private void ComputePreviousState(Vector3 mov) { GridPlanningState prevGridState = previousState as GridPlanningState; if (prevGridState != null) { currentPosition = prevGridState.currentPosition - mov; } else { FootstepPlanningState prevFootstepState = previousState as FootstepPlanningState; if (prevFootstepState != null) { currentPosition = prevFootstepState.currentPosition - mov; } } }
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List <DefaultAction> transitions) { GridPlanningState currentState = DcurrentState as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; GridPlanningState previousState = DpreviousState as GridPlanningState; foreach (Vector3 mov in movDirections) { GridPlanningAction newAction = new GridPlanningAction(previousState, mov, 0.0f); if (!CheckStateCollisions(newAction.state)) { transitions.Add(newAction); } } }
public override TaskStatus execute(float maxTime) { Stack<DefaultAction> convertedTunnel = new Stack<DefaultAction> (); // convert tunnel for(int i = tunnel.Count-1; i >=0; i--) { GridPlanningState gs = new GridPlanningState(tunnel[i].getPosition()); GridPlanningAction g = new GridPlanningAction (gs, new Vector3()); convertedTunnel.Push(g); } GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F); Debug.Log ("grid tunnel plan " + gridTunnelSearch.newPlan.Count); //// generating plan for now spaceTimePath.Clear(); while (gridTunnelSearch.newPlan.Count != 0) { DefaultAction action = gridTunnelSearch.newPlan.Pop(); GridTimeState state = action.state as GridTimeState; spaceTimePath.Add(new State(state.currentPosition, state.time)); } bool planComputed = gridTunnelSearch.goalReached; // TODO : determine task status TaskStatus taskStatus = TaskStatus.Success; if (planComputed == true) taskStatus = TaskStatus.Success; else taskStatus = TaskStatus.Failure; // TODO: I dont know, check setTaskPriority(TaskPriority.Inactive); return taskStatus; }
public override float ComputeEstimate(ref DefaultState Dfrom, ref DefaultState Dto, string estimateType) { GridPlanningState _from = Dfrom as GridPlanningState; GridPlanningState _to = Dto as GridPlanningState; if (_from == null) { FootstepPlanningState fsState = Dfrom as FootstepPlanningState; _from = new GridPlanningState(fsState); } if (estimateType == "g") { return(Mathf.Abs(Vector3.Distance(_to.currentPosition, _from.currentPosition))); } else if (estimateType == "h") { Vector3 fromStart; if (_to != null) { // A state is a goal one if it's really close to the goal fromStart = _to.currentPosition - _from.currentPosition; } else { FootstepPlanningState fsIdealGoalState = Dto as FootstepPlanningState; fromStart = fsIdealGoalState.currentPosition - _from.currentPosition; } float spaceCost = fromStart.magnitude / analyzer.meanStepSize; return(spaceCost); } else { return(0.0f); } }
//Constructor for actions using dummytype for planningstate public GridPlanningAction(GridPlanningState previousState, Vector3 mov, float dummyType) { movAction = mov; cost = 1; state = new GridPlanningState(previousState, mov, dummyType); }
public GridPlanningAction(GridPlanningState previousState, Vector3 mov) { movAction = mov; cost = 1; state = new GridPlanningState(previousState, mov); }
override public bool isAGoalState(ref DefaultState Dstate, ref DefaultState DidealGoalState) { //float startTime = Time.realtimeSinceStartup; GridPlanningState state = Dstate as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (state == null) { FootstepPlanningState fsState = Dstate as FootstepPlanningState; if (fsState != null) { state = new GridPlanningState(fsState); } else { GridTimeState gridTimeState = Dstate as GridTimeState; state = new GridPlanningState(gridTimeState); } } Vector3 toGoal; if (idealGoalState != null) { // A state is a goal one if it's really close to the goal toGoal = idealGoalState.currentPosition - state.currentPosition; } else { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; if (fsIdealGoalState != null) { toGoal = fsIdealGoalState.currentPosition - state.currentPosition; } else { GridTimeState gtIdealGoalState = DidealGoalState as GridTimeState; toGoal = gtIdealGoalState.currentPosition - state.currentPosition; } } //bool b = toGoal.magnitude/analyzer.maxStepSize < 0.5; bool b = toGoal.magnitude / 2.0 < 0.5; /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ return(b); }
public override bool isAGoalState(ref DefaultState Dstate, ref DefaultState DidealGoalState) { //float startTime = Time.realtimeSinceStartup; GridPlanningState state = Dstate as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (state == null) { FootstepPlanningState fsState = Dstate as FootstepPlanningState; if (fsState != null) state = new GridPlanningState(fsState); else { GridTimeState gridTimeState = Dstate as GridTimeState; state = new GridPlanningState(gridTimeState); } } Vector3 toGoal; if (idealGoalState != null) // A state is a goal one if it's really close to the goal toGoal = idealGoalState.currentPosition - state.currentPosition; else { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; if (fsIdealGoalState != null) toGoal = fsIdealGoalState.currentPosition - state.currentPosition; else { GridTimeState gtIdealGoalState = DidealGoalState as GridTimeState; toGoal = gtIdealGoalState.currentPosition - state.currentPosition; } } //bool b = toGoal.magnitude/analyzer.maxStepSize < 0.5; bool b = toGoal.magnitude/2.0 < 0.5; /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); numCalls = 0; timeSum = 0; } */ return b; }
private float ComputeEstimatedTime(GridPlanningState state, float velocity) { float distance = (state.currentPosition - startState.currentPosition).magnitude; float time = distance / velocity; return time; }
public override float estimateTotalCost(ref DefaultState DcurrentState, ref DefaultState DidealGoalState, float currentg) { //float startTime = Time.realtimeSinceStartup; GridPlanningState currentState = DcurrentState as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (currentState == null) { FootstepPlanningState fsState = DcurrentState as FootstepPlanningState; currentState = new GridPlanningState(fsState); } //Debug.Log("Estimating cost"); Vector3 toGoal; if (idealGoalState != null) // A state is a goal one if it's really close to the goal toGoal = idealGoalState.currentPosition - currentState.currentPosition; else { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; toGoal = fsIdealGoalState.currentPosition - currentState.currentPosition; } float h = toGoal.magnitude/analyzer.meanStepSize; float f = currentg + W*h; //Debug.Log("Estimated cost = " + f); /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); numCalls = 0; timeSum = 0; } */ return f; }
public override void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List<DefaultAction> transitions) { GridPlanningState currentState = DcurrentState as GridPlanningState; GridPlanningState idealGoalState = DidealGoalState as GridPlanningState; if (currentState == null) { FootstepPlanningState fsState = DcurrentState as FootstepPlanningState; if (fsState != null) currentState = new GridPlanningState(fsState); else currentState = new GridPlanningState(DcurrentState as GridTimeState); } if (idealGoalState == null) { FootstepPlanningState fsIdealGoalState = DidealGoalState as FootstepPlanningState; if (fsIdealGoalState != null) idealGoalState = new GridPlanningState(fsIdealGoalState); else idealGoalState = new GridPlanningState(DidealGoalState as GridTimeState); } foreach ( Vector3 mov in movDirections ) { GridPlanningAction newAction = new GridPlanningAction(currentState,mov); //if (!CheckTransitionCollisions(newAction.state,DcurrentState)) if (!CheckStateCollisions(newAction.state)) { //Debug.Log(Time.time + ": grid successor generated"); transitions.Add(newAction); } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
public override bool ComputePlan() { bool stateWasNull = false; if (currentState == null) { currentState = InitState(); stateWasNull = true; } goalState = new FootstepPlanningState(currentGoal, Quaternion.identity,goalTime); outputPlan = new Stack<DefaultAction>(); if (stateWasNull || goalReached && !stateWasNull) { if (currentState.previousState != null) { float actionLength = currentState.time - currentState.previousState.time; currentState.previousState.time = Time.time - actionLength; } currentState.time = Time.time; } DefaultState DcurrentState = currentState as DefaultState; DefaultState DgoalState = goalState as DefaultState; if (useGridDomain) { GridPlanningState currentGridState = new GridPlanningState(root.position); DcurrentState = currentGridState as DefaultState; GridPlanningState goalGridState = new GridPlanningState(currentGoal); DgoalState = goalGridState as DefaultState; } if (useGridTimeDomain) { GridTimeState currentTimeState = new GridTimeState(root.position, Time.time, currentSpeed); DcurrentState = currentTimeState as DefaultState; GridTimeState goalTimeState = new GridTimeState(currentGoal,goalTime); DgoalState = goalTimeState as DefaultState; //Debug.Log("current time = " + currentTimeState.time); } //float startTime = Time.realtimeSinceStartup; bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, maxTime); /*************************/ /* TUNNEL SEARCH TESTING */ //List<Vector3> movDirections = (domainList[0] as GridPlanningDomain).getMovDirections(); //TunnelSearch tunnelSearch = new TunnelSearch(outputPlan, 3, analyzer, movDirections); if (TunnelSearch) { if (planComputed) { GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(outputPlan, root.position, Time.time, goalState.time, gtd, MaxNumberOfNodes, maxTime, //tunnelThresholdX, tunnelThresholdZ, tunnelThresholdT); tunnelThresholdD, tunnelThresholdT, currentSpeed); outputPlanBeforeTunnel = outputPlan; outputPlan = gridTunnelSearch.newPlan; planComputed = gridTunnelSearch.goalReached; //Debug.Log("At time " + Time.time + " tunnel plan computed? " + planComputed); } } /*************************/ /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " Mean Computing Plan Time = " + meanTime); numCalls = 0; timeSum = 0; } */ if (goalMeshRenderer!= null && goalStateTransform.position == currentGoal) { if(planComputed && goalCompletedMaterial != null) goalMeshRenderer.material = goalCompletedMaterial; else goalMeshRenderer.material = goalMaterial; } //Debug.Log("new plan! at time: " + Time.time + " with " + outputPlan.Count + " actions"); goalReached = false; /* if (planComputed) { AnimationCurve[] curves = GetPlanAnimationCurve(); curveX = curves[0]; curveY = curves[1]; curveZ = curves[2]; } */ return (planComputed && outputPlan.Count > 0); }
public override float ComputeEstimate(ref DefaultState Dfrom, ref DefaultState Dto, string estimateType) { GridPlanningState _from = Dfrom as GridPlanningState; GridPlanningState _to = Dto as GridPlanningState; if (_from == null) { FootstepPlanningState fsState = Dfrom as FootstepPlanningState; _from = new GridPlanningState(fsState); } if(estimateType == "g") return (Mathf.Abs(Vector3.Distance(_to.currentPosition, _from.currentPosition))); else if(estimateType == "h") { Vector3 fromStart; if (_to != null) // A state is a goal one if it's really close to the goal fromStart = _to.currentPosition - _from.currentPosition; else { FootstepPlanningState fsIdealGoalState = Dto as FootstepPlanningState; fromStart = fsIdealGoalState.currentPosition - _from.currentPosition; } float spaceCost = fromStart.magnitude/analyzer.meanStepSize; return spaceCost; } else return 0.0f; }
public GridPlanningState(GridPlanningState original) { this.currentPosition = original.currentPosition; this.actionMov = original.actionMov; this.previousState = original.previousState; }
public AnimationCurve[] GetPlanAnimationCurve() { if (outputPlan == null) { return(null); } int numKeys = outputPlan.Count; AnimationCurve curveX = new AnimationCurve(); AnimationCurve curveY = new AnimationCurve(); AnimationCurve curveZ = new AnimationCurve(); AnimationCurve curveSpeed = new AnimationCurve(); float time = Time.time; Vector3 pos = currentStateTransform.position; float speed = currentSpeed; curveX.AddKey(time, pos.x); curveY.AddKey(time, pos.y); curveZ.AddKey(time, pos.z); curveSpeed.AddKey(time, speed); for (int i = 0; i < numKeys; i++) { DefaultAction action = outputPlan.ElementAt(i); if (action != null && action.state != null) { FootstepPlanningState fsState = action.state as FootstepPlanningState; if (fsState != null) { time = fsState.time; pos = fsState.currentPosition; speed = fsState.currentSpeed; } else { GridPlanningState gridState = action.state as GridPlanningState; if (gridState != null) { time += 1.0f; pos = gridState.currentPosition; } else { GridTimeState gridTimeState = action.state as GridTimeState; if (gridTimeState != null) { time = gridTimeState.time; pos = gridTimeState.currentPosition; speed = gridTimeState.speed; } } } curveX.AddKey(time, pos.x); curveY.AddKey(time, pos.y); curveZ.AddKey(time, pos.z); curveSpeed.AddKey(time, speed); } } AnimationCurve[] curves = new AnimationCurve[4]; curves[0] = curveX; curves[1] = curveY; curves[2] = curveZ; curves[3] = curveSpeed; return(curves); }
public FootstepPlanningState(GridPlanningState gridState) { //float startTime = Time.realtimeSinceStartup; this.actionName = "GridAction"; //this.actionSpeed = 1.0f; //this.currentAcceleration = 1.0f; this.previousState = gridState.previousState as FootstepPlanningState; this.currentPosition = gridState.currentPosition; //this.currentRotation = gridState.previousState.currentRotation; //this.currentSpeed = 1.0f; //this.leftFoot = gridState.currentPosition; //this.leftHand = gridState.currentPosition; //this.obstaclePos = original.obstaclePos; //this.preconditions = original.preconditions; //this.rightFoot = gridState.currentPosition; //this.rightHand = gridState.currentPosition; //this.time = original.time; /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); numCalls = 0; timeSum = 0; } */ }
/* * private Tunnel ConvertPlan(float minVelocity, float midVelocity, float maxVelocity) * { * Tunnel tunnel; * tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length]; * tunnel.tunnelMinVelocity = new float[currentPlan.Length]; * tunnel.tunnelMaxVelocity = new float[currentPlan.Length]; * * tunnel.thresholdD = T_d; * //tunnel.thresholdX = T_x; * //tunnel.thresholdZ = T_z; * tunnel.thresholdT = T_t; * * int i = 0; * foreach ( DefaultAction action in currentPlan ) * { * GridPlanningState state = action.state as GridPlanningState; * * if (state != null) * { * float midTime = ComputeEstimatedTime(state, midVelocity); * * GridTimeState gridTimeState = new GridTimeState(state.currentPosition,midTime); * * tunnel.tunnelMidVelocity[i] = gridTimeState; * * tunnel.tunnelMinVelocity[i] = ComputeEstimatedTime(state, minVelocity); * tunnel.tunnelMaxVelocity[i] = ComputeEstimatedTime(state, maxVelocity); * } * * i++; * } * * return tunnel; * } */ private Tunnel ConvertPlan(float startTime, float minVelocity, float midVelocity, float maxVelocity) { Tunnel tunnel; tunnel.tunnelMidVelocity = new GridTimeState[currentPlan.Length]; tunnel.tunnelMinVelocity = new float[currentPlan.Length]; tunnel.tunnelMaxVelocity = new float[currentPlan.Length]; tunnel.thresholdD = T_d; //tunnel.thresholdX = T_x; //tunnel.thresholdZ = T_z; tunnel.thresholdT = T_t; GridPlanningState lastGridPlanningState = currentPlan[currentPlan.Length - 1].state as GridPlanningState; float maxGoalTime = ComputeEstimatedTime(lastGridPlanningState, minVelocity); float midGoalTime = ComputeEstimatedTime(lastGridPlanningState, midVelocity); float minGoalTime = ComputeEstimatedTime(lastGridPlanningState, maxVelocity); //Debug.Log("minGoalTime = " + minGoalTime); //Debug.Log("midGoalTime = " + midGoalTime); //Debug.Log("maxGoalTime = " + maxGoalTime); float[] distances = ComputeNodeDistances(currentPlan); //for (int aux=0; aux<distances.Length; aux++) // Debug.Log("distance "+aux+": "+distances[aux]); float totalDistance = ComputeTotalDistance(distances); int i = 0; float minSum = startTime; float midSum = startTime; float maxSum = startTime; while (i < currentPlan.Length) { GridPlanningState state = currentPlan[i].state as GridPlanningState; if (state != null) { float midTime = midSum + midGoalTime * distances[i] / totalDistance; midSum = midTime; GridTimeState gridTimeState = new GridTimeState(state.currentPosition, midTime); tunnel.tunnelMidVelocity[i] = gridTimeState; } else { tunnel.tunnelMidVelocity[i] = null; } tunnel.tunnelMaxVelocity[i] = minSum + minGoalTime * distances[i] / totalDistance; tunnel.tunnelMinVelocity[i] = maxSum + maxGoalTime * distances[i] / totalDistance; minSum = tunnel.tunnelMaxVelocity[i]; maxSum = tunnel.tunnelMinVelocity[i]; i++; } return(tunnel); }
public override bool CheckStateCollisions(DefaultState Dstate) { //float startTime = Time.realtimeSinceStartup; GridPlanningState state = Dstate as GridPlanningState; bool b = false; // If we are checking a state of this domain if (state != null) b = CheckRootCollisions(state);//return CheckRootCollisions(state); else // If not { // We need to transform the state of the coarser domain // to our low resolution domain FootstepPlanningState footstepState = Dstate as FootstepPlanningState; if (footstepState != null) { state = new GridPlanningState(footstepState); b = CheckRootCollisions(state); //return CheckRootCollisions(state); } else b = false;//return false; } /* float endTime = Time.realtimeSinceStartup; timeSum += (endTime - startTime); numCalls++; float meanTime = 0.0f; if (numCalls == 100) { meanTime = timeSum / numCalls; Debug.Log("At time " + Time.time + " MeanTime = " + meanTime); numCalls = 0; timeSum = 0; } */ return b; }
public void PlaceSequenceFootSteps(bool tunnel = false) { FootstepPlanningAction[] plan = planning.GetOutputPlan(); if (plan == null) { return; } numberOfActions = plan.Length; //Debug.Log("Number of actions = " + numberOfActions); // Debug.Log("Footsteps " + this.gameObject.name + " " + numberOfActions); if (!tunnel) { steps = new Object[numberOfActions]; } else { steps2 = new Object[numberOfActions]; steps2Pos = new Vector3[numberOfActions]; } if (drawTimes) { if (!tunnel) { texts = new Object[numberOfActions]; } else { texts2 = new Object[numberOfActions]; } } for (int i = 0; i < numberOfActions; i++) { if (plan[i] != null) { FootstepPlanningState state = plan[i].state as FootstepPlanningState; if (state != null) { AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(state.actionName); Quaternion rotation = state.currentRotation; Vector3 position; if (animInfo.swing == Joint.LeftFoot) { position = state.leftFoot; position[1] = auxHeight; if (!tunnel) { steps[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation); } else { steps2[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation); } debugText.alignment = TextAlignment.Left; } else { position = state.rightFoot; position[1] = auxHeight; if (!tunnel) { steps[i] = GameObject.Instantiate((Object)rightFootStep, position, rotation); } else { steps2[i] = GameObject.Instantiate((Object)leftFootStep, position, rotation); } debugText.alignment = TextAlignment.Left; } if (drawTimes) { debugText.text = "" + state.time; if (!tunnel) { texts[i] = GameObject.Instantiate((Object)debugText, position, rotation); } else { texts2[i] = GameObject.Instantiate((Object)debugText, position, rotation); } } } else // if state is not a footstep state (it is a grid state or a gridTime state) { // we instantiate a gridstep GridTimeState gridTimeState = plan[i].state as GridTimeState; if (gridTimeState != null) { //Debug.Log("we place a gridTimeStep"); if (!tunnel) { steps[i] = GameObject.Instantiate((Object)gridTimeSphere, gridTimeState.currentPosition, Quaternion.identity); } else { steps2[i] = GameObject.Instantiate((Object)gridTimeSphere, gridTimeState.currentPosition, Quaternion.identity); steps2Pos[i] = gridTimeState.currentPosition; } if (drawTimes) { debugText.text = "" + gridTimeState.time; if (!tunnel) { texts[i] = GameObject.Instantiate((Object)debugText, gridTimeState.currentPosition, Quaternion.identity); } else { texts2[i] = GameObject.Instantiate((Object)debugText, gridTimeState.currentPosition, Quaternion.identity); } } } else { //Debug.Log("We Should have a gridPlanningStep"); GridPlanningState gridState = plan[i].state as GridPlanningState; if (gridState != null) { if (!tunnel) { steps[i] = GameObject.Instantiate((Object)sphere, gridState.currentPosition, Quaternion.identity); } else { steps2[i] = GameObject.Instantiate((Object)sphere, gridState.currentPosition, Quaternion.identity); steps2Pos[i] = gridState.currentPosition; } } } } } } footstepsPlaced = true; }
bool doTunnelTask() { Stack<DefaultAction> convertedTunnel = new Stack<DefaultAction> (); // convert tunnel for(int i = 0; i < path.Count; i++) { GridPlanningState gs = new GridPlanningState(path[i].getPosition()); GridPlanningAction g = new GridPlanningAction (gs, new Vector3()); convertedTunnel.Push(g); } GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(convertedTunnel, this.startState.getPosition(),startState._time, goalState._time,gridTimeDomain,300,2.0f,1.0f,1.0f,startState._speed); //new GridTunnelSearch(convertedTunnel, startState.getPosition(), 0.0F, 10.0F, gridTimeDomain, 250, 2.0F, 1.0F, 1.0F); //gridTunnelSearch.tryARAStarPlanner(spaceTimePath); //Debug.Log ("grid time plan using ara star planner " + spaceTimePath.Count); //return true; //// generating plan for now spaceTimePath.Clear(); /* // this is the state plan Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newStatePlan.Count); while (gridTunnelSearch.newStatePlan.Count != 0) { GridTimeState state = gridTunnelSearch.newStatePlan.Pop() as GridTimeState; //Debug.LogWarning ("space time state " + state.time); spaceTimePath.Add(new State(state.currentPosition, state.time)); } */ // this is the action plan Debug.Log ("grid tunnel plan using best first planner " + gridTunnelSearch.newPlan.Count); while (gridTunnelSearch.newPlan.Count != 0) { DefaultAction action = gridTunnelSearch.newPlan.Pop(); GridTimeState state = action.state as GridTimeState; //Debug.LogWarning ("space time state " + state.time + " " + state.currentPosition); spaceTimePath.Add(new State(state.currentPosition, state.time, state.speed)); } bool planComputed = gridTunnelSearch.goalReached; //Debug.LogError("grid tunnel plan status " + planComputed); return planComputed; }
public GridTimeState(GridPlanningState gridState) { currentPosition = gridState.currentPosition; time = 0; speed = 0; // ¿? }
public override bool ComputePlan() { bool stateWasNull = false; if (currentState == null) { currentState = InitState(); stateWasNull = true; } goalState = new FootstepPlanningState(currentGoal, Quaternion.identity, goalTime); outputPlan = new Stack <DefaultAction>(); if (stateWasNull || goalReached && !stateWasNull) { if (currentState.previousState != null) { float actionLength = currentState.time - currentState.previousState.time; currentState.previousState.time = Time.time - actionLength; } currentState.time = Time.time; } DefaultState DcurrentState = currentState as DefaultState; DefaultState DgoalState = goalState as DefaultState; if (useGridDomain) { GridPlanningState currentGridState = new GridPlanningState(root.position); DcurrentState = currentGridState as DefaultState; GridPlanningState goalGridState = new GridPlanningState(currentGoal); DgoalState = goalGridState as DefaultState; } if (useGridTimeDomain) { GridTimeState currentTimeState = new GridTimeState(root.position, Time.time, currentSpeed); DcurrentState = currentTimeState as DefaultState; GridTimeState goalTimeState = new GridTimeState(currentGoal, goalTime); DgoalState = goalTimeState as DefaultState; //Debug.Log("current time = " + currentTimeState.time); } //float startTime = Time.realtimeSinceStartup; bool planComputed = planner.computePlan(ref DcurrentState, ref DgoalState, ref outputPlan, maxTime); /*************************/ /* TUNNEL SEARCH TESTING */ //List<Vector3> movDirections = (domainList[0] as GridPlanningDomain).getMovDirections(); //TunnelSearch tunnelSearch = new TunnelSearch(outputPlan, 3, analyzer, movDirections); if (TunnelSearch) { if (planComputed) { GridTunnelSearch gridTunnelSearch = new GridTunnelSearch(outputPlan, root.position, Time.time, goalState.time, gtd, MaxNumberOfNodes, maxTime, //tunnelThresholdX, tunnelThresholdZ, tunnelThresholdT); tunnelThresholdD, tunnelThresholdT, currentSpeed); outputPlanBeforeTunnel = outputPlan; outputPlan = gridTunnelSearch.newPlan; planComputed = gridTunnelSearch.goalReached; //Debug.Log("At time " + Time.time + " tunnel plan computed? " + planComputed); } } /*************************/ /* * float endTime = Time.realtimeSinceStartup; * * timeSum += (endTime - startTime); * numCalls++; * * float meanTime = 0.0f; * if (numCalls == 100) * { * meanTime = timeSum / numCalls; * Debug.Log("At time " + Time.time + " Mean Computing Plan Time = " + meanTime); * numCalls = 0; * timeSum = 0; * } */ if (goalMeshRenderer != null && goalStateTransform.position == currentGoal) { if (planComputed && goalCompletedMaterial != null) { goalMeshRenderer.material = goalCompletedMaterial; } else { goalMeshRenderer.material = goalMaterial; } } //Debug.Log("new plan! at time: " + Time.time + " with " + outputPlan.Count + " actions"); goalReached = false; /* * if (planComputed) * { * AnimationCurve[] curves = GetPlanAnimationCurve(); * curveX = curves[0]; * curveY = curves[1]; * curveZ = curves[2]; * } */ return(planComputed && outputPlan.Count > 0); }