public Entry(Episode eps, int startIndex, int endIndex, TrajectoryResult trajResult)
{
this.eps = eps;
this.startIndex = startIndex;
this.endIndex = endIndex;
this.trajResult = trajResult;
}
private static TrajectoryResult CalculateStraightPositions(Transform transform, Trajectory pTrajectory, float pCharge)
{
Vector3 origin = transform.position;
TrajectoryResult result = new TrajectoryResult()
{
points = new Vector3[]
{
origin + transform.TransformDirection(pTrajectory.offset),
origin + transform.TransformDirection(Vector3.forward * pTrajectory.maxDistance)
},
count = 2,
hit = new RaycastHit()
};
RaycastHit raycastHit;
bool linecast = Physics.Linecast(
result.points[0],
result.points[1],
out raycastHit,
pTrajectory.layerMask,
QueryTriggerInteraction.Ignore
);
if (linecast)
{
result.points[1] = raycastHit.point;
result.hit = raycastHit;
}
return(result);
}
private static TrajectoryResult CalculateCurvePositions(Transform transform, Trajectory pTrajectory, float pCharge)
{
List <Vector3> points = new List <Vector3>();
RaycastHit raycastHit = new RaycastHit();
float v = Mathf.Lerp(pTrajectory.minVelocity, pTrajectory.maxVelocity, pCharge);
Vector3 initVelocity = transform.TransformDirection(Vector3.forward) * v;
Vector3 prevPosition = transform.position;
int maxCount = (int)(pTrajectory.maxDistance / pTrajectory.resolution);
float distanceAmount = 0f;
for (int i = 0; i < maxCount; ++i)
{
float t = i * pTrajectory.resolution;
points.Add(CalculateCurvePoint(
transform.position + transform.TransformDirection(pTrajectory.offset),
initVelocity,
t
));
bool linecast = Physics.Linecast(
prevPosition,
points[i],
out raycastHit,
pTrajectory.layerMask,
QueryTriggerInteraction.Ignore
);
if (linecast)
{
points[i] = raycastHit.point;
break;
}
distanceAmount += (points[i] - prevPosition).magnitude;
if (distanceAmount > pTrajectory.maxDistance)
{
break;
}
prevPosition = points[i];
}
TrajectoryResult result = new TrajectoryResult()
{
points = points.ToArray(),
count = points.Count,
hit = raycastHit
};
return(result);
}
private void UpdateTrajectory()
{
if (this.trajectory.shooter != null)
{
this.charge = this.trajectory.shooter.GetCharge();
}
TrajectoryResult result = GetTrajectory(
transform,
this.trajectory,
this.charge
);
this.lineRenderer.positionCount = result.count;
this.lineRenderer.SetPositions(result.points);
}
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);
}
public void addTrajectory(Episode eps, int startIndex, int endIndex, TrajectoryResult trajResult)
{
this.entries.Add(new Entry(eps, startIndex, endIndex, trajResult));
}
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));
}
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));
}
// A test function used when running in simulator
private void testLocally()
{
GameObject cube = GameObject.Find("Cube");
GameObject sampleDigits = GameObject.Find("Digits");
this.dataset = DatasetUtil.parseJson(this.dataFileName, this.decoration);
System.IO.StreamWriter file = new System.IO.StreamWriter("./training_instructions.txt");
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();
file.WriteLine(dp.getInstruction());
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);
}
List <int> actions = this.cachedTraj.getTrajectory(eps, startIndex, endIndex).getTrajectory();
}
file.Flush();
file.Close();
return;
/*Episode eps = null;
* int start = -1, end = -1;
* int flag = 0;
* Decoration pointDecoration = Decoration.Blank;
* string inst = "";
* int ctr = 0;
* for (int i = 0; i < this.dataset.Count; i++) {
*
* eps = this.dataset [i].getEpisode ();
* start = this.dataset [i].getStartFrame ();
* end = this.dataset [i].getEndFrame ();
* pointDecoration = this.dataset [i].getDecoration ();
* inst = this.dataset [i].getInstruction ();
*
* ObjectSystem system = eps.getEnvironmentByIndex (start);
* ObjectSystem toReach = eps.getEnvironmentByIndex (end);
*
* this.logoCubes = this.createLogoCubes (system, cube);
* this.digitCubes = this.createDigitCubes (system, cube, sampleDigits);
*
* GameObject ground = GameObject.Find ("Ground");
* ground.transform.localScale = new Vector3 (1.0f, 1.0f, 1.0f) * 0.25f;
*
* this.render (system, pointDecoration);
* this.highlightMainBlockAndGoal (system, toReach);
*
* Bounds bound = this.digitCubes[0].GetComponent<MeshFilter> ().mesh.bounds;
* Debug.Log ("Bound is " + bound.size.x);
*
* return;
*
* TrajectoryResult trajResult = system.findShortestPathAStar (toReach);
* List<int> actions = trajResult.getTrajectory ();
* string s = "";
* foreach (int action in actions) {
* int block = (int)(action / 4);
* int dir = action % 4;
* string dir_ = "";
* if (dir == 0) {
* dir_ = "west";//"north";
* } else if (dir == 1) {
* dir_ = "east";//"south";
* } else if (dir == 2) {
* dir_ = "north";//"east";
* } else if (dir == 3) {
* dir_ = "south";//"west";
* }
*
* s = s + block + "-" + dir_ + "; ";
* }
* Debug.Log ("Traj is " + s);
* Debug.Log ("Inst is " + inst.ToLower ());
* Debug.Log ("Traj count " + actions.Count);
*
* //GameObject goalStatePlane = GameObject.Find ("GoalState");
* //goalStatePlane.SetActive (false);
*
* /Application.CaptureScreenshot ("Intro_Example.png");
*
* break;
* }
*
* float groundSizeX = GameObject.Find("Ground").GetComponent<Collider> ().bounds.size.x;
* float groundSizeZ = GameObject.Find("Ground").GetComponent<Collider> ().bounds.size.z;
* Debug.Log ("Size of groud is " + groundSizeX + " , " + groundSizeZ);
* return;*/
}
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);
}
