├── .travis.yml ├── src ├── main │ ├── resources │ │ ├── images │ │ │ ├── Up.png │ │ │ ├── bg.jpg │ │ │ ├── bg2.jpg │ │ │ ├── Down.png │ │ │ ├── Left.png │ │ │ ├── Right.png │ │ │ ├── aabout.jpg │ │ │ ├── about.jpg │ │ │ ├── ghost.gif │ │ │ ├── robot.gif │ │ │ ├── No_Move.png │ │ │ ├── Up-Left.png │ │ │ ├── Up-Right.png │ │ │ ├── particle.gif │ │ │ ├── Down-Left.png │ │ │ └── Down-Right.png │ │ ├── blog │ │ │ ├── _fork_me.png │ │ │ └── _download-sf.png │ │ └── config │ │ │ ├── Kalman.properties │ │ │ ├── landmarks.properties │ │ │ ├── PathPlanner.properties │ │ │ ├── PidController.properties │ │ │ ├── Histogram.properties │ │ │ ├── Particle.properties │ │ │ ├── PathSimulator.properties │ │ │ └── PathSmoother.properties │ └── java │ │ └── pk │ │ └── com │ │ └── habsoft │ │ └── robosim │ │ ├── planning │ │ ├── internal │ │ │ ├── WorldListener.java │ │ │ ├── AlgorithmListener.java │ │ │ ├── Path.java │ │ │ ├── PathNode.java │ │ │ └── WorldNode.java │ │ ├── algos │ │ │ ├── Heuristic.java │ │ │ ├── Algorithm.java │ │ │ ├── GradientDescent.java │ │ │ ├── TestDP.java │ │ │ ├── TestDPPP.java │ │ │ ├── BFSAlgorithm.java │ │ │ └── DynamicPrograming2D.java │ │ ├── common │ │ │ └── StatisticsPanel.java │ │ └── pathsmoother │ │ │ └── views │ │ │ └── DrawingControlPanel.java │ │ ├── internal │ │ ├── PropertiesListener.java │ │ ├── RPanel.java │ │ └── RootView.java │ │ ├── filters │ │ ├── particles │ │ │ ├── internal │ │ │ │ ├── SimulationObject.java │ │ │ │ ├── SimulationListener.java │ │ │ │ └── IRobot.java │ │ │ ├── RobotType.java │ │ │ ├── LandMark.java │ │ │ ├── views │ │ │ │ ├── SimulationPanel.java │ │ │ │ └── ParticleFilterView.java │ │ │ └── BigRobot.java │ │ ├── core │ │ │ ├── actions │ │ │ │ ├── ActionObserver.java │ │ │ │ └── SADomain.java │ │ │ ├── ObjectInstance.java │ │ │ ├── values │ │ │ │ ├── UnsetValueException.java │ │ │ │ ├── StringValue.java │ │ │ │ ├── IntValue.java │ │ │ │ ├── RealValue.java │ │ │ │ ├── RelationalValue.java │ │ │ │ ├── DiscreteValue.java │ │ │ │ ├── IntArrayValue.java │ │ │ │ ├── DoubleArrayValue.java │ │ │ │ └── MultiTargetRelationalValue.java │ │ │ ├── TransitionProbability.java │ │ │ ├── KeyActionBinding.java │ │ │ ├── ui │ │ │ │ ├── RenderLayer.java │ │ │ │ ├── ObjectPainter.java │ │ │ │ ├── StaticPainter.java │ │ │ │ ├── HistogramMain.java │ │ │ │ ├── MultiLayerRenderer.java │ │ │ │ ├── Visualizer.java │ │ │ │ └── GridWorldExplorer.java │ │ │ ├── GridLocation.java │ │ │ ├── objects │ │ │ │ ├── GridRobotBelief.java │ │ │ │ └── GridRobot.java │ │ │ └── State.java │ │ ├── sensors │ │ │ ├── RangeSensor.java │ │ │ ├── RobotDirection.java │ │ │ ├── RobotControl.java │ │ │ └── SonarRangeSensor.java │ │ ├── kalman │ │ │ ├── KalmanFilterTest.java │ │ │ ├── KalmanFilterGaussian.java │ │ │ ├── KalmanFilter.java │ │ │ ├── LineChartPanel.java │ │ │ ├── BarChartPanel.java │ │ │ └── KalmanFilterSimulator.java │ │ └── histogram │ │ │ ├── HistogramSimulator.java │ │ │ └── RobotBeliefMap.java │ │ ├── utils │ │ ├── CObservable.java │ │ ├── Util.java │ │ ├── UIUtils.java │ │ ├── ImageUtil.java │ │ ├── RobotLogger.java │ │ ├── RoboMathUtils.java │ │ └── PositionGeometryTools.java │ │ └── smoothing │ │ ├── controller │ │ ├── Controller.java │ │ └── PIDController.java │ │ ├── views │ │ ├── PIDControllerView.java │ │ └── PIDSimulator.java │ │ └── LineChartPanel.java └── test │ └── java │ └── pk │ └── com │ └── habsoft │ └── robosim │ └── AppTest.java ├── .gitignore ├── log4j.properties ├── ToDo.txt ├── README.md └── pom.xml /.travis.yml: -------------------------------------------------------------------------------- 1 | language: java 2 | -------------------------------------------------------------------------------- /src/main/resources/images/Up.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Up.png -------------------------------------------------------------------------------- /src/main/resources/images/bg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/bg.jpg -------------------------------------------------------------------------------- /src/main/resources/images/bg2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/bg2.jpg -------------------------------------------------------------------------------- /src/main/resources/blog/_fork_me.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/blog/_fork_me.png -------------------------------------------------------------------------------- /src/main/resources/images/Down.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Down.png -------------------------------------------------------------------------------- /src/main/resources/images/Left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Left.png -------------------------------------------------------------------------------- /src/main/resources/images/Right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Right.png -------------------------------------------------------------------------------- /src/main/resources/images/aabout.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/aabout.jpg -------------------------------------------------------------------------------- /src/main/resources/images/about.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/about.jpg -------------------------------------------------------------------------------- /src/main/resources/images/ghost.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/ghost.gif -------------------------------------------------------------------------------- /src/main/resources/images/robot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/robot.gif -------------------------------------------------------------------------------- /src/main/resources/images/No_Move.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/No_Move.png -------------------------------------------------------------------------------- /src/main/resources/images/Up-Left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Up-Left.png -------------------------------------------------------------------------------- /src/main/resources/images/Up-Right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Up-Right.png -------------------------------------------------------------------------------- /src/main/resources/images/particle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/particle.gif -------------------------------------------------------------------------------- /src/main/resources/blog/_download-sf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/blog/_download-sf.png -------------------------------------------------------------------------------- /src/main/resources/images/Down-Left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Down-Left.png -------------------------------------------------------------------------------- /src/main/resources/images/Down-Right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/habsoft/robosim/HEAD/src/main/resources/images/Down-Right.png -------------------------------------------------------------------------------- /src/main/resources/config/Kalman.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Fri Jun 26 23:36:15 PKT 2015 3 | TOTAL_TIME=30 4 | CAR_SPEED=0.5 5 | VARIANCE=3.0 6 | -------------------------------------------------------------------------------- /src/main/resources/config/landmarks.properties: -------------------------------------------------------------------------------- 1 | 150,100 2 | 300,100 3 | 450,100 4 | 600,100 5 | 150,300 6 | 300,300 7 | 450,300 8 | 600,300 -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/internal/WorldListener.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.internal; 2 | 3 | public interface WorldListener { 4 | public void worldUpdate(DiscreteWorld world); 5 | } 6 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/internal/PropertiesListener.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.internal; 2 | 3 | public interface PropertiesListener { 4 | 5 | public boolean loadProperties(); 6 | 7 | public void saveProperties(); 8 | } 9 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/internal/SimulationObject.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles.internal; 2 | 3 | import java.awt.Graphics2D; 4 | 5 | public interface SimulationObject { 6 | public void onPaint(Graphics2D g); 7 | } 8 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/internal/AlgorithmListener.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.internal; 2 | 3 | import pk.com.habsoft.robosim.planning.algos.Algorithm; 4 | 5 | public interface AlgorithmListener { 6 | public void algorithmUpdate(Algorithm algorithm); 7 | } 8 | -------------------------------------------------------------------------------- /src/main/resources/config/PathPlanner.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Sat Dec 26 20:30:32 PKT 2015 3 | MAP_ROW_5=0,0,0,0,1,0, 4 | GOAL_NODE_TAG=4,5 5 | MAP_ROW_4=0,0,0,0,0,0, 6 | NO_OF_COLUMNS=6 7 | MAP_ROW_3=1,0,1,0,1,0, 8 | START_NODE_TAG=0,0 9 | MAP_ROW_2=0,0,0,0,0,0, 10 | MAP_ROW_1=0,0,1,0,0,0, 11 | NO_OF_ROWS=5 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.class 2 | 3 | # Mobile Tools for Java (J2ME) 4 | .mtj.tmp/ 5 | 6 | # Package Files # 7 | *.jar 8 | *.war 9 | *.ear 10 | 11 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 12 | hs_err_pid* 13 | /target/ 14 | *.classpath 15 | *.project 16 | *.prefs 17 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/internal/Path.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.internal; 2 | 3 | import java.util.LinkedList; 4 | 5 | public class Path extends LinkedList { 6 | 7 | /** 8 | * 9 | */ 10 | private static final long serialVersionUID = 1L; 11 | 12 | } 13 | -------------------------------------------------------------------------------- /src/main/resources/config/PidController.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Sat Dec 26 20:30:32 PKT 2015 3 | SHOWPI=true 4 | SHOWPD=true 5 | SHOWPIDDRIFT=false 6 | TAU_P=0.2 7 | SHOWDDRIFT=false 8 | SHOWPIDRIFT=false 9 | TAU_I=0.004 10 | SPEED=1 11 | SHOWREF=true 12 | TAU_D=5.0 13 | SHOWP=true 14 | SHOWPDDRIFT=false 15 | SHOWPID=true 16 | SHOWI=true 17 | SHOWPDRIFT=false 18 | SHOWD=true 19 | SHOWIDRIFT=false 20 | ITERATIONS=100 21 | -------------------------------------------------------------------------------- /src/main/resources/config/Histogram.properties: -------------------------------------------------------------------------------- 1 | MAP_ROW_5=0,0,0,0,0 2 | NO_OF_COLUMNS=5 3 | MAP_ROW_4=0,1,0,1,0 4 | MAP_ROW_3=0,0,0,0,0 5 | MAP_ROW_2=0,1,0,1,0 6 | NO_OF_ROWS=5 7 | MAP_ROW_1=0,0,0,0,0 8 | MAP_ROW_0=1,2,3,4,2,4 9 | SENSOR_NOISE=0.0 10 | NO_OF_COLORS=3 11 | MOTION_NOISE=0.0 12 | CYCLIC_WORLD=true 13 | MAP_ROW_6=0,0,0,0,0,0 14 | ROBOT_COMMANDS=0,0;7,1;0,0;7,1;0,0;5,1;0,0;5,1;2,0;5,1;2,0;2,1;1,0;3,1;0,0;3,1 15 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/actions/ActionObserver.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.actions; 2 | 3 | import pk.com.habsoft.robosim.filters.core.State; 4 | 5 | public interface ActionObserver { 6 | 7 | /** 8 | * 9 | * @param oldState 10 | * @param newState 11 | * @param actionName TODO 12 | */ 13 | public void actionEvent(State oldState, State newState, String actionName); 14 | } 15 | -------------------------------------------------------------------------------- /src/main/resources/config/Particle.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Fri Jun 26 23:38:01 PKT 2015 3 | LASER_RANGE=200 4 | FORWARD_NOISE=1.05 5 | UNSAMPLED_PARTICLES_RATIO=0.0 6 | MOTIONS_SPEED=20.0 7 | PARTICLES=5000 8 | ROBOT_SIZE=25 9 | LANDMARK_SIZE=15 10 | PARTICLE_SIZE=4 11 | SIMULATION_SPEED=337 12 | GHOST_SIZE=15 13 | SENSE_NOISE=5.0 14 | STEERING_NOISE=2 15 | BOUNDED_VISION=true 16 | LASER_ANGLE=180 17 | NEW_PARTICLES_RATIO=0.0 18 | MOTION_ANGLE=3 19 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/RobotType.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles; 2 | 3 | public enum RobotType { 4 | PARTICLE(0, "Particle"), ROBOT(1, "Robot"), GHOST(2, "Ghost"); 5 | RobotType(int type, String name) { 6 | this.type = type; 7 | this.name = name; 8 | } 9 | 10 | int type; 11 | String name; 12 | 13 | @Override 14 | public String toString() { 15 | return name; 16 | } 17 | 18 | } 19 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ObjectInstance.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core; 2 | 3 | public interface ObjectInstance { 4 | 5 | public String getName(); 6 | 7 | public void setName(String newName); 8 | 9 | public String getClassName(); 10 | 11 | public String getObjectDescription(); 12 | 13 | public ObjectInstance copy(); 14 | 15 | public ObjectInstance setValue(String attrib, int val); 16 | 17 | public int getIntValForAttribute(String attrib); 18 | 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/internal/SimulationListener.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles.internal; 2 | 3 | import java.awt.Graphics2D; 4 | 5 | public interface SimulationListener { 6 | // public void init(CarController crtl, Background background); 7 | public void onUpdate(float dt); 8 | 9 | // public void onGPS(float[] pos); 10 | // public void onCamera(int[][] img); 11 | public void onPaint(Graphics2D g); 12 | // public void onScanner(int[][] dots); 13 | } 14 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/UnsetValueException.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | /** 4 | * A class for indicating that a OO-MDP object instance value is unset. 5 | * 6 | * @author James MacGlashan 7 | * 8 | */ 9 | public class UnsetValueException extends RuntimeException { 10 | 11 | private static final long serialVersionUID = 1L; 12 | 13 | public UnsetValueException() { 14 | super("OO-MDP Object Instance Value is Unset"); 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/sensors/RangeSensor.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.sensors; 2 | 3 | import pk.com.habsoft.robosim.filters.core.State; 4 | 5 | public interface RangeSensor { 6 | 7 | int getMaxRange(); 8 | 9 | void setMaxRange(int maxRange); 10 | 11 | double getNoise(); 12 | 13 | void setNoise(double noise); 14 | 15 | int getMeasurement(); 16 | 17 | void setMeasurement(int measurement); 18 | 19 | RobotDirection getDirection(); 20 | 21 | void sense(State s, int cx, int cy, int theta, int[][] map); 22 | 23 | } 24 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/CObservable.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import java.util.Observable; 4 | 5 | public class CObservable extends Observable { 6 | 7 | public CObservable() { 8 | } 9 | 10 | /* 11 | * (non-Javadoc) 12 | * 13 | * @see java.util.Observable#clearChanged() 14 | */ 15 | public synchronized void clearChanged() { 16 | super.clearChanged(); 17 | } 18 | 19 | /* 20 | * (non-Javadoc) 21 | * 22 | * @see java.util.Observable#setChanged() 23 | */ 24 | public synchronized void setChanged() { 25 | super.setChanged(); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/TransitionProbability.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core; 2 | 3 | /** 4 | * Represents the probability of transition to a given state. 5 | * 6 | * @author James MacGlashan 7 | * 8 | */ 9 | public class TransitionProbability { 10 | 11 | /** 12 | * The state to which the agent may transition. 13 | */ 14 | public State s; 15 | 16 | /** 17 | * the probability of transitioning to state s 18 | */ 19 | public double p; 20 | 21 | public TransitionProbability(State s, double p) { 22 | this.s = s; 23 | this.p = p; 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/KeyActionBinding.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core; 2 | 3 | public class KeyActionBinding { 4 | 5 | String key; 6 | String actionName; 7 | 8 | public KeyActionBinding(String key, String actionName) { 9 | super(); 10 | this.key = key; 11 | this.actionName = actionName; 12 | } 13 | 14 | public String getKey() { 15 | return key; 16 | } 17 | 18 | public void setKey(String key) { 19 | this.key = key; 20 | } 21 | 22 | public String getActionName() { 23 | return actionName; 24 | } 25 | 26 | public void setActionName(String actionName) { 27 | this.actionName = actionName; 28 | } 29 | 30 | } 31 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/RenderLayer.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Graphics2D; 4 | 5 | /** 6 | * A RenderLayer is a 2 dimensional layer that paints to a provided 2D graphics 7 | * context. The {@link MultiLayerRenderer} can take a list of these objects and 8 | * will paint them sequentially to the same 2D graphics context. This allows 9 | * different kinds of renderers that display different kinds of information to 10 | * be layered on top of each other. 11 | * 12 | * @author James MacGlashan 13 | * 14 | */ 15 | public interface RenderLayer { 16 | public void render(Graphics2D g2, float width, float height); 17 | } 18 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/sensors/RobotDirection.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.sensors; 2 | 3 | public enum RobotDirection { 4 | 5 | EAST(0, 0), // East 6 | NORTH(1, 90), // North 7 | WEST(2, 180), // West 8 | SOUTH(3, 270); // South 9 | 10 | private int index; 11 | private int angle; 12 | 13 | private RobotDirection(int index, int angle) { 14 | this.index = index; 15 | this.angle = angle; 16 | } 17 | 18 | public int getIndex() { 19 | return index; 20 | } 21 | 22 | public void setIndex(int index) { 23 | this.index = index; 24 | } 25 | 26 | public int getAngle() { 27 | return angle; 28 | } 29 | 30 | public void setAngle(int angle) { 31 | this.angle = angle; 32 | } 33 | 34 | } 35 | -------------------------------------------------------------------------------- /log4j.properties: -------------------------------------------------------------------------------- 1 | log4j.rootLogger=debug, stdout, R 2 | 3 | log4j.appender.stdout=org.apache.log4j.ConsoleAppender 4 | log4j.appender.stdout.layout=org.apache.log4j.PatternLayout 5 | 6 | # Pattern to output the caller's file name and line number. 7 | log4j.appender.stdout.layout.ConversionPattern=%d{dd-MM-yyyy hh:mm:ss:ms a} %5p %c{1}:%L - %m%n 8 | 9 | #Setting for file appender 10 | 11 | log4j.appender.R=org.apache.log4j.RollingFileAppender 12 | log4j.appender.R.File=EmailUtil.log 13 | 14 | # Set the logger level of File Appender to WARN 15 | log4j.appender.R.Threshold=ALL 16 | 17 | log4j.appender.R.MaxFileSize=1MB 18 | # Keep one backup file 19 | log4j.appender.R.MaxBackupIndex=1 20 | 21 | log4j.appender.R.layout=org.apache.log4j.PatternLayout 22 | log4j.appender.R.layout.ConversionPattern=%d{DATE} %5p %c{1}:%L - %m%n 23 | -------------------------------------------------------------------------------- /ToDo.txt: -------------------------------------------------------------------------------- 1 | 1. Changelist 2 | 2. Simplify property saving. 3 | 4 | High Priority. 5 | 1. JavaDoc 6 | 2. Installation steps 7 | 3. Maven Build 8 | 4. Contribution to code 9 | 5. Eclipse Formatter 10 | 6. CheckStyle 11 | 7. Travis build 12 | 8. Gradle build 13 | 9.i18n support 14 | 10.Logging 15 | 16 | Low Priority 17 | 2. Stochastic motion for DFS, BFS and A* 18 | 5.Twiddle 19 | 7.Group nearest particles for multiple ghost 20 | 8.Count total created instances in path planning and smoothing 21 | 9.Multiple worlds for path planning 22 | 10.Tie brakers and multiple goal points. 23 | 11.8-puzzule game 24 | 12. Generic algorithms 25 | 13. D* algorithm 26 | 14. Different obstacle to be included(tree, mud, water , etc) 27 | 15. comments/documentation to be added 28 | 16. Impl of test cases 29 | 30 | World properties and collision cost addition -------------------------------------------------------------------------------- /src/test/java/pk/com/habsoft/robosim/AppTest.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim; 2 | 3 | import junit.framework.Test; 4 | import junit.framework.TestCase; 5 | import junit.framework.TestSuite; 6 | 7 | /** 8 | * Unit test for simple App. 9 | */ 10 | public class AppTest 11 | extends TestCase 12 | { 13 | /** 14 | * Create the test case 15 | * 16 | * @param testName name of the test case 17 | */ 18 | public AppTest( String testName ) 19 | { 20 | super( testName ); 21 | } 22 | 23 | /** 24 | * @return the suite of tests being tested 25 | */ 26 | public static Test suite() 27 | { 28 | return new TestSuite( AppTest.class ); 29 | } 30 | 31 | /** 32 | * Rigourous Test :-) 33 | */ 34 | public void testApp() 35 | { 36 | assertTrue( true ); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/ObjectPainter.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Graphics2D; 4 | 5 | import pk.com.habsoft.robosim.filters.core.ObjectInstance; 6 | import pk.com.habsoft.robosim.filters.core.State; 7 | 8 | /** 9 | * And interface for defining painters that can render object instances to a 10 | * graphics context. 11 | * 12 | * @author James MacGlashan 13 | * 14 | */ 15 | public interface ObjectPainter { 16 | 17 | /** 18 | * Paints object instance ob to graphics context g2 19 | * 20 | * @param g2 21 | * graphics context to which the object should be painted 22 | * @param s 23 | * the state of the object to be painted 24 | * @param ob 25 | * the instantiated object to be painted 26 | * @param cWidth 27 | * width of the canvas size 28 | * @param cHeight 29 | * height of the canvas size 30 | */ 31 | public void paintObject(Graphics2D g2, State s, ObjectInstance ob, float cWidth, float cHeight); 32 | 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/GridLocation.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core; 2 | 3 | public class GridLocation { 4 | 5 | private GridLocation() { 6 | 7 | } 8 | 9 | public static int[] getGridLocation(int theta) { 10 | return getGridLocation(1, theta); 11 | } 12 | 13 | public static int[] getGridLocation(int distance, int theta) { 14 | if (distance == 0) { 15 | return new int[] { 0, 0 }; 16 | } else { 17 | switch (theta) { 18 | case 0: 19 | return new int[] { distance * 1, 0 }; 20 | case 45: 21 | return new int[] { distance * 1, distance * 1 }; 22 | case 90: 23 | return new int[] { 0, distance * 1 }; 24 | case 135: 25 | return new int[] { distance * (-1), distance * 1 }; 26 | case 180: 27 | return new int[] { distance * (-1), 0 }; 28 | case 225: 29 | return new int[] { distance * (-1), distance * (-1) }; 30 | case 270: 31 | return new int[] { 0, distance * (-1) }; 32 | case 315: 33 | return new int[] { distance * 1, distance * (-1) }; 34 | } 35 | } 36 | throw new RuntimeException(String.format("Invalid motion command.[s:%d, T:%d]", distance, theta)); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/StaticPainter.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Graphics2D; 4 | 5 | import pk.com.habsoft.robosim.filters.core.State; 6 | 7 | /** 8 | * This class paints general properties of a state/domain that may not be 9 | * represented by any specific object instance data. For instance, the GridWorld 10 | * class may have walls that need to be painted, but the walls are part of the 11 | * transition dynamics of the domain and not captured in the object instance 12 | * values assignments. 13 | * 14 | * @author James MacGlashan 15 | * 16 | */ 17 | public interface StaticPainter { 18 | 19 | /** 20 | * Paints general state information not to graphics context g2 21 | * 22 | * @param g2 23 | * graphics context to which the static data should be painted 24 | * @param s 25 | * the state to be painted 26 | * @param cWidth 27 | * the width of the canvas 28 | * @param cHeight 29 | * the height of the canvas 30 | */ 31 | public void paint(Graphics2D g2, State s, float cWidth, float cHeight); 32 | 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/LandMark.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles; 2 | 3 | import java.awt.Color; 4 | import java.awt.Graphics2D; 5 | 6 | import pk.com.habsoft.robosim.filters.particles.internal.SimulationObject; 7 | 8 | public class LandMark implements SimulationObject { 9 | 10 | int x, y; 11 | String text = ""; 12 | 13 | private boolean blink = false; 14 | 15 | public LandMark(int x, int y) { 16 | this.x = x; 17 | this.y = y; 18 | } 19 | 20 | public void blink() { 21 | blink = true; 22 | } 23 | 24 | public void unblink() { 25 | blink = false; 26 | } 27 | 28 | public void setText(String text) { 29 | this.text = text; 30 | } 31 | 32 | public int getX() { 33 | return x; 34 | } 35 | 36 | public void setX(int x) { 37 | this.x = x; 38 | } 39 | 40 | public int getY() { 41 | return y; 42 | } 43 | 44 | public void setY(int y) { 45 | this.y = y; 46 | } 47 | 48 | @Override 49 | public String toString() { 50 | return "\nLandMark [x=" + x + ", y=" + y + "]"; 51 | } 52 | 53 | @Override 54 | public void onPaint(Graphics2D g) { 55 | 56 | if (blink) 57 | g.setColor(Color.RED); 58 | else 59 | g.setColor(Color.BLACK); 60 | g.fillRect(x, y, World.LANDMARK_SIZE, World.LANDMARK_SIZE); 61 | 62 | } 63 | 64 | } 65 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/sensors/RobotControl.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.sensors; 2 | 3 | public enum RobotControl { 4 | 5 | MOVE_FORWARD("MoveForward", "w", 1, 0), // Forward 6 | MOVE_BACKWARD("MoveBackward", "s", -1, 0), // Backward 7 | TURN_RIGHT("TurnRight", "d", 0, -90), // Right 8 | TURN_LEFT("TurnLeft", "a", 0, 90); // Left 9 | 10 | private String actionName; 11 | private String shortKey; 12 | private int distance; 13 | private int angle; 14 | 15 | private RobotControl(String actionName, String shortKey, int distance, int angle) { 16 | this.actionName = actionName; 17 | this.shortKey = shortKey; 18 | this.distance = distance; 19 | this.angle = angle; 20 | } 21 | 22 | public String getActionName() { 23 | return actionName; 24 | } 25 | 26 | public void setActionName(String actionName) { 27 | this.actionName = actionName; 28 | } 29 | 30 | public String getShortKey() { 31 | return shortKey; 32 | } 33 | 34 | public void setShortKey(String shortKey) { 35 | this.shortKey = shortKey; 36 | } 37 | 38 | public int getDistance() { 39 | return distance; 40 | } 41 | 42 | public void setDistance(int distance) { 43 | this.distance = distance; 44 | } 45 | 46 | public int getAngle() { 47 | return angle; 48 | } 49 | 50 | public void setAngle(int angle) { 51 | this.angle = angle; 52 | } 53 | 54 | } 55 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/internal/PathNode.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.internal; 2 | 3 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 4 | 5 | /* 6 | * This is used to represent path location for path from start to goal. 7 | * This contains minimum information required to represent path node. 8 | */ 9 | public class PathNode implements Cloneable { 10 | private double x; 11 | private double y; 12 | 13 | public PathNode(double x, double y) { 14 | this.x = x; 15 | this.y = y; 16 | } 17 | 18 | public PathNode(PathNode node) { 19 | this.x = node.getX(); 20 | this.y = node.getY(); 21 | } 22 | 23 | public double getX() { 24 | return x; 25 | } 26 | 27 | public void setX(double x) { 28 | this.x = x; 29 | } 30 | 31 | public double getY() { 32 | return y; 33 | } 34 | 35 | public void setY(double y) { 36 | this.y = y; 37 | } 38 | 39 | @Override 40 | public boolean equals(Object obj) { 41 | if (obj == null) 42 | return false; 43 | if (this.getClass() != obj.getClass()) 44 | return false; 45 | PathNode that = (PathNode) obj; 46 | if (this.getX() == that.getX() && this.getY() == that.getY()) 47 | return true; 48 | return false; 49 | } 50 | 51 | @Override 52 | public PathNode clone() throws CloneNotSupportedException { 53 | return (PathNode) super.clone(); 54 | } 55 | 56 | @Override 57 | public String toString() { 58 | return "Path [x=" + RoboMathUtils.round(x, 3) + ", y=" + RoboMathUtils.round(y, 3) + "]"; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/internal/IRobot.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles.internal; 2 | 3 | import pk.com.habsoft.robosim.filters.particles.RobotType; 4 | 5 | public interface IRobot extends Cloneable, SimulationObject { 6 | 7 | IRobot clone() throws CloneNotSupportedException; 8 | 9 | public void random(); 10 | 11 | public double[] sense(boolean addNoise); 12 | 13 | public void move(double[] motions); 14 | 15 | public void move(double steering, double speed); 16 | 17 | public void setLocation(double x, double y, double orientation); 18 | 19 | public void setLocation(double[] measurements); 20 | 21 | public void setNoise(double senseNoise, double steeringNoise, double forwardNoise); 22 | 23 | public double measurementProb(double[] measurements); 24 | 25 | public double getX(); 26 | 27 | public void setX(double x); 28 | 29 | public double getY(); 30 | 31 | public void setY(double y); 32 | 33 | public double getOrientation(); 34 | 35 | public void setOrientation(double orientation); 36 | 37 | public double getLength(); 38 | 39 | public double getSenseNoise(); 40 | 41 | public double getSteeringNoise(); 42 | 43 | public double getForwardNoise(); 44 | 45 | public RobotType getRobotType(); 46 | 47 | public void update(IRobot obj); 48 | 49 | public void setLaserAngle(int laserAngle); 50 | 51 | public void setLaserRange(int laserRange); 52 | 53 | public void setBoundedVision(boolean boundedVision); 54 | 55 | public void setCheckBoundaries(boolean checkBoundaries); 56 | 57 | public double getSteeringDrift(); 58 | 59 | public void setSteeringDrift(double steeringDrift); 60 | 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/views/SimulationPanel.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles.views; 2 | 3 | import java.awt.Color; 4 | import java.awt.Graphics; 5 | import java.awt.Graphics2D; 6 | 7 | import javax.swing.JPanel; 8 | 9 | import pk.com.habsoft.robosim.filters.particles.ParticleSimulator; 10 | import pk.com.habsoft.robosim.filters.particles.World; 11 | import pk.com.habsoft.robosim.filters.particles.internal.SimulationObject; 12 | 13 | public class SimulationPanel extends JPanel { 14 | private static final long serialVersionUID = 1L; 15 | private ParticleSimulator simulation; 16 | public JPanel canvas; 17 | 18 | public SimulationPanel(ParticleSimulator s) { 19 | this.simulation = s; 20 | setLayout(null); 21 | canvas = new Canvas(); 22 | canvas.setBackground(Color.WHITE); 23 | canvas.setBounds(0, 0, World.getWidth() + World.getWallSize(), World.getHeight() + World.getWallSize()); 24 | add(canvas); 25 | } 26 | 27 | @Override 28 | public void paint(Graphics g) { 29 | canvas.repaint(); 30 | } 31 | 32 | private class Canvas extends JPanel { 33 | 34 | private static final long serialVersionUID = 1L; 35 | 36 | @Override 37 | public void paint(Graphics g) { 38 | super.paint(g); 39 | Graphics2D g2d = (Graphics2D) g; 40 | for (SimulationObject o : simulation) { 41 | o.onPaint(g2d); 42 | } 43 | 44 | g.setColor(Color.MAGENTA); 45 | g.fillRect(0, 0, World.getWidth(), World.getWallSize());// top 46 | g.fillRect(0, 0, World.getWallSize(), World.getHeight());// left 47 | g.fillRect(World.getWidth() - World.getWallSize(), 0, World.getWallSize(), World.getHeight());// right 48 | g.fillRect(0, World.getHeight() - World.getWallSize(), World.getWidth(), World.getWallSize());// bottom 49 | 50 | } 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/KalmanFilterTest.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | //http://www.udacity-forums.com/cs373/questions/10153/what-are-all-those-matrices-for-the-kalman-filter-part-i-x-f-p-h-r-u 4 | 5 | public class KalmanFilterTest { 6 | 7 | private KalmanFilterTest() { 8 | 9 | } 10 | 11 | public static void main(String[] args) { 12 | 13 | // 1D Kalman Filters 14 | double[][] measurement = { { 1 }, { 2 }, { 3 }, { 4 }, { 5 } }; 15 | double[][] dataX = { { 0 }, { 0 } }; 16 | double[][] dataP = { { 1000.0, 0.0 }, { 0.0, 1000.0 } }; 17 | double[][] dataU = { { 0.0 }, { 0.0 } }; 18 | double[][] dataF = { { 1.0, 1.0 }, { 0.0, 1.0 } }; 19 | double[][] dataH = { { 1.0, 0.0 } }; 20 | double[][] dataR = { { 1.0 } }; 21 | 22 | // 2D kalman filter 23 | // double[][] measurement = { { 2.0, 17.0 }, { 0.0, 15.0 }, { 2.0, 13.0 24 | // }, { 0.0, 11.0 } }; 25 | // double dt = 0.1; 26 | // double[] initial_xy = { 1.0, 19.0 }; 27 | // double[][] dataX = { { initial_xy[0] }, { initial_xy[1] }, { 0 }, { 0 28 | // } }; 29 | // double[][] dataP = { { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0, 0.0 }, 30 | // { 0.0, 0.0, 1000.0, 0.0 }, { 0.0, 0.0, 0.0, 1000.0 } }; 31 | // double[][] dataU = { { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 } }; 32 | // double[][] dataF = { { 1.0, 0.0, dt, 0.0 }, { 0.0, 1.0, 0.0, dt }, { 33 | // 0.0, 0.0, 1.0, 0.0 }, { 0.0, 0.0, 0.0, 1.0 } }; 34 | // double[][] dataH = { { 1.0, 0.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0, 0.0 } 35 | // }; 36 | // double[][] dataR = { { 0.1, 0.0 }, { 0.0, 0.1 } }; 37 | 38 | KalmanFilter filter = new KalmanFilter(dataX, dataP, dataU, dataF, dataH, dataR); 39 | for (int i = 0; i < measurement.length; i++) { 40 | filter.filter(measurement[i]); 41 | System.out.println(filter.getX()); 42 | 43 | } 44 | 45 | } 46 | } -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/Util.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import java.text.DecimalFormat; 4 | 5 | import org.apache.commons.math3.linear.RealMatrix; 6 | 7 | public class Util { 8 | public static final DecimalFormat df = new DecimalFormat("####0.##########"); 9 | 10 | public static void printMatrix(RealMatrix m) { 11 | printArrayP(m.getData()); 12 | } 13 | 14 | public static void printArrayP(double[][] p) { 15 | 16 | for (int i = 0; i < p.length; i++) { 17 | for (int j = 0; j < p[i].length; j++) { 18 | System.out.print(df.format(p[i][j])); 19 | // System.out.print(df.format(p[i][j])+" , "); 20 | if (!(j + 1 == p[i].length)) { 21 | System.out.print(" , "); 22 | } 23 | } 24 | System.out.println(""); 25 | } 26 | System.out.println(""); 27 | 28 | } 29 | 30 | public static void printArrayP(int[][] p) { 31 | 32 | for (int i = 0; i < p.length; i++) { 33 | for (int j = 0; j < p[i].length; j++) { 34 | System.out.print(df.format(p[i][j])); 35 | // System.out.print(df.format(p[i][j])+" , "); 36 | if (!(j + 1 == p[i].length)) { 37 | System.out.print(" , "); 38 | } 39 | } 40 | System.out.println(""); 41 | } 42 | System.out.println(""); 43 | 44 | } 45 | 46 | public static double findMax(double[] arr) { 47 | double max = arr[0]; 48 | for (int i = 0; i < arr.length; i++) { 49 | if (max < arr[i]) { 50 | max = arr[i]; 51 | } 52 | } 53 | return max; 54 | } 55 | 56 | public static String padRight(String s, int n) { 57 | return String.format("%1$-" + n + "s", s); 58 | } 59 | 60 | public static String padLeft(String s, int n) { 61 | return String.format("%1$#" + n + "s", s); 62 | } 63 | 64 | public boolean isValidValue(double value, double min, double max) { 65 | return value >= min && value <= max; 66 | } 67 | 68 | public static void debug(String msg) { 69 | System.out.println(msg); 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/UIUtils.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import java.awt.Dimension; 4 | import java.awt.Font; 5 | import java.awt.GridLayout; 6 | 7 | import javax.swing.BorderFactory; 8 | import javax.swing.JLabel; 9 | import javax.swing.JPanel; 10 | import javax.swing.JSpinner; 11 | import javax.swing.SpinnerNumberModel; 12 | import javax.swing.SwingConstants; 13 | import javax.swing.border.EtchedBorder; 14 | 15 | public class UIUtils { 16 | 17 | private UIUtils() { 18 | 19 | } 20 | 21 | public static final Font labelFont = new Font("Comic", Font.BOLD, 16); 22 | 23 | public static JPanel createSpinnerPanel(String text, JSpinner spn, int value, int min, int max, int step) { 24 | JPanel pnl = new JPanel(); 25 | pnl.setLayout(new GridLayout(1, 2)); 26 | pnl.add(new JLabel(" " + text + " ")); 27 | spn.setModel(new SpinnerNumberModel(value, min, max, step)); 28 | pnl.add(spn); 29 | return pnl; 30 | } 31 | 32 | public static JPanel createSpinnerPanel(String text, JSpinner spn, double value, double min, double max, 33 | double step) { 34 | JPanel pnl = new JPanel(); 35 | pnl.setLayout(new GridLayout(1, 2)); 36 | pnl.add(new JLabel(" " + text + " ")); 37 | spn.setModel(new SpinnerNumberModel(value, min, max, step)); 38 | pnl.add(spn); 39 | return pnl; 40 | } 41 | 42 | public static JLabel createLabel(int width, int height, String text) { 43 | JLabel header = new JLabel(); 44 | // header.setSize(width, height); 45 | header.setVerticalTextPosition(SwingConstants.CENTER); 46 | header.setHorizontalTextPosition(SwingConstants.CENTER); 47 | header.setPreferredSize(new Dimension(width, height)); 48 | header.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED)); 49 | header.setHorizontalAlignment(SwingConstants.CENTER); 50 | header.setBounds(0, 0, width, height); 51 | header.setFont(labelFont); 52 | header.setText(text); 53 | return header; 54 | 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/objects/GridRobotBelief.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.objects; 2 | 3 | import java.util.Arrays; 4 | 5 | import pk.com.habsoft.robosim.filters.core.ObjectClass; 6 | import pk.com.habsoft.robosim.filters.core.ObjectInstance; 7 | 8 | public class GridRobotBelief implements ObjectInstance { 9 | 10 | ObjectClass objectClass; 11 | String objectName; 12 | 13 | double[][][] beliefMap; 14 | 15 | public GridRobotBelief(ObjectClass objectClass, String objectName) { 16 | super(); 17 | this.objectClass = objectClass; 18 | this.objectName = objectName; 19 | } 20 | 21 | public GridRobotBelief(GridRobotBelief ob) { 22 | super(); 23 | this.objectClass = ob.objectClass; 24 | this.objectName = ob.objectName; 25 | this.beliefMap = ob.getBeliefMap().clone(); 26 | 27 | } 28 | 29 | @Override 30 | public String getName() { 31 | return objectName; 32 | } 33 | 34 | @Override 35 | public String getClassName() { 36 | return objectClass.name; 37 | } 38 | 39 | @Override 40 | public String getObjectDescription() { 41 | return this.toString(); 42 | } 43 | 44 | @Override 45 | public void setName(String newName) { 46 | this.objectName = newName; 47 | 48 | } 49 | 50 | @Override 51 | public ObjectInstance copy() { 52 | return new GridRobotBelief(this); 53 | } 54 | 55 | public double[][][] getBeliefMap() { 56 | return beliefMap; 57 | } 58 | 59 | public void setBeliefMap(double[][][] beliefMap) { 60 | this.beliefMap = beliefMap; 61 | } 62 | 63 | @Override 64 | public String toString() { 65 | return "GridRobotBelief [objectClass=" + objectClass + ", objectName=" + objectName + ", beliefMap=" + Arrays.toString(beliefMap) + "]"; 66 | } 67 | 68 | @Override 69 | public int getIntValForAttribute(String attx) { 70 | // TODO Auto-generated method stub 71 | return 0; 72 | } 73 | 74 | @Override 75 | public ObjectInstance setValue(String attName, int v) { 76 | // TODO Auto-generated method stub 77 | return null; 78 | } 79 | 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/KalmanFilterGaussian.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | import pk.com.habsoft.robosim.utils.Util; 4 | 5 | public class KalmanFilterGaussian { 6 | 7 | private KalmanFilterGaussian() { 8 | 9 | } 10 | 11 | /** 12 | * 13 | * @param mean1 14 | * @param var1 15 | * @param mean2 16 | * @param var2 17 | * @return measurement update 18 | */ 19 | public static MuSigma update(double mean1, double var1, double mean2, double var2) { 20 | double newMean = (mean1 * var2 + mean2 * var1) / (var1 + var2); 21 | double newVar = 1 / (1 / var1 + 1 / var2); 22 | return new MuSigma(newMean, newVar); 23 | } 24 | 25 | /** 26 | * 27 | * @param mean1 28 | * @param var1 29 | * @param mean2 30 | * @param var2 31 | * @return Motion/Prediction Update(Correction) 32 | */ 33 | public static MuSigma predict(double mean1, double var1, double mean2, double var2) { 34 | double newMean = mean1 + mean2; 35 | double newVar = var1 + var2; 36 | return new MuSigma(newMean, newVar); 37 | } 38 | 39 | public static void main(String[] args) { 40 | 41 | double[] measurements = { 5.0, 6.0, 7.0, 9.0, 10.0 }; 42 | double measurementSig = 4.0; 43 | double[] motions = { 1.0, 1.0, 2.0, 1.0, 1.0 }; 44 | double motionSig = 2.0; 45 | 46 | // initial parameters 47 | double mu = 0; 48 | double var = 10000.0; 49 | 50 | MuSigma newVal = new MuSigma(mu, var); 51 | for (int j = 0; j < measurements.length; j++) { 52 | // System.out.println("Actual "+(newVal.mu)); 53 | newVal = update(newVal.mu, newVal.var, measurements[j], measurementSig); 54 | System.out.println("Update " + newVal); 55 | newVal = predict(newVal.mu, newVal.var, motions[j], motionSig); 56 | System.out.println("Predict " + newVal); 57 | } 58 | 59 | } 60 | } 61 | 62 | class MuSigma { 63 | double mu; 64 | double var; 65 | 66 | public MuSigma(double mu, double var) { 67 | this.mu = mu; 68 | this.var = var; 69 | } 70 | 71 | @Override 72 | public String toString() { 73 | return "Mu_Sigma [mu=" + Util.df.format(mu) + ", sigma=" + Util.df.format(var) + "]"; 74 | } 75 | 76 | } 77 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/sensors/SonarRangeSensor.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.sensors; 2 | 3 | import pk.com.habsoft.robosim.filters.core.GridLocation; 4 | import pk.com.habsoft.robosim.filters.core.GridWorldDomain; 5 | import pk.com.habsoft.robosim.filters.core.State; 6 | 7 | public class SonarRangeSensor implements RangeSensor { 8 | 9 | RobotDirection direction; 10 | int maxRange; 11 | double noise; 12 | /* 13 | * Will be calculated on sense() 14 | */ 15 | int measurement; 16 | 17 | public SonarRangeSensor(RobotDirection direction, int maxRange, double noise) { 18 | super(); 19 | this.direction = direction; 20 | this.maxRange = maxRange; 21 | this.noise = noise; 22 | } 23 | 24 | @Override 25 | public void sense(State s, int cx, int cy, int theta, int[][] map) { 26 | measurement = 0; 27 | int nd = GridWorldDomain.INSTANCE.trimValue(GridWorldDomain.ATT_THETA, direction.getAngle() + theta, true); 28 | int[] dcomp = GridLocation.getGridLocation(nd); 29 | int nx = cx + dcomp[0]; 30 | int ny = cy + dcomp[1]; 31 | 32 | while (GridWorldDomain.INSTANCE.isOpen(nx, ny) && measurement < getMaxRange()) { 33 | nx += GridLocation.getGridLocation(direction.getAngle())[0]; 34 | ny += GridLocation.getGridLocation(direction.getAngle())[1]; 35 | measurement++; 36 | } 37 | 38 | } 39 | 40 | @Override 41 | public int getMaxRange() { 42 | return maxRange; 43 | } 44 | 45 | @Override 46 | public void setMaxRange(int maxRange) { 47 | this.maxRange = maxRange; 48 | } 49 | 50 | @Override 51 | public double getNoise() { 52 | return noise; 53 | } 54 | 55 | @Override 56 | public void setNoise(double noise) { 57 | this.noise = noise; 58 | } 59 | 60 | @Override 61 | public int getMeasurement() { 62 | return measurement; 63 | } 64 | 65 | @Override 66 | public void setMeasurement(int measurement) { 67 | this.measurement = measurement; 68 | } 69 | 70 | @Override 71 | public RobotDirection getDirection() { 72 | return direction; 73 | } 74 | 75 | @Override 76 | public String toString() { 77 | return "Reading [Dir=" + direction.getAngle() + ", Dist=" + measurement + "]"; 78 | } 79 | 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/smoothing/controller/Controller.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Player Java Client 2 - Controller.java 3 | * Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | * $Id: Controller.java,v 1.3 2006/03/06 08:33:31 veedee Exp $ 20 | * 21 | */ 22 | package pk.com.habsoft.robosim.smoothing.controller; 23 | 24 | /** 25 | * Abstract controller implementation. Used as a starting point for P, PI, PD, 26 | * PID controllers. 27 | * 28 | * @author Radu Bogdan Rusu 29 | */ 30 | public abstract class Controller { 31 | 32 | /** set the controller's goal */ 33 | protected double goal; 34 | 35 | /** the sum of all errors so far */ 36 | protected double eSum; 37 | 38 | /** current error */ 39 | protected double currE; 40 | /** last error */ 41 | protected double lastE; 42 | 43 | /** 44 | * Set a new goal for the controller. 45 | * 46 | * @param newGoal 47 | * the new goal for the controller 48 | */ 49 | public void setGoal(double newGoal) { 50 | this.goal = newGoal; 51 | } 52 | 53 | public abstract double getCommand(double curretnOutput); 54 | 55 | /** 56 | * Get the difference between the current error and the last error. 57 | * 58 | * @return the difference between the current error and the last error 59 | */ 60 | protected double deltaE() { 61 | // System.out.println("Delta : "+(currE - lastE)); 62 | return currE - lastE; 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/ImageUtil.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import java.awt.AlphaComposite; 4 | import java.awt.Graphics2D; 5 | import java.awt.Image; 6 | import java.awt.RenderingHints; 7 | import java.awt.image.BufferedImage; 8 | import java.awt.image.WritableRaster; 9 | import java.io.File; 10 | import java.io.IOException; 11 | 12 | import javax.imageio.ImageIO; 13 | 14 | public class ImageUtil { 15 | 16 | private ImageUtil() { 17 | 18 | } 19 | 20 | public static double[] loadImageData(String name) { 21 | BufferedImage bi = null; 22 | double[] arr = null; 23 | try { 24 | File f = new File(name); 25 | bi = ImageIO.read(f); 26 | int w = bi.getWidth(null); 27 | int h = bi.getHeight(null); 28 | arr = new double[w * h]; 29 | System.out.println("w " + w + " : h " + h); 30 | bi.getRaster().getPixels(0, 0, w, h, arr); 31 | } catch (IOException e) { 32 | System.out.println("Image could not be read"); 33 | System.exit(1); 34 | } 35 | return arr; 36 | } 37 | 38 | public static Image createImageFromArray(double[] pixels, int oldWidth, int oldHeight, int newWidth, int newHeight, 39 | int imgType) throws Exception { 40 | BufferedImage image = new BufferedImage(oldWidth, oldHeight, imgType); 41 | WritableRaster raster = (WritableRaster) image.getData(); 42 | raster.setPixels(0, 0, oldWidth, oldHeight, pixels); 43 | image.setData(raster); 44 | return resize(image, newWidth, newHeight); 45 | } 46 | 47 | public static BufferedImage resize(BufferedImage image, int w, int h) { 48 | int type = image.getType() == 0 ? BufferedImage.TYPE_INT_ARGB : image.getType(); 49 | BufferedImage resizedImage = new BufferedImage(w, h, type); 50 | Graphics2D g = resizedImage.createGraphics(); 51 | g.setComposite(AlphaComposite.Src); 52 | 53 | g.setRenderingHint(RenderingHints.KEY_INTERPOLATION, RenderingHints.VALUE_INTERPOLATION_BILINEAR); 54 | 55 | g.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY); 56 | 57 | g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); 58 | 59 | g.drawImage(image, 0, 0, w, h, null); 60 | g.dispose(); 61 | return resizedImage; 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RoboSim (Robot Simulator) 2 | 3 | Java based portable simulator to visualize and understand the *Robot Localization*, *Path planning*, *Path Smoothing* and *PID controller* concepts. 4 | 5 | ![Path Planning](https://a.fsdn.com/con/app/proj/r-localization/screenshots/5.Path%20Smoother.png/1 "Path Planning Algorithms") 6 | 7 | ## Blog : http://www.robosimblog.wordpress.com 8 | ## Download : https://sourceforge.net/projects/r-localization/ 9 | 10 | ### Modules 11 | * Filters 12 | * Histogram Filters 13 | * Kalman Filters 14 | * Particles Filters 15 | * Path Planning 16 | * Path Smoothing 17 | * PID Controller 18 | 19 | ### Path Planning Algorithms 20 | 21 | * BFS 22 | * DFS 23 | * A Star 24 | * Dynamic Programming 25 | 26 | ### Heuristics 27 | 28 | * Euclidean Distance 29 | * Euclidean Distance(+) 30 | * Euclidean Distance(*) 31 | * Euclidean Distance Squared 32 | * Manhattan Distance 33 | * Chebyshev Distance 34 | 35 | SYSTEM REQUIREMENTS 36 | ------------------- 37 | 38 | RoboSim needs a JRE of at least version 1.6 ([Java SE 6.0](http://www.oracle.com/technetwork/java/javase/downloads/index.html)). If you want to build the jar from source, you will 39 | also need [Maven](http://maven.apache.org/). 40 | 41 | INSTALLATION FROM SOURCE 42 | ------------------------ 43 | 44 | git clone https://github.com/habsoft/robosim.git 45 | cd robosim 46 | mvn install 47 | 48 | After the build is complete, the jar will then be built as target/RoboSim/RoboSim.jar. 49 | 50 | Suggestions, Feedback 51 | ---- 52 | This project is still under development, and we are working on many new features and controls. Your suggestions and feedback is required to improve this **Robot Simulator**. 53 | Please email [Faisal Hameed](mailto:faisal.hameed.pk@gmail.com) with any feedback. 54 | 55 | Background and Contributors 56 | --------------------------- 57 | Inspired from an online course [Artificial Intelligence for Robotics] (https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373) taught by [Sebastian Thrun](https://en.wikipedia.org/wiki/Sebastian_Thrun). 58 | This **Robot Simulator** was originally started by [Faisal Hameed](https://pk.linkedin.com/in/faisalhameedpk 59 | ) in April 2012 and its first version [1.0.1] (https://sourceforge.net/projects/r-localization/files/RoboSim%20ver%201.0.1.rar/download) was release in July 2012. 60 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/Heuristic.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | public class Heuristic { 4 | public final static int NONE = 0, EUCLIDEAN = 1, EUCLIDEAN_MULTIPLY = 2, EUCLIDEAN_SQUARED = 3, MANHATTAN = 4, 5 | CHEBYSHEV = 5; 6 | public final static String[] HURISTIC_NAMES = { "None", "Euclidean Diatance(+)", "Euclidean Diatance(*)", 7 | "Euclidean Diatance Squared", "Manhattan Distance", "Chebyshev Distance" }; 8 | public final static String[] HURISTIC_FUNCTIONS = { "h(n)=0", "h(n)=sqrt((x - gX)^2 + (y - gY)^2))", 9 | "h(n)=sqrt((x - gX)^2 * (y - gY)^2))", "h(n)=(x - gX)^2 + (y - gY)^2)", "h(n)=abs(x - gX) + abs(y - gY))", 10 | "h(n)=max(abs(x - gX),abs(y - gY)))" }; 11 | double[][] heuristic; 12 | private int goalX, goalY; 13 | private int heuristicType; 14 | 15 | public Heuristic(double[][] heuristics) { 16 | this.heuristic = heuristics.clone(); 17 | } 18 | 19 | public Heuristic(int rows, int cols, int goalX, int goalY, int heuristicType) { 20 | heuristic = new double[rows][cols]; 21 | this.goalX = goalX; 22 | this.goalY = goalY; 23 | this.heuristicType = heuristicType; 24 | for (int i = 0; i < rows; i++) { 25 | for (int j = 0; j < cols; j++) { 26 | heuristic[i][j] = getHeuristic(i, j, true); 27 | } 28 | } 29 | // System.out.println("Euclidean Distance Heuristics"); 30 | // for (int[] a : heuristic) { 31 | // System.out.println(Arrays.toString(a)); 32 | // } 33 | } 34 | 35 | /** 36 | * Used to build array of heuristic 37 | */ 38 | private double getHeuristic(int x, int y, boolean flag) { 39 | double h = 0; 40 | if (heuristicType == NONE) { 41 | h = 0; 42 | } else if (heuristicType == EUCLIDEAN) { 43 | h = Math.sqrt(Math.pow((double)x - goalX, 2) + Math.pow(y - goalY, 2)); 44 | } else if (heuristicType == EUCLIDEAN_MULTIPLY) { 45 | h = Math.sqrt(Math.pow((double)x - goalX, 2) * Math.pow(y - goalY, 2)); 46 | } else if (heuristicType == EUCLIDEAN_SQUARED) { 47 | h = Math.pow(x - goalX, 2) + Math.pow(y - goalY, 2); 48 | } else if (heuristicType == MANHATTAN) { 49 | h = (double)Math.abs(x - goalX) + Math.abs(y - goalY); 50 | } else if (heuristicType == CHEBYSHEV) { 51 | h = Math.max(Math.abs(x - goalX), Math.abs(y - goalY)); 52 | } 53 | return h; 54 | } 55 | 56 | public double getHeuristic(int x, int y) { 57 | return heuristic[x][y]; 58 | } 59 | 60 | } 61 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/internal/RPanel.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.internal; 2 | 3 | import java.awt.Color; 4 | import java.awt.Component; 5 | import java.awt.Dimension; 6 | import java.awt.Font; 7 | import java.awt.LayoutManager; 8 | import java.awt.Toolkit; 9 | 10 | import javax.swing.BorderFactory; 11 | import javax.swing.JFrame; 12 | import javax.swing.JPanel; 13 | 14 | import pk.com.habsoft.robosim.utils.UIUtils; 15 | 16 | public class RPanel extends JPanel { 17 | 18 | private static final long serialVersionUID = 1L; 19 | public static final Font labelFont = new Font("Comic", Font.BOLD, 16); 20 | public static final int LABEL_HEIGHT = 30; 21 | private JPanel pnlPrivate; 22 | public JPanel pnlPublic; 23 | 24 | public RPanel(double width, double height, String label) { 25 | pnlPrivate = new JPanel(null); 26 | pnlPrivate.setBounds(0, 0, (int) width, LABEL_HEIGHT); 27 | pnlPrivate.add(UIUtils.createLabel((int) width, LABEL_HEIGHT, label)); 28 | 29 | pnlPublic = new JPanel(null); 30 | pnlPublic.setBounds(0, LABEL_HEIGHT, (int) width, (int) height - LABEL_HEIGHT); 31 | 32 | pnlPublic.setBorder(BorderFactory.createLineBorder(Color.BLACK)); 33 | // pnlPublic.setBorder(BorderFactory.createTitledBorder("abc")); 34 | // pnlPublic.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.RAISED)); 35 | super.setLayout(null); 36 | super.setBounds(0, 0, (int) width, (int) height); 37 | 38 | super.add(pnlPrivate); 39 | super.add(pnlPublic); 40 | } 41 | 42 | @Override 43 | public Component add(Component comp) { 44 | return pnlPublic.add(comp); 45 | } 46 | 47 | @Override 48 | public void add(Component comp, Object border) { 49 | pnlPublic.add(comp, border); 50 | } 51 | 52 | public void setLayoutMgr(LayoutManager mgr) { 53 | pnlPublic.setLayout(mgr); 54 | } 55 | 56 | @Override 57 | public void removeAll() { 58 | pnlPublic.removeAll(); 59 | } 60 | 61 | @Override 62 | public void doLayout() { 63 | pnlPublic.doLayout(); 64 | } 65 | 66 | public static void main(String[] args) { 67 | JFrame frame = new JFrame(); 68 | frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); 69 | 70 | Dimension size = Toolkit.getDefaultToolkit().getScreenSize(); 71 | int width = (int) size.getWidth(); 72 | int height = (int) size.getHeight(); 73 | 74 | RPanel pnl = new RPanel(300, 300, "Robot World"); 75 | frame.add(pnl); 76 | 77 | frame.setSize(width, height); 78 | frame.setVisible(true); 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/RobotLogger.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import org.apache.log4j.BasicConfigurator; 4 | import org.apache.log4j.Level; 5 | import org.apache.log4j.Logger; 6 | import org.apache.log4j.PropertyConfigurator; 7 | 8 | public class RobotLogger { 9 | 10 | private RobotLogger() { 11 | 12 | } 13 | 14 | public static Logger getLogger(String name) { 15 | 16 | // very simple configuration to print on console 17 | // Its patterns is = "%-4r [%t] %-5p %c %x - %m%n". 18 | if (name != null && name.length() > 0) { 19 | PropertyConfigurator.configure("log4j.properties"); 20 | // BasicConfigurator.configure(); 21 | } else { 22 | BasicConfigurator.configure(); 23 | } 24 | 25 | // get a logger instance named "com.foo" 26 | // Logger logger = Logger.getLogger("com.foo"); 27 | // ApacheLogTest.class.getResource("log4j.properties"); 28 | Logger logger = Logger.getLogger(name); 29 | 30 | // Now set its level. Normally you do not need to set the 31 | // level of a logger programmatically. This is usually done 32 | // in configuration files. 33 | logger.setLevel(Level.ALL); 34 | 35 | // Logger barlogger = Logger.getLogger("com.foo.Bar"); 36 | 37 | // The logger instance barlogger, named "com.foo.Bar", 38 | // will inherit its level from the logger named 39 | // "com.foo" Thus, the following request is enabled 40 | // because INFO >= INFO. 41 | // barlogger.info("Located nearest gas station."); 42 | 43 | // This request is disabled, because DEBUG < INFO. 44 | // barlogger.debug("Exiting gas station search"); 45 | 46 | // ///////////// Appenders //////////////////////// 47 | 48 | // Log4j allows logging requests to print to multiple destinations. In 49 | // log4j speak, an output destination is called an appender. 50 | 51 | // Currently, appenders exist for the console, files, GUI components, 52 | // remote socket servers, JMS, NT Event Loggers, and remote UNIX Syslog 53 | // daemons. It is also possible to log asynchronously. 54 | 55 | // ///////////// Layouts //////////////////////// 56 | 57 | // More often than not, users wish to customize not only the output 58 | // destination but also the output format. This is accomplished by 59 | // associating a layout with an appender. The layout is responsible for 60 | // formatting the logging request according to the user's wishes, 61 | // whereas an appender takes care of sending the formatted output to its 62 | // destination 63 | return logger; 64 | } 65 | 66 | } 67 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/RoboMathUtils.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.utils; 2 | 3 | import java.math.BigDecimal; 4 | import java.text.DecimalFormat; 5 | import java.util.Random; 6 | 7 | /** 8 | * 9 | * @author faisal.hameed 10 | * 11 | */ 12 | public class RoboMathUtils { 13 | 14 | private RoboMathUtils() { 15 | 16 | } 17 | 18 | /** 19 | * 20 | * @param mean 21 | * @param variance 22 | * @return Gaussian with uniformally distributed 23 | */ 24 | public static double nextGaussian(double mean, double variance) { 25 | Random r = new Random(); 26 | return mean + r.nextGaussian() * variance; 27 | } 28 | 29 | public static double gaussian(double mu, double sigma, double x) { 30 | return Math.exp(-0.5 * Math.pow((x - mu), 2) / Math.pow(sigma, 2)) / (Math.sqrt(2 * Math.PI * Math.pow(sigma, 2))); 31 | } 32 | 33 | public static double euclideanDistance(double[] m, double[] z) { 34 | double dist = 0; 35 | for (int i = 0; i < z.length; i++) { 36 | dist += Math.pow(z[i] - m[i], 2); 37 | } 38 | 39 | return Math.sqrt(dist); 40 | } 41 | 42 | public static double manhattanDistance(double[] m, double[] z) { 43 | double dist = 0; 44 | for (int i = 0; i < z.length; i++) { 45 | dist += Math.pow(z[i] - m[i], 2); 46 | } 47 | 48 | return Math.sqrt(dist); 49 | } 50 | 51 | public static String round(double unrounded, int precision) { 52 | DecimalFormat df = new DecimalFormat("####0.##########"); 53 | BigDecimal bd = new BigDecimal(unrounded); 54 | BigDecimal rounded = bd.setScale(precision, BigDecimal.ROUND_HALF_UP); 55 | return df.format(rounded.doubleValue()); 56 | } 57 | 58 | /** 59 | * 60 | * @param value 61 | * @param truncate 62 | * @return modulus value equivalent to python modulus 63 | */ 64 | public static double modulus(double value, double truncate) { 65 | double newValue = value % truncate; 66 | // to get the same result as python (%) gives 67 | if (newValue < 0) { 68 | newValue += truncate; 69 | } 70 | 71 | return newValue; 72 | } 73 | 74 | /** 75 | * 76 | * @param value 77 | * @param truncate 78 | * @return modulus value equivalent to python modulus 79 | */ 80 | public static int modulus(int value, int truncate, boolean flag) { 81 | int newValue = value % truncate; 82 | // to get the same result as python (%) gives 83 | while (newValue < 0) { 84 | newValue += truncate; 85 | } 86 | 87 | return newValue; 88 | } 89 | 90 | public static void main(String[] args) { 91 | // System.out.println(RoboMathUtils.gaussian(5, 0.1, 4)); 92 | System.out.println(modulus(270-90-90-90, 360, true)); 93 | } 94 | 95 | } 96 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/HistogramMain.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Dimension; 4 | import java.awt.Toolkit; 5 | import java.util.List; 6 | 7 | import javax.swing.JDesktopPane; 8 | import javax.swing.JFrame; 9 | 10 | import pk.com.habsoft.robosim.filters.core.Domain; 11 | import pk.com.habsoft.robosim.filters.core.GridWorldDomain; 12 | import pk.com.habsoft.robosim.filters.core.KeyActionBinding; 13 | import pk.com.habsoft.robosim.filters.core.State; 14 | import pk.com.habsoft.robosim.internal.RootView; 15 | 16 | public class HistogramMain extends RootView { 17 | 18 | /** 19 | * 20 | */ 21 | private static final long serialVersionUID = 1L; 22 | private static GridWorldExplorer exp; 23 | 24 | public HistogramMain() { 25 | super("Histogram Filter(Sonar Range Finder)", ""); 26 | } 27 | 28 | public static void main(String[] args) { 29 | 30 | JFrame frame = new JFrame(); 31 | frame.setDefaultCloseOperation(EXIT_ON_CLOSE); 32 | JDesktopPane desk = new JDesktopPane(); 33 | frame.setContentPane(desk); 34 | 35 | HistogramMain view1 = new HistogramMain(); 36 | view1.initGUI(); 37 | 38 | desk.add(view1); 39 | view1.setVisible(true); 40 | 41 | Dimension size = Toolkit.getDefaultToolkit().getScreenSize(); 42 | int width = (int) size.getWidth(); 43 | int height = (int) size.getHeight(); 44 | 45 | frame.setSize(width, height); 46 | frame.setVisible(true); 47 | 48 | } 49 | 50 | @Override 51 | public void initGUI() { 52 | GridWorldDomain domain = new GridWorldDomain(11, 11); 53 | domain.initDefaultWorld(); 54 | 55 | Domain d = domain.generateDomain(); 56 | 57 | // Setup Noise 58 | double motionNoise = 0.1; 59 | double sensorNoise = 0.1; 60 | 61 | // setup initial state 62 | State s = domain.getOneRobotBeliefState(d); 63 | domain.setAgent(s, 4, 2, 0); 64 | domain.setCyclicWorld(false); 65 | domain.initUniformBelief(s); 66 | domain.initializeSensors(s, d, motionNoise, sensorNoise); 67 | 68 | Visualizer v = GridWorldVisualizer.getVisualizer(domain.getMap()); 69 | v.updateState(s); 70 | 71 | exp = new GridWorldExplorer(this, d, v, s); 72 | 73 | // TODO 74 | List act = domain.getKeyActionsBindings(); 75 | for (KeyActionBinding action : act) { 76 | exp.addKeyAction(action.getKey(), action.getActionName()); 77 | } 78 | // set control keys to use w-s-a-d 79 | 80 | exp.addKeyAction("l", GridWorldDomain.ACTION_SENSE); 81 | 82 | exp.initGUI(); 83 | 84 | // super.add(v); 85 | 86 | setSize(screenSize.getWidth(), screenSize.getHeight() - 60); 87 | setVisible(true); 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/Algorithm.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import java.awt.Color; 4 | 5 | import pk.com.habsoft.robosim.planning.internal.DiscreteWorld; 6 | import pk.com.habsoft.robosim.planning.internal.Path; 7 | 8 | public abstract class Algorithm { 9 | 10 | int[][] policy; 11 | double[][] expand; 12 | Path path; 13 | int exploredSize; 14 | int pathSize = 1; 15 | int unExploredSize; 16 | int blocked; 17 | int instances = 1; 18 | String result = "Goal Not Found"; 19 | 20 | public final static int A_STAR = 0, DFS = 1, BFS = 2, DP = 3; 21 | DiscreteWorld world; 22 | 23 | public static final int UP = 0, UP_LEFT = 1, LEFT = 2, DOWN_LEFT = 3, DOWN = 4, DOWN_RIGHT = 5, RIGHT = 6, UP_RIGHT = 7, 24 | EXPLORED = 8, NOT_EXPLORED = 9, BLOCK = 10, HIDDEN = 11, START = 12, GOAL = 13; 25 | int[] actionArray = { UP, UP_LEFT, LEFT, DOWN_LEFT, DOWN, DOWN_RIGHT, RIGHT, UP_RIGHT, EXPLORED, NOT_EXPLORED, 26 | BLOCK, HIDDEN, START, GOAL }; 27 | public static final Color pathColor = Color.ORANGE; 28 | public static final Color[] colors = { pathColor, pathColor, pathColor, pathColor, pathColor, pathColor, pathColor, 29 | pathColor, Color.MAGENTA, Color.WHITE, Color.BLACK, Color.LIGHT_GRAY, Color.RED, Color.GREEN }; 30 | 31 | // Used for images(up,left,down,right) 32 | public static final String[] DELTA_NAMES = { "Up", "Up-Left", "Left", "Down-Left", "Down", "Down-Right", "Right", 33 | "Up-Right" }; 34 | // public static final String[] DELTA_NAMES = { "^", "<", "v", 35 | // ">","Up-Left", "Up-Right", "Down-Left", "Down-Right" };; 36 | int[][] deltas = { { -1, 0 }, // up 37 | { -1, -1 }, // up-left 38 | { 0, -1 }, // left 39 | { 1, -1 }, // down-left 40 | { 1, 0 }, // down 41 | { 1, 1 }, // down-right 42 | { 0, 1 }, // right 43 | { -1, 1 } // up-right 44 | }; 45 | 46 | protected static final String[] DELTA_NAMES2 = { "^", "", "<", "", "v", "", ">", "" };; 47 | int[][] deltas2 = { { -1, 0 }, // go up 48 | { 0, -1 }, // go left 49 | { 1, 0 }, // go down 50 | { 0, 1 } };// go right 51 | 52 | abstract void solve(); 53 | 54 | public abstract int[][] getPolicy(); 55 | 56 | public abstract double[][] getExpand(); 57 | 58 | public abstract double[][] getHeuristic(); 59 | 60 | public abstract int getExploredSize(); 61 | 62 | public abstract int getUnExploredSize(); 63 | 64 | public abstract int getBlockedSize(); 65 | 66 | public abstract int getTotalSize(); 67 | 68 | public abstract int getPathSize(); 69 | 70 | public abstract int getTotalInstances(); 71 | 72 | public abstract String getResult(); 73 | 74 | public abstract Path getPath(); 75 | 76 | } 77 | -------------------------------------------------------------------------------- /src/main/resources/config/PathSimulator.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Sat Feb 15 16:13:20 PKT 2014 3 | MAP_ROW_18=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 4 | MAP_ROW_17=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 5 | MAP_ROW_16=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 6 | MAP_ROW_15=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 7 | MAP_ROW_14=0,0,0,0,0,0,1,1,1,1,0,0,0,1,1,1,1,1,0,0,0,0,0,1,0,0,0,0,0,0, 8 | MAP_ROW_13=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0, 9 | MAP_ROW_12=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0, 10 | MAP_ROW_11=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0, 11 | ALGORITHM=0 12 | MAP_ROW_10=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0, 13 | SENSOR_RANGE=27 14 | GO_STRAIGHT_COST=1 15 | NO_OF_COLUMNS=30 16 | LEFT_TURN_COST=1 17 | USE_ORIENTATION=true 18 | USE_HEURISTICS=true 19 | SIMULATION_SPEED=9 20 | MAP_ROW_30=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0, 21 | MAP_ROW_9=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0, 22 | MAP_ROW_8=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0, 23 | MAP_ROW_7=0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0, 24 | HEURISTICS=2 25 | MAP_ROW_6=0,0,1,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,1,1,0,0,0, 26 | SHOW_HEURISTICS=false 27 | MAP_ROW_5=0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0, 28 | MAP_ROW_4=0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0, 29 | MAP_ROW_3=0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0, 30 | MAP_ROW_2=0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 31 | MAP_ROW_1=0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 32 | RIGHT_TURN_COST=1 33 | MAP_ROW_29=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0, 34 | MAP_ROW_28=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0, 35 | MAP_ROW_27=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,1,0,0, 36 | MAP_ROW_26=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0, 37 | MAP_ROW_25=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0, 38 | MAP_ROW_24=0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0, 39 | MAP_ROW_23=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,1,1,1,0,0, 40 | MAP_ROW_22=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0, 41 | MAP_ROW_21=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0, 42 | DIAGONAL_MOTION=false 43 | MAP_ROW_20=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 44 | GOAL_NODE_TAG=29,29 45 | NO_OF_ROWS=30 46 | START_NODE_TAG=0,0 47 | MAP_ROW_19=0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0, 48 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/smoothing/views/PIDControllerView.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.smoothing.views; 2 | 3 | 4 | import java.awt.Frame; 5 | 6 | import javax.swing.JDesktopPane; 7 | import javax.swing.JFrame; 8 | 9 | import pk.com.habsoft.robosim.internal.RootView; 10 | import pk.com.habsoft.robosim.smoothing.LineChartPanel; 11 | 12 | 13 | public class PIDControllerView extends RootView { 14 | 15 | private static final long serialVersionUID = 1L; 16 | 17 | static int PNL_CHART_WIDTH = 700; 18 | static int PNL_CHART_HEIGHT = 500; 19 | static int PNL_CONTROL_WIDTH = 300; 20 | static int PNL_CONTROL_HEIGHT = 250; 21 | 22 | LineChartPanel pnlChart; 23 | PIDControlPanel pnlControl; 24 | 25 | public PIDControllerView() { 26 | super("PID Controller", "config/PidController.properties"); 27 | setLayout(null); 28 | loadProperties(); 29 | initGUI(); 30 | } 31 | 32 | @Override 33 | public void initGUI() { 34 | isInit = true; 35 | 36 | pnlChart = new LineChartPanel(PNL_CHART_WIDTH, PNL_CHART_HEIGHT, "X-Axis", "Y-Axis"); 37 | pnlChart.setLocation(0, 0); 38 | pnlChart.setSize(PNL_CHART_WIDTH, PNL_CHART_HEIGHT); 39 | 40 | pnlControl = new PIDControlPanel(prop, PNL_CONTROL_WIDTH, PNL_CONTROL_HEIGHT, pnlChart); 41 | pnlControl.setLocation(PNL_CHART_WIDTH, 0); 42 | pnlControl.setSize(PNL_CONTROL_WIDTH, PNL_CONTROL_HEIGHT); 43 | 44 | add(pnlChart); 45 | add(pnlControl); 46 | 47 | setBounds(0, 0, screenSize.getWidth(), screenSize.getHeight()); 48 | 49 | } 50 | 51 | @Override 52 | public boolean loadProperties() { 53 | System.out.println("Property File = " + propertyFile); 54 | 55 | double ratio = 0.70; 56 | PNL_CHART_WIDTH = (int) (screenSize.getWidth() * ratio); 57 | PNL_CONTROL_WIDTH = (int) (screenSize.getWidth() * (1 - ratio)); 58 | PNL_CHART_HEIGHT = PNL_CONTROL_HEIGHT = (int) screenSize.getHeight() - 120; 59 | 60 | super.loadProperties(); 61 | 62 | return true; 63 | } 64 | 65 | @Override 66 | public void saveProperties() { 67 | pnlControl.saveProperties(); 68 | super.saveProperties(); 69 | } 70 | 71 | 72 | public static void main(String[] args) { 73 | JFrame frame = new JFrame(); 74 | frame.setDefaultCloseOperation(EXIT_ON_CLOSE); 75 | JDesktopPane desk = new JDesktopPane(); 76 | frame.setContentPane(desk); 77 | 78 | PIDControllerView view1 = new PIDControllerView(); 79 | 80 | desk.add(view1); 81 | view1.setVisible(true); 82 | // Dimension size = Toolkit.getDefaultToolkit().getScreenSize(); 83 | // frame.setLocation((int) size.getWidth() - 600, (int) size.getHeight() 84 | // - 800); 85 | // frame.setSize(600, 700); 86 | frame.setExtendedState(Frame.MAXIMIZED_BOTH); 87 | frame.setVisible(true); 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/histogram/HistogramSimulator.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.histogram; 2 | 3 | public class HistogramSimulator implements Runnable { 4 | 5 | int timeDelay = 1000; 6 | boolean running = false; 7 | Thread currentThread = null; 8 | public final static int SENSE = 0, MOVE = 1; 9 | int[][] commands = new int[0][0]; 10 | int currentMove = 0; 11 | int count = 0;// How many moves you want to run 12 | 13 | HistogramFilter filter; 14 | HistogramFilterView output; 15 | 16 | public HistogramSimulator(HistogramFilterView output, HistogramFilter filter) { 17 | this.output = output; 18 | this.filter = filter; 19 | } 20 | 21 | @Override 22 | public void run() { 23 | setRunning(true); 24 | int i = 0; 25 | while (isRunning() && currentMove < commands.length && i < count) { 26 | i++; 27 | // if command is SENSE then perform sense 28 | if (commands[currentMove][1] == SENSE) { 29 | output.showOutPut( 30 | currentMove + " Sense >> " + HistogramFilterView.sensorNames[commands[currentMove][0]]); 31 | filter.sense(commands[currentMove][0]); 32 | } else if (commands[currentMove][1] == MOVE) { 33 | output.showOutPut( 34 | currentMove + " Move >> " + HistogramFilterView.btnNames[commands[currentMove][0]]); 35 | filter.move(commands[currentMove][0]); 36 | } 37 | currentMove++; 38 | output.repaint(); 39 | 40 | try { 41 | Thread.sleep(timeDelay); 42 | } catch (InterruptedException e) { 43 | } 44 | } 45 | if (currentMove == commands.length) { 46 | currentMove = 0; 47 | count = 0; 48 | output.showOutPut("----------------------------------------------"); 49 | output.rbStart.setSelected(false); 50 | output.rbStop.setSelected(true); 51 | output.btnNext.setEnabled(true); 52 | output.btnBuildSimulation.setEnabled(true); 53 | output.btnResetSimulation.setEnabled(true); 54 | 55 | } 56 | 57 | currentThread = null; 58 | } 59 | 60 | private boolean start() { 61 | if (currentThread == null) { 62 | currentThread = new Thread(this); 63 | currentThread.start(); 64 | return true; 65 | } 66 | return false; 67 | } 68 | 69 | public void simulate() { 70 | count = commands.length - currentMove; 71 | start(); 72 | } 73 | 74 | public void nextStep() { 75 | count = 1; 76 | start(); 77 | } 78 | 79 | public void setCommands(int[][] commands) { 80 | this.count = commands.length; 81 | this.commands = commands; 82 | } 83 | 84 | public void reset() { 85 | filter.resetBelief(); 86 | count = 0; 87 | currentMove = 0; 88 | } 89 | 90 | public synchronized void setRunning(boolean b) { 91 | running = b; 92 | } 93 | 94 | public synchronized boolean isRunning() { 95 | return running; 96 | } 97 | 98 | } 99 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/StringValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import pk.com.habsoft.robosim.filters.core.Attribute; 4 | 5 | /** 6 | * This class provides a value for a string. 7 | * 8 | * @author Greg Yauney (gyauney) 9 | * 10 | */ 11 | public class StringValue extends OOMDPValue implements Value { 12 | private static final String UNSET = ""; 13 | /** 14 | * The string value 15 | */ 16 | protected final String stringVal; 17 | 18 | /** 19 | * Initializes for a given attribute. The default value will be set to 0. 20 | * 21 | * @param attribute 22 | */ 23 | public StringValue(Attribute attribute) { 24 | super(attribute); 25 | this.stringVal = UNSET; 26 | } 27 | 28 | /** 29 | * Initializes from an existing value. 30 | * 31 | * @param v 32 | * the value to copy 33 | */ 34 | public StringValue(StringValue v) { 35 | super(v); 36 | this.stringVal = v.stringVal; 37 | } 38 | 39 | public StringValue(Attribute attribute, String stringVal) { 40 | super(attribute); 41 | this.stringVal = stringVal; 42 | } 43 | 44 | @Override 45 | public Value copy() { 46 | return new StringValue(this); 47 | } 48 | 49 | @Override 50 | public boolean valueHasBeenSet() { 51 | return true; 52 | } 53 | 54 | @Override 55 | public Value setValue(int v) { 56 | return new StringValue(this.attribute, Integer.toString(v)); 57 | } 58 | 59 | @Override 60 | public Value setValue(double v) { 61 | return new StringValue(this.attribute, Double.toString(v)); 62 | } 63 | 64 | @Override 65 | public Value setValue(String v) { 66 | return new StringValue(this.attribute, v); 67 | } 68 | 69 | @Override 70 | public StringBuilder buildStringVal(StringBuilder builder) { 71 | return builder.append(this.stringVal); 72 | } 73 | 74 | @Override 75 | public int hashCode() { 76 | final int prime = 31; 77 | int result = 1; 78 | result = prime * result + ((stringVal == null) ? 0 : stringVal.hashCode()); 79 | return result; 80 | } 81 | 82 | @Override 83 | public boolean equals(Object obj) { 84 | if (this == obj) { 85 | return true; 86 | } 87 | if (!(obj instanceof StringValue)) { 88 | return false; 89 | } 90 | 91 | StringValue o = (StringValue) obj; 92 | 93 | if (!o.attribute.equals(attribute)) { 94 | return false; 95 | } 96 | 97 | return this.stringVal.equals(o.stringVal); 98 | 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/actions/SADomain.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.actions; 2 | 3 | import java.util.ArrayList; 4 | import java.util.HashMap; 5 | import java.util.List; 6 | import java.util.Map; 7 | 8 | import pk.com.habsoft.robosim.filters.core.Domain; 9 | 10 | /** 11 | * A domain subclass for single agent domains. This class adds data structures 12 | * to index the actions that can be taken by the agent in the domain. 13 | * 14 | * @author James MacGlashan 15 | * 16 | */ 17 | public class SADomain extends Domain { 18 | 19 | protected List actions; // list of actions 20 | protected Map actionMap; // lookup actions by name 21 | 22 | public SADomain() { 23 | super(); 24 | actions = new ArrayList(); 25 | actionMap = new HashMap(); 26 | } 27 | 28 | /** 29 | * Clears all action observers for all actions in this domain and then sets 30 | * them to have the single action observer provided 31 | * 32 | * @param observer 33 | * the single action observer to set all actions to use. 34 | */ 35 | public void setActionObserverForAllAction(ActionObserver observer) { 36 | for (Action a : this.actions) { 37 | a.clearAllActionsObservers(); 38 | a.addActionObserver(observer); 39 | } 40 | } 41 | 42 | /** 43 | * Adss the action observer to all actions associated with this domain. 44 | * Actions added to this domain after this method is called will have to 45 | * have the observer set for them independently or by a subsequent call to 46 | * this method. 47 | * 48 | * @param observer 49 | * the observer to set all actions to use. 50 | */ 51 | public void addActionObserverForAllAction(ActionObserver observer) { 52 | for (Action a : this.actions) { 53 | a.addActionObserver(observer); 54 | } 55 | } 56 | 57 | /** 58 | * Clears all action observers for all action in this domain. 59 | */ 60 | public void clearAllActionObserversForAllActions() { 61 | for (Action a : this.actions) { 62 | a.clearAllActionsObservers(); 63 | } 64 | } 65 | 66 | @Override 67 | public void addAction(Action act) { 68 | if (!actionMap.containsKey(act.getName())) { 69 | actions.add(act); 70 | actionMap.put(act.getName(), act); 71 | } 72 | } 73 | 74 | @Override 75 | public List getActions() { 76 | return new ArrayList(actions); 77 | } 78 | 79 | @Override 80 | public Action getAction(String name) { 81 | return actionMap.get(name); 82 | } 83 | 84 | @Override 85 | protected Domain newInstance() { 86 | return new SADomain(); 87 | } 88 | 89 | } 90 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/GradientDescent.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import java.util.ArrayList; 4 | import java.util.List; 5 | 6 | import pk.com.habsoft.robosim.planning.internal.PathNode; 7 | 8 | public class GradientDescent { 9 | 10 | private GradientDescent() { 11 | 12 | } 13 | 14 | public static void main(String[] args) { 15 | List path = new ArrayList(); 16 | path.add(new PathNode(0, 0)); 17 | path.add(new PathNode(0, 1)); 18 | path.add(new PathNode(0, 2)); 19 | path.add(new PathNode(1, 2)); 20 | path.add(new PathNode(2, 2)); 21 | path.add(new PathNode(3, 2)); 22 | path.add(new PathNode(4, 2)); 23 | path.add(new PathNode(4, 3)); 24 | path.add(new PathNode(4, 4)); 25 | 26 | // List newPath = smooth(path, 0.5, 0.1, 0.000001); 27 | List newPath = smooth(path, 0.5, 0.1, 0.0000001, 10000, false); 28 | 29 | for (int i = 0; i < path.size(); i++) { 30 | System.out.println(path.get(i) + " : " + newPath.get(i)); 31 | } 32 | 33 | } 34 | 35 | public static List smooth(List pathList, double weightData, double weightSmooth, 36 | double tolerance, int timeout, boolean smoothBoundries) { 37 | double[][] path = new double[pathList.size()][2]; 38 | double[][] newpath = new double[pathList.size()][2]; 39 | for (int i = 0; i < pathList.size(); i++) { 40 | PathNode node = pathList.get(i); 41 | newpath[i][0] = node.getX(); 42 | newpath[i][1] = node.getY(); 43 | path[i][0] = node.getX(); 44 | path[i][1] = node.getY(); 45 | } 46 | 47 | double change = tolerance; 48 | int turns = 0; 49 | while (change >= tolerance) { 50 | turns++; 51 | if (turns > timeout) { 52 | System.out.println("Timeout"); 53 | break; 54 | } 55 | change = 0.0; 56 | 57 | // do not smooth 1st and last index 58 | for (int i = smoothBoundries ? 0 : 1; i < (smoothBoundries ? newpath.length : newpath.length - 1); i++) { 59 | for (int j = 0; j < newpath[i].length; j++) { 60 | double aux = newpath[i][j]; 61 | // here we should apply simultaneous update 62 | double alphaValue = weightData * ((path[i][j]) - newpath[i][j]); 63 | double beetaValue = 0; 64 | if (smoothBoundries) { 65 | if (i < newpath.length - 1) 66 | beetaValue = weightSmooth * (newpath[i + 1][j] - newpath[i][j]); 67 | } else { 68 | beetaValue = weightSmooth * (newpath[i - 1][j] + newpath[i + 1][j] - (2.0 * newpath[i][j])); 69 | } 70 | newpath[i][j] += alphaValue + beetaValue; 71 | change += Math.abs(aux - newpath[i][j]); 72 | } 73 | } 74 | } 75 | 76 | // System.out.println("Path smoothed in Turns = " + turns); 77 | 78 | List newPathList = new ArrayList(pathList.size()); 79 | for (int i = 0; i < newpath.length; i++) { 80 | newPathList.add(new PathNode(newpath[i][0], newpath[i][1])); 81 | // System.out.println(i + " " + newPathList.get(i)); 82 | } 83 | 84 | return newPathList; 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/histogram/RobotBeliefMap.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.histogram; 2 | 3 | import java.awt.Color; 4 | import java.awt.Font; 5 | import java.awt.FontMetrics; 6 | import java.awt.Graphics; 7 | import java.awt.Graphics2D; 8 | import java.awt.Image; 9 | import java.awt.image.BufferedImage; 10 | 11 | import pk.com.habsoft.robosim.internal.RPanel; 12 | 13 | public class RobotBeliefMap extends RPanel { 14 | 15 | /** 16 | * 17 | */ 18 | private static final long serialVersionUID = 1L; 19 | 20 | private static int spacing = 1; // pixel 21 | 22 | private double dWidth; 23 | private double dHeight; 24 | 25 | private Image image; 26 | HistogramFilterAdvView view; 27 | 28 | public RobotBeliefMap(HistogramFilterAdvView view, double width, double height, String label) { 29 | super(width, height, label); 30 | this.view = view; 31 | this.dWidth = width; 32 | this.dHeight = height; 33 | 34 | image = new BufferedImage((int) width, (int) height, BufferedImage.BITMASK); 35 | } 36 | 37 | public void paint(Graphics g) { 38 | super.paint(g); 39 | Graphics2D graphics = (Graphics2D) image.getGraphics(); 40 | 41 | int rows = view.world.length; 42 | int cols = view.world[0].length; 43 | int cellWidth = (int) dWidth / cols; 44 | int cellHeight = (int) dHeight / rows; 45 | 46 | graphics.setBackground(Color.red); 47 | graphics.setPaint(Color.RED); 48 | for (int i = 0; i < rows; i++) { 49 | for (int j = 0; j < cols; j++) { 50 | 51 | graphics.setPaint(Color.WHITE); 52 | graphics.fillRect(j * cellHeight, i * cellWidth, cellHeight, cellWidth); 53 | 54 | double intensity = Double.parseDouble(view.filter.getProbabilityAt(i, j)); 55 | // Paint p = new Color(0, 0, 0, (int) (255 * intensity)); 56 | // graphics.setPaint(p); 57 | Color p = new Color(200, 0, 0); 58 | graphics.setPaint(new Color(p.getRed(), p.getGreen(), p.getBlue(), 55 + (int) (200 * intensity))); 59 | 60 | graphics.fillRect(j * cellWidth + spacing, i * cellWidth + spacing, cellWidth - spacing, cellWidth 61 | - spacing); 62 | 63 | graphics.setColor(Color.BLACK); 64 | graphics.setFont(new Font("Arial", Font.BOLD, Math.min(cellWidth, cellWidth) / 4)); 65 | String str = String.valueOf(view.filter.getProbabilityAt(i, j)); 66 | 67 | FontMetrics matrix = graphics.getFontMetrics(); 68 | int ht = matrix.getAscent(); 69 | int wd = matrix.stringWidth(str); 70 | 71 | // Draw Image in center of the cell 72 | graphics.drawString(str, (j * (cellWidth) + cellWidth / 2 - wd / 2), 73 | (i * (cellWidth) + cellWidth / 2 + ht / 2)); 74 | 75 | } 76 | } 77 | 78 | g.drawImage(image, 0, LABEL_HEIGHT, this); 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/common/StatisticsPanel.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this template, choose Tools | Templates 3 | * and open the template in the editor. 4 | */ 5 | 6 | package pk.com.habsoft.robosim.planning.common; 7 | 8 | import java.awt.BorderLayout; 9 | import java.awt.Color; 10 | import java.awt.GridLayout; 11 | 12 | import javax.swing.JLabel; 13 | import javax.swing.JPanel; 14 | 15 | import pk.com.habsoft.robosim.internal.RPanel; 16 | import pk.com.habsoft.robosim.planning.algos.Algorithm; 17 | import pk.com.habsoft.robosim.planning.internal.AlgorithmListener; 18 | import pk.com.habsoft.robosim.utils.Util; 19 | 20 | public class StatisticsPanel extends RPanel implements AlgorithmListener { 21 | 22 | private static final long serialVersionUID = 1L; 23 | 24 | String strExplored = " Explored", strBlocked = "Blocked", strUnExplored = " Un-Explored", strTotal = " Total", 25 | strPathSize = " Path Length", strResult = " Result", strInstances = " Total Instances"; 26 | JLabel lblExplored, lblBlocked, lblUnExplored, lblTotal, lblPathSize, lblResult, lblInstances; 27 | 28 | public StatisticsPanel(int width, int height, String label) { 29 | super(width, height, label); 30 | 31 | setLayoutMgr(new BorderLayout()); 32 | 33 | JPanel pnl = new JPanel(); 34 | createOutputPanelContents(pnl); 35 | 36 | add(pnl, BorderLayout.NORTH); 37 | } 38 | 39 | private void createOutputPanelContents(JPanel pnlOutput) { 40 | 41 | pnlOutput.setLayout(new GridLayout(4, 2, 5, 3)); 42 | 43 | pnlOutput.add(new JLabel(strExplored)); 44 | lblExplored = new JLabel(); 45 | pnlOutput.add(lblExplored); 46 | pnlOutput.add(new JLabel(strBlocked)); 47 | lblBlocked = new JLabel(); 48 | pnlOutput.add(lblBlocked); 49 | pnlOutput.add(new JLabel(strUnExplored)); 50 | lblUnExplored = new JLabel(); 51 | pnlOutput.add(lblUnExplored); 52 | pnlOutput.add(new JLabel(strTotal)); 53 | lblTotal = new JLabel(); 54 | pnlOutput.add(lblTotal); 55 | pnlOutput.add(new JLabel(strPathSize)); 56 | lblPathSize = new JLabel(); 57 | pnlOutput.add(lblPathSize); 58 | pnlOutput.add(new JLabel(strInstances)); 59 | lblInstances = new JLabel(); 60 | pnlOutput.add(lblInstances); 61 | pnlOutput.add(new JLabel(strResult)); 62 | lblResult = new JLabel(); 63 | pnlOutput.add(lblResult); 64 | lblResult.setForeground(Color.RED); 65 | 66 | } 67 | 68 | @Override 69 | public void algorithmUpdate(Algorithm algorithm) { 70 | if (algorithm != null) { 71 | lblExplored.setText(Util.padRight(Integer.toString(algorithm.getExploredSize()), 20)); 72 | lblUnExplored.setText(Util.padRight(Integer.toString(algorithm.getUnExploredSize()), 20)); 73 | lblBlocked.setText(Util.padRight(Integer.toString(algorithm.getBlockedSize()), 20)); 74 | lblTotal.setText(Util.padRight(Integer.toString(algorithm.getTotalSize()), 20)); 75 | lblPathSize.setText(Util.padRight(Integer.toString(algorithm.getPathSize()), 20)); 76 | lblResult.setText(Util.padRight(algorithm.getResult(), 20)); 77 | lblInstances.setText(Util.padRight(Integer.toString(algorithm.getTotalInstances()), 20)); 78 | } else { 79 | lblExplored.setText("0"); 80 | lblUnExplored.setText("0"); 81 | lblBlocked.setText("0"); 82 | lblTotal.setText("0"); 83 | lblPathSize.setText("0"); 84 | lblResult.setText("NA"); 85 | lblTotal.setText("0"); 86 | } 87 | 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/objects/GridRobot.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.objects; 2 | 3 | import java.util.ArrayList; 4 | import java.util.List; 5 | 6 | import pk.com.habsoft.robosim.filters.core.Attribute; 7 | import pk.com.habsoft.robosim.filters.core.ObjectClass; 8 | import pk.com.habsoft.robosim.filters.core.ObjectInstance; 9 | import pk.com.habsoft.robosim.filters.core.values.Value; 10 | 11 | public class GridRobot implements ObjectInstance { 12 | 13 | ObjectClass obClass; 14 | String objectName; 15 | protected List values; 16 | 17 | public GridRobot(ObjectClass objectClass, String objectName) { 18 | super(); 19 | this.obClass = objectClass; 20 | this.objectName = objectName; 21 | this.values = this.initializeValueObjects(); 22 | } 23 | 24 | /** 25 | * Creates a new object instance that is a deep copy of the specified object 26 | * instance's values. The object class and name is a shallow copy. 27 | * 28 | * @param o 29 | * the source object instance from which this will object will 30 | * copy. 31 | */ 32 | public GridRobot(GridRobot o) { 33 | 34 | this.obClass = o.obClass; 35 | this.objectName = o.objectName; 36 | 37 | this.values = new ArrayList(o.values); 38 | 39 | } 40 | 41 | /** 42 | * Creates new value object assignments for each of this object instance 43 | * class's attributes. 44 | */ 45 | public List initializeValueObjects() { 46 | 47 | List values = new ArrayList(obClass.numAttributes()); 48 | for (Attribute att : obClass.attributeList) { 49 | values.add(att.valueConstructor()); 50 | } 51 | return values; 52 | } 53 | 54 | @Override 55 | public String getName() { 56 | return objectName; 57 | } 58 | 59 | @Override 60 | public String getClassName() { 61 | return obClass.name; 62 | } 63 | 64 | @Override 65 | public String getObjectDescription() { 66 | return this.toString(); 67 | } 68 | 69 | @Override 70 | public void setName(String newName) { 71 | objectName = newName; 72 | } 73 | 74 | @Override 75 | public ObjectInstance copy() { 76 | return new GridRobot(this); 77 | } 78 | 79 | /** 80 | * Sets the value of the attribute named attName for this object instance. 81 | * 82 | * @param attName 83 | * the name of the attribute whose value is to be set. 84 | * @param v 85 | * the int rep value to which the attribute of this object 86 | * instance should be set. 87 | */ 88 | @Override 89 | public ObjectInstance setValue(String attName, int v) { 90 | int ind = obClass.attributeIndex(attName); 91 | Value value = values.get(ind); 92 | Value newValue = value.setValue(v); 93 | values.set(ind, newValue); 94 | return this; 95 | } 96 | 97 | /** 98 | * Returns the int value assignment for the discrete-valued attribute named 99 | * attName. Will throw a runtime exception is the attribute named attName is 100 | * not of type DISC 101 | * 102 | * @param attName 103 | * the name of the attribute whose value should be returned 104 | * @return the int value assignment for the discrete-valued attribute named 105 | * attName. 106 | */ 107 | public int getIntValForAttribute(String attName) { 108 | int ind = obClass.attributeIndex(attName); 109 | return values.get(ind).getDiscVal(); 110 | } 111 | 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/IntValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import pk.com.habsoft.robosim.filters.core.Attribute; 4 | 5 | /** 6 | * This class provides a value for an integer. The value may be negative or 7 | * postive and exist in any range. This value object should be preferred for 8 | * discrete values that span a large numeric range and for which it would be 9 | * ineffecient to store an array of all possible values in the attribtue. 10 | * Otherwise the {@link DiscreteValue} class should be preferred. 11 | * 12 | * @author James MacGlashan 13 | * 14 | */ 15 | public class IntValue extends OOMDPValue implements Value { 16 | 17 | /** 18 | * The int value 19 | */ 20 | protected final int intVal; 21 | 22 | /** 23 | * Initializes for a given attribute. The default value will be set to 0. 24 | * 25 | * @param attribute 26 | */ 27 | public IntValue(Attribute attribute) { 28 | super(attribute); 29 | this.intVal = 0; 30 | } 31 | 32 | /** 33 | * Initializes from an existing IntUnBound value. 34 | * 35 | * @param v 36 | * the value to copy 37 | */ 38 | public IntValue(IntValue v) { 39 | super(v); 40 | this.intVal = v.intVal; 41 | } 42 | 43 | public IntValue(Attribute attribute, int intVal) { 44 | super(attribute); 45 | this.intVal = intVal; 46 | } 47 | 48 | @Override 49 | public boolean valueHasBeenSet() { 50 | return true; 51 | } 52 | 53 | @Override 54 | public Value copy() { 55 | return new IntValue(this); 56 | } 57 | 58 | @Override 59 | public Value setValue(int v) { 60 | return new IntValue(this.attribute, v); 61 | } 62 | 63 | @Override 64 | public Value setValue(double v) { 65 | return new IntValue(this.attribute, (int) v); 66 | } 67 | 68 | @Override 69 | public Value setValue(String v) { 70 | return new IntValue(this.attribute, Integer.parseInt(v)); 71 | } 72 | 73 | @Override 74 | public Value setValue(boolean v) { 75 | return new IntValue(this.attribute, (v) ? 1 : 0); 76 | } 77 | 78 | @Override 79 | public int getDiscVal() { 80 | return this.intVal; 81 | } 82 | 83 | @Override 84 | public StringBuilder buildStringVal(StringBuilder builder) { 85 | return builder.append(this.intVal); 86 | } 87 | 88 | @Override 89 | public double getNumericRepresentation() { 90 | return (double) this.intVal; 91 | } 92 | 93 | @Override 94 | public int hashCode() { 95 | final int prime = 31; 96 | int result = 1; 97 | result = prime * result + intVal; 98 | return result; 99 | } 100 | 101 | @Override 102 | public boolean equals(Object obj) { 103 | if (this == obj) { 104 | return true; 105 | } 106 | 107 | if (!(obj instanceof IntValue)) { 108 | return false; 109 | } 110 | 111 | IntValue o = (IntValue) obj; 112 | 113 | if (!o.attribute.equals(attribute)) { 114 | return false; 115 | } 116 | 117 | return this.intVal == o.intVal; 118 | 119 | } 120 | 121 | @Override 122 | public boolean getBooleanValue() { 123 | return this.intVal != 0; 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /src/main/resources/config/PathSmoother.properties: -------------------------------------------------------------------------------- 1 | # 2 | #Sun Jul 26 01:46:32 PKT 2015 3 | MAP_ROW_18=0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 4 | MAP_ROW_17=0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0,0,0, 5 | MAP_ROW_16=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,1,1,0,0,1,1,0,0,0,0,0,0,0, 6 | MAP_ROW_15=0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0,0,1,1,1,1,1,1,0, 7 | MAP_ROW_14=0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1, 8 | MAP_ROW_13=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 9 | MAP_ROW_12=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 10 | ALGORITHM=3 11 | MAP_ROW_11=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 12 | MAP_ROW_10=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0, 13 | WEIGHT_SMOOTH=0.9 14 | GO_STRAIGHT_COST=0 15 | SMOOTHING_TIMEOUT=500000 16 | SHOW_SMOOTH_PATH=true 17 | NO_OF_COLUMNS=35 18 | SHOW_GRID=false 19 | LEFT_TURN_COST=0 20 | SMOOTH_BOUNDRIES=false 21 | USE_ORIENTATION=true 22 | USE_HEURISTICS=true 23 | MAP_ROW_35=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 24 | MAP_ROW_34=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 25 | MAP_ROW_33=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,1,1,1,1,1,1, 26 | MAP_ROW_32=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,1,1,1,0,0,0,0,0, 27 | MAP_ROW_31=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 28 | CELL_DIVISIONS=1 29 | MAP_ROW_30=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 30 | MAP_ROW_9=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0, 31 | MAP_ROW_8=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0, 32 | HEURISTICS=3 33 | MAP_ROW_7=1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 34 | SHOW_HEURISTICS=false 35 | MAP_ROW_6=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 36 | MAP_ROW_5=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 37 | MAP_ROW_4=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 38 | MAP_ROW_3=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 39 | MAP_ROW_2=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 40 | MAP_ROW_1=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 41 | USE_STOCHASTIC_MOTION=true 42 | RIGHT_TURN_COST=0 43 | MAP_ROW_29=0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 44 | MAP_ROW_28=0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 45 | MAP_ROW_27=0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 46 | MAP_ROW_26=1,1,1,1,1,0,0,1,1,0,0,0,0,0,0,1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0,0, 47 | MAP_ROW_25=0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0, 48 | MAP_ROW_24=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0, 49 | MAP_ROW_23=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 50 | MAP_ROW_22=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 51 | DIAGONAL_MOTION=false 52 | MAP_ROW_21=0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 53 | MAP_ROW_20=0,0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 54 | WEIGHT_DATA=0.14 55 | GOAL_NODE_TAG=34,0 56 | P_SUCCESS=0.9 57 | SHOW_ACTUAL_PATH=false 58 | NO_OF_ROWS=35 59 | START_NODE_TAG=0,0 60 | MAP_ROW_19=0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 61 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/RealValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import pk.com.habsoft.robosim.filters.core.Attribute; 4 | 5 | /** 6 | * A real-valued value subclass in which real-values are stored as doubles. 7 | * 8 | * @author James MacGlashan 9 | * 10 | */ 11 | public class RealValue extends OOMDPValue implements Value { 12 | private static final double UNSET = Double.NaN; 13 | /** 14 | * The real value stored as a double. Default value of NaN indicates that 15 | * the value is unset 16 | */ 17 | protected final double realVal; 18 | 19 | /** 20 | * Initializes this value to be an assignment for Attribute attribute. 21 | * 22 | * @param attribute 23 | */ 24 | public RealValue(Attribute attribute) { 25 | super(attribute); 26 | this.realVal = UNSET; 27 | } 28 | 29 | /** 30 | * Initializes this value as a copy from the source Value object v. 31 | * 32 | * @param v 33 | * the source Value to make this object a copy of. 34 | */ 35 | public RealValue(RealValue v) { 36 | super(v); 37 | RealValue rv = v; 38 | this.realVal = rv.realVal; 39 | } 40 | 41 | public RealValue(Attribute attribute, double realVal) { 42 | super(attribute); 43 | this.realVal = realVal; 44 | } 45 | 46 | @Override 47 | public Value copy() { 48 | return new RealValue(this); 49 | } 50 | 51 | @Override 52 | public boolean valueHasBeenSet() { 53 | return !Double.isNaN(this.realVal); 54 | } 55 | 56 | @Override 57 | public Value setValue(int v) { 58 | return new RealValue(this.attribute, v); 59 | } 60 | 61 | @Override 62 | public Value setValue(double v) { 63 | return new RealValue(this.attribute, v); 64 | } 65 | 66 | @Override 67 | public Value setValue(String v) { 68 | return new RealValue(this.attribute, Double.parseDouble(v)); 69 | } 70 | 71 | @Override 72 | public double getRealVal() { 73 | if (Double.isNaN(this.realVal)) { 74 | throw new UnsetValueException(); 75 | } 76 | return this.realVal; 77 | } 78 | 79 | @Override 80 | public StringBuilder buildStringVal(StringBuilder builder) { 81 | if (Double.isNaN(this.realVal)) { 82 | throw new UnsetValueException(); 83 | } 84 | return builder.append(this.realVal); 85 | } 86 | 87 | @Override 88 | public double getNumericRepresentation() { 89 | if (Double.isNaN(this.realVal)) { 90 | throw new UnsetValueException(); 91 | } 92 | return this.realVal; 93 | } 94 | 95 | @Override 96 | public int hashCode() { 97 | final int prime = 31; 98 | int result = 1; 99 | long temp; 100 | temp = Double.doubleToLongBits(realVal); 101 | result = prime * result + (int) (temp ^ (temp >>> 32)); 102 | return result; 103 | } 104 | 105 | @Override 106 | public boolean equals(Object obj) { 107 | if (this == obj) { 108 | return true; 109 | } 110 | if (!(obj instanceof RealValue)) { 111 | return false; 112 | } 113 | 114 | RealValue op = (RealValue) obj; 115 | if (!op.attribute.equals(attribute)) { 116 | return false; 117 | } 118 | 119 | return realVal == op.realVal; 120 | 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /pom.xml: -------------------------------------------------------------------------------- 1 | 3 | 4.0.0 4 | 5 | pk.com.habsoft 6 | robosim 7 | 0.0.1-SNAPSHOT 8 | jar 9 | 10 | robosim 11 | http://maven.apache.org 12 | 13 | 14 | UTF-8 15 | 16 | 17 | 18 | 19 | junit 20 | junit 21 | 3.8.1 22 | test 23 | 24 | 25 | org.apache.commons 26 | commons-math3 27 | 3.0 28 | 29 | 30 | jfree 31 | jcommon 32 | 1.0.16 33 | 34 | 35 | org.jfree 36 | jfreechart 37 | 1.0.14 38 | 39 | 40 | log4j 41 | log4j 42 | 1.2.17 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | org.apache.maven.plugins 51 | maven-compiler-plugin 52 | 3.3 53 | 54 | 1.7 55 | 1.7 56 | 57 | 58 | 59 | 60 | 61 | maven-resources-plugin 62 | 2.7 63 | 64 | 65 | copy-resources01 66 | process-classes 67 | 68 | copy-resources 69 | 70 | 71 | ${basedir}/target/Robosim/config 72 | UTF-8 73 | 74 | 75 | ${basedir}/src/main/resources/config 76 | 77 | **/*.properties 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | maven-assembly-plugin 89 | 90 | 91 | 92 | pk.com.habsoft.robosim.main.RoboSim 93 | 94 | 95 | 96 | jar-with-dependencies 97 | 98 | false 99 | 100 | 101 | 102 | make-assembly 103 | package 104 | 105 | single 106 | 107 | 108 | ${basedir}/target/Robosim 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/TestDP.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import java.util.Arrays; 4 | 5 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 6 | 7 | public class TestDP { 8 | 9 | public static void main(String[] args) { 10 | TestDP d = new TestDP(); 11 | d.search(); 12 | } 13 | 14 | int[][] grid = { { 1, 1, 1, 0, 0, 0 }, { 1, 1, 1, 0, 1, 0 }, { 0, 0, 0, 0, 0, 0 }, { 1, 1, 1, 0, 1, 1 }, 15 | { 1, 1, 1, 0, 1, 1 } }; 16 | 17 | int[] goal = { 2, 0 }; 18 | int[] init = { 4, 3, 0 }; 19 | 20 | int[][] forward = { { -1, 0 }, // go up 21 | { 0, -1 }, // go left 22 | { 1, 0 }, // go down 23 | { 0, 1 } };// go right 24 | // String[] forward_names = { "up", "left", "down", "right" }; 25 | char[] forward_names = { 'U', 'L', 'D', 'R' }; 26 | 27 | int[] cost = { 1, 1, 12 }; 28 | int[] action = { -1, 0, 1 }; 29 | char[] action_names = { 'R', '#', 'L' }; 30 | 31 | void search() { 32 | 33 | int[][][] value = new int[4][grid.length][grid[0].length]; 34 | char[][][] policy = new char[4][grid.length][grid[0].length]; 35 | char[][] policy2D = new char[grid.length][grid[0].length]; 36 | char[][] pol = new char[grid.length][grid[0].length]; 37 | for (int i = 0; i < 4; i++) { 38 | for (int j = 0; j < grid.length; j++) { 39 | for (int k = 0; k < grid[j].length; k++) { 40 | value[i][j][k] = 9999; 41 | policy[i][j][k] = ' '; 42 | policy2D[j][k] = ' '; 43 | pol[j][k] = ' '; 44 | } 45 | } 46 | } 47 | boolean change = true; 48 | while (change) { 49 | change = false; 50 | for (int x = 0; x < grid.length; x++) { 51 | for (int y = 0; y < grid[x].length; y++) { 52 | for (int j = 0; j < forward.length; j++) { 53 | if (goal[0] == x && goal[1] == y) { 54 | if (value[j][x][y] > 0) { 55 | value[j][x][y] = 0; 56 | policy[j][x][y] = '*'; 57 | change = true; 58 | } 59 | } else if (grid[x][y] == 0) { 60 | for (int i = 0; i < action.length; i++) { 61 | int o2 = RoboMathUtils.modulus((j + action[i]), 4, false); 62 | int x2 = x + forward[o2][0]; 63 | int y2 = y + forward[o2][1]; 64 | if (x2 >= 0 && x2 < grid.length && y2 >= 0 && y2 < grid[0].length 65 | && grid[x2][y2] == 0) { 66 | int v2 = value[o2][x2][y2] + cost[i]; 67 | if (v2 < value[j][x][y]) { 68 | change = true; 69 | value[j][x][y] = v2; 70 | policy[j][x][y] = action_names[i]; 71 | // pol[x][y] = forward_names[j]; 72 | 73 | } 74 | } 75 | } 76 | } 77 | } 78 | } 79 | } 80 | } 81 | 82 | int x = init[0]; 83 | int y = init[1]; 84 | int orientation = init[2]; 85 | int o2 = orientation; 86 | 87 | policy2D[x][y] = policy[orientation][x][y]; 88 | // pol[x][y] = forward_names[orientation]; 89 | while (policy[orientation][x][y] != '*') { 90 | if (policy[orientation][x][y] == '#') { 91 | o2 = orientation; 92 | } else if (policy[orientation][x][y] == 'R') { 93 | o2 = RoboMathUtils.modulus((orientation - 1), 4, false); 94 | } else if (policy[orientation][x][y] == 'L') { 95 | o2 = RoboMathUtils.modulus((orientation + 1), 4, false); 96 | } 97 | pol[x][y] = forward_names[o2]; 98 | x = x + forward[o2][0]; 99 | y = y + forward[o2][1]; 100 | orientation = o2; 101 | policy2D[x][y] = policy[orientation][x][y]; 102 | } 103 | 104 | for (char[] arr : policy2D) { 105 | System.out.println(Arrays.toString(arr)); 106 | } 107 | System.out.println("-------------"); 108 | for (char[] arr : pol) { 109 | System.out.println(Arrays.toString(arr)); 110 | } 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/internal/WorldNode.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.internal; 2 | 3 | import java.util.PriorityQueue; 4 | 5 | /* 6 | * This class is used to represent a single node while searching 7 | * the world 8 | */ 9 | public class WorldNode implements Comparable { 10 | 11 | private static int instances = 0; 12 | 13 | /** 14 | * x,y location of this node in the worlds 15 | */ 16 | private int xLoc, yLoc; 17 | /** 18 | * Heuristic value to be used for algorithm. 19 | */ 20 | private double heuristic = 0; 21 | private int depth = 0; 22 | // private int cost = 0; 23 | /** 24 | * By following which action this node is populated. To trace back the path. 25 | */ 26 | private int action = -1; 27 | 28 | public WorldNode(int xLoc, int yLoc) { 29 | this.xLoc = xLoc; 30 | this.yLoc = yLoc; 31 | instances++; 32 | } 33 | 34 | public WorldNode(WorldNode node) { 35 | setxLoc(node.getxLoc()); 36 | setyLoc(node.getyLoc()); 37 | setDepth(node.getDepth()); 38 | setAction(node.getAction()); 39 | } 40 | 41 | public static int getInstances() { 42 | return instances; 43 | } 44 | 45 | public int getxLoc() { 46 | return xLoc; 47 | } 48 | 49 | public void setxLoc(int xLoc) { 50 | this.xLoc = xLoc; 51 | } 52 | 53 | public int getyLoc() { 54 | return yLoc; 55 | } 56 | 57 | public void setyLoc(int yLoc) { 58 | this.yLoc = yLoc; 59 | } 60 | 61 | public double getHeuristic() { 62 | return heuristic; 63 | } 64 | 65 | public void setHeuristic(double heuristic) { 66 | this.heuristic = heuristic; 67 | } 68 | 69 | public int getDepth() { 70 | return depth; 71 | } 72 | 73 | public void setDepth(int depth) { 74 | this.depth = depth; 75 | } 76 | 77 | public double getCost() { 78 | return this.depth + this.heuristic; 79 | } 80 | 81 | public int getAction() { 82 | return action; 83 | } 84 | 85 | public void setAction(int action) { 86 | this.action = action; 87 | } 88 | 89 | @Override 90 | public String toString() { 91 | return "WorldState [xLoc=" + xLoc + ", yLoc=" + yLoc + ", heuristic=" + heuristic + ", depth=" + depth 92 | + ", Cost=" + getCost() + ", action=" + action + "]"; 93 | } 94 | 95 | @Override 96 | public boolean equals(Object obj) { 97 | if(obj == null) 98 | return false; 99 | if(this.getClass() != obj.getClass()) 100 | return false; 101 | WorldNode that = (WorldNode) obj; 102 | return this.xLoc == that.xLoc && this.yLoc == that.yLoc; 103 | } 104 | 105 | public int hashCode() { 106 | final int prime = 31; 107 | int result = 1; 108 | result = prime * result + action; 109 | result = prime * result + depth; 110 | long temp; 111 | temp = Double.doubleToLongBits(heuristic); 112 | result = prime * result + (int) (temp ^ (temp >>> 32)); 113 | result = prime * result + xLoc; 114 | result = prime * result + yLoc; 115 | return result; 116 | } 117 | 118 | @Override 119 | public int compareTo(WorldNode that) { 120 | int returnValue; 121 | // f = g + h 122 | 123 | if (this.getCost() == that.getCost()) 124 | returnValue = 0; 125 | else if (this.getCost() < that.getCost()) 126 | returnValue = -1; 127 | else 128 | returnValue = 1; 129 | return returnValue; 130 | } 131 | 132 | public static void main(String[] args) { 133 | PriorityQueue q = new PriorityQueue(); 134 | for (int i = 0; i < 5; i++) { 135 | WorldNode s = new WorldNode(i, i); 136 | s.setHeuristic((double)5 - i); 137 | s.setDepth(i * 2); 138 | q.add(s); 139 | System.out.println("Adding " + s); 140 | } 141 | while (!q.isEmpty()) { 142 | System.out.println("Poll " + q.poll()); 143 | } 144 | } 145 | } 146 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/smoothing/controller/PIDController.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Player Java Client 2 - PIDController.java 3 | * Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | * $Id: PIDController.java,v 1.3 2006/03/06 08:33:31 veedee Exp $ 20 | * 21 | */ 22 | package pk.com.habsoft.robosim.smoothing.controller; 23 | 24 | /** 25 | * Proportional-Integral-Derivative controller implementation. 26 | * 27 | * @author Radu Bogdan Rusu 28 | */ 29 | public class PIDController extends Controller { 30 | 31 | /** Proportional constant */ 32 | protected double kp; 33 | /** Integral constant */ 34 | protected double ki; 35 | /** Derivative constant */ 36 | protected double kd; 37 | 38 | /** 39 | * Constructor for PIDController. 40 | * 41 | * @param Kp 42 | * the proportional constant 43 | * @param Ki 44 | * the integral constant 45 | * @param Kd 46 | * the derivative constant 47 | */ 48 | public PIDController(double Kp, double Ki, double Kd) { 49 | this.kp = Kp; 50 | this.ki = Ki; 51 | this.kd = Kd; 52 | } 53 | 54 | /** 55 | * Calculate and return the controller's command for the controlled system. 56 | * 57 | * @param currentOutput 58 | * the current output of the system 59 | * @return the new calculated command for the system 60 | */ 61 | public double getCommand(double currentOutput) { 62 | this.currE = this.goal - currentOutput; 63 | eSum += currE; 64 | 65 | double Pgain = this.kp * currE; 66 | double Igain = this.ki * eSum; 67 | double Dgain = this.kd * deltaE(); 68 | 69 | // System.out.println("P " + Pgain + " , D " + Dgain + " , I " + Igain); 70 | 71 | lastE = currE; 72 | 73 | return Pgain + Igain + Dgain; 74 | } 75 | 76 | /** 77 | * Get the current value of the proportional constant. 78 | * 79 | * @return Kp as a double 80 | */ 81 | public double getKp() { 82 | return this.kp; 83 | } 84 | 85 | /** 86 | * Set a new value for the proportional constant. 87 | * 88 | * @param newKp 89 | * the new value for Kp 90 | */ 91 | public void setKp(double newKp) { 92 | this.kp = newKp; 93 | } 94 | 95 | /** 96 | * Get the current value of the integral constant. 97 | * 98 | * @return Ki as a double 99 | */ 100 | public double getKi() { 101 | return this.ki; 102 | } 103 | 104 | /** 105 | * Set a new value for the integral constant. 106 | * 107 | * @param newKi 108 | * the new value for Ki 109 | */ 110 | public void setKi(double newKi) { 111 | this.ki = newKi; 112 | } 113 | 114 | /** 115 | * Get the current value of the derivative constant. 116 | * 117 | * @return Kd as a double 118 | */ 119 | public double getKd() { 120 | return this.kd; 121 | } 122 | 123 | /** 124 | * Set a new value for the derivative constant. 125 | * 126 | * @param newKd 127 | * the new value for Kd 128 | */ 129 | public void setKd(double newKd) { 130 | this.kd = newKd; 131 | } 132 | } 133 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/KalmanFilter.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | import org.apache.commons.math3.linear.Array2DRowRealMatrix; 4 | import org.apache.commons.math3.linear.LUDecomposition; 5 | import org.apache.commons.math3.linear.MatrixUtils; 6 | import org.apache.commons.math3.linear.RealMatrix; 7 | 8 | public class KalmanFilter { 9 | RealMatrix x;// initial 10 | // state(location,velocit) 11 | 12 | RealMatrix p;// initial uncertainty 13 | 14 | private RealMatrix u;// external motion 15 | 16 | private RealMatrix f;// next state function 17 | 18 | private RealMatrix h;// measurement function 19 | 20 | private RealMatrix r;// measurement uncertainty 21 | 22 | private RealMatrix identity;// identity matrix 23 | 24 | public KalmanFilter(double[][] xx, double[][] pp, double[][] uu, double[][] ff, double[][] hh, double[][] rr) { 25 | 26 | x = new Array2DRowRealMatrix(xx); 27 | p = new Array2DRowRealMatrix(pp); 28 | u = new Array2DRowRealMatrix(uu); 29 | f = new Array2DRowRealMatrix(ff); 30 | h = new Array2DRowRealMatrix(hh); 31 | r = new Array2DRowRealMatrix(rr); 32 | identity = MatrixUtils.createRealIdentityMatrix(pp[0].length); 33 | } 34 | 35 | /** 36 | * 37 | * @param measurement 38 | */ 39 | public void filter(double[] measurement) { 40 | double[][] dataZ = { measurement }; 41 | 42 | // Measurement 43 | RealMatrix z = new Array2DRowRealMatrix(dataZ); 44 | 45 | // MOTION UPDATE(PREDICT) ///////////////// 46 | // The time update projects the current state estimate ahead in time. 47 | // We are predicting the next state here 48 | // x = F(2x2) * x(2x1) + u(2x1) 49 | x = (f.multiply(x)).add(u); 50 | // P(2x2) = F(2x2) * P (2x2)* F.transpose()(2x2) 51 | p = f.multiply(p.multiply(f.transpose())); 52 | 53 | // MEASUREMENT UPDATE ////////////////////// 54 | // The measurement update adjusts the projected estimate by an actual 55 | // measurement at that time. 56 | // Y(1x1) = Z(1x1) - H(1x2) * x(2x1) 57 | // y = Measurement Error 58 | RealMatrix y = (z.transpose()).subtract(h.multiply(x)); 59 | // S(1x1) = H(1x2) * P(2x2) * H.transpose()(2x1) + R(1x1) 60 | RealMatrix s = (h.multiply(p)).multiply(h.transpose()).add(r); 61 | RealMatrix sInverse = new LUDecomposition(s).getSolver().getInverse(); 62 | // K(2x1) = P(2x2) * H.transpose()(2x1) * S.invers()(1x1) 63 | // k = Kalman Gain 64 | RealMatrix k = (p.multiply(h.transpose())).multiply(sInverse); 65 | // x(2x1) = x(2x1) + K(2x1) * Y(1x1) 66 | x = x.add(k.multiply(y)); 67 | // P(2x2) =(I(2x2) - K(2x1) * H(1x2)) * P(2x2) 68 | p = (identity.subtract(k.multiply(h))).multiply(p); 69 | 70 | // System.out.println("X = "); 71 | // Util.printMatrix(x); 72 | // System.out.println("P = "); 73 | // Util.printMatrix(p); 74 | 75 | } 76 | 77 | /** 78 | * 79 | * @param xx 80 | * (initial state) 81 | * @param pp 82 | * (initial uncertainty) 83 | * @param uu 84 | * (external motion) 85 | * @param ff 86 | * (next state function) 87 | * @param hh 88 | * (measurement function) 89 | * @param rr 90 | * (measurement uncertainty) 91 | */ 92 | public void setData(double[][] xx, double[][] pp, double[][] uu, double[][] ff, double[][] hh, double[][] rr) { 93 | x = new Array2DRowRealMatrix(xx); 94 | p = new Array2DRowRealMatrix(pp); 95 | u = new Array2DRowRealMatrix(uu); 96 | f = new Array2DRowRealMatrix(ff); 97 | h = new Array2DRowRealMatrix(hh); 98 | r = new Array2DRowRealMatrix(rr); 99 | identity = MatrixUtils.createRealIdentityMatrix(pp[0].length); 100 | } 101 | 102 | public RealMatrix getX() { 103 | return new Array2DRowRealMatrix(x.getData()); 104 | } 105 | 106 | public RealMatrix getP() { 107 | return new Array2DRowRealMatrix(p.getData()); 108 | } 109 | 110 | } 111 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/TestDPPP.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import java.util.Arrays; 4 | 5 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 6 | 7 | public class TestDPPP { 8 | 9 | public static void main(String[] args) { 10 | TestDPPP d = new TestDPPP(); 11 | d.search(); 12 | } 13 | 14 | int[][] grid = { { 1, 1, 1, 0, 0, 0 }, { 1, 1, 1, 0, 1, 0 }, { 0, 0, 0, 0, 0, 0 }, { 1, 1, 1, 0, 1, 1 }, 15 | { 1, 1, 1, 0, 1, 1 } }; 16 | 17 | int[] goal = { 2, 0 }; 18 | int[] init = { 4, 3, 0 }; 19 | 20 | int[][] forward = { { -1, 0 }, // go up 21 | { 0, -1 }, // go left 22 | { 1, 0 }, // go down 23 | { 0, 1 } };// go right 24 | // String[] forward_names = { "up", "left", "down", "right" }; 25 | // char[] forward_names = { 'U', 'L', 'D', 'R' }; 26 | 27 | int[] cost = { 1, 1, 15 }; 28 | int[] action = { -1, 0, 1 }; 29 | char[] action_names = { 'R', '#', 'L' }; 30 | int g = -100; 31 | 32 | void search() { 33 | 34 | int[][][] value = new int[4][grid.length][grid[0].length]; 35 | int[][][] policy3D = new int[4][grid.length][grid[0].length]; 36 | // int[][] policy2D = new int[grid.length][grid[0].length]; 37 | int[][] policy = new int[grid.length][grid[0].length]; 38 | for (int i = 0; i < 4; i++) { 39 | for (int j = 0; j < grid.length; j++) { 40 | for (int k = 0; k < grid[j].length; k++) { 41 | value[i][j][k] = 9999; 42 | policy3D[i][j][k] = 0; 43 | // policy2D[j][k] = -2; 44 | policy[j][k] = Algorithm.NOT_EXPLORED; 45 | } 46 | } 47 | } 48 | boolean change = true; 49 | while (change) { 50 | change = false; 51 | for (int x = 0; x < grid.length; x++) { 52 | for (int y = 0; y < grid[x].length; y++) { 53 | for (int j = 0; j < forward.length; j++) { 54 | if (goal[0] == x && goal[1] == y) { 55 | if (value[j][x][y] > 0) { 56 | value[j][x][y] = 0; 57 | policy3D[j][x][y] = g; 58 | change = true; 59 | } 60 | } else if (grid[x][y] == 0) { 61 | for (int i = 0; i < action.length; i++) { 62 | int o2 = RoboMathUtils.modulus((j + action[i]), 4, false); 63 | int x2 = x + forward[o2][0]; 64 | int y2 = y + forward[o2][1]; 65 | if (x2 >= 0 && x2 < grid.length && y2 >= 0 && y2 < grid[0].length 66 | && grid[x2][y2] == 0) { 67 | int v2 = value[o2][x2][y2] + cost[i]; 68 | if (v2 < value[j][x][y]) { 69 | change = true; 70 | value[j][x][y] = v2; 71 | policy3D[j][x][y] = i; 72 | 73 | } 74 | } 75 | } 76 | } 77 | } 78 | } 79 | } 80 | } 81 | 82 | int x = init[0]; 83 | int y = init[1]; 84 | int orientation = init[2]; 85 | int o2; 86 | 87 | // policy2D[x][y] = policy[orientation][x][y]; 88 | // pol[x][y] = forward_names[orientation]; 89 | while (policy3D[orientation][x][y] != g) { 90 | // if (action_names[policy[orientation][x][y]] == '#') { 91 | // o2 = orientation; 92 | // } else if (action_names[policy[orientation][x][y]] == 'R') { 93 | // o2 = Util.modulus((orientation - 1), 4, false); 94 | // } else if (action_names[policy[orientation][x][y]] == 'L') { 95 | // o2 = Util.modulus((orientation + 1), 4, false); 96 | // } 97 | o2 = RoboMathUtils.modulus((orientation + action[policy3D[orientation][x][y]]), 4, false); 98 | policy[x][y] = o2; 99 | x = x + forward[o2][0]; 100 | y = y + forward[o2][1]; 101 | orientation = o2; 102 | // if (policy[orientation][x][y]<4 && policy[orientation][x][y]>=0) 103 | // { 104 | // policy2D[x][y] = action[policy[orientation][x][y]]; 105 | // } 106 | } 107 | 108 | // for (int[] arr : policy2D) { 109 | // System.out.println(Arrays.toString(arr)); 110 | // } 111 | System.out.println("-------------"); 112 | for (int[] arr : policy) { 113 | System.out.println(Arrays.toString(arr)); 114 | } 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/RelationalValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import java.util.Set; 4 | import java.util.TreeSet; 5 | 6 | import pk.com.habsoft.robosim.filters.core.Attribute; 7 | 8 | /** 9 | * A relational valued value subclass in which values are stored as a single 10 | * String object for the name of the object instance to which it is linked. If 11 | * the relational value is not linked to any object, then the String value is 12 | * set to the empty String: "". 13 | * 14 | * @author James MacGlashan 15 | * 16 | */ 17 | public class RelationalValue extends OOMDPValue implements Value { 18 | private static final String UNSET = ""; 19 | /** 20 | * A string representing the object target of this value. Targets are 21 | * specified by the object name identifier. If the relational target is 22 | * unset, then this value will be set to the empty string "", which is the 23 | * default value. 24 | */ 25 | protected final String target; 26 | 27 | /** 28 | * Initializes this value to be an assignment for Attribute attribute. 29 | * 30 | * @param attribute 31 | */ 32 | public RelationalValue(Attribute attribute) { 33 | super(attribute); 34 | this.target = UNSET; 35 | } 36 | 37 | /** 38 | * Initializes this value as a copy from the source Value object v. 39 | * 40 | * @param v 41 | * the source Value to make this object a copy of. 42 | */ 43 | public RelationalValue(RelationalValue v) { 44 | super(v); 45 | RelationalValue rv = v; 46 | this.target = rv.target; 47 | } 48 | 49 | public RelationalValue(Attribute attribute, String target) { 50 | super(attribute); 51 | this.target = target; 52 | } 53 | 54 | @Override 55 | public Value copy() { 56 | return new RelationalValue(this); 57 | } 58 | 59 | @Override 60 | public boolean valueHasBeenSet() { 61 | return true; 62 | } 63 | 64 | @Override 65 | public Value setValue(String v) { 66 | return new RelationalValue(this.attribute, v); 67 | } 68 | 69 | @Override 70 | public Value addRelationalTarget(String t) { 71 | return new RelationalValue(this.attribute, t); 72 | } 73 | 74 | @Override 75 | public Value clearRelationTargets() { 76 | return new RelationalValue(this.attribute); 77 | } 78 | 79 | @Override 80 | public Value removeRelationalTarget(String target) { 81 | if (this.target.equals(target)) { 82 | return new RelationalValue(this.attribute); 83 | } 84 | return this; 85 | } 86 | 87 | @Override 88 | public Set getAllRelationalTargets() { 89 | Set res = new TreeSet(); 90 | res.add(this.target); 91 | return res; 92 | } 93 | 94 | @Override 95 | public StringBuilder buildStringVal(StringBuilder builder) { 96 | return builder.append(this.target); 97 | } 98 | 99 | @Override 100 | public double getNumericRepresentation() { 101 | return 0; 102 | } 103 | 104 | @Override 105 | public int hashCode() { 106 | final int prime = 31; 107 | int result = 1; 108 | result = prime * result + ((target == null) ? 0 : target.hashCode()); 109 | return result; 110 | } 111 | 112 | @Override 113 | public boolean equals(Object obj) { 114 | if (this == obj) { 115 | return true; 116 | } 117 | if (!(obj instanceof RelationalValue)) { 118 | return false; 119 | } 120 | 121 | RelationalValue op = (RelationalValue) obj; 122 | if (!op.attribute.equals(attribute)) { 123 | return false; 124 | } 125 | 126 | return this.target.equals(op.target); 127 | 128 | } 129 | 130 | } 131 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/BigRobot.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles; 2 | 3 | import pk.com.habsoft.robosim.filters.particles.internal.IRobot; 4 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 5 | 6 | public class BigRobot extends Robot { 7 | 8 | double MAX_STEERING_ANGLE = Math.PI / 4.0; 9 | 10 | public BigRobot() { 11 | super(); 12 | } 13 | 14 | public BigRobot(double length) { 15 | super(length); 16 | } 17 | 18 | public BigRobot(double length, RobotType type) { 19 | super(length, type); 20 | 21 | } 22 | 23 | public BigRobot(IRobot r) { 24 | super(r); 25 | 26 | } 27 | 28 | @Override 29 | public double[] sense(boolean addNoise) { 30 | double[] z = new double[World.getLandmark().size()]; 31 | for (int i = 0; i < z.length; i++) { 32 | LandMark landmark = World.getLandmark().get(i); 33 | double dx = landmark.getX() - this.x; 34 | double dy = landmark.getY() - this.y; 35 | double bearing = Math.atan2(dy, dx) - this.orientation; 36 | if (addNoise) 37 | bearing += RoboMathUtils.nextGaussian(0, senseNoise); 38 | bearing = RoboMathUtils.modulus(bearing, 2 * Math.PI); 39 | z[i] = bearing; 40 | } 41 | return z; 42 | } 43 | 44 | @Override 45 | public double measurementProb(double[] measurements) { 46 | // calculate the correct measurement 47 | double[] predictedMeasurements = sense(false); 48 | 49 | // compute errors 50 | double error = 1.0; 51 | for (int j = 0; j < predictedMeasurements.length; j++) { 52 | double errorBearing = Math.abs(measurements[j] - predictedMeasurements[j]); 53 | errorBearing = RoboMathUtils.modulus(errorBearing + Math.PI, 2.0 * Math.PI) - Math.PI; 54 | 55 | // update Gaussian 56 | double e1 = RoboMathUtils.gaussian(0, Math.pow(senseNoise, 2), Math.pow(errorBearing, 2)); 57 | error *= e1; 58 | // System.out.println("Error bearing " + error_bearing + 59 | // " ; ERROR = " + error); 60 | } 61 | return error; 62 | } 63 | 64 | @Override 65 | public void move(double steering, double speed) { 66 | move(new double[] { steering, speed }); 67 | } 68 | 69 | @Override 70 | public void move(double[] motions) { 71 | double stearing = motions[0]; 72 | double forward = motions[1]; 73 | if (stearing > MAX_STEERING_ANGLE) { 74 | // System.err.println("Turning angle is greater than maximum"); 75 | stearing = MAX_STEERING_ANGLE; 76 | } 77 | if (stearing < -MAX_STEERING_ANGLE) { 78 | // System.err.println("Turning angle is greater than maximum"); 79 | stearing = -MAX_STEERING_ANGLE; 80 | } 81 | // if (forward < 0) { 82 | // // raise ValueError, 'Robot cant move backwards' 83 | // System.err.println("Robot cant move backwards"); 84 | // } 85 | double stearing2 = RoboMathUtils.nextGaussian(stearing, steeringNoise); 86 | double distance2 = RoboMathUtils.nextGaussian(forward, forwardNoise); 87 | // # move, and add randomness to the motion command 88 | // # calculate beeta 89 | 90 | // add steering drift 91 | stearing2 += steeringDrift; 92 | 93 | double beeta = Math.tan(stearing2) * distance2 / length; 94 | // # calculate global coordinates 95 | if (Math.abs(beeta) < 0.001) { 96 | x = x + (distance2 * Math.cos(orientation)); 97 | y = y + (distance2 * Math.sin(orientation)); 98 | orientation = orientation + beeta; 99 | setOrientation(orientation); 100 | } else { 101 | // Apply by-cycle modal 102 | double turningRadius = distance2 / beeta; 103 | double cx = x - (Math.sin(orientation) * turningRadius); 104 | double cy = y + (Math.cos(orientation) * turningRadius); 105 | 106 | // # calculate the local coordinates 107 | x = cx + (Math.sin(orientation + beeta) * turningRadius); 108 | y = cy - (Math.cos(orientation + beeta) * turningRadius); 109 | 110 | setOrientation(orientation + beeta); 111 | } 112 | 113 | setX(x);// # cyclic truncate 114 | setY(y); 115 | 116 | // # turn, and add randomness to the turning command 117 | } 118 | 119 | } 120 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/internal/RootView.java: -------------------------------------------------------------------------------- 1 | /** 2 | * M Faisal Hameed 3 | */ 4 | package pk.com.habsoft.robosim.internal; 5 | 6 | import java.awt.Color; 7 | import java.awt.Dimension; 8 | import java.awt.Toolkit; 9 | import java.io.File; 10 | import java.io.FileInputStream; 11 | import java.io.FileOutputStream; 12 | import java.util.Properties; 13 | 14 | import javax.swing.BorderFactory; 15 | import javax.swing.JInternalFrame; 16 | import javax.swing.WindowConstants; 17 | import javax.swing.border.Border; 18 | import javax.swing.event.InternalFrameAdapter; 19 | import javax.swing.event.InternalFrameEvent; 20 | 21 | abstract public class RootView extends JInternalFrame implements PropertiesListener { 22 | 23 | private static final long serialVersionUID = 1L; 24 | protected String viewName = ""; 25 | public int LABEL_HEIGHT = 30; 26 | public Border lineBorder; 27 | public boolean isInit = false; 28 | // TODO subtract 100 pixels 29 | public static final Dimension screenSize = Toolkit.getDefaultToolkit().getScreenSize(); 30 | 31 | protected String propertyFile = ""; 32 | protected Properties prop = new Properties(); 33 | 34 | public RootView(String title, String propertyFile) { 35 | super(title, true, // resizable 36 | true, // closable 37 | true, // maximizable 38 | true);// iconifiable 39 | this.propertyFile = propertyFile; 40 | viewName = title; 41 | 42 | setDefaultCloseOperation(WindowConstants.HIDE_ON_CLOSE); 43 | try { 44 | jbInit(); 45 | } catch (Exception e) { 46 | e.printStackTrace(); 47 | } 48 | } 49 | 50 | @Override 51 | public boolean loadProperties() { 52 | if (propertyFile != null && propertyFile.trim().length() > 0) { 53 | File f = new File(propertyFile); 54 | // If property file not exists then copy from default config folder. 55 | if (!f.exists()) { 56 | File ff = new File(getClass().getClassLoader().getResource(propertyFile).getFile()); 57 | System.out.println(ff); 58 | f = ff; 59 | } 60 | System.out.println(f.getPath()); 61 | if (f.exists()) { 62 | try { 63 | FileInputStream fis = new FileInputStream(f); 64 | prop.load(fis); 65 | fis.close(); 66 | } catch (Exception e) { 67 | System.out.println("Unable to read property file " + f.getAbsolutePath() + " ." + e.getMessage()); 68 | return false; 69 | } 70 | } else { 71 | 72 | System.out.println("Property file not exists : " + f.getAbsolutePath()); 73 | return false; 74 | } 75 | } 76 | return true; 77 | } 78 | 79 | @Override 80 | public void saveProperties() { 81 | try { 82 | // File f = new 83 | // File(getClass().getClassLoader().getResource(propertyFile).getFile()); 84 | File f = new File(propertyFile); 85 | FileOutputStream out = new FileOutputStream(f); 86 | prop.store(out, ""); 87 | out.close(); 88 | } catch (Exception e) { 89 | e.printStackTrace(); 90 | } 91 | System.out.println("** Saving properties file. " + propertyFile); 92 | } 93 | 94 | public void setBounds(double x, double y, double width, double height) { 95 | super.setBounds((int) x, (int) y, (int) width, (int) height); 96 | } 97 | 98 | public void setSize(double width, double height) { 99 | super.setSize((int) width, (int) height); 100 | } 101 | 102 | private void jbInit() throws Exception { 103 | this.setFont(new java.awt.Font("Dialog", 0, 10)); 104 | lineBorder = BorderFactory.createLineBorder(Color.BLACK); 105 | 106 | this.addInternalFrameListener(new InternalFrameAdapter() { 107 | @Override 108 | public void internalFrameActivated(InternalFrameEvent e) { 109 | this_internalFrameActivated(e); 110 | } 111 | 112 | @Override 113 | public void internalFrameClosing(InternalFrameEvent e) { 114 | this_internalFrameClosing(); 115 | } 116 | }); 117 | } 118 | 119 | public void this_internalFrameActivated(InternalFrameEvent e) { 120 | } 121 | 122 | public void this_internalFrameClosing() { 123 | System.out.println("saving"); 124 | if (isInit) { 125 | saveProperties(); 126 | } 127 | } 128 | 129 | public abstract void initGUI(); 130 | 131 | public void showView() { 132 | setVisible(true); 133 | try { 134 | setSelected(true); 135 | setIcon(false); 136 | } catch (java.beans.PropertyVetoException ex) { 137 | ex.printStackTrace(); 138 | } 139 | } 140 | 141 | } 142 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/DiscreteValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import pk.com.habsoft.robosim.filters.core.Attribute; 4 | 5 | /** 6 | * A discrete value subclass in which discrete values are stored as int values. 7 | * The int values correspond to the attributes categorical list of discrete 8 | * values, so the int values should always be >= 0 unless it is unset, which 9 | * is specified by a value of -1. 10 | * 11 | * @author James MacGlashan 12 | * 13 | */ 14 | public class DiscreteValue extends OOMDPValue implements Value { 15 | public static final int UNSET = -1; 16 | /** 17 | * The discrete value stored as an integer. If the attribute defines 18 | * categorical values with strings, this int value represents the index in 19 | * the list of categorical values. The default value of -1 indicates an 20 | * unset attribute value. 21 | */ 22 | protected final int discVal; 23 | 24 | /** 25 | * Initializes this value to be an assignment for Attribute attribute. 26 | * 27 | * @param attribute 28 | */ 29 | public DiscreteValue(Attribute attribute) { 30 | super(attribute); 31 | this.discVal = UNSET; 32 | } 33 | 34 | /** 35 | * Initializes this value as a copy from the source Value object v. 36 | * 37 | * @param v 38 | * the source Value to make this object a copy of. 39 | */ 40 | public DiscreteValue(DiscreteValue v) { 41 | super(v); 42 | DiscreteValue dv = v; 43 | this.discVal = dv.discVal; 44 | } 45 | 46 | public DiscreteValue(Attribute attribute, int discVal) { 47 | super(attribute); 48 | this.discVal = discVal; 49 | } 50 | 51 | @Override 52 | public boolean valueHasBeenSet() { 53 | return this.discVal != UNSET; 54 | } 55 | 56 | @Override 57 | public Value copy() { 58 | return new DiscreteValue(this); 59 | } 60 | 61 | @Override 62 | public Value setValue(int v) { 63 | return new DiscreteValue(this.attribute, v); 64 | } 65 | 66 | @Override 67 | public Value setValue(double v) { 68 | return new DiscreteValue(this.attribute, (int) v); 69 | } 70 | 71 | @Override 72 | public Value setValue(boolean v) { 73 | int intV = (v) ? 1 : 0; 74 | return new DiscreteValue(this.attribute, intV); 75 | } 76 | 77 | @Override 78 | public Value setValue(String v) { 79 | Integer intv = attribute.discValuesHash.get(v); 80 | if (intv == null) { 81 | throw new RuntimeException("String value " + v + " is not applicable for attribute " + this.attribute.name); 82 | } 83 | return new DiscreteValue(this.attribute, intv); 84 | } 85 | 86 | @Override 87 | public int getDiscVal() { 88 | if (this.discVal == -1) { 89 | throw new UnsetValueException(); 90 | } 91 | return this.discVal; 92 | } 93 | 94 | @Override 95 | public StringBuilder buildStringVal(StringBuilder builder) { 96 | if (this.discVal == -1) { 97 | throw new UnsetValueException(); 98 | } 99 | return builder.append(attribute.discValues.get(discVal)); 100 | } 101 | 102 | @Override 103 | public double getNumericRepresentation() { 104 | if (this.discVal == -1) { 105 | throw new UnsetValueException(); 106 | } 107 | return (double) this.discVal; 108 | } 109 | 110 | @Override 111 | public int hashCode() { 112 | final int prime = 31; 113 | int result = 1; 114 | result = prime * result + discVal; 115 | return result; 116 | } 117 | 118 | @Override 119 | public boolean equals(Object obj) { 120 | if (this == obj) { 121 | return true; 122 | } 123 | 124 | if (!(obj instanceof DiscreteValue)) { 125 | return false; 126 | } 127 | 128 | DiscreteValue op = (DiscreteValue) obj; 129 | if (!op.attribute.equals(attribute)) { 130 | return false; 131 | } 132 | 133 | return discVal == op.discVal; 134 | 135 | } 136 | 137 | @Override 138 | public boolean getBooleanValue() { 139 | return this.discVal != 0; 140 | } 141 | 142 | } 143 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/LineChartPanel.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | import java.awt.Color; 4 | import java.util.ArrayList; 5 | import java.util.List; 6 | 7 | import javax.swing.JPanel; 8 | 9 | import org.jfree.chart.ChartFactory; 10 | import org.jfree.chart.ChartPanel; 11 | import org.jfree.chart.JFreeChart; 12 | import org.jfree.chart.axis.NumberAxis; 13 | import org.jfree.chart.plot.PlotOrientation; 14 | import org.jfree.chart.plot.XYPlot; 15 | import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer; 16 | import org.jfree.data.xy.XYDataset; 17 | import org.jfree.data.xy.XYSeries; 18 | import org.jfree.data.xy.XYSeriesCollection; 19 | 20 | class LineChartPanel extends JPanel { 21 | 22 | private static final long serialVersionUID = 1L; 23 | private String xLabel, yLabel; 24 | ChartPanel chartPanel = null; 25 | List dataList = new ArrayList(); 26 | 27 | private class ChartData { 28 | String lable; 29 | double[] data; 30 | 31 | public ChartData(String lable, double[] data) { 32 | this.data = data; 33 | this.lable = lable; 34 | } 35 | 36 | public String getLable() { 37 | return lable; 38 | } 39 | 40 | public double[] getData() { 41 | return data; 42 | } 43 | 44 | } 45 | 46 | public LineChartPanel(String xLabel, String yLabel) { 47 | this.xLabel = xLabel; 48 | this.yLabel = yLabel; 49 | 50 | chartPanel = new ChartPanel(null); 51 | add(chartPanel); 52 | } 53 | 54 | public void clearData() { 55 | dataList.clear(); 56 | } 57 | 58 | public void addData(String lable, double[] data) { 59 | dataList.add(new ChartData(lable, data)); 60 | updateChart(); 61 | } 62 | 63 | private void updateChart() { 64 | // First Panel 65 | XYDataset dataset = createPositionDataset(); 66 | JFreeChart chart = createChart(dataset); 67 | chartPanel.setChart(chart); 68 | } 69 | 70 | @Override 71 | public void setSize(int width, int height) { 72 | // setPreferredSize(new java.awt.Dimension(width, height)); 73 | chartPanel.setPreferredSize(new java.awt.Dimension(width - 10, height - 10)); 74 | } 75 | 76 | /** 77 | * Creates a sample dataset. 78 | * 79 | * @return a sample dataset. 80 | */ 81 | private XYDataset createPositionDataset() { 82 | 83 | final XYSeriesCollection dataset = new XYSeriesCollection(); 84 | 85 | for (int k = 0; k < dataList.size(); k++) { 86 | ChartData data = dataList.get(k); 87 | final XYSeries series1 = new XYSeries(data.getLable()); 88 | for (int i = 0; i < data.getData().length; i++) { 89 | series1.add(i, data.getData()[i]); 90 | } 91 | dataset.addSeries(series1); 92 | } 93 | 94 | return dataset; 95 | 96 | } 97 | 98 | /** 99 | * Creates a chart. 100 | * 101 | * @param dataset 102 | * the data for the chart. 103 | * 104 | * @return a chart. 105 | */ 106 | private JFreeChart createChart(final XYDataset dataset) { 107 | 108 | // create the chart... 109 | final JFreeChart chart = ChartFactory.createXYLineChart("", // chart 110 | // title 111 | xLabel, // x axis label 112 | yLabel, // y axis label 113 | dataset, // data 114 | PlotOrientation.VERTICAL, true, // include legend 115 | true, // tooltips 116 | false // urls 117 | ); 118 | 119 | // NOW DO SOME OPTIONAL CUSTOMISATION OF THE CHART... 120 | chart.setBackgroundPaint(Color.white); 121 | 122 | // final StandardLegend legend = (StandardLegend) chart.getLegend(); 123 | // legend.setDisplaySeriesShapes(true); 124 | 125 | // get a reference to the plot for further customisation... 126 | final XYPlot plot = chart.getXYPlot(); 127 | plot.setBackgroundPaint(Color.lightGray); 128 | // plot.setAxisOffset(new Spacer(Spacer.ABSOLUTE, 5.0, 5.0, 5.0, 5.0)); 129 | plot.setDomainGridlinePaint(Color.white); 130 | plot.setRangeGridlinePaint(Color.white); 131 | 132 | final XYLineAndShapeRenderer renderer = new XYLineAndShapeRenderer(); 133 | renderer.setSeriesLinesVisible(0, false); 134 | renderer.setSeriesShapesVisible(1, false); 135 | plot.setRenderer(renderer); 136 | 137 | // change the auto tick unit selection to integer units only... 138 | final NumberAxis rangeAxis = (NumberAxis) plot.getRangeAxis(); 139 | rangeAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits()); 140 | // OPTIONAL CUSTOMISATION COMPLETED. 141 | 142 | return chart; 143 | 144 | } 145 | 146 | } 147 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/MultiLayerRenderer.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Canvas; 4 | import java.awt.Color; 5 | import java.awt.Graphics; 6 | import java.awt.Graphics2D; 7 | import java.awt.Image; 8 | import java.awt.Rectangle; 9 | import java.util.ArrayList; 10 | import java.util.List; 11 | 12 | /** 13 | * A MultiLayerRenderer is a canvas that will sequentially render a set of 14 | * render layers, one on top of the other, to the same 2D graphics context. 15 | * Rendering is performed offscreen to a buffered image before being displayed 16 | * on the screen. 17 | * 18 | * @author James MacGlashan 19 | * 20 | */ 21 | public class MultiLayerRenderer extends Canvas { 22 | 23 | private static final long serialVersionUID = 1L; 24 | 25 | /** 26 | * The layers that will be rendered in order from index 0 to n 27 | */ 28 | protected List renderLayers; 29 | 30 | /** 31 | * the background color of the canvas 32 | */ 33 | protected Color bgColor = Color.white; 34 | 35 | /** 36 | * Offscreen image to render to first 37 | */ 38 | protected Image offscreen = null; 39 | 40 | /** 41 | * The graphics context of the offscreen image 42 | */ 43 | protected Graphics2D bufferedGraphics = null; 44 | 45 | private int lastRenderWidth = 0; 46 | private int lastRenderHeight = 0; 47 | 48 | public MultiLayerRenderer() { 49 | this.renderLayers = new ArrayList(); 50 | } 51 | 52 | /** 53 | * Adds the specified {@link RenderLayer} to the end of the render layer 54 | * ordered list. 55 | * 56 | * @param l 57 | * the {@link RenderLayer} to add 58 | */ 59 | public void addRenderLayer(RenderLayer l) { 60 | this.renderLayers.add(l); 61 | } 62 | 63 | /** 64 | * Inserts a render layer at the specified position 65 | * 66 | * @param i 67 | * the position in which the render layer should be inserted 68 | * @param l 69 | * the render layer to insert 70 | */ 71 | public void insertRenderLayerTo(int i, RenderLayer l) { 72 | this.renderLayers.add(i, l); 73 | } 74 | 75 | /** 76 | * Removes the render layer at teh specified position. 77 | * 78 | * @param i 79 | * the position of the render layer to remove 80 | */ 81 | public void removeRenderLayer(int i) { 82 | this.renderLayers.remove(i); 83 | } 84 | 85 | /** 86 | * Returns the number of render layers 87 | * 88 | * @return the number of render layers 89 | */ 90 | public int numRenderLayers() { 91 | return this.renderLayers.size(); 92 | } 93 | 94 | /** 95 | * Sets the color that will fill the canvas before rendering begins 96 | * 97 | * @param col 98 | * the background color 99 | */ 100 | public void setBGColor(Color col) { 101 | this.bgColor = col; 102 | } 103 | 104 | /** 105 | * Returns the background color of the renderer 106 | * 107 | * @return the background color of the renderer 108 | */ 109 | public Color getBgColor() { 110 | return bgColor; 111 | } 112 | 113 | @Override 114 | public void paint(Graphics g) { 115 | this.initializeOffscreen(); 116 | 117 | this.bufferedGraphics.setColor(bgColor); 118 | this.bufferedGraphics.fill(new Rectangle(this.getWidth(), this.getHeight())); 119 | 120 | for (RenderLayer l : this.renderLayers) { 121 | l.render(bufferedGraphics, this.getWidth(), this.getHeight()); 122 | } 123 | 124 | Graphics2D g2 = (Graphics2D) g; 125 | g2.drawImage(offscreen, 0, 0, this); 126 | 127 | } 128 | 129 | /** 130 | * Initializes a new offscreen image and context 131 | */ 132 | protected void initializeOffscreen() { 133 | if (this.bufferedGraphics == null || (this.lastRenderWidth != this.getWidth()) 134 | && this.lastRenderHeight != this.getHeight()) { 135 | this.offscreen = createImage(this.getWidth(), this.getHeight()); 136 | this.bufferedGraphics = (Graphics2D) offscreen.getGraphics(); 137 | this.lastRenderHeight = this.getHeight(); 138 | this.lastRenderWidth = this.getWidth(); 139 | } 140 | } 141 | 142 | } 143 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/State.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core; 2 | 3 | import java.util.Collection; 4 | import java.util.List; 5 | import java.util.Set; 6 | 7 | public interface State { 8 | 9 | /** 10 | * Returns a copy of this state, if mutable the copy should be deep. 11 | * 12 | * @return a copy of this state. 13 | */ 14 | State copy(); 15 | 16 | /** 17 | * Adds object instance o to this state. 18 | * 19 | * @param o 20 | * the object instance to be added to this state. 21 | * @return the modified state 22 | */ 23 | State addObject(ObjectInstance o); 24 | 25 | /** 26 | * Adds the collection of objects to the state 27 | * 28 | * @param objects 29 | * @return the modified state 30 | */ 31 | State addAllObjects(Collection objects); 32 | 33 | /** 34 | * Removes the object instance with the name oname from this state. 35 | * 36 | * @param oname 37 | * the name of the object instance to remove. 38 | * @return the modified state 39 | */ 40 | State removeObject(String oname); 41 | 42 | /** 43 | * Removes the object instance o from this state. 44 | * 45 | * @param o 46 | * the object instance to remove from this state. 47 | * @return the modified state 48 | */ 49 | State removeObject(ObjectInstance o); 50 | 51 | /** 52 | * Removes the collection of objects from the state 53 | * 54 | * @param objects 55 | * @return the modified state 56 | */ 57 | State removeAllObjects(Collection objects); 58 | 59 | /** 60 | * Renames the identifier for object instance o in this state to newName. 61 | * 62 | * @param o 63 | * the object instance to rename in this state 64 | * @param newName 65 | * the new name of the object instance 66 | * @return the modified state 67 | */ 68 | State renameObject(ObjectInstance o, String newName); 69 | 70 | /** 71 | * Returns the number of object instances in this state. 72 | * 73 | * @return the number of object instances in this state. 74 | */ 75 | int numTotalObjects(); 76 | 77 | /** 78 | * Returns the object in this state with the name oname 79 | * 80 | * @param oname 81 | * the name of the object instance to return 82 | * @return the object instance with the name oname or null if there is no 83 | * object in this state named oname 84 | */ 85 | ObjectInstance getObject(String oname); 86 | 87 | /** 88 | * Returns the list of observable and hidden object instances in this state. 89 | * 90 | * @return the list of observable and hidden object instances in this state. 91 | */ 92 | List getAllObjects(); 93 | 94 | /** 95 | * Returns all objects that belong to the object class named oclass 96 | * 97 | * @param oclass 98 | * the name of the object class for which objects should be 99 | * returned 100 | * @return all objects that belong to the object class named oclass 101 | */ 102 | List getObjectsOfClass(String oclass); 103 | 104 | /** 105 | * Returns the first indexed object of the object class named oclass 106 | * 107 | * @param oclass 108 | * the name of the object class for which the first indexed 109 | * object should be returned. 110 | * @return the first indexed object of the object class named oclass 111 | */ 112 | ObjectInstance getFirstObjectOfClass(String oclass); 113 | 114 | /** 115 | * Returns a set of of the object class names for all object classes that 116 | * have instantiated objects in this state. 117 | * 118 | * @return a set of of the object class names for all object classes that 119 | * have instantiated objects in this state. 120 | */ 121 | Set getObjectClassesPresent(); 122 | 123 | /** 124 | * Returns a list of list of object instances, grouped by object class 125 | * 126 | * @return a list of list of object instances, grouped by object class 127 | */ 128 | List> getAllObjectsByClass(); 129 | 130 | /** 131 | * Returns a string representation of this state using observable and hidden 132 | * object instances. 133 | * 134 | * @return a string representation of this state using observable and hidden 135 | * object instances. 136 | */ 137 | String getCompleteStateDescription(); 138 | 139 | State setObjectsValue(String name, String attrib, int val); 140 | 141 | } 142 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/particles/views/ParticleFilterView.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.particles.views; 2 | 3 | 4 | import java.awt.BorderLayout; 5 | import java.awt.Frame; 6 | import java.awt.GridLayout; 7 | 8 | import javax.swing.JDesktopPane; 9 | import javax.swing.JFrame; 10 | import javax.swing.JScrollPane; 11 | import javax.swing.JTextArea; 12 | 13 | import pk.com.habsoft.robosim.filters.particles.ParticleSimulator; 14 | import pk.com.habsoft.robosim.filters.particles.World; 15 | import pk.com.habsoft.robosim.internal.RPanel; 16 | import pk.com.habsoft.robosim.internal.RootView; 17 | 18 | public class ParticleFilterView extends RootView { 19 | 20 | private static final long serialVersionUID = 1L; 21 | 22 | RPanel pnlOutput; 23 | SimulationPanel pnlWorld; 24 | ControlPanel pnlControls; 25 | JTextArea ta; 26 | 27 | final static int PNL_CONTROL_SIZE = 180; 28 | final static double PNL_WORLD_RATIO = 0.6; 29 | 30 | public static int PNL_WORLD_WIDTH; 31 | public static int PNL_WORLD_HEIGHT; 32 | 33 | static int PNL_OUTPUT_WIDTH; 34 | static int PNL_OUTPUT_HEIGHT; 35 | 36 | static int PNL_CONTROL_WIDTH; 37 | static int PNL_CONTROL_HEIGHT; 38 | 39 | ParticleSimulator sim; 40 | 41 | public ParticleFilterView() { 42 | super("Particle Filter", "config/Particle.properties"); 43 | setLayout(new GridLayout(2, 2)); 44 | setLayout(null); 45 | loadProperties(); 46 | } 47 | 48 | @Override 49 | public void initGUI() { 50 | isInit = true; 51 | 52 | pnlOutput = new RPanel(PNL_OUTPUT_WIDTH, PNL_OUTPUT_HEIGHT, "Output Panel"); 53 | pnlOutput.setLocation(PNL_WORLD_WIDTH, 0); 54 | pnlOutput.setLayoutMgr(new BorderLayout()); 55 | ta = new JTextArea(); 56 | // ta.setLineWrap(false); 57 | // ta.setWrapStyleWord(false); 58 | JScrollPane scroll = new JScrollPane(ta); 59 | scroll.setAutoscrolls(false); 60 | pnlOutput.add(scroll, BorderLayout.CENTER); 61 | 62 | add(pnlOutput); 63 | 64 | World.setMaxWidth(PNL_WORLD_WIDTH); 65 | World.setMaxHeight(PNL_WORLD_HEIGHT); 66 | sim = new ParticleSimulator(); 67 | pnlWorld = new SimulationPanel(sim); 68 | pnlWorld.setBounds(0, 0, PNL_WORLD_WIDTH, PNL_WORLD_HEIGHT); 69 | pnlWorld.setBorder(lineBorder); 70 | sim.setSimulationPanel(pnlWorld); 71 | sim.setOutPutPanel(this); 72 | // sim.paint(); 73 | add(pnlWorld); 74 | 75 | pnlControls = new ControlPanel(sim, prop, PNL_CONTROL_WIDTH, PNL_CONTROL_HEIGHT); 76 | pnlControls.setBounds(0, PNL_WORLD_HEIGHT, PNL_CONTROL_WIDTH, PNL_CONTROL_HEIGHT); 77 | add(pnlControls); 78 | 79 | setBounds(0, 0, screenSize.getWidth(), screenSize.getHeight()); 80 | 81 | } 82 | 83 | public void showOutPut(String txt) { 84 | ta.append(" " + txt + "\n"); 85 | ta.setCaretPosition(ta.getDocument().getLength()); 86 | } 87 | 88 | @Override 89 | public boolean loadProperties() { 90 | System.out.println("Property File = " + propertyFile); 91 | PNL_WORLD_WIDTH = (int) (screenSize.getWidth() * PNL_WORLD_RATIO) - 15; 92 | PNL_WORLD_HEIGHT = (int) (screenSize.getHeight() - PNL_CONTROL_SIZE - 150); 93 | 94 | PNL_OUTPUT_HEIGHT = PNL_WORLD_HEIGHT; 95 | PNL_OUTPUT_WIDTH = (int) (screenSize.getWidth() * (1 - PNL_WORLD_RATIO)) - 15; 96 | 97 | PNL_CONTROL_HEIGHT = PNL_CONTROL_SIZE; 98 | PNL_CONTROL_WIDTH = (int) screenSize.getWidth() - 30; 99 | return super.loadProperties(); 100 | } 101 | 102 | @Override 103 | public void saveProperties() { 104 | prop.clear(); 105 | pnlControls.saveProperties(); 106 | super.saveProperties(); 107 | } 108 | 109 | @Override 110 | public void dispose() { 111 | if (sim != null) { 112 | sim.setRunning(false); 113 | } 114 | super.dispose(); 115 | } 116 | 117 | @Override 118 | public void hide() { 119 | if (sim != null) { 120 | sim.setRunning(false); 121 | } 122 | super.hide(); 123 | } 124 | 125 | public static void main(String[] args) { 126 | JFrame frame = new JFrame(); 127 | frame.setDefaultCloseOperation(EXIT_ON_CLOSE); 128 | JDesktopPane desk = new JDesktopPane(); 129 | frame.setContentPane(desk); 130 | 131 | ParticleFilterView view1 = new ParticleFilterView(); 132 | view1.initGUI(); 133 | 134 | desk.add(view1); 135 | view1.setVisible(true); 136 | // Dimension size = Toolkit.getDefaultToolkit().getScreenSize(); 137 | // frame.setLocation((int) size.getWidth() - 600, (int) size.getHeight() 138 | // - 800); 139 | // frame.setSize(600, 700); 140 | frame.setExtendedState(Frame.MAXIMIZED_BOTH); 141 | frame.setVisible(true); 142 | } 143 | 144 | } 145 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/Visualizer.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.Color; 4 | 5 | import pk.com.habsoft.robosim.filters.core.State; 6 | 7 | /** 8 | * This class extends the {@link MultiLayerRenderer} class to provide a base 9 | * instance of a {@link StateRenderLayer} in its render list and provides 10 | * methods to directly access and interface with the {@link StateRenderLayer} 11 | * instance. 12 | *

