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); }