public MechanismCSpace(Mechanism mechanism, MechanismEnviroment scene, int randomSeed) { this.mechanism = (Mechanism)mechanism.Clone(); this.enviroment = scene; double[] dimensionLowLimit = new double[mechanism.Joints.Count]; double[] dimensionHighLimit = new double[mechanism.Joints.Count]; double[] dimensionWeight = new double[mechanism.Joints.Count]; for (int i = 0; i < mechanism.Joints.Count; i++) { dimensionLowLimit[i] = -180; dimensionHighLimit[i] = 180; dimensionWeight[i] = 1; } this.cSpace = new CSpace(mechanism.Joints.Count, dimensionLowLimit, dimensionHighLimit, dimensionWeight, CheckCollision, randomSeed); }
public void RunTests() { Matrix world = Matrix.CreateWorld(Vector3.Zero, Vector3.Forward, Vector3.Up); Vector3 minBound = new Vector3(-10, -10, -5); Vector3 maxBound = new Vector3(120, 10, 5); Mechanism robot = NArticulatedPlanarMechanism.Create(linkModel, new Vector3(100, 0, 0), minBound, maxBound, 6, world); MechanismEnviroment enviroment = new MechanismEnviroment(sceneModel); double[] origin = new double[6] { 150, 0, 0, 0, 0, 0 }; double[] dest = new double[6] { 90, 0, 0, 0, 0, 0 }; FileStream fs = new FileStream("test1.csv", FileMode.Create); StreamWriter sw = new StreamWriter(fs); ManualResetEvent stopEvent = new ManualResetEvent(false); for (int i = 0; i < 10; i++) { Node originNode, destNode; DateTime start, end; TimeSpan elapsedTime; int iterations; MechanismCSpace mechanismCSpace; CSpaceRRT RRT; originNode = null; destNode = null; mechanismCSpace = null; RRT = null; GC.Collect(); GC.WaitForPendingFinalizers(); mechanismCSpace = new MechanismCSpace(robot, enviroment, i); RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000); Thread.Sleep(1000); start = DateTime.Now; iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Edge, false, double.PositiveInfinity, stopEvent); end = DateTime.Now; elapsedTime = end - start; sw.WriteLine("Edge;Infinity;false;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString()); sw.Flush(); for (int j = 0; j < 4; j++) { double maxSize; switch (j) { case 0: maxSize = 5.0; break; case 1: maxSize = 10.0; break; case 2: maxSize = 25.0; break; case 3: maxSize = 50.0; break; default: maxSize = double.PositiveInfinity; break; } originNode = null; destNode = null; mechanismCSpace = null; RRT = null; GC.Collect(); GC.WaitForPendingFinalizers(); mechanismCSpace = new MechanismCSpace(robot, enviroment, i); RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000); Thread.Sleep(1000); start = DateTime.Now; iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, false, maxSize, stopEvent); end = DateTime.Now; elapsedTime = end - start; sw.WriteLine("Node;" + maxSize.ToString()+ ";false;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString()); sw.Flush(); originNode = null; destNode = null; mechanismCSpace = null; RRT = null; GC.Collect(); GC.WaitForPendingFinalizers(); mechanismCSpace = new MechanismCSpace(robot, enviroment, i); RRT = new CSpaceRRT(mechanismCSpace.CSpace, 10000); Thread.Sleep(1000); start = DateTime.Now; iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, true, maxSize, stopEvent); end = DateTime.Now; elapsedTime = end - start; sw.WriteLine(";Node;" + maxSize.ToString()+ ";true;" + i.ToString() + ";" + iterations.ToString() + ";" + elapsedTime.TotalSeconds.ToString()); sw.Flush(); } } sw.Close(); }