IVIGIT.

CMakeLists.txt

+CMAKE_MINIMUM_REQUIRED(VERSION 2.8 FATAL_ERROR)
+PROJECT(pinocchio_tutos)
+
+FIND_PROGRAM(ASCIIDOC NAMES asciidoc)
+set(ASCIIDOC ${ASCIIDOC} --filter latex -a toc -a icons -o)
+
+# --- ASCIIDOC RULES ---------------------------------------------------------------
+MACRO(ADD_ASCIIDOC_TARGET asciidocfile htmlfile)
+  # Create a "CUSTOM_COMMAND" to create the htmlfile from the asciidoc file.
+  add_custom_command(OUTPUT ${htmlfile}
+    COMMAND ${ASCIIDOC}  ${htmlfile} ${asciidocfile} 
+    DEPENDS ${asciidocfile}
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+    COMMENT "Compiling ASCIIDOC  ${asciidocfile}"
+    VERBATIM)
+  get_filename_component(targetname ${htmlfile} NAME_WE)
+  add_custom_target(${targetname} DEPENDS ${htmlfile})
+  MESSAGE(STATUS "Add target ${targetname}")
+ENDMACRO(ADD_ASCIIDOC_TARGET)
+
+MACRO(ASCIIDOC_FROM_PYTHON_TARGET pythonfile asciidocfile)
+  # Create a "CUSTOM_COMMAND" to create a asciidoc file from a script file.
+  get_filename_component(pythonshort ${pythonfile} NAME)
+  set(SED_COMMAND     
+    -e 1s@^@The\ script\ below\ can\ be\ found\ in\ link:${pythonfile}[this\ file].\\n[source,python]\\n----\\n@
+    -e \$s@\$@\\n----\\n@)
+  add_custom_command(OUTPUT ${asciidocfile}
+    COMMAND sed  ${SED_COMMAND}  ${pythonfile} > ${asciidocfile}
+    DEPENDS ${pythonfile}
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+    COMMENT "Building ASCIIDOC from ${pythonfile}"
+    VERBATIM)
+  MESSAGE(STATUS "Add target ${pythonfile}")
+ENDMACRO(ASCIIDOC_FROM_PYTHON_TARGET)
+
+MACRO(TUTO_NEW id name)
+  # Set the variables to define a new tutorial.
+  # <id> is typically a number, <name> a string, defining the directory in source_dir tp<id>_<name>
+  set(fullid "tp${id}")
+  set(dirtargets ${dirtargets} ${fullid})
+  set(${fullid}_name ${name})
+  set(${fullid}_dir "${fullid}_${name}")
+  set(${fullid}_secundaries)
+  set(${fullid}_pythons)
+ENDMACRO(TUTO_NEW)
+
+MACRO(TUTO_ADD_PYTHON id pythons)
+  # Add a list of python to the tutorial <id>. <asciidocs> is a list of files with relative
+  # path from source_dir/tp<id>/.
+  set(fullid "tp${id}")
+  LIST(APPEND ${fullid}_pythons ${pythons})
+ENDMACRO(TUTO_ADD_PYTHON)
+
+MACRO(TUTO_ADD_ASCIIDOC id asciidocs)
+  # Add a list of asciidoc to the tutorial <id>. <asciidocs> is a list of files with relative
+  # path from source_dir/tp<id>/.
+  set(fullid "tp${id}")
+  LIST(APPEND ${fullid}_secundaries ${asciidocs})
+ENDMACRO(TUTO_ADD_ASCIIDOC)
+
+# -----------------------------------------------------------------------------------
+set(dirtargets)
+OPTION (WITH_SOLUTION "Copy and install solution scripts" OFF)
+
+tuto_new(0 install)
+tuto_add_python(0 "viewer_unittest.py;install.sh")
+
+tuto_new(1 directgeom)
+if(WITH_SOLUTION)
+  tuto_add_python(1 tp1.py)
+endif(WITH_SOLUTION)
+
+tuto_new(2 invgeom)
+tuto_add_python(2 ur5x4.py)
+if(WITH_SOLUTION)
+  tuto_add_python(2
+    "q10_parallelik.py;q1q2q3_optim_translation.py;q4q5_optim_redundancy.py;q6_optim_placement.py;q7q8_optim_quaternion.py")
+endif(WITH_SOLUTION)
+
+tuto_new(3 invkine)
+tuto_add_python(3 "mobilerobot.py;example_mobilerobot.py")
+if(WITH_SOLUTION)
+  tuto_add_python(3 tp3.py)
+endif(WITH_SOLUTION)
+
+tuto_new(4 dyn)
+tuto_add_python(4 "display.py;example_hand.py;example_qp.py;robot_hand.py")
+if(WITH_SOLUTION)
+  tuto_add_python(4 "q1q2q3_freesimu.py;q4q5_contactsimu.py")
+endif(WITH_SOLUTION)
+
+
+tuto_new(5 planner)
+tuto_add_python(5 "example_collision.py;graph.py;prm_display.py")
+if(WITH_SOLUTION)
+  tuto_add_python(5 "planner.py;")
+endif(WITH_SOLUTION)
+
+tuto_new(6 wpg)
+tuto_add_python(6 "factor.py;foot_steps.py;example_factor.py;example_foot_steps.py;example_matplotlib.py;example_romeo.py;example_scipy.py")
+
+tuto_new(7 learn)
+tuto_add_python(7 "display.py;dpendulum.py;pendulum.py")
+if(WITH_SOLUTION)
+  tuto_add_python(7 "continuous.py;ocp.py;qnet.py;qtable.py")
+endif(WITH_SOLUTION)
+
+
+# -----------------------------------------------------------------------------------
+set(dest ${CMAKE_BINARY_DIR}/html)
+file(MAKE_DIRECTORY ${dest})
+set(destpy ${dest}/python)
+file(MAKE_DIRECTORY ${destpy})
+
+set(alldeps)
+foreach(dirname ${dirtargets})
+  # --- Compile main.asciidoc
+  add_asciidoc_target(${CMAKE_SOURCE_DIR}/${${dirname}_dir}/main.asciidoc ${dest}/${dirname}.html)
+  set(dirdeps ${dest}/${dirname}.html)
+  # --- Compile secundary asciidoc files.
+  foreach(secname ${${dirname}_secundaries})
+    LIST(APPEND dirdeps ${dest}/${dirname}_${secname}.html)
+    add_asciidoc_target(
+      ${CMAKE_SOURCE_DIR}/${${dirname}_dir}/${secname}.asciidoc
+      ${dest}/${dirname}_${secname}.html
+      )
+  endforeach(secname)
+  # --- Add python files
+  foreach(pyname ${${dirname}_pythons})
+    get_filename_component(basename ${pyname} NAME_WE)
+    get_filename_component(extname ${pyname} EXT)
+    string(SUBSTRING ${extname} 1 -1 extname)
+    set(shortname ${dirname}_${basename}_${extname})
+    file(MAKE_DIRECTORY ${destpy}/${dirname})
+    add_custom_command(
+      OUTPUT ${destpy}/${dirname}/${pyname}
+      COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_SOURCE_DIR}/${${dirname}_dir}/${pyname} 
+                                        ${destpy}/${dirname}/${pyname}
+      DEPENDS ${CMAKE_SOURCE_DIR}/${${dirname}_dir}/${pyname}                                     
+      COMMENT "Copy ${pyname}")
+    LIST(APPEND dirdeps ${destpy}/${dirname}/${pyname})
+    asciidoc_from_python_target(${destpy}/${dirname}/${pyname} ${shortname}.asciidoc )
+    add_asciidoc_target(${shortname}.asciidoc ${dest}/${shortname}.html)
+    LIST(APPEND dirdeps ${dest}/${shortname}.html)
+  endforeach(pyname)
+  # Add rule tpXX_ to compile all the directory.
+  add_custom_target(${dirname}_ DEPENDS ${dirdeps})
+  LIST(APPEND alldeps ${dirname}_)
+endforeach(dirname)
+
+add_custom_target(tutos ALL DEPENDS ${alldeps})

