.. _control-walk-tuto2:

Walk control Tutorial: The Robot Position
=========================================

:ref:`Overview <control-walk>` | :ref:`API <control-walk-api>` |
:ref:`foot planner Tutorial <control-walk-tuto1>` |
robot position Tutorial


.. seealso::

    - :ref:`naoqi-motion`

------------

Introduction
------------

This tutorial explains how to use the robotPosition and getFootSteps API. These both API will help
you to have a better control on the walk algorithm.


.. note::

   The tutorial is written in Python.


.. _control-walk-tuto2-download:

Download
--------

You can download the robot position example here:
:download:`motion_robotPosition.py </examples/python/motion/motion_robotPosition.py>`

To be executed, this tutorial require a config file:
:download:`config.py </examples/python/motion/config.py>`. Modify the config file to enter
your robot IP, and place it in the same folder as the example.

Please refer to the section: :ref:`python-install-guide` for any troubleshooting linked to python.

This tutorial uses matplotlib for plotting data: http://matplotlib.sourceforge.net/.


Code review
-----------

In this section we describe each important piece of code of the example.

NAOqi tools
+++++++++++

First, we import some external library:
 - config: the config file (see above the :ref:`control-walk-tuto2-download` section)
 - motion: some useful defintion such as SPACE.
 - math: a classical python mathematic library useful for cos, sinus ...
 - almath: an optimized mathematic toolbox for robotics.
   For further details, see: `libalmath API reference <../../ref/libalmath/index.html>`_.
 - pylab: matplotlib useful for plotting data (http://matplotlib.sourceforge.net/).

Then, the proxy to **ALMotion** module is created. This proxy is useful to called **ALMotion** API.

.. code-block:: python

  import config
  import time
  import math
  import almath

  try:
      import pylab as pyl
      HAS_PYLAB = True
  except ImportError:
      print "Matplotlib not found. this example will not plot data"
      HAS_PYLAB = False


  def main():
      ''' robot Position: Small example to know how to deal
                          with robotPosition and getFootSteps
      '''
      motionProxy = config.loadProxy("ALMotion")


NAO initialization
++++++++++++++++++

| Set stiffness into the joint and execute a PoseInit.

.. code-block:: python

    # Set NAO in stiffness On
    config.StiffnessOn(motionProxy)
    config.PoseInit(motionProxy, 0.2)

    # Initialize the walk
    motionProxy.walkInit()

The experience
++++++++++++++

We define here an experience where someone send a first walk command and, few seconds after, a
second one.

We observe phenomenon of unchangeable and changeable foot step. The second walk
command is executed after the unchangeable foot generated by the first walk command.

.. code-block:: python

    # First call of walk API
    # with post prefix to not be bloquing here.
    motionProxy.post.walkTo(0.3, 0.0, 0.5)

    # wait that the walk process start running
    time.sleep(0.1)

    # get robotPosition and nextRobotPosition
    robotPosition     = almath.Pose2D(almath.vectorFloat(motionProxy.getRobotPosition(False)))
    nextRobotPosition = almath.Pose2D(almath.vectorFloat(motionProxy.getNextRobotPosition(False)))

    # get the first foot steps vector
    # (footPosition, unChangeable and changeable steps)
    footSteps1 = motionProxy.getFootSteps()

    # Second call of walk API
    motionProxy.post.walkTo(0.3, 0.0, -0.5)

    # get the second foot steps vector
    footSteps2 = motionProxy.getFootSteps()


Compute robot move
++++++++++++++++++

Here, using walk API, we compute the move made by the robot. The goal is to find the
value of the second walk command.

The robot begins the second walk command after the first command unchangeable foot step.
In this case the robot position is the result of nextRobotPosition.

Then, we wait the end of the walk process (waitUntilWalkIsFinished) and we get the final robot
position.

The distance is equivalent to :math:`(nextRobotPosition)^{-1}*robotPositionFinal`.

And the result should be [0.3, 0.0, -0.5]

.. code-block:: python

    # here we wait until the walk process is over
    motionProxy.waitUntilWalkIsFinished()
    # then we get the final robot position
    robotPositionFinal = almath.Pose2D(almath.vectorFloat(motionProxy.getRobotPosition(False)))

    # compute robot Move with the second call of walk API
    # so between nextRobotPosition and robotPositionFinal
    robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
    print "Robot Move :", robotMove


Print Result
++++++++++++

We use matplotlib function to print the foot step result of the experience.

Functions printRobotPosition and printFootSteps are definied in the
:download:`motion_robotPosition.py </examples/python/motion/motion_robotPosition.py>` file.

.. code-block:: python

    if (HAS_PYLAB):
      #################
      # Plot the data #
      #################
      pyl.figure()
      printRobotPosition(robotPosition, 'black')
      printRobotPosition(nextRobotPosition, 'blue')
      printFootSteps(footSteps1, 'green', 'red')

      pyl.figure()
      printRobotPosition(robotPosition, 'black')
      printRobotPosition(nextRobotPosition, 'blue')
      printFootSteps(footSteps2, 'blue', 'orange')

      pyl.show()

This first figure represents the result of the fisrt walk command:

.. image:: /medias/dev/modules/motion/motion_robotPositionLegend1.png
   :height: 114 px
   :width: 266 px


.. image:: /medias/dev/modules/motion/motion_robotPosition1.png
   :height: 447 px
   :width: 600 px



| The second one shows the effect of the second walk command.
| We see that unChangeable footStep of the first command will be executed before the second command:
|

.. image:: /medias/dev/modules/motion/motion_robotPositionLegend2.png
   :height: 114 px
   :width: 266 px


.. image:: /medias/dev/modules/motion/motion_robotPosition2.png
   :height: 447 px
   :width: 600 px