public double calculateShortestPathMetric()
{
ObjectSystem newState = this.getCurrentObjectSystem();
//Compute distance between this newState and the old state
TrajectoryResult result = newState.findShortestPathAStar(this.end);
List <int> trajectory = result.getTrajectory();
int count = trajectory.Count;
if (trajectory [trajectory.Count - 1] == 80)
{
count--;
}
double distance = this.stepSize * count + result.getMinDistanceAchieved();
double shortestPathDistance = distance / this.blockSize;
return(shortestPathDistance);
}
void Update1()
{
if (this.first)
{
if (Screen.height != this.screenSize || Screen.width != this.screenSize)
{
return;
}
this.first = false;
}
else
{
return;
}
this.dataset = DatasetUtil.parseJson(this.dataFileName, this.decoration);
this.cachedTraj = new CachedTrajectory();
int datasetSize = this.dataset.Count;
System.IO.StreamWriter file = new System.IO.StreamWriter(@"datafile_" + this.dataFileName + ".txt");
for (int i = 0; i < datasetSize; i++)
{
DataPoint dp = this.dataset [i];
Episode eps = dp.getEpisode();
int startIndex = dp.getStartFrame();
int endIndex = dp.getEndFrame();
file.WriteLine(dp.getInstruction());
List <int> blocksMoved = dp.blocksMoved();
if (blocksMoved.Count != 1)
{
Debug.LogError("Moved more than 1 block!!!");
}
int goldBlock = blocksMoved [0];
Point3D pt = dp.getEpisode().getEnvironmentByIndex(dp.getEndFrame()).getObjectLocations() [goldBlock];
string info = goldBlock + " " + pt.getX() + " " + pt.getY() + " " + pt.getZ();
file.WriteLine(info);
if (!this.cachedTraj.isExist(eps, startIndex, endIndex))
{
ObjectSystem system1 = eps.getEnvironmentByIndex(startIndex);
ObjectSystem toReach1 = eps.getEnvironmentByIndex(endIndex);
TrajectoryResult trajResult = system1.findShortestPathAStar(toReach1);
this.cachedTraj.addTrajectory(eps, startIndex, endIndex, trajResult);
}
}
file.Flush();
file.Close();
// Start with the environment in the training data
this.datapointIndex = 0;
DataPoint current = this.dataset [this.datapointIndex];
int start = current.getStartFrame();
int end = current.getEndFrame();
Episode episode = current.getEpisode();
ObjectSystem system = episode.getEnvironmentByIndex(start);
ObjectSystem toReach = episode.getEnvironmentByIndex(end);
TrajectoryResult trajResult1 = this.cachedTraj.getTrajectory(episode, start, end);
List <int> trajectory = trajResult1.getTrajectory();
List <Point3D> trajectoryPoints = trajResult1.getTrajectoryPoints();
string instruction = current.getInstruction();
Decoration pointDecoration = current.getDecoration();
Debug.Log("Up and running");
//Read the grid
GameObject ground = GameObject.Find("Ground");
Bounds groundBounds = ground.GetComponent <MeshFilter> ().mesh.bounds;
float groundWidth = groundBounds.size.x;
float groundHeight = groundBounds.size.z;
Debug.Log("Ground: Width " + groundWidth + " height " + groundHeight);
//Read the sample cube information
GameObject cube = GameObject.Find("Cube");
Bounds cubeBounds = cube.GetComponent <MeshFilter> ().mesh.bounds;
float cubeWidth = cubeBounds.size.x;
float cubeHeight = cubeBounds.size.z;
Debug.Log("Cube: Width " + cubeWidth + " height " + cubeHeight);
//Render the system
this.logoCubes = this.createLogoCubes(system, cube);
GameObject sampleDigits = GameObject.Find("Digits");
this.digitCubes = this.createDigitCubes(system, cube, sampleDigits);
ground.transform.localScale = new Vector3(1.0f, 1.0f, 1.0f) * 0.25f;
this.render(system, pointDecoration);
this.highlightMainBlockAndGoal(system, toReach);
List <GameObject> cubes = null;
if (pointDecoration == Decoration.Logos)
{
cubes = this.logoCubes;
}
else if (pointDecoration == Decoration.Digits)
{
cubes = this.digitCubes;
}
// Add robot controller
GameObject robot = GameObject.Find("Robot");
robot.AddComponent <RobotController> ();
RobotController robotController = robot.GetComponent <RobotController> ();
robotController.init(cubes, (float)system.getSizeOfBlock(), this.stepSize, this.horizon);
// Initialize the MDP Manager which computes rewards.
double gamma = 0.1;
double failureReward = -5;
double winReward = 5;
robotController.initMDPManager(gamma, failureReward, winReward, this.stopActionReward,
this.rewardFunctionType, this.simplifiedProblem, this.horizon);
// Set the robot controller to work on the current problem
robotController.changeEpisode(cubes, system, toReach, trajectory, trajectoryPoints, brandsList.Count, episode.getBlockSize());
// Add agent messenger for listening
AgentMessenger.uselocalhost = this.uselocalhost;
robot.AddComponent <AgentMessenger> ();
this.agentMessenger = robot.GetComponent <AgentMessenger> ();
// Attach the robot controller to agent messenger
this.agentMessenger.attachRobotController(robotController);
//Find trajectory
string s = "";
foreach (int i in trajectory)
{
s = s + i + ",";
}
// TODO Clearly define message protocol
StartCoroutine(initConnection("Reset#0#img/Screenshot.png#" + instruction + "#" + s));
}
void Update()
{
if (this.first)
{
if (Screen.height != this.screenSize || Screen.width != this.screenSize)
{
return;
}
this.first = false;
}
else
{
return;
}
this.dataset = DatasetUtil.parseJson(this.dataFileName, this.decoration);
//this.dataset = this.readFromInstructionFile ();
/// Remove pairs of replicated state-end environments
/// in simplified problem
if (this.simplifiedProblem)
{
List <DataPoint> newDataset = new List <DataPoint> ();
int oldDatasetSize = this.dataset.Count;
for (int i = 0; i < oldDatasetSize; i++)
{
DataPoint dp = this.dataset [i];
Episode eps = dp.getEpisode();
int startIndex = dp.getStartFrame();
int endIndex = dp.getEndFrame();
bool seen = false;
foreach (DataPoint newDp in newDataset)
{
if (newDp.getStartFrame() == startIndex &&
newDp.getEndFrame() == endIndex &&
newDp.getEpisode() == eps)
{
seen = true;
break;
}
}
if (!seen)
{
newDataset.Add(dp);
}
}
this.dataset = newDataset;
Debug.Log("After removing commons. The dataset file " +
this.dataFileName + " has size " + this.dataset.Count);
}
Debug.Log("Total dataset size " + this.dataset.Count);
if (this.numDatapoints == -1)
{
this.numDatapoints = this.dataset.Count;
}
// Separate tune data and add it later
List <DataPoint> tuning = new List <DataPoint> ();
HashSet <Episode> epsLogo = new HashSet <Episode> ();
HashSet <Episode> epsDigit = new HashSet <Episode> ();
for (int i = 0; i < this.dataset.Count; i++)
{
DataPoint dp = this.dataset [i];
Decoration type = dp.getDecoration();
Episode eps = dp.getEpisode();
if (type == Decoration.Logos)
{
if (epsLogo.Contains(eps))
{
tuning.Add(dp);
}
else if (epsLogo.Count < 2)
{
//Space to add more episodes
epsLogo.Add(eps);
tuning.Add(dp);
}
}
if (type == Decoration.Digits)
{
if (epsDigit.Contains(eps))
{
tuning.Add(dp);
}
else if (epsDigit.Count < 2)
{
// Space to add more episodes
epsDigit.Add(eps);
tuning.Add(dp);
}
}
}
this.cachedTraj = new CachedTrajectory();
int datasetSize = this.dataset.Count;
for (int i = 0; i < datasetSize; i++)
{
DataPoint dp = this.dataset [i];
Episode eps = dp.getEpisode();
int startIndex = dp.getStartFrame();
int endIndex = dp.getEndFrame();
if (!this.cachedTraj.isExist(eps, startIndex, endIndex))
{
ObjectSystem system1 = eps.getEnvironmentByIndex(startIndex);
ObjectSystem toReach1 = eps.getEnvironmentByIndex(endIndex);
TrajectoryResult trajResult = system1.findShortestPathAStar(toReach1);
this.cachedTraj.addTrajectory(eps, startIndex, endIndex, trajResult);
}
}
// Remove the tuning data
foreach (DataPoint dp in tuning)
{
this.dataset.Remove(dp);
}
Debug.Log("Dataset: Tuning " + tuning.Count + " and train is " + this.dataset.Count);
if (this.shuffleBeforeSelect)
{
InitScript.Shuffle(this.dataset);
}
// Add the tuning data in the beginning
List <DataPoint> smallerDataset = new List <DataPoint> ();
foreach (DataPoint dp in tuning)
{
smallerDataset.Add(dp);
}
//////////////////////////////////////
List <SeenDemonstrations> uniqueDemonstration = new List <SeenDemonstrations>();
foreach (DataPoint dp in this.dataset)
{
bool isSeen = this.isSeenDemonstration(uniqueDemonstration, dp.getEpisode(), dp.getStartFrame(), dp.getEndFrame());
if (!isSeen)
{
SeenDemonstrations seen = new SeenDemonstrations(dp.getEpisode(), dp.getStartFrame(), dp.getEndFrame());
uniqueDemonstration.Add(seen);
}
}
// Number of demonstations to consider
int toUse = (int)(this.percentDemonstrations * uniqueDemonstration.Count);
Debug.Log("Number of demonstrations to are " + uniqueDemonstration.Count + " using " + toUse);
List <SeenDemonstrations> usedDemonstrations = new List <SeenDemonstrations>();
for (int i = 0; i < toUse; i++)
{
usedDemonstrations.Add(uniqueDemonstration [i]);
}
Debug.Log("Using " + usedDemonstrations.Count + " out of " + uniqueDemonstration.Count);
List <DataPoint> dpsSeen = new List <DataPoint>();
List <DataPoint> dpsUnseen = new List <DataPoint> ();
foreach (DataPoint dp in this.dataset)
{
if (this.isSeenDemonstration(usedDemonstrations, dp.getEpisode(), dp.getStartFrame(), dp.getEndFrame()))
{
dpsSeen.Add(dp);
}
else
{
dpsUnseen.Add(dp);
}
}
Debug.Log("Size of seen dataset is " + dpsSeen.Count + " and unseen is " + dpsUnseen.Count);
int total = tuning.Count + dpsSeen.Count + dpsUnseen.Count;
Debug.Log("Total accounted points are " + total);
foreach (DataPoint dp in dpsSeen)
{
smallerDataset.Add(dp);
}
foreach (DataPoint dp in dpsUnseen)
{
smallerDataset.Add(dp);
}
Debug.Log("Size of smaller dataset is " + smallerDataset.Count);
this.numDatapointsWithDemonstrations = tuning.Count + dpsSeen.Count;
Debug.Log("MAGIC: Consider the first " + this.numDatapointsWithDemonstrations + " points ");
//////////////////////////////////////
/*foreach (DataPoint dp in this.dataset) {
* smallerDataset.Add (dp);
* }*/
this.dataset = smallerDataset;
// Start with the environment in the training data
this.datapointIndex = 0;
DataPoint current = this.dataset [this.datapointIndex];
int start = current.getStartFrame();
int end = current.getEndFrame();
Episode episode = current.getEpisode();
ObjectSystem system = episode.getEnvironmentByIndex(start);
ObjectSystem toReach = episode.getEnvironmentByIndex(end);
TrajectoryResult trajResult1 = this.cachedTraj.getTrajectory(episode, start, end);
List <int> trajectory = trajResult1.getTrajectory();
List <Point3D> trajectoryPoints = trajResult1.getTrajectoryPoints();
string instruction = current.getInstruction();
Decoration pointDecoration = current.getDecoration();
Debug.Log("Up and running");
//Read the grid
GameObject ground = GameObject.Find("Ground");
Bounds groundBounds = ground.GetComponent <MeshFilter> ().mesh.bounds;
float groundWidth = groundBounds.size.x;
float groundHeight = groundBounds.size.z;
Debug.Log("Ground: Width " + groundWidth + " height " + groundHeight);
//Read the sample cube information
GameObject cube = GameObject.Find("Cube");
Bounds cubeBounds = cube.GetComponent <MeshFilter> ().mesh.bounds;
float cubeWidth = cubeBounds.size.x;
float cubeHeight = cubeBounds.size.z;
Debug.Log("Cube: Width " + cubeWidth + " height " + cubeHeight);
//Render the system
this.logoCubes = this.createLogoCubes(system, cube);
GameObject sampleDigits = GameObject.Find("Digits");
this.digitCubes = this.createDigitCubes(system, cube, sampleDigits);
ground.transform.localScale = new Vector3(1.0f, 1.0f, 1.0f) * 0.25f;
this.render(system, pointDecoration);
this.highlightMainBlockAndGoal(system, toReach);
List <GameObject> cubes = null;
if (pointDecoration == Decoration.Logos)
{
cubes = this.logoCubes;
}
else if (pointDecoration == Decoration.Digits)
{
cubes = this.digitCubes;
}
// Add robot controller
GameObject robot = GameObject.Find("Robot");
robot.AddComponent <RobotController> ();
RobotController robotController = robot.GetComponent <RobotController> ();
robotController.init(cubes, (float)system.getSizeOfBlock(), this.stepSize, this.horizon);
// Initialize the MDP Manager which computes rewards.
double gamma = 0.1;
double failureReward = -5;
double winReward = 5;
robotController.initMDPManager(gamma, failureReward, winReward, this.stopActionReward,
this.rewardFunctionType, this.simplifiedProblem, this.horizon);
// Set the robot controller to work on the current problem
robotController.changeEpisode(cubes, system, toReach, trajectory, trajectoryPoints, brandsList.Count, episode.getBlockSize());
// Add agent messenger for listening
AgentMessenger.uselocalhost = this.uselocalhost;
robot.AddComponent <AgentMessenger> ();
this.agentMessenger = robot.GetComponent <AgentMessenger> ();
// Attach the robot controller to agent messenger
this.agentMessenger.attachRobotController(robotController);
//Find trajectory
string s = "";
foreach (int i in trajectory)
{
s = s + i + ",";
}
// TODO Clearly define message protocol
StartCoroutine(initConnection("Reset#0#img/Screenshot.png#" + instruction + "#" + s));
// StartCoroutine (sendMessageWithGoalState ("Reset#0#img/Screenshot.png#" + instruction + "#" + s));
}
public string reset()
{
if (this.repeat)
{
if (shownAlready)
{
// Jump to the next datapoint
this.datapointIndex = (this.datapointIndex + 1) % this.dataset.Count;
shownAlready = false;
}
else
{
// Shown the task again
shownAlready = true;
}
}
else
{
// Jump to the next datapoint
this.datapointIndex = (this.datapointIndex + 1) % this.dataset.Count;
}
DataPoint current = this.dataset [this.datapointIndex];
int start = current.getStartFrame();
int end = current.getEndFrame();
Episode episode = current.getEpisode();
ObjectSystem system = episode.getEnvironmentByIndex(start);
ObjectSystem toReach = episode.getEnvironmentByIndex(end);
Decoration pointDecoration = current.getDecoration();
this.render(system, pointDecoration);
this.highlightMainBlockAndGoal(system, toReach);
GameObject robot = GameObject.Find("Robot");
RobotController robotController = robot.GetComponent <RobotController> ();
/////////////////////////////////////
Episode myEpisode = this.dataset [this.datapointIndex].getEpisode();
if (this.datapointIndex == 0)
{
// Part of seen demo. therefore
// change the reward function to add demonstrations (type 4)
robotController.changeRewardFunctionType(4);
}
if (this.datapointIndex == this.numDatapointsWithDemonstrations)
{
// Part of unseen demo. therefore
// change the reward function to remove demonstrations (type 2)
robotController.changeRewardFunctionType(2);
}
/////////////////////////////////////
TrajectoryResult trajResult1 = this.cachedTraj.getTrajectory(episode, start, end);
List <int> trajectory = trajResult1.getTrajectory();
List <Point3D> trajectoryPoints = trajResult1.getTrajectoryPoints();
List <GameObject> cubes = null;
if (pointDecoration == Decoration.Logos)
{
cubes = this.logoCubes;
}
else if (pointDecoration == Decoration.Digits)
{
cubes = this.digitCubes;
}
robotController.changeEpisode(cubes, system, toReach, trajectory, trajectoryPoints, brandsList.Count, episode.getBlockSize());
string s = "";
foreach (int i in trajectory)
{
s = s + i + ",";
}
return(current.getInstruction() + "#" + s);
}