tp0_install/install.sh

+#! /bin/sh
+
+### Install basic dependencies 
+sudo apt-get install -y cmake cmake-curses-gui cmake-qt-gui curl doxygen g++ ipython libassimp-dev assimp-utils libboost-dev omniidl-python omniorb omniorb-idl omniorb-nameserver libomniorb4-dev libomnithread3-dev python python-matplotlib python-numpy python-scipy python2.7 qgit libbz2-dev zlib1g-dev libssl-dev pax tar libeigen3-dev libtinyxml-dev liburdfdom-dev libboost-all-dev libpcre3-dev libopenscenegraph-dev libqt4-dev python-qt4-dev libncurses-dev python-pip cython python-h5py
+echo 'export ROBOTPKG_BASE=/opt/openrobots' >> .bashrc
+echo 'export PKG_CONFIG_PATH=$ROBOTPKG_BASE/lib/pkgconfig:${PKG_CONFIG_PATH}' >> .bashrc
+echo 'export LD_LIBRARY_PATH=$ROBOTPKG_BASE/lib/:$ROBOTPKG_BASE/lib64/:${LD_LIBRARY_PATH}' >> .bashrc
+echo 'export PATH=$PATH:$ROBOTPKG_BASE/sbin:$ROBOTPKG_BASE/bin' >> .bashrc
+echo 'export PYTHON_LOCAL_PATH=lib/python2.7/dist-packages' >> .bashrc
+echo 'export PYTHON_SITE_PATH=lib/python2.7/site-packages' >> .bashrc
+echo 'export PYTHONPATH=${PYTHONPATH}:${ROBOTPKG_BASE}/${PYTHON_LOCAL_PATH}:${ROBOTPKG_BASE}/${PYTHON_SITE_PATH} ' >> .bashrc
+
+### Setup RobotPKG repository
+relcode=`lsb_release -cs`
+sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
+deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $relcode robotpkg
+EOF
+
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+sudo apt-get update
+
+### Install Pinocchio
+sudo apt-get install -y robotpkg-pinocchio robotpkg-osg-dae robotpkg-gepetto-viewer-corba
+
+### Install other requirement (quadprog, tensorflow)
+sudo chmod -R a+w /usr/local/
+sudo pip install quadprog
+export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.4.0-cp27-none-linux_x86_64.whl
+pip install --upgrade $TF_BINARY_URL
+pip install tflearn
+
+

