Permalink
Browse files

[doc] Add a tutorial for noise and ghost

  • Loading branch information...
1 parent 86f7407 commit 9b9941698c1d329acaa07302b6ec588788e587c2 @lesire lesire committed with adegroote Apr 3, 2013
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
@@ -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
--------------------
@@ -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
+
@@ -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)
@@ -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)
@@ -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()

0 comments on commit 9b99416

Please sign in to comment.