public WorldState parse() { XmlDocument doc = new XmlDocument(); doc.Load(this.file); XmlNodeList entities = doc.GetElementsByTagName("SerializedEntities")[0].ChildNodes; IEnumerator ienum = entities.GetEnumerator(); List<ObjectState> objects = new List<ObjectState>(); int barriers = 0; while (ienum.MoveNext()) { XmlNode entity = (XmlNode)ienum.Current; if (entity.Name == "IRobotCreate") { XmlNodeList state = entity.FirstChild.ChildNodes; XmlNode n = state[0]; string name = n.InnerText; //Console.WriteLine(name); string type = "robot"; //Console.WriteLine(type); float[] position = new float[3]; float[] orientation = new float[3]; float[] velocity = new float[3]; float[] dimension = new float[3]; dimension[0] = 0; dimension[1] = 0; dimension[2] = 0; XmlNodeList pose = state[2].ChildNodes; XmlNodeList p = pose[0].ChildNodes; position[0] = float.Parse(p[0].InnerText); //Console.WriteLine(p[0].InnerText); position[1] = -float.Parse(p[2].InnerText); //Console.WriteLine(p[2].InnerText); position[2] = float.Parse(p[1].InnerText); //Console.WriteLine(p[1].InnerText); XmlNodeList o = pose[1].ChildNodes; orientation[0] = float.Parse(o[0].InnerText); //Console.WriteLine(o[1].InnerText); orientation[1] = -float.Parse(o[2].InnerText); //Console.WriteLine(o[2].InnerText); orientation[2] = float.Parse(o[1].InnerText); //Console.WriteLine(o[1].InnerText); XmlNodeList v = state[3].ChildNodes; velocity[0] = float.Parse(v[0].InnerText); //Console.WriteLine(v[0].InnerText); velocity[1] = -float.Parse(v[2].InnerText); //Console.WriteLine(v[2].InnerText); velocity[2] = float.Parse(v[1].InnerText); //Console.WriteLine(v[1].InnerText); ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension); objects.Add(os); } //else if (entity.Name == "ENTER ENTITY NAME HERE") //{ // XmlNodeList state = entity.FirstChild.ChildNodes; // XmlNode n = state[0]; // string name = n.InnerText; // //Console.WriteLine(name); // string type = "robot"; // //Console.WriteLine(type); // float[] position = new float[3]; // float[] orientation = new float[3]; // float[] velocity = new float[3]; // float[] dimension = new float[3]; // dimension[0] = 0; // dimension[1] = 0; // dimension[2] = 0; // XmlNodeList pose = state[2].ChildNodes; // XmlNodeList p = pose[0].ChildNodes; // position[0] = float.Parse(p[0].InnerText); // //Console.WriteLine(p[0].InnerText); // position[1] = -float.Parse(p[2].InnerText); // //Console.WriteLine(p[2].InnerText); // position[2] = float.Parse(p[1].InnerText); // //Console.WriteLine(p[1].InnerText); // XmlNodeList o = pose[1].ChildNodes; // orientation[0] = float.Parse(o[0].InnerText); // //Console.WriteLine(o[1].InnerText); // orientation[1] = -float.Parse(o[2].InnerText); // //Console.WriteLine(o[2].InnerText); // orientation[2] = float.Parse(o[1].InnerText); // //Console.WriteLine(o[1].InnerText); // XmlNodeList v = state[3].ChildNodes; // velocity[0] = float.Parse(v[0].InnerText); // //Console.WriteLine(v[0].InnerText); // velocity[1] = -float.Parse(v[2].InnerText); // //Console.WriteLine(v[2].InnerText); // velocity[2] = float.Parse(v[1].InnerText); // //Console.WriteLine(v[1].InnerText); // ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension); // objects.Add(os); //} else if (entity.Name == "SingleShapeEntity") { if (barriers == 4) { XmlNodeList state = entity.FirstChild.ChildNodes; XmlNode n = state[0]; string name = n.InnerText; //Console.WriteLine(name); string type = "obstacle"; //Console.WriteLine(type); float[] position = new float[3]; float[] orientation = new float[3]; float[] velocity = new float[3]; float[] dimension = new float[3]; dimension[0] = 0; dimension[1] = 0; dimension[2] = 0; XmlNodeList pose = state[2].ChildNodes; XmlNodeList p = pose[0].ChildNodes; position[0] = float.Parse(p[0].InnerText); //Console.WriteLine(p[0].InnerText); position[1] = -float.Parse(p[2].InnerText); //Console.WriteLine(p[2].InnerText); position[2] = float.Parse(p[1].InnerText); //Console.WriteLine(p[1].InnerText); XmlNodeList o = pose[1].ChildNodes; orientation[0] = float.Parse(o[0].InnerText); //Console.WriteLine(o[1].InnerText); orientation[1] = -float.Parse(o[2].InnerText); //Console.WriteLine(o[2].InnerText); orientation[2] = float.Parse(o[1].InnerText); //Console.WriteLine(o[1].InnerText); XmlNodeList v = state[3].ChildNodes; velocity[0] = float.Parse(v[0].InnerText); //Console.WriteLine(v[0].InnerText); velocity[1] = -float.Parse(v[2].InnerText); //Console.WriteLine(v[2].InnerText); velocity[2] = float.Parse(v[1].InnerText); //Console.WriteLine(v[1].InnerText); ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension); objects.Add(os); } else { barriers++; } } } WorldState w = new WorldState(objects); return w; }
public WorldPair(WorldDimensions WD, WorldState WS) { this.WD = WD; this.WS = WS; }
//NEW!!!!! Changed! public void SetWorldState(WorldState World) { this.W = World; }
//NEW!!!!!! Changed! public void PopulateWorld(WorldDimensions WD, WorldState World, GenAlgorithm Algo) { // Orient sim camera view point this.SetupCamera(WD); AddSky(); AddGround(Algo.GridSquareSize); AddBoundaries(WD.xdim, WD.zdim); for (int i = 0; i < World.objects.Count; i++) { if (World.objects[i].type == "obstacle") { AddObstacle(World.objects[i].dimension, World.objects[i].position, World.objects[i].name); } else if (World.objects[i].type == "robot") { this.RobotList.Add(AddRobot(World.objects[i])); } else if (World.objects[i].type == "food") { AddFood(World.objects[i].position, World.objects[i].name); } } }
public WorldState GetWorldState() { List<ObjectState> objs = new List<ObjectState>(); string name = ""; string type = "obstacle"; float[] position = new float[3]; float[] orientation = new float[3]; float[] velocity = new float[3]; float[] dimension = new float[3]; double newdegreesfromx; for (int i = 0; i < this.BoundaryList.Count; i++) { name = this.BoundaryList[i].State.Name; if (i < 4) { type = "Wall"; } else { type = "obstacle"; } position[0] = this.BoundaryList[i].State.Pose.Position.X; position[1] = -this.BoundaryList[i].State.Pose.Position.Z; position[2] = this.BoundaryList[i].State.Pose.Position.Y; orientation[0] = this.BoundaryList[i].State.Pose.Orientation.X; orientation[1] = -this.BoundaryList[i].State.Pose.Orientation.Z; orientation[2] = this.BoundaryList[i].State.Pose.Orientation.Y; velocity[0] = this.BoundaryList[i].State.Velocity.X; velocity[1] = -this.BoundaryList[i].State.Velocity.Z; velocity[2] = this.BoundaryList[i].State.Velocity.Y; for (int j = 0; j < this.W.objects.Count; j++) { if (name == this.W.objects[j].name) { dimension[0] = this.W.objects[j].dimension.X; dimension[1] = this.W.objects[j].dimension.Y; dimension[2] = this.W.objects[j].dimension.Z; } } ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension); objs.Add(o); } for (int i = 0; i < this.RobotList.Count; i++) { name = this.RobotList[i].State.Name; type = "robot"; position[0] = this.RobotList[i].State.Pose.Position.X; position[1] = -this.RobotList[i].State.Pose.Position.Z; position[2] = this.RobotList[i].State.Pose.Position.Y; orientation[0] = this.RobotList[i].State.Pose.Orientation.X; orientation[1] = -this.RobotList[i].State.Pose.Orientation.Z; orientation[2] = this.RobotList[i].State.Pose.Orientation.Y; velocity[0] = this.RobotList[i].State.Velocity.X; velocity[1] = -this.RobotList[i].State.Velocity.Z; velocity[2] = this.RobotList[i].State.Velocity.Y; dimension[0] = 1; dimension[1] = 1; dimension[2] = 1; double x = this.RobotList[i].State.Pose.Orientation.X / Math.Sqrt(1 - this.RobotList[i].State.Pose.Orientation.W * this.RobotList[i].State.Pose.Orientation.W); //Wrong orientation when negative double y = -(this.RobotList[i].State.Pose.Orientation.Z / Math.Sqrt(1 - this.RobotList[i].State.Pose.Orientation.W * this.RobotList[i].State.Pose.Orientation.W)); //Console.WriteLine(this.RobotList[i].Rotation); //This is correct!!! newdegreesfromx = (this.RobotList[i].Rotation.Y + 90); if (newdegreesfromx > 180) newdegreesfromx = 360 - newdegreesfromx; ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension); Console.WriteLine("Orientation is: " + newdegreesfromx); o.SetNewDegreesFromX(newdegreesfromx); objs.Add(o); } for (int i = 0; i < this.FoodList.Count; i++) { name = this.FoodList[i].State.Name; type = "FoodUnit"; position[0] = this.FoodList[i].State.Pose.Position.X; position[1] = -this.FoodList[i].State.Pose.Position.Z; position[2] = this.FoodList[i].State.Pose.Position.Y; orientation[0] = this.FoodList[i].State.Pose.Orientation.X; orientation[1] = -this.FoodList[i].State.Pose.Orientation.Z; orientation[2] = this.FoodList[i].State.Pose.Orientation.Y; velocity[0] = this.FoodList[i].State.Velocity.X; velocity[1] = -this.FoodList[i].State.Velocity.Z; velocity[2] = this.FoodList[i].State.Velocity.Y; dimension[0] = 0.5f; dimension[1] = 0.4f; dimension[2] = 0.5f; ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension); objs.Add(o); } WorldState ws = new WorldState(objs); this.W = ws; // create an updated world state // set the simulator field return W; }
//NEW!!!!! Changed! public void ExecuteActions(RobotActions ActionsVector) { //Console.WriteLine("SEND RECEIVED ACTION"); this.W = this.RobotsAct(ActionsVector); }