tp0_install/main.asciidoc

+0. Once upon a time (startup guide)
+===================================
+
+Objective
+---------
+The objective of this initial tutorial is to explain how to set up the system. Two solutions are proposed: i) using a VirtualBox out of the shelf or ii) installing on a native Ubuntu 14.04.
+
+The first solution i) is easier to set up: simply download the virtual disk and run it on VirtualBox on any Linux, Windows or Mac system. The drawback is some additional latency due to the virtualization, in particular when rendering graphics. On the other hand, if you have a Linux 14.04 available, following the procedure ii) should be straight forward and lead to better performances. If you plan to only use Pinocchio is the frame of a class and you dont have a Ubuntu available, choose solution i). If you rather plan to make extensive use of Pinocchio, for example for a MSc project, rather choose ii).
+
+i) Using a VirtualBox
+---------------------
+
+The tutorial is performed inside a virtual machine, under VirtualBox. VirtualBox is a free software, available on most platforms (Linux, Windows, MacOS). The virtual machine runs a Ubuntu 14.04 (64bits), with our software installed on it.
+
+*    Install VirtualBox for your computer from here.
+*    Download the virtual image. The image about 2,8Gb: expect from 10 minutes to 1 hour of downloading time, depending on the bandwidth from your network to our lab. Download here.
+*    Import the virtual image in VirtualBox, using the procedure described here.
+*    Start the virtual machine. The system auto log on the main user (ID student, password student). This user owns sudoer privilege.
+*    [optional] A complete tutorial about VirtualBox, along with an explanation of what is virtualization, is available here
+*    [optional] A Md5 checksum for each image is available here. This file can be used that the .ova file has been properly downloaded. Under Linux, check by first downloading both files .ova and .md5 in the same directory and then typing md5sum -c u1404x32.md5.
+*    [optional] You can install the "Guest Additions" in your virtual box, to improve the performance of the virtual computer (e.g. enlarge the size of the display). In the VirtualBox menu (when the virtual machine is on), choose "Devices" [Périphérique], then "Insert Guest Additions" [Additions Invités], and follow the instructions (remember that the password is "student"). 
+
+ii) Installation on the native Ubuntu system
+--------------------------------------------
+
+You can alternatively install the software on a native Linux. The instructions are the same for Ubuntu 12.04, 14.04 (32 or 64bits). It should work for a Ubuntu 16.04 (except osg-dae that is not working).
+
+The link:tp0_install_sh.html[script here] collects all the instructions that were needed to set up the above Virtual Box. It should run immediately on U14.04x64.
+
+On Ubuntu 14.04, the following distrib packages are needed (similar in u16.04, just adapt some of the names with simple google searches):
+[source,python]
+----
+apt-get install cmake cmake-curses-gui cmake-qt-gui curl doxygen g++ ipython libassimp-dev assimp-utils libboost-dev omniidl-python omniorb omniorb-idl omniorb-nameserver libomniorb4-dev libomnithread3-dev python python-matplotlib python-numpy python-scipy python2.7 qgit libbz2-dev zlib1g-dev libssl-dev pax tar libeigen3-dev libtinyxml-dev liburdfdom-dev libboost-all-dev libpcre3-dev libopenscenegraph-dev libqt4-dev python-qt4-dev libncurses-dev python-pip cython python-h5py
+----
+
+Then 
+
+The Debian (APT) packages are build from a  packaging "from sources" system named robot-pkg.
+Debian packets have been tested on U14.04 and U16.04. They should run out of the box (osg-dae only works on U14.04 but may be skipped). Set up the repository of robot-pkg:
+[source,python]
+----
+relcode=`lsb_release -cs`
+sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
+deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $relcode robotpkg
+EOF
+
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+sudo apt-get update
+----
+Here <relcode> is typically trusty or xenial.
+
+Also set up the bash environment. Add the following to you environment (at the end of your bashrc)
+[source,python]
+----
+export ROBOTPKG_BASE=/opt/openrobots
+export PKG_CONFIG_PATH=$ROBOTPKG_BASE/lib/pkgconfig:${PKG_CONFIG_PATH}
+export LD_LIBRARY_PATH=$ROBOTPKG_BASE/lib/:$ROBOTPKG_BASE/lib64/:${LD_LIBRARY_PATH}
+export PATH=$PATH:$ROBOTPKG_BASE/sbin:$ROBOTPKG_BASE/bin
+export PYTHON_LOCAL_PATH=lib/python2.7/dist-packages
+export PYTHON_SITE_PATH=lib/python2.7/site-packages
+export PYTHONPATH=${PYTHONPATH}:${ROBOTPKG_BASE}/${PYTHON_LOCAL_PATH}:${ROBOTPKG_BASE}/${PYTHON_SITE_PATH}
+----
+
+Finally instal the following APT packages
+[source,python]
+----
+apt-get install -y robotpkg-pinocchio robotpkg-osg-dae robotpkg-gepetto-viewer-corba
+----
+
+In case you prefer to install from source, robot-Pkg is available link:http://robotpkg.openrobots.org/install.html[at this URL]. Follow the instructions to set up robot-pkg and robot-pkg wip:
+[source,python]
+----
+git clone git://git.openrobots.org/robots/robotpkg
+cd robotpkg
+git clone git://git.openrobots.org/robots/robotpkg/robotpkg-wip wip
+----
+(see also the dedicated menu to robot-pkg wip).
+
+Once robot-pkg is set up, install the packages wip/pinocchio (make checkout before make install), wip/osg-dae, and graphics/gepetto-viewer-corba/ .
+The installation can be tested by running link:tp0_viewer_unittest_py.html[this Python script].
+
+On u16.04, the plugin Osg-dae is not going to install properly. As most of the URDF files are using DAE mesh files, gepetto-viewer will not be able to properly display your models (note that Pinocchio will work properly, only display will fail). You then have two solutions: - re-compile OSG from source, which should also install the OSG DAE plugin. This is 2 hours of compilation, but should work fine. - In your URDF model, duplicate your mesh files to have both DAE file and OSG file (using osgconv). Then patch robot_wrapper.py to have gepetto-viewer loading the OSG files instead of DAE files (do not modify the URDF file): replace line #184 by "self.viewer.gui.addMesh(meshName, meshPath[:-3]+'osg')" 
\ No newline at end of file

