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);
}
//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);
}
//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 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 GridPlanningAction(GridPlanningState previousState, Vector3 mov)
{
movAction = mov;
cost = 1;
state = new GridPlanningState(previousState, mov);
}
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;
}
*/
}
public GridPlanningState(GridPlanningState original)
{
this.currentPosition = original.currentPosition;
this.actionMov = original.actionMov;
this.previousState = original.previousState;
}
/*
* 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);
}
