public void RepairWithComposite(OpenCondition oc, CompositeSchedulePlanStep repairStep) { var needStep = Find(oc.step); if (!needStep.Name.Equals("DummyGoal") && !needStep.Name.Equals("DummyInit")) { needStep.Fulfill(oc.precondition); } // need to merge all steps that are being connected by this predicate: if (oc.precondition.Name.Equals("obs-starts")) { var stepThatNeedsToBeMerged = oc.precondition.Terms[0]; IPlanStep ReferencedStep1 = new PlanStep(); IPlanStep ReferencedStep2 = new PlanStep(); foreach (var step in Steps) { if (step.Action.ID.ToString().Equals(stepThatNeedsToBeMerged)) { if (repairStep.SubSteps.Contains(step)) { ReferencedStep1 = step; } else { ReferencedStep2 = step; } } } if (ReferencedStep1.Name.Equals("") || ReferencedStep2.Name.Equals("")) { Debug.Log("never found steps to merge"); throw new System.Exception(); } if (ReferencedStep1.OpenConditions.Count > ReferencedStep2.OpenConditions.Count) { MergeSteps(ReferencedStep1, ReferencedStep2); } else { MergeSteps(ReferencedStep2, ReferencedStep1); } } orderings.Insert(repairStep.GoalStep as IPlanStep, needStep); var clink = new CausalLink <IPlanStep>(oc.precondition as Predicate, repairStep.GoalStep as IPlanStep, needStep); causalLinks.Add(clink); }
public new void AddStep(IPlan plan, OpenCondition oc) { long before = 0; var watch = System.Diagnostics.Stopwatch.StartNew(); foreach (var cndt in CacheMaps.GetCndts(oc.precondition)) { if (cndt == null) { continue; } if (cndt.Height == 0) { continue; } before = watch.ElapsedMilliseconds; var planClone = plan.Clone() as PlanSchedule; IPlanStep newStep; if (cndt.Height > 0) { //continue; var compCndt = cndt as CompositeSchedule; newStep = new CompositeSchedulePlanStep(compCndt.Clone() as IComposite, compCndt.Cntgs) { Depth = oc.step.Depth }; } else { // only add composite steps... //continue; newStep = new PlanStep(cndt.Clone() as IOperator) { Depth = oc.step.Depth }; } LogTime("CloneCndt", watch.ElapsedMilliseconds - before); before = watch.ElapsedMilliseconds; planClone.Insert(newStep); LogTime("InsertDecomp", watch.ElapsedMilliseconds - before); //newStep.Height = cndt.Height; // planClone.Insert(newStep); before = watch.ElapsedMilliseconds; planClone.Repair(oc, newStep); LogTime("RepairDecomp", watch.ElapsedMilliseconds - before); // check if inserting new Step (with orderings given by Repair) add cndts/risks to existing open conditions, affecting their status in the heap //planClone.Flaws.UpdateFlaws(planClone, newStep); if (oc.isDummyGoal) { if (newStep.Height > 0) { var compNewStep = newStep as CompositeSchedulePlanStep; planClone.Orderings.Insert(oc.step.InitCndt, compNewStep.InitialStep); } else { planClone.Orderings.Insert(oc.step.InitCndt, newStep); } } before = watch.ElapsedMilliseconds; planClone.DetectThreats(newStep); LogTime("DetectThreats", watch.ElapsedMilliseconds - before); before = watch.ElapsedMilliseconds; Insert(planClone); LogTime("InsertPlan", watch.ElapsedMilliseconds - before); } }
public void InsertDecomp(CompositeSchedulePlanStep newStep) { Decomps += 1; var IDMap = new Dictionary <int, IPlanStep>(); // Clone, Add, and Order Initial step var dummyInit = newStep.InitialStep.Clone() as IPlanStep; dummyInit.Depth = newStep.Depth; IDMap[newStep.InitialStep.ID] = dummyInit; Steps.Add(dummyInit); Orderings.Insert(InitialStep, dummyInit); Orderings.Insert(dummyInit, GoalStep); // Clone, Add, and order Goal step var dummyGoal = newStep.GoalStep.Clone() as IPlanStep; dummyGoal.Depth = newStep.Depth; dummyGoal.InitCndt = dummyInit; InsertPrimitiveSubstep(dummyGoal, dummyInit.Effects, true); IDMap[newStep.GoalStep.ID] = dummyGoal; Orderings.Insert(dummyInit, dummyGoal); var newStepCopy = new PlanStep(new Operator(newStep.Action.Predicate as Predicate, new List <IPredicate>(), new List <IPredicate>())); Steps.Add(newStepCopy); // orderings.Insert(dummyInit, newStepCopy); // orderings.Insert(newStepCopy, dummyGoal); newStepCopy.Height = newStep.Height; var newSubSteps = new List <IPlanStep>(); foreach (var substep in newStep.SubSteps) { // substep is either a IPlanStep or ICompositePlanStep if (substep.Height > 0) { var compositeSubStep = new CompositeSchedulePlanStep(substep.Clone() as IPlanStep) { Depth = newStep.Depth + 1 }; Orderings.Insert(compositeSubStep.GoalStep, dummyGoal); Orderings.Insert(dummyInit, compositeSubStep.InitialStep); IDMap[substep.ID] = compositeSubStep; compositeSubStep.InitialStep.InitCndt = dummyInit; newSubSteps.Add(compositeSubStep); Insert(compositeSubStep); // Don't bother updating hdepth yet because we will check on recursion } else { var newsubstep = new PlanStep(substep.Clone() as IPlanStep) { Depth = newStep.Depth + 1 }; Orderings.Insert(newsubstep, dummyGoal); Orderings.Insert(dummyInit, newsubstep); IDMap[substep.ID] = newsubstep; newSubSteps.Add(newsubstep); newsubstep.InitCndt = dummyInit; InsertPrimitiveSubstep(newsubstep, dummyInit.Effects, false); if (newsubstep.Depth > Hdepth) { Hdepth = newsubstep.Depth; } } } foreach (var tupleOrdering in newStep.SubOrderings) { // Don't bother adding orderings to dummies if (tupleOrdering.First.Equals(newStep.InitialStep)) { continue; } if (tupleOrdering.Second.Equals(newStep.GoalStep)) { continue; } var head = IDMap[tupleOrdering.First.ID]; var tail = IDMap[tupleOrdering.Second.ID]; if (head.Height > 0) { // can you pass it back? var temp = head as ICompositePlanStep; head = temp.GoalStep as IPlanStep; } if (tail.Height > 0) { var temp = tail as ICompositePlanStep; tail = temp.InitialStep as IPlanStep; } Orderings.Insert(head, tail); } // in this world, all composite plan steps are composite schedule plan steps. var schedulingStepComponent = newStep as CompositeSchedulePlanStep; foreach (var cntg in schedulingStepComponent.Cntgs) { var head = IDMap[cntg.First.ID]; var tail = IDMap[cntg.Second.ID]; if (head.Height > 0) { // how do we describe a composite as being contiguous with another step? var temp = head as ICompositePlanStep; head = temp.GoalStep as IPlanStep; } if (tail.Height > 0) { var temp = tail as ICompositePlanStep; tail = temp.InitialStep as IPlanStep; } Cntgs.Insert(head, tail); // also add orderings just in case Orderings.Insert(head, tail); } foreach (var clink in newStep.SubLinks) { var head = IDMap[clink.Head.ID]; var tail = IDMap[clink.Tail.ID]; if (head.Height > 0) { var temp = head as CompositePlanStep; head = temp.GoalStep as IPlanStep; } if (tail.Height > 0) { var temp = tail as CompositePlanStep; tail = temp.InitialStep as IPlanStep; } var newclink = new CausalLink <IPlanStep>(clink.Predicate, head, tail); CausalLinks.Add(newclink); Orderings.Insert(head, tail); // check if this causal links is threatened by a step in subplan foreach (var step in newSubSteps) { // Prerequisite criteria 1 if (step.ID == head.ID || step.ID == tail.ID) { continue; } // Prerequisite criteria 2 if (!CacheMaps.IsThreat(clink.Predicate, step)) { continue; } // If the step has height, need to evaluate differently if (step.Height > 0) { var temp = step as ICompositePlanStep; if (Orderings.IsPath(head, temp.InitialStep)) { continue; } if (Orderings.IsPath(temp.GoalStep, tail)) { continue; } } else { if (Orderings.IsPath(head, step)) { continue; } if (Orderings.IsPath(step, tail)) { continue; } } if (step.Height > 0) { // Then we need to dig deeper to find the step that threatens DecomposeThreat(clink, step as ICompositePlanStep); } else { Flaws.Add(new ThreatenedLinkFlaw(newclink, step)); } } } // This is needed because we'll check if these substeps are threatening links newStep.SubSteps = newSubSteps; newStep.InitialStep = dummyInit; newStep.GoalStep = dummyGoal; foreach (var pre in newStep.OpenConditions) { Flaws.Add(this, new OpenCondition(pre, dummyInit as IPlanStep)); } }
public void Insert(CompositeSchedulePlanStep newStep) { InsertDecomp(newStep); }