tp0_install/viewer_unittest.py

+import os
+import gepetto.corbaserver
+from pinocchio.utils import *
+import time
+
+os.system('gepetto-gui &')
+
+time.sleep(1)
+viewer=gepetto.corbaserver.Client()
+windowID = viewer.gui.createWindow ("viewtest")
+viewer.gui.createSceneWithFloor("world")
+viewer.gui.addSceneToWindow("world",windowID)
+
+viewer.gui.applyConfiguration("world/floor",[0,0,0,  .7071,.7071,0,0])
+viewer.gui.refresh()
+
+N = 100
+for i in range(10*N):
+    viewer.gui.applyConfiguration("world/floor",[0,0,0,  .7071,.7071,i/(N+.0),0])
+    viewer.gui.refresh()
+    time.sleep(1e-2)

tp1_directgeom/main.asciidoc

+1. Move your body (aka direct geoemtry)
+=======================================
+
+Objective
+---------
+In this first series of exercises, we are going to start using the library Pinocchio.
+We will load in the simulator the model of a simple manipulator arm, the Universal Robot 5, or UR5.
+This model will be used for a positioning task using simple methods.
+This set of exercices emphasizes the first part of the class, about direct geometry.
+
+The software Pinocchio is a C++ library provided with a python wrapping that allows us to control it with a python terminal.
+Let's see how it works.
+
+Tutorial 1.0. Tips
+------------------
+
+Setup
+~~~~~
+
+For all the exercices, we suppose that Pinocchio is installed in a VirtualBox and the user ID is "student" (password student).
+The VirtualBox contains all the software to install. 
+Alternatively, Pinocchio might be installed directly in any Ubuntu 14.04 / 16.04, after some work in other Linux, OSX, and likely in Windows (if you like to gamble).
+
+Two directories have been created at the root of the homedir. The directory "student" is where you should develop your scripts. The directory "models" contains the models of the robot that we will use during the class.
+
+Pinocchio has been installed using a software for "installation from sources" named +robot-pkg+. The sources of Pinocchio are available in +~/robotpkg/wip/pinocchio/work.robotics/pinochio-1.2.2+. After compilation, the software has been installed in +/opt/openrobots'. See for example the python scripts in +/opt/openrobots/lib/python2.7+.
+
+Python
+~~~~~~
+We remind you that you can open a python terminal in your own shell. Simply type :
+bash terminal
+....
+student@student-virtualbox:~$ ipython
+>>>
+....
+Afterwards you'll just have to type your commands in this newly opened terminal.
+
+To close the python terminal, just type CTRL-D (CTRL-C first to interrupt any on-going execution).
+
+You can also write you command lines in a file and launch this script without entering the interactive mode
+(ie. without starting a new python terminal). In your shell, just type:
+bash terminal
+....
+student@student-virtualbox:~$ ipython script.py
+>>>
+....
+
+Pinocchio
+~~~~~~~~~
+
+
+*Basic mathematical objects:*
+
+In the following, we will use numpy Matrix class to represent matrices and vectors.
+In numpy, vectors simply are matrices with one column. See the following example.
+[source, python]
+----
+import numpy as np
+A = np.matrix([ [1,2,3,4],[5,6,7,8]]) # Define a 2x4 matrix
+b = np.zeros([4,1])   # Define a 4 vector (ie a 4x1 matrix) initialized with 0.
+c = A*b               # Obtain c by multiplying A by b.
+----
+
+A bunch of useful functions are packaged in the utils of pinocchio.
+
+[source,python]
+----
+from pinocchio.utils import *
+eye(6)                    # Return a 6x6 identity matrix
+zero(6)                   # Return a zero 6x1 vector
+zero([6,4])               # Return az zero 6x4 matrix
+rand(6)                   # Random 6x1 vector
+isapprox(zero(6),rand(6)) # Test epsilon equality
+mprint(rand([6,6]))       # Matlab-style print
+skew(rand(3))             # Skew "cross-product" 3x3 matrix from a 3x1 vector
+cross(rand(3),rand(3))    # Cross product of R^3
+rotate('x',0.4)           # Build a rotation matrix of 0.4rad around X.
+----
+
+Specific classes are defined to represent objects of SE(3), se(3) and se(3)^*. Rigid displacements, elements of SE(3), are represented by the class SE3.
+
+[source,python]
+----
+import pinocchio as se3
+R = eye(3); p = zero(3)
+M0 = se3.SE3(R,p)
+M = se3.SE3.Random()
+M.translation = p; M.rotation = R
+----
+
+Spatial velocities, elements of se(3) = M^6, are represented by the class Motion.
+
+[source,python]
+----
+v = zero(3); w = zero(3)
+nu0 = se3.Motion(v,w)
+nu = se3.Motion.Random()
+nu.linear = v; nu.angular = w
+----
+
+Spatial forces, elements of se(3)^* = F^6, are represented by the class Force.
+
+[source,python]
+----
+f = zero(3); tau = zero(3)
+phi0 = se3.Force(f,tau)
+phi = se3.Force.Random()
+phi.linear = f; phi.angular = tau
+----
+
+
+Tutorial 1.1. Creating and displaying the robot
+-----------------------------------------------
+
+Robot kinematic tree
+~~~~~~~~~~~~~~~~~~~~
+
+The kinematic tree is represented by two C\++ objects called Model (which
+contains the model constants: lengths, masses, names, etc) and Data (which
+contains the working memory used by the model algorithms). Both C\++ objects are
+contained in a unique Python class. 
+The first class is called RobotWrapper and is generic. 
+
+For the next steps, we are going to work with the RobotWrapper.
+
+Import the class +RobotWrapper+ and create an instance of this class in the
+python terminal. At initialization, RobotWrapper will read the model
+description in the URDF file given as argument. In the following, we will use
+the model of the UR5 robot, available in the directory "models" of pinocchio (available in the homedir of the VBox).
+
+[source, python]
+----
+from pinocchio.robot_wrapper import RobotWrapper
+
+pkg  = '/home/student/models/'
+urdf = pkg + '/ur_description/urdf/ur5_gripper.urdf'
+robot = RobotWrapper(urdf,[pkg,])
+----
+The code of the RobotWrapper class is in /path/to/pinocchio//src/python/romeo_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.
+
+UR5 is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format ".dae". Both the URDF and the DAE files are available in the attached ZIP archive. Uncompressed it in the VirtualBox, for example in the directory "/home/student/src/pinocchio/models".
+
+* UR5 model and code link:ur5.zip[*ur5.zip*]
+
+Exploring the model
+~~~~~~~~~~~~~~~~~~~
+
+The robot model is available in robot.model. It contains the names of all the
+robot joint [names], the kinematic tree [parents] (i.e. the graph of parents, 0
+being the root and having no parents), the position of the current joint in the
+parent coordinate frame [jointPosition], the mass, inertia and
+center-of-gravity position of all the bodies (condensed in a spatial inertia
+6x6 matrix) [inertias] and the gravity of the associated world [gravity]. All
+these functions are documented and are available in the correponding class
+dictionnary.
+
+[source,python]
+----
+for name,function in robot.model.__class__.__dict__.items():
+  print " **** ", name, ": ", function.__doc__
+----
+
+Similarly, the robot data are available in robot.data. All the variables
+allocated by the classical rigid-body dynamics algorithms are stored in
+robot.data and are available through the python wrapping. Similarly to the
+model object, the function are documented and are available from the class
+dictionnary. The most useful in the following will be the placement of the
+frame associated which each joint output stored in robot.data.oMi.
+
+For example, the robot end effector corresponds to the output of the last joint, called +wrist_1_joint+. The ID of the joint in the joint list can be
+recovered from its name, and then used to access its placement:
+
+[source,python]
+----
+# Get index of end effector
+idx = robot.index('wrist_3_joint')
+
+# Compute and get the placement of joint number idx
+placement = robot.position(q,idx)
+# Be carreful, Python always returns references to values. 
+# You can often .copy() the object to avoid side effects
+# Only get the placement
+placement = robot.data.oMi[idx].copy()
+
+----
+
+Finally, some recurring datas (used in Model and Data) have been wrapped to functions in some
+python shortcuts, also available in RomeoWrapper:
++The size of the robot configuration is given by nq.
++The dimension of its tangent space (velocity) is nv.
++The index of a joint in the tree can be accessed from its name by index (see above).
++The classical algorithms are also binded: com, Jcom, mass, biais, joint gravity, position and velocity of each joint.
+
+[source,python]
+----
+q = zero(robot.nq)
+v = rand(robot.nv)
+robot.com(q)           # Compute the robot center of mass.
+robot.position(q,3)    # Compute the placement of joint 3
+----
+
+Display the robot
+~~~~~~~~~~~~~~~~~
+
+To display the robot, we need an external program called +Gepetto Viewer+.
+If you completed the installation in the previous page, you can launch this
+program, open a new terminal in an empty workspace.
+
+....
+student@student-virtualbox:~$ gepetto-gui
+....
+This will start a server waiting for instructions. We will now create a client that will ask
+the server to perform some requests (such as creating a window or displaying our robot)
+
+
+In a python terminal you can now load the visual model of the robot in the viewer
+[source,python]
+----
+robot.initDisplay(loadModel=True)
+----
+This will flush the robot model inside the GUI.
+The argument "loadModel=True" is mandatory when you start or restart the GUI. 
+In later call to your scripts, you can set the argument to "False". 
+A side effector of "=True" is that it will move the viewpoint inside the GUI to a reference zero position.
+
+
+More details about loading the model (optionnal)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+You can access the visual object composing the robot model by robot.visual_model.geometryObject.
+[source,python]
+----
+visualObj  = robot.visual_model.geometryObjects[4] # 3D object representing the robot forarm
+visualName = visualObj.name                        # Name associated to this object
+visualRef  = robot.viewerNodeNames(visualObj)      # Viewer reference (string) representing this object
+----
+
+Moving one object 
+[source,python]
+----
+q1 = [1,1,1, 1,0,0,0]  # x,y,z   , quaternion
+robot.viewer.gui.applyConfiguration(visualRef,q1)
+robot.viewer.gui.refresh() # Refresh the window.
+----
+
+Additional objects can be created, like a sphere as follows.
+[source,python]
+----
+rgbt =  [1.0,0.2,0.2,1.0] # red-green-blue-transparency
+robot.viewer.gui.addSphere("world/sphere", .1, rgbt) # .1 is the radius
+----
+
+The exhaustive list of the object that can be created is available in the IDL of the GUI:
++/opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl+
+
+
+Tutorial 1.2. Simple pick and place
+-----------------------------------
+
+*Objectives:* Display the robot at a given configuration or along a given trajectory
+
+Pick: 
+~~~~~
+
+Say we have a target at position [.5,.1,.2] and we would like the robot to grasp it.
+
+First display a small sphere at this position to visualize it.
+
+Then decide by any mean you want a configuration of the robot so that the end effector is touching the sphere.
+
+At the reference position you built, the end effector placement can be obtained by robot.position(q,6). Only the translation part of the placement has been selected. The rotation is free.
+
+[Optional]
+Say now that the object is a rectangle and not a sphere.
+Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.
+
+Place:
+~~~~~~
+
+Choose any trajectory you want in the configuration space, starting from the reference position built in the previous exercice (it can be sinus-cosinus waves, polynomials, splines, straight lines). 
+
+Make a for loop to display the robot at sampling positions along this trajectory. The function sleep in module time (from time import sleep) can be used to slow down the loop.
+
+At each instant of your loop, recompute the position of the ball and display it so that it always "sticks" to the robot end effector.
+
+////
+Homework
+--------
+
+Send by mail at nmansard@laas.fr a mail containing a single python file. The subject of the mail should start with +[SUPAERO] TP1+ 
+When executed, the script should place the robot at the initial starting position where the end effector touches the sphere (optionally the rectangle) and move the robot with the sphere attached to the end effector.
+
+////
+
+
+
+

