[doc] Add a tutorial for noise and ghost

doc/morse/tutorials.rst

@@ -87,6 +87,7 @@ with specific requirements.
     user/advanced_tutorials/ros_nav_tutorial
     user/advanced_tutorials/mocap_tutorial
     user/advanced_tutorials/request_tutorial
+    user/advanced_tutorials/noise_ghost_tutorial
 
 Multi-node tutorials
 --------------------

doc/morse/user/advanced_tutorials/noise_ghost_tutorial.rst

@@ -0,0 +1,143 @@
+Noise and ghost tutorial :tag:`builder` :tag:`pymorse`
+======================================================
+
+This tutorial will show how to add and configure noise modifiers
+to your components, and will illustrate the use ``ghost`` robots.
+
+.. image:: ../../../media/morse-noise-ghost-tutorial.png
+   :width: 300pt
+   :align: center
+
+Pre-requisites
+--------------
+
+- You must have completed the :doc:`first tutorial <../beginner_tutorials/tutorial>`.
+- It is also advised to have completed the :doc:`cat and mouse tutorial <cat_and_mouse>`.
+
+Creating the scenario
+---------------------
+
+We'll use the Builder API to configure the robots in the scenario.
+First we will configure the real ATRV robot.
+
+- Create a new ATRV robot:
+
+  .. code-block:: python
+
+    from morse.builder import *
+
+    robot = ATRV()
+
+- We will add a pose sensor to it, and make the data available through the socket interface:
+
+  .. code-block:: python
+    
+    pose = Pose()
+    robot.append(pose)
+    pose.add_stream('socket')
+
+- Next we make it controllable by the keyboard, using the correct actuator:
+
+  .. code-block:: python
+
+    keyboard = Keyboard()
+    robot.append(keyboard)
+
+- Finally we alter the initial pose data with a Gaussian noise:
+
+  .. code-block:: python
+    
+    pose.alter('Noise', pos_std=.5, rot_std=0, _2D=True)
+    
+  .. note::
+
+    The noise parameters are documented in the :doc:`Noise modifier <../modifiers/pose_noise>`.
+
+Now we'll create the ghost robot: it is aimed to represent some
+external computation, without conflicting with the simulated robots.
+
+- Create another ATRV robot, the *ghost*:
+
+  .. code-block:: python
+
+    ghost = ATRV()
+
+- Next we add a :doc:`Teleport <../actuators/teleport>` actuator to control it:
+
+  .. code-block:: python
+
+    teleport = Teleport()
+    ghost.append(teleport)
+
+- Finally, we make the robot a ghost, and add the socket interface to it:
+
+  .. code-block:: python
+
+    ghost.make_ghost()
+    ghost.add_default_interface('socket')
+
+And finally we complete the scene configuration:
+
+  .. code-block:: python
+
+    env = Environment('land-1/trees')
+    env.create()
+
+The complete script can be found at: ``$MORSE_SRC/examples/tutorials/noise_ghost_tutorial.py``.
+
+Ghost is noise script
+---------------------
+
+As a very simple example of how to use the ghost robot, we'll create
+a Python script to connect to MORSE and display the noisy position:
+
+.. code-block:: python
+
+    import pymorse
+    import time
+    
+    with pymorse.Morse() as morse:
+      while True:
+        pose = morse.robot.pose.get()
+        morse.ghost.teleport.publish(pose)
+        time.sleep(.1)
+    
+The whole program can be found at: ``$MORSE_SRC/examples/clients/atrv/ghost_noise_script.py``
+
+Run morse with the builder script to create the scenario. You will be 
+able to control the robot with the arrow keys on the keyboard::
+
+  $ cd $MORSE_SRC/examples/tutorials
+  $ morse run noise_ghost_tutorial.py
+
+Then run the Python noise display script from another terminal. The ghost
+position will show the noisy robot position as altered by the Noise modifier.::
+
+  $ cd $MORSE_SRC/examples/clients/atrv
+  $ python3 ghost_noise_script.py
+
+Ghost is filtered script
+------------------------
+
+Using the ghost robot to show the noise applied to the position data
+is actually not very interesting. In this script, we will see how we can
+use the ghost to display more useful information, for instance the
+robot position estimated from the noisy position.
+
+For this script, you need to install the ``numpy`` and ``scipy`` python packages::
+
+  $ sudo apt-get install python3-numpy python3-scipy
+  
+or::
+
+  $ sudo easy_install3 numpy scipy
+  
+The whole program can be found at: ``$MORSE_SRC/examples/clients/atrv/ghost_estimation_script.py``
+
+It differs from the previous one in the fact that the robot position
+is estimated from the measured noisy position of the robot.
+
+To run it, just launch this script instead of the previous one::
+
+  $ python3 ghost_estimation_script.py
+

examples/clients/atrv/ghost_estimation_script.py

@@ -0,0 +1,48 @@
+import pymorse
+import time
+import numpy as np
+from scipy.linalg import solve_continuous_are
+
+'''
+If you are confident in the measures, increase
+the confidence so that measure_confidence >> 1.
+Otherwise, make it << 1. 
+'''
+measure_confidence = 1
+
+Aext = np.mat('[0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 0 0]')
+Cext = np.mat('[1 0 0 0; 0 0 1 0]')
+n = Aext.shape[0]
+ny = Cext.shape[0]
+G = np.identity(n);
+Q = measure_confidence * np.identity(n);
+R = np.identity(ny);
+
+P = solve_continuous_are(Aext.T, Cext.T, Q, R)
+
+L = P * Cext.T * R;
+
+dt = .1
+
+A = np.identity(n) + dt * (Aext - L * Cext)
+B = L * dt
+C = Cext
+
+xest_k = np.mat('[0; 0; 0; 0]')
+
+with pymorse.Morse() as morse:
+
+    while True:
+
+        pose = morse.robot.pose.get()
+        z = np.mat([[pose['x']], [pose['y']]])
+
+        x_ = C * xest_k
+
+        pose['x'] = float(x_[0][0])
+        pose['y'] = float(x_[1][0])
+        morse.ghost.teleport.publish(pose)
+
+        xest_k = A * xest_k + B * z
+
+        time.sleep(dt)

examples/clients/atrv/ghost_noise_script.py

@@ -0,0 +1,8 @@
+import pymorse
+import time
+
+with pymorse.Morse() as morse:
+    while True:
+        pose = morse.robot.pose.get()
+        morse.ghost.teleport.publish(pose)
+        time.sleep(.1)

examples/tutorials/noise_ghost_tutorial.py

@@ -0,0 +1,18 @@
+from morse.builder import *
+
+robot = ATRV()
+pose = Pose()
+robot.append(pose)
+pose.add_stream('socket')
+keyboard = Keyboard()
+robot.append(keyboard)
+pose.alter('Noise', pos_std=.5, rot_std=0, _2D='True')
+
+ghost = ATRV()
+ghost.make_ghost()
+teleport = Teleport()
+ghost.append(teleport)
+ghost.add_default_interface('socket')
+
+env = Environment("land-1/trees")
+env.create()