13 | * The {@link StateRenderLayer} instance provides 2D visualization of states by 14 | * being provided a set of classes that can paint ObjectInstances to the canvas 15 | * as well as classes that can paint general domain information. Painters for 16 | * object classes as well as specific object instances can be provided. If there 17 | * is a painter for an object class and a painter for a specific object instance 18 | * of that same class, then the specific object instance painter will be used to 19 | * pain that object instead of the painter for that instance's OO-MDP class. 20 | * 21 | * @author James MacGlashan 22 | * 23 | */ 24 | public class Visualizer extends MultiLayerRenderer { 25 | 26 | private static final long serialVersionUID = 1L; 27 | 28 | /** 29 | * The {@link StateRenderLayer} instance for visualizing OO-MDP states. 30 | */ 31 | protected StateRenderLayer srender; 32 | 33 | public Visualizer() { 34 | super(); 35 | srender = new StateRenderLayer(); 36 | this.renderLayers.add(srender); 37 | } 38 | 39 | public Visualizer(StateRenderLayer srender) { 40 | super(); 41 | this.srender = srender; 42 | this.renderLayers.add(this.srender); 43 | } 44 | 45 | public void setSetRenderLayer(StateRenderLayer srender) { 46 | this.renderLayers.remove(this.srender); 47 | this.renderLayers.add(srender); 48 | this.srender = srender; 49 | } 50 | 51 | /** 52 | * Sets the background color of the canvas 53 | * 54 | * @param c 55 | * the background color of the canvas 56 | */ 57 | public void setBGColor(Color c) { 58 | this.bgColor = c; 59 | } 60 | 61 | /** 62 | * Adds a static painter for the domain. 63 | * 64 | * @param sp 65 | * the static painter to add. 66 | */ 67 | public void addStaticPainter(StaticPainter sp) { 68 | this.srender.addStaticPainter(sp); 69 | } 70 | 71 | /** 72 | * Adds a class that will paint objects that belong to a given OO-MDPclass. 73 | * 74 | * @param className 75 | * the name of the class that the provided painter can paint 76 | * @param op 77 | * the painter 78 | */ 79 | public void addObjectClassPainter(String className, ObjectPainter op) { 80 | this.srender.addObjectClassPainter(className, op); 81 | } 82 | 83 | /** 84 | * Adds a painter that will be used to paint a specific object in states 85 | * 86 | * @param objectName 87 | * the name of the object this painter is used to paint 88 | * @param op 89 | * the painter 90 | */ 91 | public void addSpecificObjectPainter(String objectName, ObjectPainter op) { 92 | this.srender.addSpecificObjectPainter(objectName, op); 93 | } 94 | 95 | /** 96 | * Returns the {@link StateRenderLayer} instance for visualizing OO-MDP 97 | * states. 98 | * 99 | * @return the {@link StateRenderLayer} instance for visualizing OO-MDP 100 | * states. 101 | */ 102 | public StateRenderLayer getStateRenderLayer() { 103 | return this.srender; 104 | } 105 | 106 | /** 107 | * Updates the state that needs to be painted and repaints. If a 108 | * {@link burlap.oomdp.visualizer.StateActionRenderLayer} has been 109 | * specified, then it will have its state-action cleared before repainting. 110 | * 111 | * @param s 112 | * the state to paint 113 | */ 114 | public void updateState(State s) { 115 | this.srender.updateState(s); 116 | repaint(); 117 | } 118 | 119 | public Visualizer copy() { 120 | Visualizer v = new Visualizer(this.srender); 121 | for (RenderLayer rl : this.renderLayers) { 122 | if (rl != this.srender) { 123 | this.renderLayers.add(rl); 124 | } 125 | } 126 | 127 | return v; 128 | } 129 | 130 | } 131 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/BarChartPanel.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | import java.awt.Color; 4 | import java.awt.GradientPaint; 5 | 6 | import javax.swing.JPanel; 7 | 8 | import org.jfree.chart.ChartFactory; 9 | import org.jfree.chart.ChartPanel; 10 | import org.jfree.chart.JFreeChart; 11 | import org.jfree.chart.axis.CategoryAxis; 12 | import org.jfree.chart.axis.CategoryLabelPositions; 13 | import org.jfree.chart.axis.NumberAxis; 14 | import org.jfree.chart.plot.CategoryPlot; 15 | import org.jfree.chart.plot.PlotOrientation; 16 | import org.jfree.chart.renderer.category.BarRenderer; 17 | import org.jfree.data.category.CategoryDataset; 18 | import org.jfree.data.category.DefaultCategoryDataset; 19 | 20 | public class BarChartPanel extends JPanel { 21 | 22 | private static final long serialVersionUID = 1L; 23 | private double[] measurementError; 24 | private double[] kalmanError; 25 | private String xLabel, yLabel; 26 | private String mLabel, kLabel; 27 | ChartPanel chartPanel = null; 28 | 29 | public BarChartPanel(String xLabel, String yLabel, String mLabel, String kLabel) { 30 | this.xLabel = xLabel; 31 | this.yLabel = yLabel; 32 | this.mLabel = mLabel; 33 | this.kLabel = kLabel; 34 | 35 | chartPanel = new ChartPanel(null); 36 | add(chartPanel); 37 | } 38 | 39 | public void setData(double[] measurementError, double[] kalmanError) { 40 | this.measurementError = measurementError; 41 | this.kalmanError = kalmanError; 42 | updateChart(); 43 | } 44 | 45 | private void updateChart() { 46 | // First Panel 47 | CategoryDataset dataset = createPositionDataset(); 48 | JFreeChart chart = createChart(dataset); 49 | chartPanel.setChart(chart); 50 | } 51 | 52 | @Override 53 | public void setSize(int width, int height) { 54 | // setPreferredSize(new java.awt.Dimension(width, height)); 55 | chartPanel.setPreferredSize(new java.awt.Dimension(width - 10, height - 10)); 56 | } 57 | 58 | /** 59 | * Creates a sample dataset. 60 | * 61 | * @return a sample dataset. 62 | */ 63 | private CategoryDataset createPositionDataset() { 64 | 65 | DefaultCategoryDataset dataset = new DefaultCategoryDataset(); 66 | 67 | for (int i = 0; i < measurementError.length; i++) { 68 | dataset.addValue(measurementError[i], mLabel, String.valueOf(i)); 69 | } 70 | 71 | for (int i = 0; i < kalmanError.length; i++) { 72 | dataset.addValue(kalmanError[i], kLabel, String.valueOf(i)); 73 | } 74 | 75 | return dataset; 76 | 77 | } 78 | 79 | /** 80 | * Creates a sample chart. 81 | * 82 | * @param dataset 83 | * the dataset. 84 | * 85 | * @return The chart. 86 | */ 87 | private JFreeChart createChart(CategoryDataset dataset) { 88 | 89 | // create the chart... 90 | JFreeChart chart = ChartFactory.createBarChart("", // chart 91 | // title 92 | xLabel, // domain axis label 93 | yLabel, // range axis label 94 | dataset, // data 95 | PlotOrientation.VERTICAL, // orientation 96 | true, // include legend 97 | true, // tooltips? 98 | false // URLs? 99 | ); 100 | 101 | // NOW DO SOME OPTIONAL CUSTOMISATION OF THE CHART... 102 | 103 | // set the background color for the chart... 104 | chart.setBackgroundPaint(Color.white); 105 | 106 | // get a reference to the plot for further customisation... 107 | CategoryPlot plot = chart.getCategoryPlot(); 108 | plot.setBackgroundPaint(Color.lightGray); 109 | plot.setDomainGridlinePaint(Color.white); 110 | plot.setDomainGridlinesVisible(true); 111 | plot.setRangeGridlinePaint(Color.white); 112 | 113 | // set the range axis to display integers only... 114 | final NumberAxis rangeAxis = (NumberAxis) plot.getRangeAxis(); 115 | rangeAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits()); 116 | 117 | // disable bar outlines... 118 | BarRenderer renderer = (BarRenderer) plot.getRenderer(); 119 | renderer.setDrawBarOutline(false); 120 | 121 | // set up gradient paints for series... 122 | GradientPaint gp0 = new GradientPaint(0.0f, 0.0f, Color.red, 0.0f, 0.0f, new Color(64, 0, 0)); 123 | GradientPaint gp1 = new GradientPaint(0.0f, 0.0f, Color.green, 0.0f, 0.0f, new Color(0, 64, 0)); 124 | renderer.setSeriesPaint(0, gp0); 125 | renderer.setSeriesPaint(1, gp1); 126 | 127 | CategoryAxis domainAxis = plot.getDomainAxis(); 128 | domainAxis.setCategoryLabelPositions(CategoryLabelPositions.createUpRotationLabelPositions(Math.PI / 2.0)); 129 | // OPTIONAL CUSTOMISATION COMPLETED. 130 | 131 | return chart; 132 | 133 | } 134 | } 135 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/IntArrayValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import java.util.Arrays; 4 | 5 | import pk.com.habsoft.robosim.filters.core.Attribute; 6 | 7 | /** 8 | * This class implements an attribute value that is defined with an int array. 9 | * In general, it is reccomended that a series of {@link IntValue} attributes is 10 | * defined instead of using this class, because a series of {@link IntValue}s 11 | * will have better compatibility with existing BURLAP tools and algorithms, but 12 | * this class can be used in cases where there is a very large number of int 13 | * values that have to be stored in each state to cut down on memory overhead. 14 | * 15 | * @author James MacGlashan 16 | * 17 | */ 18 | public class IntArrayValue extends OOMDPValue implements Value { 19 | 20 | protected final int[] intArray; 21 | 22 | public IntArrayValue(Attribute attribute) { 23 | super(attribute); 24 | this.intArray = null; 25 | } 26 | 27 | public IntArrayValue(IntArrayValue v) { 28 | super(v); 29 | IntArrayValue iaValue = v; 30 | if (iaValue.intArray != null) { 31 | this.intArray = iaValue.intArray.clone(); 32 | } else { 33 | this.intArray = null; 34 | } 35 | } 36 | 37 | public IntArrayValue(Attribute attribute, int[] intArray) { 38 | super(attribute); 39 | this.intArray = intArray; 40 | } 41 | 42 | @Override 43 | public Value copy() { 44 | return new IntArrayValue(this); 45 | } 46 | 47 | @Override 48 | public boolean valueHasBeenSet() { 49 | return this.intArray != null; 50 | } 51 | 52 | @Override 53 | public Value setValue(String v) { 54 | if (v.startsWith("\"") && v.endsWith("\"")) { 55 | v = v.substring(1, v.length()); 56 | } 57 | String[] comps = v.split(","); 58 | int[] intArray = new int[comps.length]; 59 | for (int i = 0; i < comps.length; i++) { 60 | intArray[i] = Integer.parseInt(comps[i]); 61 | } 62 | return new IntArrayValue(this.attribute, intArray); 63 | } 64 | 65 | @Override 66 | public StringBuilder buildStringVal(StringBuilder builder) { 67 | for (int i = 0; i < this.intArray.length; i++) { 68 | if (i > 0) { 69 | builder.append(","); 70 | } 71 | builder.append(this.intArray[i]); 72 | } 73 | return builder; 74 | } 75 | 76 | @Override 77 | public double getNumericRepresentation() { 78 | int sum = 0; 79 | for (int v : this.intArray) { 80 | sum *= 31; 81 | sum += v; 82 | } 83 | return sum; 84 | } 85 | 86 | @Override 87 | public Value setValue(int[] intArray) { 88 | return new IntArrayValue(this.attribute, intArray); 89 | } 90 | 91 | @Override 92 | public Value setValue(double[] doubleArray) { 93 | throw new UnsupportedOperationException("Cannot set int array value to double array value."); 94 | } 95 | 96 | @Override 97 | public int[] getIntArray() { 98 | return this.intArray; 99 | } 100 | 101 | @Override 102 | public double[] getDoubleArray() { 103 | double[] doubleArray = new double[this.intArray.length]; 104 | for (int i = 0; i < doubleArray.length; i++) { 105 | doubleArray[i] = this.intArray[i]; 106 | } 107 | return doubleArray; 108 | } 109 | 110 | @Override 111 | public int hashCode() { 112 | final int prime = 31; 113 | int result = 1; 114 | result = prime * result + Arrays.hashCode(intArray); 115 | return result; 116 | } 117 | 118 | @Override 119 | public boolean equals(Object obj) { 120 | if (this == obj) { 121 | return true; 122 | } 123 | if (!(obj instanceof IntArrayValue)) { 124 | return false; 125 | } 126 | 127 | IntArrayValue o = (IntArrayValue) obj; 128 | 129 | if (!o.attribute.equals(attribute)) { 130 | return false; 131 | } 132 | 133 | if (this.intArray.length != o.intArray.length) { 134 | return false; 135 | } 136 | 137 | for (int i = 0; i < this.intArray.length; i++) { 138 | if (this.intArray[i] != o.intArray[i]) { 139 | return false; 140 | } 141 | } 142 | 143 | return true; 144 | 145 | } 146 | 147 | } 148 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/kalman/KalmanFilterSimulator.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.kalman; 2 | 3 | import java.text.DecimalFormat; 4 | 5 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 6 | 7 | public class KalmanFilterSimulator { 8 | 9 | // simulation parameters 10 | int totalTime; 11 | double measurementVariance; 12 | 13 | // #Data Arrays for plotting 14 | double[] positionMeasurements; 15 | double[] carPositions; 16 | double[] positionKalman; 17 | 18 | double[] velocMeasurement; 19 | double[] carVeloc; 20 | double[] velocKalman; 21 | 22 | double[] measurementError; 23 | double[] kalmanPositionError; 24 | double[] measurementVelocError; 25 | double[] kalmanVelocError; 26 | 27 | double[][] xx = { { 0 }, { 0 } }; 28 | double[][] pp = { { 1000.0, 0.0 }, { 0.0, 1000.0 } }; 29 | double[][] uu = { { 0.0 }, { 0.0 } }; 30 | double[][] ff = { { 1.0, 1.0 }, { 0.0, 1.0 } }; 31 | double[][] hh = { { 1.0, 0.0 } }; 32 | double[][] rr = { { 1.0 } }; 33 | 34 | double carSpeed = 0.5; 35 | 36 | KalmanFilter filter = new KalmanFilter(xx, pp, uu, ff, hh, rr); 37 | 38 | public static final DecimalFormat df = new DecimalFormat("####0.000"); 39 | 40 | public KalmanFilterSimulator(int totalTime, double variance, double carSpeed) { 41 | this.totalTime = totalTime; 42 | this.measurementVariance = variance; 43 | this.carSpeed = carSpeed; 44 | 45 | positionMeasurements = new double[totalTime]; 46 | carPositions = new double[totalTime]; 47 | positionKalman = new double[totalTime]; 48 | 49 | velocMeasurement = new double[totalTime]; 50 | carVeloc = new double[totalTime]; 51 | velocKalman = new double[totalTime]; 52 | 53 | measurementError = new double[totalTime]; 54 | kalmanPositionError = new double[totalTime]; 55 | 56 | measurementVelocError = new double[totalTime]; 57 | kalmanVelocError = new double[totalTime]; 58 | } 59 | 60 | // #Returns car distance given of time(t) 61 | // #our car drives with constant speed 62 | public double carPos(double t) { 63 | return carSpeed * t; 64 | } 65 | 66 | public void simulate() { 67 | double lastPos = 0; 68 | double lastMes = 0; 69 | for (int t = 0; t < totalTime; t++) { 70 | // #generate measurement and carpos 71 | // actual position 72 | double carPos = carPos(t); 73 | // measured position 74 | double measuredPos = RoboMathUtils.nextGaussian(carPos, measurementVariance); 75 | 76 | // actual velocity 77 | double v = carPos - lastPos; 78 | // measured velocity 79 | double mv = measuredPos - lastMes; 80 | 81 | // #run Kalman Filter 82 | // x, P, u, F, H, R, I = kalman(t, m, x, P, u, F, H, R, I) 83 | double[] mm = { measuredPos }; 84 | filter.filter(mm); 85 | 86 | // predicted next position 87 | double[][] x = filter.getX().getData(); 88 | 89 | velocMeasurement[t] = mv; 90 | carVeloc[t] = v; 91 | velocKalman[t] = x[1][0]; 92 | 93 | positionMeasurements[t] = measuredPos; 94 | carPositions[t] = carPos; 95 | positionKalman[t] = x[0][0]; 96 | 97 | measurementError[t] = measuredPos - carPos; 98 | measurementVelocError[t] = mv - v; 99 | 100 | kalmanPositionError[t] = x[0][0] - carPos; 101 | kalmanVelocError[t] = x[1][0] - v; 102 | 103 | lastPos = carPos;// #assume dPos/dT ^= veloc 104 | lastMes = measuredPos;// #so no need for extra veloc func 105 | // System.out.println(car_positions[t] + " : " + 106 | // position_kalman[t]); 107 | // System.out.println(car_veloc[t] + " : " + veloc_kalman[t] + " : " 108 | // + veloc_measurement[t]); 109 | 110 | } 111 | 112 | } 113 | 114 | public static void main(String[] args) { 115 | KalmanFilterSimulator anim = new KalmanFilterSimulator(10, 0, 0.5); 116 | anim.simulate(); 117 | System.out.println(anim.filter.getX()); 118 | } 119 | 120 | public double[] getPositionMeasurements() { 121 | return this.positionMeasurements; 122 | } 123 | 124 | public double[] getCarPositions() { 125 | return this.carPositions; 126 | } 127 | 128 | public double[] getPositionsKalman() { 129 | return this.positionKalman; 130 | } 131 | 132 | public double[] getVelocityMeasurements() { 133 | return this.velocMeasurement; 134 | } 135 | 136 | public double[] getCarVelocities() { 137 | return this.carVeloc; 138 | } 139 | 140 | public double[] getVelocitiesKalman() { 141 | return this.velocKalman; 142 | } 143 | 144 | public double[] getPositionKalmanError() { 145 | return this.kalmanPositionError; 146 | } 147 | 148 | public double[] getPositionMeasurementError() { 149 | return this.measurementError; 150 | } 151 | 152 | public double[] getVelocityKalmanError() { 153 | return this.kalmanVelocError; 154 | } 155 | 156 | public double[] getVelocityMeasurementError() { 157 | return this.measurementVelocError; 158 | } 159 | 160 | } 161 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/ui/GridWorldExplorer.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.ui; 2 | 3 | import java.awt.BorderLayout; 4 | import java.awt.Dimension; 5 | import java.awt.Insets; 6 | import java.awt.event.KeyEvent; 7 | import java.awt.event.KeyListener; 8 | import java.util.HashMap; 9 | import java.util.Map; 10 | 11 | import javax.swing.JFrame; 12 | import javax.swing.JScrollPane; 13 | import javax.swing.JTextArea; 14 | import javax.swing.text.DefaultCaret; 15 | 16 | import pk.com.habsoft.robosim.filters.core.Domain; 17 | import pk.com.habsoft.robosim.filters.core.State; 18 | import pk.com.habsoft.robosim.filters.core.actions.Action; 19 | import pk.com.habsoft.robosim.internal.RootView; 20 | 21 | public class GridWorldExplorer { 22 | 23 | private static final String HELP_TEXT = "Use keys (A,S,D,W) to move left, back, right and forward respectively. Use key (L) to scan environment using Sonar Range Sensors. "; 24 | RootView frame; 25 | State baseState; 26 | Visualizer painter; 27 | 28 | protected Domain domain; 29 | protected Map keyActionMap; 30 | protected JTextArea stateConsole; 31 | 32 | public GridWorldExplorer(RootView frame, Domain domain, Visualizer painter, State baseState) { 33 | this.frame = frame; 34 | this.domain = domain; 35 | this.painter = painter; 36 | this.baseState = baseState; 37 | this.keyActionMap = new HashMap<>(); 38 | } 39 | 40 | public void initGUI() { 41 | painter.setPreferredSize(new Dimension(600, 600)); 42 | 43 | this.stateConsole = new JTextArea(40, 40); 44 | this.stateConsole.setLineWrap(true); 45 | DefaultCaret caret = (DefaultCaret) this.stateConsole.getCaret(); 46 | caret.setUpdatePolicy(DefaultCaret.ALWAYS_UPDATE); 47 | this.stateConsole.setEditable(false); 48 | this.stateConsole.setMargin(new Insets(10, 5, 10, 5)); 49 | this.stateConsole.setText(HELP_TEXT); 50 | 51 | JScrollPane shellScroll = new JScrollPane(this.stateConsole, JScrollPane.VERTICAL_SCROLLBAR_NEVER, JScrollPane.HORIZONTAL_SCROLLBAR_NEVER); 52 | shellScroll.setPreferredSize(new Dimension(600, 50)); 53 | 54 | frame.add(shellScroll, BorderLayout.SOUTH); 55 | 56 | frame.add(painter); 57 | frame.setVisible(true); 58 | frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); 59 | frame.setPreferredSize(new Dimension(600, 800)); 60 | 61 | frame.addKeyListener(new KeyListener() { 62 | public void keyPressed(KeyEvent e) { 63 | } 64 | 65 | public void keyReleased(KeyEvent e) { 66 | } 67 | 68 | public void keyTyped(KeyEvent e) { 69 | handleKeyPressed(e); 70 | } 71 | 72 | }); 73 | 74 | // also add key listener to the painter in case the focus is changed 75 | painter.addKeyListener(new KeyListener() { 76 | public void keyPressed(KeyEvent e) { 77 | } 78 | 79 | public void keyReleased(KeyEvent e) { 80 | } 81 | 82 | public void keyTyped(KeyEvent e) { 83 | handleKeyPressed(e); 84 | } 85 | 86 | }); 87 | 88 | frame.pack(); 89 | } 90 | 91 | /** 92 | * Specifies a string representation of an action to execute when the 93 | * specified key is pressed. The string representation should have the first 94 | * word be the action name, with spaces separating the parameters of the 95 | * string representation of each parameter value. 96 | * 97 | * @param key 98 | * the key that is pressed by the user 99 | * @param actionStringRep 100 | * the {@link burlap.oomdp.singleagent.GroundedAction} to take 101 | * when the key is pressed 102 | */ 103 | public void addKeyAction(String key, String actionStringRep) { 104 | Action ga = domain.getAction(actionStringRep); 105 | if (ga == null) { 106 | System.out.println("Could not parse GroundedAction string representation of " + actionStringRep + ".\n" + "It is not being assigned to VisualExplorer key " + key + "."); 107 | } else { 108 | System.out.println(key); 109 | this.keyActionMap.put(key, ga); 110 | } 111 | } 112 | 113 | protected void handleKeyPressed(KeyEvent e) { 114 | 115 | String key = String.valueOf(e.getKeyChar()); 116 | 117 | // otherwise this could be an action, see if there is an action mapping 118 | Action mappedAction = keyActionMap.get(key); 119 | if (mappedAction != null) { 120 | 121 | this.executeAction(mappedAction); 122 | 123 | } 124 | 125 | } 126 | 127 | protected void executeAction(Action ga) { 128 | 129 | State newState = ga.performAction(this.baseState); 130 | updateState(newState); 131 | 132 | } 133 | 134 | /** 135 | * Updates the currently visualized state to the input state. 136 | * 137 | * @param s 138 | * the state to visualize. 139 | */ 140 | synchronized public void updateState(State s) { 141 | this.baseState = s; 142 | this.painter.updateState(s); 143 | } 144 | 145 | } 146 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/smoothing/LineChartPanel.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.smoothing; 2 | 3 | import java.awt.Color; 4 | import java.util.ArrayList; 5 | import java.util.List; 6 | 7 | import javax.swing.JPanel; 8 | 9 | import org.jfree.chart.ChartFactory; 10 | import org.jfree.chart.ChartPanel; 11 | import org.jfree.chart.JFreeChart; 12 | import org.jfree.chart.axis.NumberAxis; 13 | import org.jfree.chart.plot.PlotOrientation; 14 | import org.jfree.chart.plot.XYPlot; 15 | import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer; 16 | import org.jfree.data.xy.XYDataset; 17 | import org.jfree.data.xy.XYSeries; 18 | import org.jfree.data.xy.XYSeriesCollection; 19 | 20 | public class LineChartPanel extends JPanel { 21 | 22 | private static final long serialVersionUID = 1L; 23 | private String xLabel, yLabel; 24 | ChartPanel chartPanel = null; 25 | List dataList = new ArrayList(); 26 | 27 | private class ChartData { 28 | String lable; 29 | double[][] data; 30 | 31 | public ChartData(String lable, double[][] data) { 32 | this.data = data; 33 | this.lable = lable; 34 | } 35 | 36 | public String getLable() { 37 | return lable; 38 | } 39 | 40 | public double[][] getData() { 41 | return data; 42 | } 43 | 44 | } 45 | 46 | public LineChartPanel(String xLabel, String yLabel) { 47 | this.xLabel = xLabel; 48 | this.yLabel = yLabel; 49 | chartPanel = new ChartPanel(null); 50 | add(chartPanel); 51 | } 52 | 53 | public LineChartPanel(int width, int height, String xLabel, String yLabel) { 54 | this.xLabel = xLabel; 55 | this.yLabel = yLabel; 56 | 57 | super.setSize(width, height); 58 | chartPanel = new ChartPanel(null); 59 | add(chartPanel); 60 | } 61 | 62 | public void clearData() { 63 | dataList.clear(); 64 | updateChart(); 65 | } 66 | 67 | public void addData(String lable, double[][] data) { 68 | dataList.add(new ChartData(lable, data)); 69 | updateChart(); 70 | } 71 | 72 | private void updateChart() { 73 | XYDataset dataset = createPositionDataset(); 74 | JFreeChart chart = createChart(dataset); 75 | chartPanel.setChart(chart); 76 | } 77 | 78 | @Override 79 | public void setLocation(int x, int y) { 80 | super.setLocation(x, y); 81 | chartPanel.setLocation(x, y); 82 | } 83 | 84 | @Override 85 | public void setSize(int width, int height) { 86 | // super.setPreferredSize(new java.awt.Dimension(width - 10, height - 87 | // 10)); 88 | chartPanel.setPreferredSize(new java.awt.Dimension(width - 10, height - 10)); 89 | } 90 | 91 | /** 92 | * Creates a sample dataset. 93 | * 94 | * @return a sample dataset. 95 | */ 96 | private XYDataset createPositionDataset() { 97 | 98 | final XYSeriesCollection dataset = new XYSeriesCollection(); 99 | 100 | for (int k = 0; k < dataList.size(); k++) { 101 | ChartData data = dataList.get(k); 102 | final XYSeries series1 = new XYSeries(data.getLable()); 103 | for (int i = 0; i < data.getData().length; i++) { 104 | series1.add(data.getData()[i][0], data.getData()[i][1]); 105 | } 106 | dataset.addSeries(series1); 107 | } 108 | 109 | return dataset; 110 | 111 | } 112 | 113 | /** 114 | * Creates a chart. 115 | * 116 | * @param dataset 117 | * the data for the chart. 118 | * 119 | * @return a chart. 120 | */ 121 | private JFreeChart createChart(final XYDataset dataset) { 122 | 123 | // create the chart... 124 | final JFreeChart chart = ChartFactory.createXYLineChart("", // chart 125 | // title 126 | xLabel, // x axis label 127 | yLabel, // y axis label 128 | dataset, // data 129 | PlotOrientation.VERTICAL, true, // include legend 130 | true, // tooltips 131 | false // urls 132 | ); 133 | 134 | // NOW DO SOME OPTIONAL CUSTOMISATION OF THE CHART... 135 | chart.setBackgroundPaint(Color.white); 136 | 137 | // final StandardLegend legend = (StandardLegend) chart.getLegend(); 138 | // legend.setDisplaySeriesShapes(true); 139 | 140 | // get a reference to the plot for further customisation... 141 | final XYPlot plot = chart.getXYPlot(); 142 | plot.setBackgroundPaint(Color.lightGray); 143 | // plot.setAxisOffset(new Spacer(Spacer.ABSOLUTE, 5.0, 5.0, 5.0, 5.0)); 144 | plot.setDomainGridlinePaint(Color.white); 145 | plot.setRangeGridlinePaint(Color.white); 146 | 147 | final XYLineAndShapeRenderer renderer = new XYLineAndShapeRenderer(); 148 | renderer.setSeriesLinesVisible(0, true); 149 | renderer.setSeriesShapesVisible(1, true); 150 | plot.setRenderer(renderer); 151 | 152 | // change the auto tick unit selection to integer units only... 153 | final NumberAxis rangeAxis = (NumberAxis) plot.getRangeAxis(); 154 | rangeAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits()); 155 | // OPTIONAL CUSTOMISATION COMPLETED. 156 | 157 | return chart; 158 | 159 | } 160 | 161 | } 162 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/utils/PositionGeometryTools.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Player Java Client 2 - PositionGeometryTools.java 3 | * Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | * $Id: PositionGeometryTools.java,v 1.3 2006/03/06 08:33:31 veedee Exp $ 20 | * 21 | */ 22 | 23 | package pk.com.habsoft.robosim.utils; 24 | 25 | import java.awt.*; 26 | 27 | /** 28 | * Several methods for position geometric calculus. 29 | * 30 | * @author Marius Borodi & Radu Bogdan Rusu 31 | */ 32 | 33 | public class PositionGeometryTools { 34 | 35 | private PositionGeometryTools() { 36 | 37 | } 38 | 39 | /** 40 | * Calculate the distance between two X and Y points assuming that their 41 | * counterparts are 0 using Pitagora's theorem. 42 | * 43 | * @param x 44 | * X's coordonate of the first point (Y=0) 45 | * @param y 46 | * Y's coordonate of the second point (X=0) 47 | * @return distance between [X, 0] and [0, Y] as a float 48 | */ 49 | public static float calcDist(float x, float y) { 50 | double d = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); 51 | return Math.round(d); 52 | } 53 | 54 | /** 55 | * Calculate the distance between two points (p1 and p2). 56 | * 57 | * @param p1 58 | * first point 59 | * @param p2 60 | * second point 61 | * @return the distance between p1 and p2 62 | */ 63 | public static float calcDist(Point p1, Point p2) { 64 | int x = p2.x - p1.x; 65 | int y = p2.y - p1.y; 66 | return calcDist(x, y); 67 | } 68 | 69 | /** 70 | * Calculate the X coordinate of a point situated at distance dist, 71 | * angle angle from a given point initP. 72 | * 73 | * @param initP 74 | * reference point 75 | * @param dist 76 | * distance from the reference point to the desired point 77 | * @param angle 78 | * angle from the reference point to the desired point 79 | * @return the X coordinate of the point 80 | */ 81 | public static float calcX(Point initP, float dist, float angle) { 82 | double tmp = dist * Math.cos(Math.toRadians(angle)); 83 | return initP.x + Math.round(tmp); 84 | } 85 | 86 | /** 87 | * Calculate the Y coordinate of a point situated at distance dist, 88 | * angle angle from a given point initP. 89 | * 90 | * @param initP 91 | * reference point 92 | * @param dist 93 | * distance from the reference point to the desired point 94 | * @param angle 95 | * angle from the reference point to the desired point 96 | * @return the Y coordinate of the point 97 | */ 98 | public static float calcY(Point initP, float dist, float angle) { 99 | double tmp = dist * Math.sin(Math.toRadians(angle)); 100 | return initP.y + Math.round(tmp); 101 | } 102 | 103 | /** 104 | * Calculate the coordinates of a point situated at distance dist, 105 | * angle angle from a given point initP. 106 | * 107 | * @param initP 108 | * reference point 109 | * @param dist 110 | * distance from the reference point to the desired point 111 | * @param angle 112 | * angle from the reference point to the desired point 113 | * @return the coordinates of the new point as a Point (AWT) 114 | */ 115 | public static Point calcDistPoint(Point initP, float dist, float angle) { 116 | double tmp = dist * Math.cos(Math.toRadians(angle)); 117 | float x = initP.x + Math.round(tmp); 118 | tmp = dist * Math.sin(Math.toRadians(angle)); 119 | float y = initP.y + Math.round(tmp); 120 | Point p = new Point(); 121 | p.setLocation(x, y); 122 | return p; 123 | } 124 | 125 | /** 126 | * Calculate the angle between the line determined by the two points and the 127 | * horizontal axis. 128 | * 129 | * @param p1 130 | * First point 131 | * @param p2 132 | * Second point 133 | * @return the angle as an integer 134 | */ 135 | public static float calcAngle(Point p1, Point p2) { 136 | return Math.round(Math.toDegrees(Math.atan2((p2.y - p1.y), (p2.x - p1.x)))); 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/pathsmoother/views/DrawingControlPanel.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this template, choose Tools | Templates 3 | * and open the template in the editor. 4 | */ 5 | 6 | package pk.com.habsoft.robosim.planning.pathsmoother.views; 7 | 8 | import java.awt.Color; 9 | import java.awt.FlowLayout; 10 | import java.awt.event.ActionEvent; 11 | import java.awt.event.ActionListener; 12 | 13 | import javax.swing.ButtonGroup; 14 | import javax.swing.JButton; 15 | import javax.swing.JLabel; 16 | import javax.swing.JPanel; 17 | import javax.swing.JRadioButton; 18 | import javax.swing.JSpinner; 19 | import javax.swing.JToggleButton; 20 | import javax.swing.SpinnerNumberModel; 21 | import javax.swing.event.ChangeEvent; 22 | import javax.swing.event.ChangeListener; 23 | 24 | public class DrawingControlPanel extends JPanel implements ActionListener, ChangeListener { 25 | 26 | private static final long serialVersionUID = 1L; 27 | 28 | DrawingPanel drawingPanel = null; 29 | 30 | private JButton clearButton = new JButton("Clear"); 31 | JRadioButton rbDraw = new JRadioButton("Draw"); 32 | JRadioButton rbErase = new JRadioButton("Erase"); 33 | JRadioButton rbStart = new JRadioButton("Start"); 34 | JRadioButton rbFinish = new JRadioButton("Finish"); 35 | JToggleButton tbEditWorld = new JToggleButton("Edit World"); 36 | 37 | JSpinner spnRows = new JSpinner(); 38 | JSpinner spnColumns = new JSpinner(); 39 | 40 | private int drawMod = DrawingPanel.NONE; 41 | 42 | public void setDrawingPanel(DrawingPanel observer) { 43 | this.drawingPanel = observer; 44 | this.drawingPanel.setDrawingMod(drawMod); 45 | 46 | } 47 | 48 | public DrawingControlPanel(int rows, int columns) { 49 | setLayout(new FlowLayout()); 50 | 51 | tbEditWorld.addActionListener(this); 52 | tbEditWorld.setBackground(Color.GREEN); 53 | add(tbEditWorld); 54 | 55 | clearButton.addActionListener(this); 56 | clearButton.setEnabled(tbEditWorld.isSelected()); 57 | add(clearButton); 58 | 59 | rbDraw.addActionListener(this); 60 | rbDraw.setSelected(drawMod == DrawingPanel.NONE); 61 | rbDraw.setEnabled(tbEditWorld.isSelected()); 62 | add(rbDraw); 63 | 64 | rbErase.addActionListener(this); 65 | rbErase.setSelected(drawMod == DrawingPanel.ERASE); 66 | rbErase.setEnabled(tbEditWorld.isSelected()); 67 | add(rbErase); 68 | 69 | rbStart.addActionListener(this); 70 | rbStart.setSelected(drawMod == DrawingPanel.START); 71 | rbStart.setEnabled(tbEditWorld.isSelected()); 72 | add(rbStart); 73 | 74 | rbFinish.addActionListener(this); 75 | rbFinish.setSelected(drawMod == DrawingPanel.FINISH); 76 | rbFinish.setEnabled(tbEditWorld.isSelected()); 77 | add(rbFinish); 78 | 79 | ButtonGroup gp = new ButtonGroup(); 80 | gp.add(rbDraw); 81 | gp.add(rbErase); 82 | gp.add(rbStart); 83 | gp.add(rbFinish); 84 | 85 | spnRows.setModel(new SpinnerNumberModel(rows, 2, 100, 1)); 86 | spnRows.addChangeListener(this); 87 | spnRows.setEnabled(false); 88 | add(spnRows); 89 | 90 | spnColumns.setModel(new SpinnerNumberModel(columns, 2, 100, 1)); 91 | spnColumns.addChangeListener(this); 92 | spnColumns.setEnabled(false); 93 | add(spnColumns); 94 | 95 | add(new JLabel("Click Edit button to modify Robot World and drag mouse on Robot World to change it.")); 96 | 97 | } 98 | 99 | @Override 100 | public void actionPerformed(ActionEvent e) { 101 | Object obj = e.getSource(); 102 | 103 | if (obj.equals(clearButton)) { 104 | if (tbEditWorld.isSelected()) { 105 | drawingPanel.clear(); 106 | } 107 | } else if (obj.equals(rbDraw)) { 108 | drawMod = tbEditWorld.isSelected() ? DrawingPanel.DRAW : DrawingPanel.NONE; 109 | } else if (obj.equals(rbErase)) { 110 | drawMod = tbEditWorld.isSelected() ? DrawingPanel.ERASE : DrawingPanel.NONE; 111 | } else if (obj.equals(rbStart)) { 112 | drawMod = tbEditWorld.isSelected() ? DrawingPanel.START : DrawingPanel.NONE; 113 | } else if (obj.equals(rbFinish)) { 114 | drawMod = tbEditWorld.isSelected() ? DrawingPanel.FINISH : DrawingPanel.NONE; 115 | } else if (obj.equals(tbEditWorld)) { 116 | spnRows.setEnabled(tbEditWorld.isSelected()); 117 | spnColumns.setEnabled(tbEditWorld.isSelected()); 118 | clearButton.setEnabled(tbEditWorld.isSelected()); 119 | rbDraw.setEnabled(tbEditWorld.isSelected()); 120 | rbErase.setEnabled(tbEditWorld.isSelected()); 121 | rbStart.setEnabled(tbEditWorld.isSelected()); 122 | rbFinish.setEnabled(tbEditWorld.isSelected()); 123 | if (tbEditWorld.isSelected()) { 124 | rbDraw.setSelected(true); 125 | drawMod = DrawingPanel.DRAW; 126 | } else { 127 | drawMod = DrawingPanel.NONE; 128 | } 129 | } 130 | drawingPanel.setDrawingMod(drawMod); 131 | } 132 | 133 | @Override 134 | public void stateChanged(ChangeEvent arg0) { 135 | drawingPanel.setWorldSize(Integer.parseInt(spnRows.getValue().toString()), 136 | Integer.parseInt(spnColumns.getValue().toString())); 137 | } 138 | 139 | } 140 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/DoubleArrayValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import java.util.Arrays; 4 | 5 | import pk.com.habsoft.robosim.filters.core.Attribute; 6 | 7 | /** 8 | * This class implements an attribute value that is defined with a double array. 9 | * In general, it is reccomended that a series of {@link RealValue} attributes 10 | * is defined instead of using this class, because the series of individual 11 | * {@link RealValue}s will have better compatibility with existing BURLAP tools 12 | * and algorithms, but this value can be used in cases where there is a very 13 | * large number of double values that have to be stored in each state to cut 14 | * down on memory overhead. 15 | * 16 | * @author James MacGlashan 17 | * 18 | */ 19 | public class DoubleArrayValue extends OOMDPValue { 20 | 21 | protected final double[] doubleArray; 22 | 23 | public DoubleArrayValue(Attribute attribute) { 24 | super(attribute); 25 | this.doubleArray = null; 26 | } 27 | 28 | public DoubleArrayValue(DoubleArrayValue v) { 29 | super(v); 30 | DoubleArrayValue daValue = v; 31 | if (daValue.doubleArray != null) { 32 | this.doubleArray = daValue.doubleArray.clone(); 33 | } else { 34 | this.doubleArray = null; 35 | } 36 | } 37 | 38 | public DoubleArrayValue(Attribute attribute, double[] doubleArray) { 39 | super(attribute); 40 | this.doubleArray = doubleArray; 41 | } 42 | 43 | @Override 44 | public Value copy() { 45 | return new DoubleArrayValue(this); 46 | } 47 | 48 | @Override 49 | public boolean valueHasBeenSet() { 50 | return this.doubleArray != null; 51 | } 52 | 53 | @Override 54 | public Value setValue(String v) { 55 | if (v.startsWith("\"") && v.endsWith("\"")) { 56 | v = v.substring(1, v.length()); 57 | } 58 | String[] comps = v.split(","); 59 | double[] doubleArray = new double[comps.length]; 60 | for (int i = 0; i < comps.length; i++) { 61 | doubleArray[i] = Double.parseDouble(comps[i]); 62 | } 63 | return new DoubleArrayValue(this.attribute, doubleArray); 64 | } 65 | 66 | @Override 67 | public StringBuilder buildStringVal(StringBuilder builder) { 68 | for (int i = 0; i < this.doubleArray.length; i++) { 69 | if (i > 0) { 70 | builder.append(","); 71 | } 72 | builder.append(this.doubleArray[i]); 73 | } 74 | return builder; 75 | } 76 | 77 | @Override 78 | public double getNumericRepresentation() { 79 | double sum = 0; 80 | for (double v : this.doubleArray) { 81 | sum *= 31; 82 | sum += v; 83 | } 84 | return sum; 85 | } 86 | 87 | @Override 88 | public Value setValue(int[] intArray) { 89 | 90 | double[] doubleArray = new double[intArray.length]; 91 | for (int i = 0; i < intArray.length; i++) { 92 | doubleArray[i] = intArray[i]; 93 | } 94 | return new DoubleArrayValue(this.attribute, doubleArray); 95 | } 96 | 97 | @Override 98 | public Value setValue(double[] doubleArray) { 99 | return new DoubleArrayValue(this.attribute, doubleArray); 100 | } 101 | 102 | @Override 103 | public int[] getIntArray() { 104 | if (this.doubleArray == null) { 105 | throw new RuntimeException("Error, double array value is unset, cannot return a value for it."); 106 | } 107 | int[] intArray = new int[this.doubleArray.length]; 108 | for (int i = 0; i < this.doubleArray.length; i++) { 109 | intArray[i] = (int) this.doubleArray[i]; 110 | } 111 | return intArray; 112 | } 113 | 114 | @Override 115 | public double[] getDoubleArray() { 116 | if (this.doubleArray == null) { 117 | throw new RuntimeException("Error, double array value is unset, cannot return a value for it."); 118 | } 119 | return this.doubleArray; 120 | } 121 | 122 | @Override 123 | public int hashCode() { 124 | final int prime = 31; 125 | int result = 1; 126 | result = prime * result + Arrays.hashCode(doubleArray); 127 | return result; 128 | } 129 | 130 | @Override 131 | public boolean equals(Object obj) { 132 | if (this == obj) { 133 | return true; 134 | } 135 | if (!(obj instanceof DoubleArrayValue)) { 136 | return false; 137 | } 138 | 139 | DoubleArrayValue o = (DoubleArrayValue) obj; 140 | 141 | if (!o.attribute.equals(this.attribute)) { 142 | return false; 143 | } 144 | 145 | if (this.doubleArray.length != o.doubleArray.length) { 146 | return false; 147 | } 148 | 149 | for (int i = 0; i < this.doubleArray.length; i++) { 150 | if (this.doubleArray[i] != o.doubleArray[i]) { 151 | return false; 152 | } 153 | } 154 | 155 | return true; 156 | 157 | } 158 | } 159 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/filters/core/values/MultiTargetRelationalValue.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.filters.core.values; 2 | 3 | import java.util.Arrays; 4 | import java.util.Collection; 5 | import java.util.Collections; 6 | import java.util.Set; 7 | import java.util.TreeSet; 8 | 9 | import pk.com.habsoft.robosim.filters.core.Attribute; 10 | 11 | /** 12 | * A multi-target relational value object subclass. Values are stored as an 13 | * ordered set (TreeSet) of string names of the object instance name 14 | * identifiers. If the attribute is not linked to any target, the set will be 15 | * empty. 16 | * 17 | * @author James MacGlashan 18 | * 19 | */ 20 | public class MultiTargetRelationalValue extends OOMDPValue implements Value { 21 | 22 | /** 23 | * The set of object targets to which this value points. Object targets are 24 | * indicated by their object name identifier. 25 | */ 26 | protected final Set targetObjects; 27 | 28 | /** 29 | * Initializes the value to be associted with the given attribute 30 | * 31 | * @param attribute 32 | * the attribute with which this value is associated 33 | */ 34 | public MultiTargetRelationalValue(Attribute attribute) { 35 | super(attribute); 36 | this.targetObjects = Collections.unmodifiableSet(new TreeSet()); 37 | } 38 | 39 | /** 40 | * Initializes this value as a copy from the source Value object v. 41 | * 42 | * @param v 43 | * the source Value to make this object a copy of. 44 | */ 45 | public MultiTargetRelationalValue(MultiTargetRelationalValue v) { 46 | super(v); 47 | MultiTargetRelationalValue rv = v; 48 | this.targetObjects = rv.targetObjects; 49 | } 50 | 51 | public MultiTargetRelationalValue(Attribute attribute, Collection targets) { 52 | super(attribute); 53 | this.targetObjects = Collections.unmodifiableSet(new TreeSet(targets)); 54 | } 55 | 56 | @Override 57 | public Value copy() { 58 | return new MultiTargetRelationalValue(this); 59 | } 60 | 61 | @Override 62 | public boolean valueHasBeenSet() { 63 | return true; 64 | } 65 | 66 | @Override 67 | public Value setValue(String v) { 68 | if (v.indexOf(';') != -1) { 69 | return new MultiTargetRelationalValue(this.attribute, Arrays.asList(v.split(";"))); 70 | } 71 | return new MultiTargetRelationalValue(this.attribute, Arrays.asList(v)); 72 | } 73 | 74 | @Override 75 | public Value addRelationalTarget(String t) { 76 | TreeSet targets = new TreeSet(this.targetObjects); 77 | targets.add(t); 78 | return new MultiTargetRelationalValue(this.attribute, targets); 79 | } 80 | 81 | @Override 82 | public Value addAllRelationalTargets(Collection targets) { 83 | TreeSet allTargets = new TreeSet(this.targetObjects); 84 | allTargets.addAll(targets); 85 | return new MultiTargetRelationalValue(this.attribute, allTargets); 86 | } 87 | 88 | @Override 89 | public Value clearRelationTargets() { 90 | return new MultiTargetRelationalValue(this.attribute); 91 | } 92 | 93 | @Override 94 | public Value removeRelationalTarget(String target) { 95 | TreeSet targets = new TreeSet(this.targetObjects); 96 | targets.remove(target); 97 | return new MultiTargetRelationalValue(this.attribute, targets); 98 | } 99 | 100 | @Override 101 | public Set getAllRelationalTargets() { 102 | return this.targetObjects; 103 | } 104 | 105 | @Override 106 | public StringBuilder buildStringVal(StringBuilder builder) { 107 | boolean didFirst = false; 108 | for (String t : this.targetObjects) { 109 | if (didFirst) { 110 | builder.append(";"); 111 | } 112 | builder.append(t); 113 | didFirst = true; 114 | } 115 | return builder; 116 | } 117 | 118 | @Override 119 | public double getNumericRepresentation() { 120 | return 0; 121 | } 122 | 123 | @Override 124 | public int hashCode() { 125 | final int prime = 31; 126 | int result = 1; 127 | result = prime * result + ((targetObjects == null) ? 0 : targetObjects.hashCode()); 128 | return result; 129 | } 130 | 131 | @Override 132 | public boolean equals(Object obj) { 133 | if (this == obj) { 134 | return true; 135 | } 136 | if (!(obj instanceof MultiTargetRelationalValue)) { 137 | return false; 138 | } 139 | 140 | MultiTargetRelationalValue op = (MultiTargetRelationalValue) obj; 141 | if (!op.attribute.equals(attribute)) { 142 | return false; 143 | } 144 | 145 | if (this.targetObjects.size() != op.targetObjects.size()) { 146 | return false; 147 | } 148 | 149 | for (String t : this.targetObjects) { 150 | if (!op.targetObjects.contains(t)) { 151 | return false; 152 | } 153 | } 154 | 155 | return true; 156 | 157 | } 158 | } 159 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/smoothing/views/PIDSimulator.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.smoothing.views; 2 | 3 | import pk.com.habsoft.robosim.filters.particles.BigRobot; 4 | import pk.com.habsoft.robosim.filters.particles.internal.IRobot; 5 | import pk.com.habsoft.robosim.smoothing.controller.Controller; 6 | import pk.com.habsoft.robosim.smoothing.controller.PIDController; 7 | 8 | public class PIDSimulator { 9 | 10 | public double tauP = 0.2, tauD = 3.0, tauI = 0.004; 11 | public int steerDrift = 10;// in degrees 12 | private double speed = 1; 13 | private int iter = 100; 14 | 15 | public double getTauP() { 16 | return tauP; 17 | } 18 | 19 | public void setTauP(double tauP) { 20 | this.tauP = tauP; 21 | } 22 | 23 | public double getTauD() { 24 | return tauD; 25 | } 26 | 27 | public void setTauD(double tauD) { 28 | this.tauD = tauD; 29 | } 30 | 31 | public double getTauI() { 32 | return tauI; 33 | } 34 | 35 | public void setTauI(double tauI) { 36 | this.tauI = tauI; 37 | } 38 | 39 | public void setSteerDrift(int steerDrift) { 40 | this.steerDrift = steerDrift; 41 | } 42 | 43 | public int getSteerDrift() { 44 | return steerDrift; 45 | } 46 | 47 | public double[][] getRefData() { 48 | double[][] data = new double[getIter()][2]; 49 | for (int i = 0; i < getIter(); i++) { 50 | data[i][0] = i * getSpeed(); 51 | data[i][1] = 0; 52 | } 53 | return data; 54 | } 55 | 56 | // public double[][] getPData(boolean drift) { 57 | // 58 | // IRobot robot = getRobot(drift); 59 | // Controller c = new PController(getTauP()); 60 | // double[][] data = new double[iter][2]; 61 | // for (int i = 0; i < iter; i++) { 62 | // double steering = c.getCommand(robot.getY()); 63 | // robot.move(steering, speed); 64 | // data[i][0] = robot.getX(); 65 | // data[i][1] = robot.getY(); 66 | // } 67 | // 68 | // return data; 69 | // } 70 | // 71 | // public double[][] getPDData(boolean drift) { 72 | // 73 | // IRobot robot = getRobot(drift); 74 | // Controller c = new PDController(getTauP(), getTauD()); 75 | // double[][] data = new double[iter][2]; 76 | // for (int i = 0; i < iter; i++) { 77 | // double steering = c.getCommand(robot.getY()); 78 | // robot.move(steering, speed); 79 | // data[i][0] = robot.getX(); 80 | // data[i][1] = robot.getY(); 81 | // } 82 | // return data; 83 | // } 84 | // 85 | // public double[][] getPIData(boolean drift) { 86 | // 87 | // IRobot robot = getRobot(drift); 88 | // Controller c = new PIController(getTauP(), getTauI()); 89 | // double[][] data = new double[iter][2]; 90 | // for (int i = 0; i < iter; i++) { 91 | // double steering = c.getCommand(robot.getY()); 92 | // robot.move(steering, speed); 93 | // data[i][0] = robot.getX(); 94 | // data[i][1] = robot.getY(); 95 | // } 96 | // return data; 97 | // } 98 | // 99 | // public double[][] getPIDData(boolean drift) { 100 | // 101 | // IRobot robot = getRobot(drift); 102 | // Controller c = new PIDController(getTauP(), getTauI(), getTauD()); 103 | // double[][] data = new double[iter][2]; 104 | // for (int i = 0; i < iter; i++) { 105 | // double steering = c.getCommand(robot.getY()); 106 | // robot.move(steering, speed); 107 | // data[i][0] = robot.getX(); 108 | // data[i][1] = robot.getY(); 109 | // } 110 | // return data; 111 | // } 112 | 113 | public double[][] getPData(boolean drift) { 114 | Controller c = new PIDController(getTauP(), 0, 0); 115 | return getData(c, drift); 116 | 117 | } 118 | 119 | public double[][] getDData(boolean drift) { 120 | Controller c = new PIDController(0, 0, getTauD()); 121 | return getData(c, drift); 122 | 123 | } 124 | 125 | public double[][] getIData(boolean drift) { 126 | Controller c = new PIDController(0, getTauI(), 0); 127 | return getData(c, drift); 128 | 129 | } 130 | 131 | public double[][] getPDData(boolean drift) { 132 | Controller c = new PIDController(getTauP(), 0, getTauD()); 133 | return getData(c, drift); 134 | 135 | } 136 | 137 | public double[][] getPIData(boolean drift) { 138 | Controller c = new PIDController(getTauP(), getTauI(), 0); 139 | return getData(c, drift); 140 | 141 | } 142 | 143 | public double[][] getPIDData(boolean drift) { 144 | Controller c = new PIDController(getTauP(), getTauI(), getTauD()); 145 | return getData(c, drift); 146 | 147 | } 148 | 149 | public double[][] getData(Controller c, boolean drift) { 150 | 151 | IRobot robot = getRobot(drift); 152 | double[][] data = new double[getIter()][2]; 153 | for (int i = 0; i < getIter(); i++) { 154 | double steering = c.getCommand(robot.getY()); 155 | robot.move(steering, getSpeed()); 156 | data[i][0] = robot.getX(); 157 | data[i][1] = robot.getY(); 158 | } 159 | return data; 160 | } 161 | 162 | private IRobot getRobot(boolean drift) { 163 | IRobot robot = new BigRobot(20); 164 | robot.setLocation(0, 1, 0); 165 | robot.setCheckBoundaries(false); 166 | if (drift) 167 | robot.setSteeringDrift(Math.toRadians(steerDrift)); 168 | return robot; 169 | } 170 | 171 | public void setIter(int iter) { 172 | this.iter = iter; 173 | } 174 | 175 | public int getIter() { 176 | return iter; 177 | } 178 | 179 | public void setSpeed(double speed) { 180 | this.speed = speed; 181 | } 182 | 183 | public double getSpeed() { 184 | return speed; 185 | } 186 | 187 | } 188 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/BFSAlgorithm.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import java.util.ArrayList; 4 | import java.util.LinkedList; 5 | import java.util.List; 6 | import java.util.Queue; 7 | 8 | import pk.com.habsoft.robosim.planning.internal.DiscreteWorld; 9 | import pk.com.habsoft.robosim.planning.internal.Path; 10 | import pk.com.habsoft.robosim.planning.internal.PathNode; 11 | import pk.com.habsoft.robosim.planning.internal.WorldNode; 12 | 13 | public class BFSAlgorithm extends Algorithm { 14 | 15 | // PriorityQueue q = new PriorityQueue(); 16 | Queue queue = new LinkedList(); 17 | List closed = new ArrayList(); 18 | private boolean allowDiagonalMotion; 19 | 20 | public BFSAlgorithm(DiscreteWorld world, boolean diagonalMotion) { 21 | this.world = world; 22 | this.allowDiagonalMotion = diagonalMotion; 23 | path = new Path(); 24 | 25 | policy = new int[world.getRows()][world.getColumns()]; 26 | expand = new double[world.getRows()][world.getColumns()]; 27 | solve(); 28 | } 29 | 30 | @Override 31 | void solve() { 32 | for (int i = 0; i < world.getRows(); i++) { 33 | for (int j = 0; j < world.getColumns(); j++) { 34 | expand[i][j] = -1; 35 | if (world.isHidden(i, j)) { 36 | policy[i][j] = HIDDEN; 37 | } else if (world.isOpen(i, j)) 38 | policy[i][j] = NOT_EXPLORED; 39 | else { 40 | policy[i][j] = BLOCK; 41 | blocked++; 42 | } 43 | } 44 | } 45 | // Cost of each node 46 | int cost = 1; 47 | // w.setStartNode(0, 0); 48 | // w.setGoalNode(world[0].length, world.length); 49 | 50 | boolean solve = false; 51 | boolean resign = false; 52 | WorldNode start = world.getTempStart(); 53 | queue.add(start); 54 | closed.add(start); 55 | while (!solve && !resign) { 56 | if (queue.isEmpty()) { 57 | resign = true; 58 | } else { 59 | WorldNode current = queue.poll(); 60 | int x = current.getxLoc(); 61 | int y = current.getyLoc(); 62 | int g = current.getDepth(); 63 | if (world.isHidden(x, y)) { 64 | policy[x][y] = HIDDEN; 65 | } else { 66 | expand[x][y] = exploredSize++; 67 | policy[x][y] = EXPLORED; 68 | } 69 | if (world.isGoal(current)) { 70 | solve = true; 71 | result = "Goal Found"; 72 | } else { 73 | int incr = allowDiagonalMotion ? 1 : 2; 74 | for (int i = 0; i < deltas.length; i = i + incr) { 75 | int x2 = x + deltas[i][0]; 76 | int y2 = y + deltas[i][1]; 77 | // if path is open 78 | if (world.isOpen(x2, y2)) { 79 | int g2 = g + cost; 80 | WorldNode toExplore = new WorldNode(x2, y2); 81 | instances++; 82 | if (!closed.contains(toExplore)) { 83 | toExplore.setDepth(g2); 84 | toExplore.setAction(i); 85 | queue.add(toExplore); 86 | closed.add(toExplore); 87 | } 88 | } 89 | } 90 | } 91 | } 92 | } 93 | unExploredSize = world.getRows() * world.getColumns() - exploredSize - blocked; 94 | 95 | WorldNode last = new WorldNode(world.getStart().getxLoc(), world.getStart().getyLoc()); 96 | if (solve) { 97 | last = new WorldNode(closed.get(closed.indexOf(world.getGoal()))); 98 | path.add(new PathNode(last.getxLoc(), last.getyLoc())); 99 | } 100 | 101 | String[][] temp = new String[policy.length][policy[0].length]; 102 | if (last != null) { 103 | while (last.getAction() != -1) { 104 | pathSize++; 105 | int x2 = last.getxLoc() - deltas[last.getAction()][0]; 106 | int y2 = last.getyLoc() - deltas[last.getAction()][1]; 107 | policy[x2][y2] = last.getAction(); 108 | temp[x2][y2] = DELTA_NAMES[last.getAction()]; 109 | last.setxLoc(x2); 110 | last.setyLoc(y2); 111 | last = closed.get(closed.indexOf(last)); 112 | path.add(new PathNode(last.getxLoc(), last.getyLoc())); 113 | 114 | } 115 | if (pathSize == 1) { 116 | pathSize = 0; 117 | } 118 | } 119 | // for (String[] strings : temp) { 120 | // System.out.println(Arrays.toString(strings)); 121 | // } 122 | // for (int[] strings : expand) { 123 | // System.out.println(Arrays.toString(strings)); 124 | // } 125 | // System.out.println(world.printWorld()); 126 | 127 | } 128 | 129 | @Override 130 | public int[][] getPolicy() { 131 | return policy; 132 | } 133 | 134 | @Override 135 | public double[][] getExpand() { 136 | return expand; 137 | } 138 | 139 | @Override 140 | public int getExploredSize() { 141 | return exploredSize; 142 | } 143 | 144 | @Override 145 | public int getUnExploredSize() { 146 | return unExploredSize; 147 | } 148 | 149 | @Override 150 | public int getPathSize() { 151 | return pathSize; 152 | } 153 | 154 | @Override 155 | public double[][] getHeuristic() { 156 | return new double[world.getRows()][world.getColumns()]; 157 | } 158 | 159 | @Override 160 | public int getBlockedSize() { 161 | return blocked; 162 | } 163 | 164 | @Override 165 | public int getTotalSize() { 166 | return exploredSize + unExploredSize + blocked; 167 | } 168 | 169 | @Override 170 | public String getResult() { 171 | return result; 172 | } 173 | 174 | @Override 175 | public Path getPath() { 176 | return path; 177 | } 178 | 179 | @Override 180 | public int getTotalInstances() { 181 | return instances; 182 | } 183 | } 184 | -------------------------------------------------------------------------------- /src/main/java/pk/com/habsoft/robosim/planning/algos/DynamicPrograming2D.java: -------------------------------------------------------------------------------- 1 | package pk.com.habsoft.robosim.planning.algos; 2 | 3 | import pk.com.habsoft.robosim.planning.internal.DiscreteWorld; 4 | import pk.com.habsoft.robosim.planning.internal.Path; 5 | import pk.com.habsoft.robosim.utils.RoboMathUtils; 6 | 7 | /* 8 | * 2D dynamic programming stochastic 9 | */ 10 | public class DynamicPrograming2D extends Algorithm { 11 | // static int[][] map = { { 0, 0, 1, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, { 0, 12 | // 0, 1, 1, 0, 0 }, { 0, 0, 0, 0, 1, 0 }, { 0, 0, 0, 0, 1, 0 } }; 13 | static int[][] map = { { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 } }; 14 | 15 | // int[][] policy; 16 | // double[][] expand; 17 | int[][] actions; 18 | 19 | int initCost = 1000000; 20 | int collisionCost = 100; 21 | double sProb = 0.8; 22 | double fProb = (1 - sProb) / 2; 23 | 24 | public static void main(String[] args) { 25 | DiscreteWorld w = new DiscreteWorld(map); 26 | w.setGoalNode(map.length - 1, map[0].length - 1); 27 | new DynamicPrograming2D(w, false, 0.8); 28 | } 29 | 30 | private boolean allowDiagonalMotion; 31 | 32 | public DynamicPrograming2D(DiscreteWorld world, boolean diagonalMotion, double successProb) { 33 | this.world = world; 34 | this.allowDiagonalMotion = diagonalMotion; 35 | sProb = successProb; 36 | fProb = (1 - sProb) / 2; 37 | 38 | if (allowDiagonalMotion) { 39 | actions = deltas; 40 | } else { 41 | actions = deltas2; 42 | } 43 | 44 | policy = new int[world.getRows()][world.getColumns()]; 45 | expand = new double[world.getRows()][world.getColumns()]; 46 | 47 | solve(); 48 | } 49 | 50 | @Override 51 | void solve() { 52 | for (int i = 0; i < world.getRows(); i++) { 53 | for (int j = 0; j < world.getColumns(); j++) { 54 | expand[i][j] = initCost; 55 | // if (world.isHidden(i, j)) { 56 | // policy[i][j] = HIDDEN; 57 | // } else 58 | if (world.isOpen(i, j)) 59 | policy[i][j] = NOT_EXPLORED; 60 | else { 61 | policy[i][j] = BLOCK; 62 | blocked++; 63 | } 64 | } 65 | } 66 | // Cost of each node 67 | int cost = 1; 68 | // int incr = allowDiagonalMotion ? 1 : 2; 69 | 70 | boolean change = true; 71 | while (change) { 72 | change = false; 73 | for (int x = 0; x < world.getRows(); x++) { 74 | for (int y = 0; y < world.getColumns(); y++) { 75 | if (world.isGoal(x, y)) { 76 | result = "Goal Found"; 77 | if (expand[x][y] > 0) { 78 | expand[x][y] = 0; 79 | change = true; 80 | if (world.isHidden(x, y)) { 81 | policy[x][y] = HIDDEN; 82 | } else if (policy[x][y] >= deltas.length) { 83 | policy[x][y] = Algorithm.EXPLORED; 84 | exploredSize++; 85 | } 86 | 87 | } 88 | } else if (world.isOpen(x, y)) { 89 | 90 | for (int k = 0; k < actions.length; k = k + 1) { 91 | instances++; 92 | double v2 = cost; 93 | v2 += sProb * getCost(x, y, k); 94 | v2 += fProb * getCost(x, y, RoboMathUtils.modulus(k - 1, actions.length, true)); 95 | v2 += fProb * getCost(x, y, RoboMathUtils.modulus(k + 1, actions.length, true)); 96 | if (v2 < expand[x][y]) { 97 | change = true; 98 | expand[x][y] = v2; 99 | if (world.isHidden(x, y)) { 100 | policy[x][y] = HIDDEN; 101 | } else if (policy[x][y] >= deltas.length) { 102 | exploredSize++; 103 | pathSize++; 104 | } 105 | policy[x][y] = allowDiagonalMotion ? k : k * 2; 106 | 107 | } 108 | } 109 | } 110 | } 111 | } 112 | } 113 | unExploredSize = world.getRows() * world.getColumns() - exploredSize - blocked; 114 | 115 | // for (double[] strings : expand) { 116 | // System.out.println(Arrays.toString(strings)); 117 | // } 118 | // System.out.println(world.printWorld()); 119 | 120 | // for (int i = 0; i < world.getHeight(); i++) { 121 | // for (int j = 0; j < world.getWidth(); j++) { 122 | // if (policy[i][j] < 8) { 123 | // System.out.print(DELTA_NAME[policy[i][j]] + " "); 124 | // } 125 | // } 126 | // System.out.println(); 127 | // } 128 | 129 | } 130 | 131 | private double getCost(int x, int y, int i) { 132 | int x2 = x + actions[i][0]; 133 | int y2 = y + actions[i][1]; 134 | if (world.isOpen(x2, y2)) 135 | return expand[x2][y2]; 136 | return collisionCost; 137 | } 138 | 139 | @Override 140 | public int[][] getPolicy() { 141 | return policy; 142 | } 143 | 144 | @Override 145 | public double[][] getExpand() { 146 | return expand; 147 | } 148 | 149 | @Override 150 | public int getExploredSize() { 151 | return exploredSize; 152 | } 153 | 154 | @Override 155 | public int getUnExploredSize() { 156 | return unExploredSize; 157 | } 158 | 159 | @Override 160 | public int getPathSize() { 161 | return pathSize; 162 | } 163 | 164 | @Override 165 | public double[][] getHeuristic() { 166 | return new double[world.getRows()][world.getColumns()]; 167 | } 168 | 169 | @Override 170 | public int getBlockedSize() { 171 | return blocked; 172 | } 173 | 174 | @Override 175 | public int getTotalSize() { 176 | return exploredSize + unExploredSize + blocked; 177 | } 178 | 179 | @Override 180 | public String getResult() { 181 | return result; 182 | } 183 | 184 | @Override 185 | public Path getPath() { 186 | return path; 187 | } 188 | 189 | @Override 190 | public int getTotalInstances() { 191 | return instances; 192 | } 193 | } 194 | --------------------------------------------------------------------------------