tp2_invgeom/main.asciidoc

+2. Grasp an object (aka inverse Geometry)
+=========================================
+
+Objectives
+----------
+
+The main objective of the first tutorial is to compute a configuration of the robot minimizing a cost (maximizing a reward) and respecting some given constraints. Additionally, the objective is to have a first hands on the difficulties of working outside of real vector spaces and to consider what are the guesses that are taken by an optimal solver.
+
+Tutorial 2.0. Technical prerequisites
+-------------------------------------
+
+Python SciPy and MatplotLib
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+You will need the two libraries Python +SciPy+ (scientific Python) and +MatPlotLib+ (plot mathematical data). 
+
+SciPy can be installed by +sudo apt-get install python-scipy+. It is already installed if you are using the VirtualBox.
+Examples of calls of these two functions are given below. We will use both solvers with numerical (finite-differencing) differenciation, to avoid the extra work of differencing the cost and constraint functions by hand. In general, it is strongly advice to first test a numerical program with finite differencing, before implementing the true derivatives only if needed. In any case, the true derivatives must always be checked by comparing the results with the finite differenciation. 
+
+Additionally, the provided implementation of BFGS allows the user to provide a callback function and track the path taken by the solver, but does not provide the possibility to specify constraints (constraints can be added as penalty functions in the cost, but this requires additional work). The constrained least-square implementation allows the user to specify equality and inequality constraints, but not the callback. In the following, start to use BFGS before moving to the constrained least-square only when constraints are really needed.
+[source,python]
+----
+# Example of use a the optimization toolbox of SciPy.
+import numpy as np
+from scipy.optimize import fmin_bfgs, fmin_slsqp
+
+def cost(x): 
+     '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''
+     x0 = x[0]
+     x1 = x[1]
+     return -1*(2*x0*x1 + 2*x0 - x0**2 - 2*x1**2)
+
+def constraint_eq(x):
+     ''' Constraint x^3 = y '''
+     return np.array([ x[0]**3-x[1] ])
+