====== SP2158 - Sistemas bioinspirados ======

===== Tarea 1: YARP/ROS =====

==== YARP ====

=== Instalación ===

[[tutorials:installing_yarp_in_debian|Installing YARP in debian]]

=== Práctica general ===

  * Jugar un poco con yarp read y write. 
    * crear dos puertos de salida y conectarlos a una entrada
    * Hacerlos persistentes


=== Sumador ===

  * Programe un módulo en python utilizando YARP que:

    - Acepte un número de punto flotante por un puerto llamado "/sumador/in:1"
    - Acepte un número de punto flotante por un puerto llamado "/sumador/in:2"
    - Sume ambos números y envíe el resultado en el puerto de salida "/sumador/out"
    - El programa debe correr en un ciclo infinito, siempre esperando por nuevos nuevos.

  * Ponga a prueba este programa utilizando yarp read y yarp write . (Hace screen shots de dichos resultados)
  * Modifique este módulo para que acepte los dos números de punto flotante utilizando un solo puerto de entrada "/sumador/in"

  * Construya un módulo que tome los datos de un eje de un joystick de un PS4 controller y los envíe por un puerto de yarp llamado "/ps3/out"
  * Use yarp-plotter para visualizarlo. (git clone https://gitlab.com/arcoslab/yarp-plotter). Para poder utilizar yarp-plotter necesita primero instalar [[https://gitlab.com/arcoslab/avispy| avispy]] y [[https://gitlab.com/arcoslab/arcospyu| arcospyu]].

<code bash>
  sudo apt-get install python-numpy python-scipy python-opengl python-pygame python-matplotlib python-sip python-sip-dev python-qt4-dev python-qt4 python-gtk2 python-gtk2-dev python-vtk6 python-pyvtk python-gtkglext1 libeigen3-dev python-yaml python-setuptools python-future python-colorlog
 
  echo "install arcospyu"
  cd $HOME/local/src
  git clone https://gitlab.com/arcoslab/arcospyu.git
  cd arcospyu
  make xstow_install
  
  echo "install avispy"
  cd $HOME/local/src
  git clone https://gitlab.com/arcoslab/avispy.git
  cd avispy
  make xstow_install
</code>

  * Modifique el programa de control de motores open-coroco para que tome los datos del joystick y los convierta en comandos de control de velocidad para el motor.
  * Coloque el módulo sumador en medio de estos dos módulos y de esta manera agregue un valor constante a la velocidad cuando usted lo deseé.
==== ROS ====

  * Siga los tutoriales de ROS en el siguiente orden:

http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment

http://wiki.ros.org/ROS/Tutorials/NavigatingTheFilesystem

http://wiki.ros.org/ROS/Tutorials/UnderstandingNodes

http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics

http://wiki.ros.org/ROS/Tutorials/CreatingPackage

http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29

  * Modifique el programa del PS4 controller y logre enviar datos mediante ROS para que la tortuga se mueva en el eje X con el joystick del controller.


===== Preliminar Orocos =====


<code python>
  import numpy as np
  import iexample as ix
  
  handler = ix.InteractiveHandler()
  box = handler.create_object("box", scale=[0.1, 0.1, 0.1])
  arm = handler.create_chain(ix.default_conf["kuka"])
  
  handler.core()
  
  box.update_twist(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 2.0]))

</code>


