diff --git a/XPlanning/data/mobilerobot/maps/GHC7-map0.json b/XPlanning/data/mobilerobot/maps/GHC7-map0.json index aae527a0..532e2ac2 100644 --- a/XPlanning/data/mobilerobot/maps/GHC7-map0.json +++ b/XPlanning/data/mobilerobot/maps/GHC7-map0.json @@ -548,5 +548,22 @@ "to-id": "L22", "from-id": "L18" } - ] + ], + "lighting": [ + { + "dark": true, + "to-id": "L22", + "from-id": "L18" + }, + { + "dark": true, + "to-id": "L21", + "from-id": "L17" + }, + { + "dark": true, + "to-id": "L24", + "from-id": "L27" + } + ] } \ No newline at end of file diff --git a/XPlanning/data/mobilerobot/missions/mission0.json b/XPlanning/data/mobilerobot/missions/mission0.json index 9c396be6..f8c86d7f 100644 --- a/XPlanning/data/mobilerobot/missions/mission0.json +++ b/XPlanning/data/mobilerobot/missions/mission0.json @@ -24,7 +24,7 @@ "objective": "energyConsumption", "scaling-const": 0.3, "min-step-value": 0.0, - "max-step-value": 2525 + "max-step-value": 4185.0 } ], "start-id": "L1" diff --git a/XPlanning/src/examples/mobilerobot/demo/MobileRobotXMDPLoader.java b/XPlanning/src/examples/mobilerobot/demo/MobileRobotXMDPLoader.java index 5e3c3668..c359fb47 100644 --- a/XPlanning/src/examples/mobilerobot/demo/MobileRobotXMDPLoader.java +++ b/XPlanning/src/examples/mobilerobot/demo/MobileRobotXMDPLoader.java @@ -18,12 +18,14 @@ import examples.mobilerobot.dsm.Mission; import examples.mobilerobot.dsm.MobileRobotXMDPBuilder; import examples.mobilerobot.dsm.parser.AreaParser; +import examples.mobilerobot.dsm.parser.DarknessParser; import examples.mobilerobot.dsm.parser.IEdgeAttributeParser; import examples.mobilerobot.dsm.parser.INodeAttributeParser; import examples.mobilerobot.dsm.parser.MapTopologyReader; import examples.mobilerobot.dsm.parser.MissionReader; import examples.mobilerobot.dsm.parser.OcclusionParser; import examples.mobilerobot.models.Area; +import examples.mobilerobot.models.Darkness; import examples.mobilerobot.models.Occlusion; import language.exceptions.XMDPException; import language.mdp.XMDP; @@ -44,15 +46,18 @@ public MobileRobotXMDPLoader(File mapsJsonDir) { mMapsJsonDir = mapsJsonDir; AreaParser areaParser = new AreaParser(); OcclusionParser occlusionParser = new OcclusionParser(); + DarknessParser darknessParser = new DarknessParser(); Set> nodeAttributeParsers = new HashSet<>(); nodeAttributeParsers.add(areaParser); Set> edgeAttributeParsers = new HashSet<>(); edgeAttributeParsers.add(occlusionParser); + edgeAttributeParsers.add(darknessParser); mMapReader = new MapTopologyReader(nodeAttributeParsers, edgeAttributeParsers); // Default node/edge attribute values mDefaultNodeAttributes.put(areaParser.getAttributeName(), DEFAULT_AREA); mDefaultEdgeAttributes.put(occlusionParser.getAttributeName(), DEFAULT_OCCLUSION); + mDefaultEdgeAttributes.put(darknessParser.getAttributeName(), Darkness.LIGHT); } @Override diff --git a/XPlanning/src/examples/mobilerobot/dsm/MobileRobotXMDPBuilder.java b/XPlanning/src/examples/mobilerobot/dsm/MobileRobotXMDPBuilder.java index 974ba98c..133abb0f 100644 --- a/XPlanning/src/examples/mobilerobot/dsm/MobileRobotXMDPBuilder.java +++ b/XPlanning/src/examples/mobilerobot/dsm/MobileRobotXMDPBuilder.java @@ -16,13 +16,17 @@ import examples.mobilerobot.metrics.TravelTimeDomain; import examples.mobilerobot.metrics.TravelTimeQFunction; import examples.mobilerobot.models.Area; +import examples.mobilerobot.models.Darkness; import examples.mobilerobot.models.Distance; +import examples.mobilerobot.models.HeadlampActionDescription; +import examples.mobilerobot.models.HeadlampState; import examples.mobilerobot.models.Location; import examples.mobilerobot.models.MoveToAction; import examples.mobilerobot.models.Occlusion; import examples.mobilerobot.models.RobotLocationActionDescription; import examples.mobilerobot.models.RobotSpeed; import examples.mobilerobot.models.RobotSpeedActionDescription; +import examples.mobilerobot.models.SetHeadlampAction; import examples.mobilerobot.models.SetSpeedAction; import language.domain.metrics.CountQFunction; import language.domain.metrics.EventBasedMetric; @@ -78,6 +82,20 @@ public class MobileRobotXMDPBuilder { // SetSpeed action definition private ActionDefinition setSpeedDef = new ActionDefinition<>("setSpeed", setSpeedHalf, setSpeedFull); + + // --- Headlamp setting --- // + private HeadlampState headlampOn = new HeadlampState(true); + private HeadlampState headlampOff = new HeadlampState(false); + + // Robot's headlamp state variable + private StateVarDefinition rHeadlampDef = new StateVarDefinition<>("rHeadlamp", headlampOn, headlampOff); + + // Headlamp-setting actions + private SetHeadlampAction setHeadlampOff = new SetHeadlampAction(rHeadlampDef.getStateVar(headlampOff)); + private SetHeadlampAction setHeadlampOn = new SetHeadlampAction(rHeadlampDef.getStateVar(headlampOn)); + + // SetHeadlamp action defintion + private ActionDefinition setHeadlampDef = new ActionDefinition<>("setHeadlamp", setHeadlampOff, setHeadlampOn); // ------ // // --- QA functions --- // @@ -130,6 +148,7 @@ private StateSpace buildStateSpace(MapTopology map) throws NodeAttributeNotFound StateSpace stateSpace = new StateSpace(); stateSpace.addStateVarDefinition(rLocDef); stateSpace.addStateVarDefinition(rSpeedDef); + stateSpace.addStateVarDefinition(rHeadlampDef); return stateSpace; } @@ -139,24 +158,29 @@ private ActionSpace buildActionSpace(MapTopology map) throws MapTopologyExceptio // Assume that all locations are reachable for (Location locDest : rLocDef.getPossibleValues()) { - MoveToAction moveTo = new MoveToAction(rLocDef.getStateVar(locDest)); - - // Derived attributes for each move action are obtained from edges in the map - LocationNode node = map.lookUpLocationNode(locDest.getId()); - Set connections = map.getConnections(node); - for (Connection conn : connections) { - Location locSrc = mLocMap.get(conn.getOtherNode(node)); - - // Distance - Distance distance = new Distance(conn.getDistance()); - moveTo.putDistanceValue(distance, rLocDef.getStateVar(locSrc)); - - // Occlusion - Occlusion occlusion = conn.getConnectionAttribute(Occlusion.class, "occlusion"); - moveTo.putOcclusionValue(occlusion, rLocDef.getStateVar(locSrc)); + for (HeadlampState hlState : rHeadlampDef.getPossibleValues()) { + MoveToAction moveTo = new MoveToAction(rLocDef.getStateVar(locDest), rHeadlampDef.getStateVar(hlState)); + + // Derived attributes for each move action are obtained from edges in the map + LocationNode node = map.lookUpLocationNode(locDest.getId()); + Set connections = map.getConnections(node); + for (Connection conn : connections) { + Location locSrc = mLocMap.get(conn.getOtherNode(node)); + + // Distance + Distance distance = new Distance(conn.getDistance()); + moveTo.putDistanceValue(distance, rLocDef.getStateVar(locSrc)); + + // Occlusion + Occlusion occlusion = conn.getConnectionAttribute(Occlusion.class, "occlusion"); + moveTo.putOcclusionValue(occlusion, rLocDef.getStateVar(locSrc)); + + Darkness darkness = conn.getConnectionAttribute(Darkness.class, "lighting"); + moveTo.putDarknessValue(darkness, rLocDef.getStateVar(locSrc)); + } + + moveTos.add(moveTo); } - - moveTos.add(moveTo); } // MoveTo action definition @@ -165,6 +189,7 @@ private ActionSpace buildActionSpace(MapTopology map) throws MapTopologyExceptio ActionSpace actionSpace = new ActionSpace(); actionSpace.addActionDefinition(moveToDef); actionSpace.addActionDefinition(setSpeedDef); + actionSpace.addActionDefinition(setHeadlampDef); return actionSpace; } @@ -173,6 +198,7 @@ private StateVarTuple buildInitialState(LocationNode startNode) { StateVarTuple initialState = new StateVarTuple(); initialState.addStateVar(rLocDef.getStateVar(loc)); initialState.addStateVar(rSpeedDef.getStateVar(DEFAULT_SPEED)); + initialState.addStateVar(rHeadlampDef.getStateVar(headlampOff)); return initialState; } @@ -182,6 +208,10 @@ private StateVarTuple buildGoal(LocationNode goalNode) { goal.addStateVar(rLocDef.getStateVar(loc)); return goal; } + + private boolean isDark(Connection conn) throws MapTopologyException{ + return conn.getConnectionAttribute(Darkness.class, "lighting") == Darkness.DARK; + } private TransitionFunction buildTransitionFunction(MapTopology map) throws XMDPException, MapTopologyException { // MoveTo: @@ -190,6 +220,7 @@ private TransitionFunction buildTransitionFunction(MapTopology map) throws XMDPE for (MoveToAction moveTo : moveToDef.getActions()) { Location locDest = moveTo.getDestination(); + HeadlampState hlState = moveTo.getHeadlampState(); // Source location for each move action from the map LocationNode node = map.lookUpLocationNode(locDest.getId()); @@ -197,12 +228,15 @@ private TransitionFunction buildTransitionFunction(MapTopology map) throws XMDPE for (Connection conn : connections) { Location locSrc = mLocMap.get(conn.getOtherNode(node)); preMoveTo.add(moveTo, rLocDef, locSrc); + if (isDark(conn) && !hlState.getValue()) { + preMoveTo.add(moveTo, rHeadlampDef, headlampOn); + } } } // Action description for rLoc RobotLocationActionDescription rLocActionDesc = new RobotLocationActionDescription(moveToDef, preMoveTo, - rLocDef); + rLocDef, rHeadlampDef); // PSO FactoredPSO moveToPSO = new FactoredPSO<>(moveToDef, preMoveTo); @@ -217,14 +251,28 @@ private TransitionFunction buildTransitionFunction(MapTopology map) throws XMDPE // Action description for rSpeed RobotSpeedActionDescription rSpeedActionDesc = new RobotSpeedActionDescription(setSpeedDef, preSetSpeed, rSpeedDef); - + // PSO FactoredPSO setSpeedPSO = new FactoredPSO<>(setSpeedDef, preSetSpeed); setSpeedPSO.addActionDescription(rSpeedActionDesc); + + // SetHeadlamp + // Precondition + Precondition preSetHeadlamp = new Precondition<>(setHeadlampDef); + preSetHeadlamp.add(setHeadlampOn, rHeadlampDef, headlampOff); + preSetHeadlamp.add(setHeadlampOff, rHeadlampDef, headlampOn); + + // Action description for rHeadlamp + HeadlampActionDescription rHeadlampActionDesc = new HeadlampActionDescription(setHeadlampDef, preSetHeadlamp, rHeadlampDef); + + // PSO + FactoredPSO setHeadlampPSO = new FactoredPSO<>(setHeadlampDef, preSetHeadlamp); + setHeadlampPSO.addActionDescription(rHeadlampActionDesc); TransitionFunction transFunction = new TransitionFunction(); transFunction.add(moveToPSO); transFunction.add(setSpeedPSO); + transFunction.add(setHeadlampPSO); return transFunction; } @@ -256,9 +304,10 @@ private QSpace buildQFunctions() { metric); // Energy Consumption - EnergyConsumptionDomain energyDomain = new EnergyConsumptionDomain(rLocDef, rSpeedDef, moveToDef); + EnergyConsumptionDomain energyDomain = new EnergyConsumptionDomain(rLocDef, rSpeedDef, rHeadlampDef, moveToDef); EnergyConsumptionQFunction energyQFunction = new EnergyConsumptionQFunction(energyDomain); - + + QSpace qSpace = new QSpace(); qSpace.addQFunction(timeQFunction); qSpace.addQFunction(collisionQFunction); diff --git a/XPlanning/src/examples/mobilerobot/dsm/parser/DarknessParser.java b/XPlanning/src/examples/mobilerobot/dsm/parser/DarknessParser.java new file mode 100644 index 00000000..c8044f0c --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/dsm/parser/DarknessParser.java @@ -0,0 +1,25 @@ +package examples.mobilerobot.dsm.parser; + +import org.json.simple.JSONObject; + +import examples.mobilerobot.models.Darkness; + +public class DarknessParser implements IEdgeAttributeParser { + + @Override + public String getAttributeName() { + return "lighting"; + } + + @Override + public String getJSONObjectKey() { + return "lighting"; + } + + @Override + public Darkness parseAttribute(JSONObject edgeObject) { + Boolean dark = (Boolean )edgeObject.getOrDefault("dark", false); + return dark ? Darkness.DARK : Darkness.LIGHT; + } + +} diff --git a/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionDomain.java b/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionDomain.java index b2ae879a..4affc61a 100644 --- a/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionDomain.java +++ b/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionDomain.java @@ -1,44 +1,52 @@ package examples.mobilerobot.metrics; +import examples.mobilerobot.models.HeadlampState; import examples.mobilerobot.models.Location; import examples.mobilerobot.models.MoveToAction; import examples.mobilerobot.models.RobotSpeed; import language.domain.metrics.TransitionStructure; import language.domain.models.ActionDefinition; import language.domain.models.StateVarDefinition; + /** * Defines the domain for the energy quality domain * - * The energy consumption quality is calculated based on the robot's location and speed, - * gets information from the MoveToAction, and effects the robot's location (because MoveToAction does) + * The energy consumption quality is calculated based on the robot's location and speed, gets information from the + * MoveToAction, and effects the robot's location (because MoveToAction does) * * */ public class EnergyConsumptionDomain extends TransitionStructure { - private StateVarDefinition mrLocDef; private StateVarDefinition mrSpeedDef; + private StateVarDefinition mrHeadlampDef; public EnergyConsumptionDomain(StateVarDefinition rLocDef, StateVarDefinition rSpeedDef, - ActionDefinition moveToDef) { - + StateVarDefinition rHeadlampDef, ActionDefinition moveToDef) { + this.mrLocDef = rLocDef; this.mrSpeedDef = rSpeedDef; + this.mrHeadlampDef = rHeadlampDef; addSrcStateVarDef(rLocDef); addSrcStateVarDef(rSpeedDef); - + addSrcStateVarDef(rHeadlampDef); + setActionDef(moveToDef); - + addDestStateVarDef(rLocDef); } - + public StateVarDefinition getLocationStateVar() { return mrLocDef; } - + public StateVarDefinition getSpeedStateVar() { return mrSpeedDef; } + + public StateVarDefinition getHeadlampStateVar() { + return mrHeadlampDef; + } } diff --git a/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionQFunction.java b/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionQFunction.java index 60c98654..88b1f9fc 100644 --- a/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionQFunction.java +++ b/XPlanning/src/examples/mobilerobot/metrics/EnergyConsumptionQFunction.java @@ -1,6 +1,7 @@ package examples.mobilerobot.metrics; import examples.mobilerobot.models.Distance; +import examples.mobilerobot.models.HeadlampState; import examples.mobilerobot.models.Location; import examples.mobilerobot.models.MoveToAction; import examples.mobilerobot.models.RobotSpeed; @@ -24,6 +25,7 @@ public class EnergyConsumptionQFunction implements IStandardMetricQFunction transit RobotSpeed speed = transition.getSrcStateVarValue(RobotSpeed.class, mDomain.getSpeedStateVar()); + // If the headlamp is on, then need to discharge at a higher rate + HeadlampState hl = transition.getSrcStateVarValue(HeadlampState.class, mDomain.getHeadlampStateVar()); + double consumption = distance.getDistance() / speed.getSpeed() * - (BASE_DISCHARGE_RATE + speed.getSpeed() * SPEED_INCREASE_DISCHARGE_RATE * 10); + (BASE_DISCHARGE_RATE + + speed.getSpeed() * SPEED_INCREASE_DISCHARGE_RATE * 10 + + (hl.getValue() ? HEADLAMP_DISCHARGE_RATE : 0)); return consumption; } diff --git a/XPlanning/src/examples/mobilerobot/models/Darkness.java b/XPlanning/src/examples/mobilerobot/models/Darkness.java new file mode 100644 index 00000000..48faf980 --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/models/Darkness.java @@ -0,0 +1,8 @@ +package examples.mobilerobot.models; + +import examples.mobilerobot.dsm.IEdgeAttribute; +import language.domain.models.IActionAttribute; + +public enum Darkness implements IEdgeAttribute, IActionAttribute { + LIGHT, DARK +} diff --git a/XPlanning/src/examples/mobilerobot/models/HeadlampActionDescription.java b/XPlanning/src/examples/mobilerobot/models/HeadlampActionDescription.java new file mode 100644 index 00000000..7801ffae --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/models/HeadlampActionDescription.java @@ -0,0 +1,20 @@ +package examples.mobilerobot.models; + +import language.domain.models.ActionDefinition; +import language.domain.models.IProbabilisticTransitionFormula; +import language.domain.models.StateVarDefinition; +import language.mdp.DiscriminantClass; +import language.mdp.EffectClass; +import language.mdp.FormulaActionDescription; +import language.mdp.Precondition; + +public class HeadlampActionDescription extends FormulaActionDescription { + + public HeadlampActionDescription(ActionDefinition setHeadlampDef, + Precondition precondition, StateVarDefinition rHeadlampDef) { + super(setHeadlampDef, precondition, + new DiscriminantClass(rHeadlampDef), new EffectClass(rHeadlampDef), + new HeadlampStateFormula(rHeadlampDef)); + } + +} diff --git a/XPlanning/src/examples/mobilerobot/models/HeadlampState.java b/XPlanning/src/examples/mobilerobot/models/HeadlampState.java new file mode 100644 index 00000000..7a37db95 --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/models/HeadlampState.java @@ -0,0 +1,58 @@ +package examples.mobilerobot.models; + +import language.domain.models.IStateVarAttribute; +import language.domain.models.IStateVarBoolean; +import language.exceptions.AttributeNameNotFoundException; + +public class HeadlampState implements IStateVarBoolean { + + private boolean mOn; + + public HeadlampState(boolean on) { + mOn = on; + } + + @Override + public IStateVarAttribute getAttributeValue(String name) throws AttributeNameNotFoundException { + throw new AttributeNameNotFoundException(name); + } + + @Override + public boolean getValue() { + return mOn; + } + + @Override + public String toString() { + return Boolean.toString(mOn); + } + + + /* + * Cached hashCode -- Effective Java + */ + private volatile int hashCode; + + @Override + public boolean equals(Object obj) { + if (obj == this) { + return true; + } + if (!(obj instanceof HeadlampState)) { + return false; + } + HeadlampState hlstate = (HeadlampState) obj; + return hlstate.mOn == mOn; + } + + @Override + public int hashCode() { + int result = hashCode; + if (result == 0) { + result = Boolean.hashCode(mOn); + hashCode = result; + } + return hashCode; + } + +} diff --git a/XPlanning/src/examples/mobilerobot/models/HeadlampStateFormula.java b/XPlanning/src/examples/mobilerobot/models/HeadlampStateFormula.java new file mode 100644 index 00000000..0d1b40c0 --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/models/HeadlampStateFormula.java @@ -0,0 +1,60 @@ +package examples.mobilerobot.models; + +import language.domain.models.IProbabilisticTransitionFormula; +import language.domain.models.StateVar; +import language.domain.models.StateVarDefinition; +import language.exceptions.XMDPException; +import language.mdp.Discriminant; +import language.mdp.Effect; +import language.mdp.EffectClass; +import language.mdp.ProbabilisticEffect; + +public class HeadlampStateFormula implements IProbabilisticTransitionFormula { + + private StateVarDefinition mrHeadlampDef; + private EffectClass mEffectClass; + + public HeadlampStateFormula (StateVarDefinition rHeadlampDef) { + this.mrHeadlampDef = rHeadlampDef; + this.mEffectClass = new EffectClass(rHeadlampDef); + } + + @Override + public ProbabilisticEffect formula(Discriminant discriminant, SetHeadlampAction action) throws XMDPException { + ProbabilisticEffect pe = new ProbabilisticEffect(mEffectClass); + Effect newHeadlampEffect = new Effect(mEffectClass); + StateVar rHeadhlamp = mrHeadlampDef.getStateVar(action.getValue()); + newHeadlampEffect.add(rHeadhlamp); + pe.put(newHeadlampEffect, 1.0); + return pe; + } + + /* + * Cached hashCode -- Effective Java + */ + private volatile int hashCode; + + @Override + public boolean equals(Object obj) { + if (obj == this) { + return true; + } + if (!(obj instanceof HeadlampStateFormula)) { + return false; + } + HeadlampStateFormula formula = (HeadlampStateFormula) obj; + return formula.mrHeadlampDef.equals(mrHeadlampDef); + } + + @Override + public int hashCode() { + int result = hashCode; + if (result == 0) { + result = 17; + result = 31 * result + mrHeadlampDef.hashCode(); + hashCode = result; + } + return hashCode; + } + +} diff --git a/XPlanning/src/examples/mobilerobot/models/MoveToAction.java b/XPlanning/src/examples/mobilerobot/models/MoveToAction.java index d5d94a8b..2c56013d 100644 --- a/XPlanning/src/examples/mobilerobot/models/MoveToAction.java +++ b/XPlanning/src/examples/mobilerobot/models/MoveToAction.java @@ -28,9 +28,12 @@ public class MoveToAction implements IAction { private Action mAction; private StateVar mrLocDest; - public MoveToAction(StateVar rLocDest) { - mAction = new Action("moveTo", rLocDest.getValue()); + private StateVar mrHeadlamp; + + public MoveToAction(StateVar rLocDest, StateVar rHeadlamp) { + mAction = new Action("moveTo", rLocDest.getValue(), rHeadlamp.getValue()); mrLocDest = rLocDest; + mrHeadlamp = rHeadlamp; } public void putDistanceValue(Distance distance, StateVar rLocSrc) { @@ -44,11 +47,20 @@ public void putOcclusionValue(Occlusion occlusion, StateVar rLocSrc) { varSet.add(rLocSrc); mAction.putDerivedAttributeValue("occlusion", occlusion, varSet); } + + public void putDarknessValue(Darkness darkness, StateVar rLocSrc) { + Set> varSet = new HashSet<>(); + varSet.add(rLocSrc); + mAction.putDerivedAttributeValue("darkness", darkness, varSet); + } public Location getDestination() { return mrLocDest.getValue(); } + public HeadlampState getHeadlampState() { + return mrHeadlamp.getValue(); + } public Distance getDistance(StateVar rLocSrc) throws AttributeNameNotFoundException { Set> varSet = new HashSet<>(); varSet.add(rLocSrc); @@ -60,6 +72,12 @@ public Occlusion getOcclusion(StateVar rLocSrc) throws AttributeNameNo varSet.add(rLocSrc); return (Occlusion) getDerivedAttributeValue("occlusion", varSet); } + + public Darkness getDarkness(StateVar rLocSrc) throws AttributeNameNotFoundException { + Set> varSet = new HashSet<>(); + varSet.add(rLocSrc); + return (Darkness) getDerivedAttributeValue("darkness", varSet); + } @Override public String getName() { diff --git a/XPlanning/src/examples/mobilerobot/models/RobotLocationActionDescription.java b/XPlanning/src/examples/mobilerobot/models/RobotLocationActionDescription.java index b1fb6241..2117f891 100644 --- a/XPlanning/src/examples/mobilerobot/models/RobotLocationActionDescription.java +++ b/XPlanning/src/examples/mobilerobot/models/RobotLocationActionDescription.java @@ -34,12 +34,13 @@ public class RobotLocationActionDescription implements IActionDescription mrLocActionDesc; public RobotLocationActionDescription(ActionDefinition moveToDef, - Precondition precondition, StateVarDefinition rLocDef) { + Precondition precondition, StateVarDefinition rLocDef, StateVarDefinition rHeadlampDef) { DiscriminantClass discrClass = new DiscriminantClass(); discrClass.add(rLocDef); + discrClass.add(rHeadlampDef); EffectClass effectClass = new EffectClass(); effectClass.add(rLocDef); - RobotLocationFormula rLocFormula = new RobotLocationFormula(rLocDef); + RobotLocationFormula rLocFormula = new RobotLocationFormula(rLocDef, rHeadlampDef); mrLocActionDesc = new FormulaActionDescription<>(moveToDef, precondition, discrClass, effectClass, rLocFormula); } diff --git a/XPlanning/src/examples/mobilerobot/models/RobotLocationFormula.java b/XPlanning/src/examples/mobilerobot/models/RobotLocationFormula.java index b9b7086d..fe29d128 100644 --- a/XPlanning/src/examples/mobilerobot/models/RobotLocationFormula.java +++ b/XPlanning/src/examples/mobilerobot/models/RobotLocationFormula.java @@ -28,9 +28,11 @@ public class RobotLocationFormula implements IProbabilisticTransitionFormula mrLocDef; private EffectClass mEffectClass; // of rLoc + private StateVarDefinition mrHeadlampDef; - public RobotLocationFormula(StateVarDefinition rLocDef) { + public RobotLocationFormula(StateVarDefinition rLocDef, StateVarDefinition rHeadlampDef) { mrLocDef = rLocDef; + mrHeadlampDef = rHeadlampDef; mEffectClass = new EffectClass(); mEffectClass.add(rLocDef); @@ -39,8 +41,10 @@ public RobotLocationFormula(StateVarDefinition rLocDef) { @Override public ProbabilisticEffect formula(Discriminant discriminant, MoveToAction moveTo) throws XMDPException { Location srcLoc = discriminant.getStateVarValue(Location.class, mrLocDef); + HeadlampState srcHeadlamp = discriminant.getStateVarValue(HeadlampState.class, mrHeadlampDef); StateVar rLocSrc = mrLocDef.getStateVar(srcLoc); Occlusion occlusion = moveTo.getOcclusion(rLocSrc); + Darkness darkness = moveTo.getDarkness(rLocSrc); ProbabilisticEffect rLocProbEffect = new ProbabilisticEffect(mEffectClass); // Possible effects on rLoc Effect newLoc = new Effect(mEffectClass); @@ -48,12 +52,18 @@ public ProbabilisticEffect formula(Discriminant discriminant, MoveToAction moveT newLoc.add(mrLocDef.getStateVar(moveTo.getDestination())); oldLoc.add(rLocSrc); - if (occlusion == Occlusion.OCCLUDED) { - rLocProbEffect.put(newLoc, MOVE_PROB_OCCLUDED); - rLocProbEffect.put(oldLoc, 1 - MOVE_PROB_OCCLUDED); - } else { - rLocProbEffect.put(newLoc, MOVE_PROB_NON_OCCLUDED); - rLocProbEffect.put(oldLoc, 1 - MOVE_PROB_NON_OCCLUDED); + if (darkness == Darkness.DARK && !srcHeadlamp.getValue()) { + rLocProbEffect.put(newLoc, 0); + rLocProbEffect.put(oldLoc, 1); + } + else { + if (occlusion == Occlusion.OCCLUDED) { + rLocProbEffect.put(newLoc, MOVE_PROB_OCCLUDED); + rLocProbEffect.put(oldLoc, 1 - MOVE_PROB_OCCLUDED); + } else { + rLocProbEffect.put(newLoc, MOVE_PROB_NON_OCCLUDED); + rLocProbEffect.put(oldLoc, 1 - MOVE_PROB_NON_OCCLUDED); + } } return rLocProbEffect; diff --git a/XPlanning/src/examples/mobilerobot/models/SetHeadlampAction.java b/XPlanning/src/examples/mobilerobot/models/SetHeadlampAction.java new file mode 100644 index 00000000..2e4468d2 --- /dev/null +++ b/XPlanning/src/examples/mobilerobot/models/SetHeadlampAction.java @@ -0,0 +1,19 @@ +package examples.mobilerobot.models; + +import language.domain.models.Action; +import language.domain.models.StateVar; + +public class SetHeadlampAction extends Action { + + private StateVar mrHeadlampState; + + public SetHeadlampAction(StateVar rHeadlampState) { + super("setHeadlamp", rHeadlampState.getValue()); + mrHeadlampState = rHeadlampState; + } + + public HeadlampState getValue() { + return mrHeadlampState.getValue(); + } + +} diff --git a/XPlanning/src/examples/mobilerobot/viz/MapBasedPolicyRenderer.java b/XPlanning/src/examples/mobilerobot/viz/MapBasedPolicyRenderer.java index 39647d29..3c720531 100644 --- a/XPlanning/src/examples/mobilerobot/viz/MapBasedPolicyRenderer.java +++ b/XPlanning/src/examples/mobilerobot/viz/MapBasedPolicyRenderer.java @@ -28,12 +28,14 @@ import examples.mobilerobot.dsm.MapTopology; import examples.mobilerobot.dsm.Mission; import examples.mobilerobot.dsm.parser.AreaParser; +import examples.mobilerobot.dsm.parser.DarknessParser; import examples.mobilerobot.dsm.parser.IEdgeAttributeParser; import examples.mobilerobot.dsm.parser.INodeAttributeParser; import examples.mobilerobot.dsm.parser.MapTopologyReader; import examples.mobilerobot.dsm.parser.MissionReader; import examples.mobilerobot.dsm.parser.OcclusionParser; import examples.mobilerobot.models.Area; +import examples.mobilerobot.models.Darkness; import examples.mobilerobot.models.Occlusion; import explanation.rendering.IPolicyRenderer; import graphscii.Edge; @@ -86,6 +88,8 @@ public static int[] getTerminalSize() throws Throwable { "| Edges: |", "| () : Sparse occlusion |", "| [] : Dense occlusion |", + "| o : Dark corridor |", + "| * : Headlamp on |", "| " + (char )0x21D2 + " : 0.7m/s traversal |", "| " + (char )0x2192 + " : 0.35m/s traversal |", "+--------------------------+" }; @@ -145,6 +149,10 @@ public String labelFor(Node t) { public String labelFor(Edge e) { Character dir = getAngleChar(e); String label = dir == null ? "" : ("" + dir); + Boolean headlamp = (Boolean )e.getAttribute("headlamp"); + if (headlamp != null && headlamp) { + label += "*"; + } Occlusion occ = (Occlusion) e.getAttribute("occlusion"); switch (occ) { case OCCLUDED: @@ -154,6 +162,10 @@ public String labelFor(Edge e) { label = String.format("(%s)", label); break; } + + Darkness dark = (Darkness) e.getAttribute("lighting"); + if (dark == Darkness.DARK) + label = "o" + label; return label; } @@ -187,6 +199,7 @@ else if (angle >= 292 && angle <= 338) } // System.out.println( // String.format("%s -- %s: %.0f", e.getSource().getLabel(), e.getTarget().getLabel(), angle, ret)); + return ret; } @@ -209,15 +222,18 @@ public MapBasedPolicyRenderer(File mapsJsonDir, File missionJsonFile, int consol } AreaParser areaParser = new AreaParser(); OcclusionParser occlusionParser = new OcclusionParser(); + DarknessParser darknessParser = new DarknessParser(); Set> nodeAttributeParsers = new HashSet<>(); nodeAttributeParsers.add(areaParser); Set> edgeAttributeParsers = new HashSet<>(); edgeAttributeParsers.add(occlusionParser); + edgeAttributeParsers.add(darknessParser); mMapReader = new MapTopologyReader(nodeAttributeParsers, edgeAttributeParsers); // Default node/edge attribute values mDefaultNodeAttributes.put(areaParser.getAttributeName(), DEFAULT_AREA); mDefaultEdgeAttributes.put(occlusionParser.getAttributeName(), DEFAULT_OCCLUSION); + mDefaultEdgeAttributes.put(darknessParser.getAttributeName(), Darkness.LIGHT); Mission mission; try { @@ -283,6 +299,10 @@ public MapBasedPolicyRenderer(File mapsJsonDir, File missionJsonFile, int consol if (occlusion != null) { edge.setAttribute("occlusion", occlusion); } + Darkness darkness = e.getConnectionAttribute(Darkness.class, "lighting"); + if (darkness != null) { + edge.setAttribute("lighting", darkness); + } } } @@ -320,6 +340,7 @@ public void renderPolicy(String policyFile, String prefix) throws IOException { if ("moveTo".equals(actionType)) { String sourceLabel = (String) state.get("rLoc"); double speed = (Double) state.get("rSpeed"); + boolean headlamp = (Boolean )state.get("rHeadlamp"); String targetLabel = (String) ((JSONArray) action.get("params")).get(0); Edge e = m_graph.getEdgeBySourceTarget(sourceLabel, targetLabel); Node source = null; @@ -340,6 +361,7 @@ public void renderPolicy(String policyFile, String prefix) throws IOException { } e.setAttribute("dirAngle", angle); e.setAttribute("speed", speed == 0.35 ? "normal" : "fast"); + e.setAttribute("headlamp", headlamp); } }