===== Tarea 2: Orocos-KDL =====

  * Instale el programa ipython
  * En una consola de linux corra el programa ipython
  * En la consola de ipython importe la biblioteca KDL:

  import PyKDL as k

  * De ahora en adelante puede usar k para utilizar las facilidades incluidas en orocos-kdl para python
  * Realice las siguientes actividades:

  - Cree dos vectores
  - Súmelos
  - Réstelos
  - Cree una matriz de rotación identidad
  - Cree una matriz de rotación de una rotación en X de 45
  - Cree otra matriz de rotación de una rotación en Z de 45
  - Componga esas dos rotaciones. Primero con respecto a marcos de referencia locales y nuevamente con respecto a marcos de referencia globales. Dibuje el resultado en ambos casos. (llamemos a estas dos rotaciones Rl, Rg
  - Cree una matriz de rotación de una rotación en x (roll) de 45, y(pitch) 0,  y z(yaw) de 45 utilizando RPY. Rrpy  (de acuerdo a KDL, en el libro de siciliano es así: z(roll), y(pitch), x(yaw))
  - Utilice el método "GetRPY" sobre Rl y Rg, Cuál de las dos coincide con los valores de Rrpy?
  - Invierta Rrpy y multiplique su resultado con Rrpy original. Comente el resultado.
  - Cree un Frame (F1) (matrix homogenea) con una matrix de rotacion en X de 45 y un punto en 1 2 3.
  - Cree un Frame (F2) (matrix homogenea) con una matrix de rotacion en Z de 45 y un punto en 1 2 3.
  - Cree un twist restando los dos frames de arriba
  - Componga ambos Frames en una dirección y luego en otra (F1*F2) y (F2*F1)
  - Extraiga la rotación del resultado en ambos casos (GetRPY). Cuál corresponde al caso de rotaciones con respecto a marcos de referencia locales?
  - Utilizando "Joint", "segment" y "Chain" cree la cadena cinemática que el profesor puso en la pizarra. 
  - Utilizando el robot expresado por el profesor en clase y "Frames" calcule la orientación del Frame final de toda la cadena kinemática.
  - Realice el cálculo de kinemática directa en el caso de los ángulos 45, 45, 45 y 10, 20, 30. Mencione la posición del end effector en ambos casos.
  - Construya un frame rotado en Z 10 grados y en la posición 40, 20, 0. Realice el cálculo de la kinemática inversa en este caso. Cuáles son los ángulos encontrados para cada articulación? (Extra)

===== Tarea 3: RT stm32 =====

  * Descargue el repositorio del proyecto open-coroco:

  mkdir -p ~/local/src/git-arcoslab/
  cd ~/local/src/
  git clone git@git.arcoslab.org:humanoid-hardware/open-coroco.git git-arcoslab

  * Compílelo:

  cd git-arcoslab/
  git checkout hardware-register
  git submodule init
  git submodule update
  cd lib/libopencm3/
  make -j3
  cd ../libopencm3-plus
  make -j3

  * Compile el ejemplo de control para el motor Vextra:

  cd ~/local/src/git-arcoslab/
  sed -i 's/libopencm3_stm32f4.ld/stm32f405x6.ld/g' lib/libopencm3-plus/lib/libopencm3_plus_stm32f4discovery.ld
  cd ~/local/src/git-arcoslab/src/vextra_AXHM230KC-GFH
  make

  * Conecte el stm32f4discovery y cargue el programa al microcontrolador:

  make flash

  * Conecte el stm32f4discovery a la pcb de open-coroco, al motor y a la fuente de poder como el profesor le indique.

{{ :teaching:conectar_stm.jpg?nolink&400 |}}

  * Corra el programa open-coroco-pc.py en la computadora:

  cd ~/local/src/git-arcoslab/utils/
  ./open-coroco-pc.py

  * Dicho programa debería cambiar la velocidad continuamente del motor y desplegar datos de los sensores de la tarjeta open-coroco.

  * Cambien el programa open-coroco-pc.py para que el motor acelere hasta 400Hz (self.max_speed) a un paso de aceleración de 0.1 (self.speed_inc=0.1) y que se quede en esa velocidad de 400Hz de manera indefinida.
  * El programa open-coroco-pc.py genera datos en un archivo llamado "output.dat". Examine dicho archivo y grafique (utilizando kst2 o gnuplot o matplotlib) los datos de la corriente principal, la velocidad estimada (est_freq) y la posición (raw_pos) vs el tiempo.
  * Cambie el programa open-coroco-pc.py para que el motor se detenga (enviar 0 de velocidad) cuando la corriente principal (primera corriente) sean menor o igual a 1500. Observe el tiempo que le toma al motor detenerse desde que se detecta dicha condición (grafíquelo).
  * Haga nuevamente el experimento pero ahora disminuya el tamaño del buffer del filtro de promedio de la corriente principal de 10 valores a 5 valores. Observe los resultados (grafíquelo).
  * Haga nuevamente el experimento, pero ahora modifique el archivo bincom.c para realizar el detenido del motor desde el microcontrolador, bajo las mismas condiciones (corriente principal igual o menor a 1500). Observe los resultados y grafíquelos.
  * Compare los tres experimentos y explique la razón de los diferentes resultados.