Difference between revisions of "Control brazo MYRAbot (bioloid+arduino)"

From robotica.unileon.es
Jump to: navigation, search
(→‎Creación de mensajes personalizados en ROS)
 
(29 intermediate revisions by 2 users not shown)
Line 1: Line 1:
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]
  
−
==Brazo Bioloid==
+
 
 +
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Artículos realcionados
 +
----
 +
!Otros artículos
 +
----
 +
|-
 +
| valign="top" width="50%" |
 +
 
 +
[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
 +
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
 +
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
 +
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
 +
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
 +
 
 +
| valign="top" |
 +
 
 +
[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
 +
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
 +
----
 +
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
 +
 
 +
|}
 +
----
 +
 
 +
 
 +
=Brazo Bioloid=
  
 
El brazo en el que se basa este proyecto es el desarrollado en el proyecto [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]], realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:
 
El brazo en el que se basa este proyecto es el desarrollado en el proyecto [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]], realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:
Line 13: Line 44:
 
* Piezas realizadas para pinza y fijación al MYRAbot.
 
* Piezas realizadas para pinza y fijación al MYRAbot.
  
−
 
+
=Arduino IDE y rosserial=
−
 
 
−
 
 
−
==Arduino IDE y rosserial==
 
  
 
Para la comunicación e intercambio de información entre [http://www.arduino.cc arduino] y [http://www.ros.org ROS] es necesario instalar [http://www.arduino.cc arduino] IDE y rosserial (''stack'' de [http://www.ros.org ROS] que contiene el ''package'' rosserial_arduino con las librerias para [http://www.arduino.cc arduino]). Comenzaremos instalando el [http://www.arduino.cc arduino] IDE, para lo que ejecutaremos los siguientes comandos en un terminal:
 
Para la comunicación e intercambio de información entre [http://www.arduino.cc arduino] y [http://www.ros.org ROS] es necesario instalar [http://www.arduino.cc arduino] IDE y rosserial (''stack'' de [http://www.ros.org ROS] que contiene el ''package'' rosserial_arduino con las librerias para [http://www.arduino.cc arduino]). Comenzaremos instalando el [http://www.arduino.cc arduino] IDE, para lo que ejecutaremos los siguientes comandos en un terminal:
Line 32: Line 60:
 
cp -r ros_lib /home/”nombre_sesion”/sketchbook/libraries/ros_lib</syntaxhighlight>
 
cp -r ros_lib /home/”nombre_sesion”/sketchbook/libraries/ros_lib</syntaxhighlight>
  
−
===Creación de un package para nuestros programas===
+
==Creación de un package para nuestros programas==
  
 
Deberemos tener previamente creado un [[Fernando-TFM-ROS02#Creando un espacio de trabajo|espacio de trabajo]] para nuestra versión de [http://www.ros.org ROS]. Para poder compilar y enviar nuestros programas a la placa [http://www.arduino.cc arduino] sin tener que pasar por [http://www.arduino.cc arduino] IDE vamos a crear un ''package'' llamado “arduino_fer” con las dependencias necesarias para nuestros programas, para ello ejecutaremos los siguientes comandos en un terminal:
 
Deberemos tener previamente creado un [[Fernando-TFM-ROS02#Creando un espacio de trabajo|espacio de trabajo]] para nuestra versión de [http://www.ros.org ROS]. Para poder compilar y enviar nuestros programas a la placa [http://www.arduino.cc arduino] sin tener que pasar por [http://www.arduino.cc arduino] IDE vamos a crear un ''package'' llamado “arduino_fer” con las dependencias necesarias para nuestros programas, para ello ejecutaremos los siguientes comandos en un terminal:
Line 52: Line 80:
 
cmake .</syntaxhighlight>
 
cmake .</syntaxhighlight>
  
−
===Primer programa (A toda potencia)===
+
==Primer programa (A toda potencia)==
  
 
El primer programa que vamos a realizar crea un nodo llamado “potencia” que publica un ''topic'' llamado “cifra” y está suscrito a un ''topic'' llamado “resultado”. Cuando se publique un número en el ''topic'' “cifra” lo multiplicará por si mismo y publicará el resultado en el ''topic'' “resultado”. El código del programa es el siguiente:
 
El primer programa que vamos a realizar crea un nodo llamado “potencia” que publica un ''topic'' llamado “cifra” y está suscrito a un ''topic'' llamado “resultado”. Cuando se publique un número en el ''topic'' “cifra” lo multiplicará por si mismo y publicará el resultado en el ''topic'' “resultado”. El código del programa es el siguiente:
Line 117: Line 145:
 
<syntaxhighlight>rostopic echo resultado</syntaxhighlight>
 
<syntaxhighlight>rostopic echo resultado</syntaxhighlight>
  
−
==Arduino y servomotores Dinamixel==
+
=Arduino y servomotores Dinamixel=
  
 
En [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] se encuentran las adaptaciones hardware realizadas a la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] para la correcta comunicación con los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] de [http://www.robotis.com/xe/ ROBOTICS], así como las librerías utilizadas para la programación ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).
 
En [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] se encuentran las adaptaciones hardware realizadas a la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] para la correcta comunicación con los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] de [http://www.robotis.com/xe/ ROBOTICS], así como las librerías utilizadas para la programación ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).
  
−
===Segundo programa (Muévete)===
+
==Segundo programa (Muévete)==
  
 
En este programa simplemente se realiza un prueba de la comunicación entre la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] con un servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. El programa publica el ''topic'' “angulo” con la posición del motor (entero entre 0 y 1023) y está suscrito al ''topic'' “giro” del que recibe la posición a la que debe moverse (entero entre 0 y 1023), con una velocidad constante de 128 (entero entre 0 y 1023). El código del programa es el siguiente:
 
En este programa simplemente se realiza un prueba de la comunicación entre la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] con un servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. El programa publica el ''topic'' “angulo” con la posición del motor (entero entre 0 y 1023) y está suscrito al ''topic'' “giro” del que recibe la posición a la que debe moverse (entero entre 0 y 1023), con una velocidad constante de 128 (entero entre 0 y 1023). El código del programa es el siguiente:
Line 179: Line 207:
 
<syntaxhighlight>rostopic echo angulo</syntaxhighlight>
 
<syntaxhighlight>rostopic echo angulo</syntaxhighlight>
  
−
==Modelo cinemático del brazo==
+
=Modelo cinemático del brazo=
  
 
Para el correcto posicionamiento del brazo es necesario obtener las transformaciones geométricas para determinar el ángulo que deben girar los servomotores para alcanzar un punto (cinemática inversa). Al tratarse de un brazo que trabaja en un solo plano con giro (4 articulaciones) es un proceso sencillo que se ha realizado de forma directa, sino se puede recurrir al algoritmo de [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters parametrización Denavit- Hartenberg] que permite obtener la cinemática directa y a partir de esta obtener la inversa.
 
Para el correcto posicionamiento del brazo es necesario obtener las transformaciones geométricas para determinar el ángulo que deben girar los servomotores para alcanzar un punto (cinemática inversa). Al tratarse de un brazo que trabaja en un solo plano con giro (4 articulaciones) es un proceso sencillo que se ha realizado de forma directa, sino se puede recurrir al algoritmo de [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters parametrización Denavit- Hartenberg] que permite obtener la cinemática directa y a partir de esta obtener la inversa.
Line 185: Line 213:
 
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagramas para el estudio de la cinemática]]
 
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagramas para el estudio de la cinemática]]
  
−
===Cinemática inversa===
+
==Cinemática inversa==
  
 
Siguiendo los diagramas planteados y considerando ''x'',''y'' y ''z'' como las coordenadas del punto destino, y &epsilon; el ángulo que forma la pinza respecto al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:
 
Siguiendo los diagramas planteados y considerando ''x'',''y'' y ''z'' como las coordenadas del punto destino, y &epsilon; el ángulo que forma la pinza respecto al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:
Line 202: Line 230:
  
 
[[file:brazo_ecuacion_arti3.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
 
[[file:brazo_ecuacion_arti3.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
−
 
  
 
Para enviar a los servomotores valores dentro del rango de trabajo, se multiplica por 1023 (0x3FF), valor máximo del rango de posiciones del servo, y se divide por 300 (300º), el ángulo total de giro del servomotor.
 
Para enviar a los servomotores valores dentro del rango de trabajo, se multiplica por 1023 (0x3FF), valor máximo del rango de posiciones del servo, y se divide por 300 (300º), el ángulo total de giro del servomotor.
  
−
===Cinemática directa===
+
==Cinemática directa==
  
 
A partir de las ecuaciones anteriores podemos plantear también el proceso inverso, la obtención del punto en que se encuentra el brazo a partir de las posiciones de los servomotores. Las ecuaciones obtenidas son las siguientes:
 
A partir de las ecuaciones anteriores podemos plantear también el proceso inverso, la obtención del punto en que se encuentra el brazo a partir de las posiciones de los servomotores. Las ecuaciones obtenidas son las siguientes:
Line 212: Line 239:
 
[[file:brazo_ecuaciones_directa.jpg]]
 
[[file:brazo_ecuaciones_directa.jpg]]
  
−
===Espacio de trabajo y limitaciones===
+
==Espacio de trabajo y limitaciones==
  
 
Para evitar colisiones del brazo con las diferentes partes del MYRAbot y con sus propios componentes, es necesario fijar unos límites de giro a los servomotores y restringir el acceso a regiones del espacio. Para esta tarea se ha realizado una hoja de calculo con las formulas de la cinemática inversa y se ha representado en unos gráficos las posiciones de los ejes y las articulaciones del brazo en el espacio.
 
Para evitar colisiones del brazo con las diferentes partes del MYRAbot y con sus propios componentes, es necesario fijar unos límites de giro a los servomotores y restringir el acceso a regiones del espacio. Para esta tarea se ha realizado una hoja de calculo con las formulas de la cinemática inversa y se ha representado en unos gráficos las posiciones de los ejes y las articulaciones del brazo en el espacio.
Line 218: Line 245:
 
[[file:brazo_cinematica_excel.jpg|thumb|750px|center|Hoja de cálculo cinemática inversa y directa, [http://www.fernando.casadogarcia.es/files/cinematica_brazo_MYRAbot.xlsx descargar]]]
 
[[file:brazo_cinematica_excel.jpg|thumb|750px|center|Hoja de cálculo cinemática inversa y directa, [http://www.fernando.casadogarcia.es/files/cinematica_brazo_MYRAbot.xlsx descargar]]]
  
−
==Programas de control==
+
=Programas de control=
  
−
===Tercer programa (Ven aquí)===
+
==Tercer programa (Ven aquí)==
  
 
Primero crearemos un programa que actuará de intermediario entre el programa de control y los servomotores del brazo, que situaremos en la placa [http://www.arduino.cc arduino]. Este programa se encargará de enviar las ordenes recibidas a los servomotores y enviar los datos más relevantes de los servomotores. Como se van a ''publicar'' y ''suscribir'' los datos de 5 servomotores, vamos a comenzar creando nuestros ''mensajes personalizados''.
 
Primero crearemos un programa que actuará de intermediario entre el programa de control y los servomotores del brazo, que situaremos en la placa [http://www.arduino.cc arduino]. Este programa se encargará de enviar las ordenes recibidas a los servomotores y enviar los datos más relevantes de los servomotores. Como se van a ''publicar'' y ''suscribir'' los datos de 5 servomotores, vamos a comenzar creando nuestros ''mensajes personalizados''.
  
−
====Creación de mensajes personalizados en ROS====
+
===Creación de mensajes personalizados en ROS===
  
 
Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas (std_msgs geometry_msgs roscpp). Dentro de este ''package'' crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre de campo).  
 
Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas (std_msgs geometry_msgs roscpp). Dentro de este ''package'' crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre de campo).  
Line 242: Line 269:
 
Servos par</syntaxhighlight>
 
Servos par</syntaxhighlight>
  
−
Para la recepción de datos de los servomotores crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:
+
Para la recepción de datos de los servomotores (posición, estado y corriente) crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:
  
 
<syntaxhighlight>Servos posicion
 
<syntaxhighlight>Servos posicion
Line 248: Line 275:
 
Servos corriente</syntaxhighlight>
 
Servos corriente</syntaxhighlight>
  
−
Para la creación de los mensajes en el sistema [http://www.ros.org ROS] debemos editar el archivo "CMakeLists.txt" y eliminar el símbolo "#" para que deje de estar comentada la siguiente línea:
+
Para permitir la creación de los mensajes en el sistema [http://www.ros.org ROS] debemos editar el archivo "CMakeLists.txt" eliminando el símbolo "#" para que deje de estar comentada la siguiente línea:
  
 
<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>
 
<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>
Line 261: Line 288:
 
<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>
  
−
====Programa arduino====
+
===Programa arduino===
  
 
El programa para la placa [http://www.arduino.cc arduino] está suscrito al ''topic'' "move_arm", por el cual recibe los movimientos de los servomotores, y publica el ''topic'' "pose_arm", con la información de posición, par y corriente de los servomotores. El código del programa es el siguiente:
 
El programa para la placa [http://www.arduino.cc arduino] está suscrito al ''topic'' "move_arm", por el cual recibe los movimientos de los servomotores, y publica el ''topic'' "pose_arm", con la información de posición, par y corriente de los servomotores. El código del programa es el siguiente:
Line 405: Line 432:
 
Para compilar y cargar el programa en la placa emplearemos el programa [http://www.arduino.cc arduino] IDE. Este programa nos servirá para otros programas de control que desarrollemos, ya que se trata de un mero intermediario.
 
Para compilar y cargar el programa en la placa emplearemos el programa [http://www.arduino.cc arduino] IDE. Este programa nos servirá para otros programas de control que desarrollemos, ya que se trata de un mero intermediario.
  
−
====Programa ROS====
+
===Programa ROS===
  
−
Este es el programa que indica la posición de los servomotores para alcanzar un punto del espacio de trabajo. Está suscrito al ''topic'' "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al ''topic'' "pose_arm", donde recibe la información de los servomotores que publica el programa [http://www.arduino.cc arduino], y publica el ''topic'' "move_arm", donde indica al programa [http://www.arduino.cc arduino] la posición y par de los servomotores. Dentro del ''pakage'' creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido:
+
Usando las ecuaciones de la cinemática inversa este programa indica a los servomotores la posición de giro para alcanzar el punto de destino que se encuentre dentro del espacio de trabajo. Está suscrito al ''topic'' "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al ''topic'' "pose_arm", donde recibe la información de los servomotores que publica el programa de [http://www.arduino.cc arduino], y publica el ''topic'' "move_arm", donde indica al programa de [http://www.arduino.cc arduino] la posición, velocidad y par de los servomotores. Dentro del ''pakage'' creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido:
  
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
Line 554: Line 581:
 
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:  
 
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del ''package'' creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:  
  
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v01 src/control_v01.cpp)</syntaxhighlight>
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(control_v01 src/control_v01.cpp)</syntaxhighlight>
  
 
Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
 
Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
Line 561: Line 588:
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
====Ejecutando el programa====
+
===Ejecutando el programa===
  
 
Para ejecutar el programa debemos lanzar el núcleo de [http://www.ros.org ROS], si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
 
Para ejecutar el programa debemos lanzar el núcleo de [http://www.ros.org ROS], si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
Line 577: Line 604:
 
Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el ''topic'' "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
 
Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el ''topic'' "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
  
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>
+
<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>
  
 
El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde uno de partida:
 
El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde uno de partida:
Line 585: Line 612:
 
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Ver vídeo en YouTube]</center>
 
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Ver vídeo en YouTube]</center>
  
−
===Cuarto programa (Agarra la botella)===
+
==Cuarto programa (Agarra la botella)==
  
−
Este programa ejecuta la secuencia para coger una botella pequeña de plástico (vacía, ya que la potencia de los servomotores es insuficiente para una llena, deberían emplearse más servomotores por articulación). Los pasos que realiza son los siguientes:
+
===Archivo include con funciones===
  
−
# Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).
+
Con el objetivo de simplificar y hacer más legibles los programas se ha creado este archivo que contiene las funciones de control del brazo. Crearemos un archivo llamado "brazo_fer.h" dentro del directorio "include/brazo_fer" del ''package'' creado con el siguiente contenido:
−
# Posicionamiento en el punto indicado.
 
−
# Cierre de la pinza (hasta tener agarrada la botella).
 
−
# Levantamiento y acercamiento (levanta y acerca la botella al MYRAbot)
 
  
−
Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v02.cpp" con el siguiente contenido:
+
<syntaxhighlight lang="cpp" enclose="div">
  
−
<syntaxhighlight lang="cpp" enclose="div">
+
#ifndef brazo_fer_H
 +
#define brazo_fer_H
  
−
   #include "ros/ros.h"   //Librería de funciones del sistema ROS
+
   #include "ros/ros.h"
 
   #include "brazo_fer/Servos.h"
 
   #include "brazo_fer/Servos.h"
 
   #include "brazo_fer/WriteServos.h"   
 
   #include "brazo_fer/WriteServos.h"   
Line 604: Line 629:
 
   #include "std_msgs/Int16.h"
 
   #include "std_msgs/Int16.h"
 
   #include "geometry_msgs/Point.h"
 
   #include "geometry_msgs/Point.h"
−
   #include "math.h" //Librería de funciones matemáticas
+
   #include "math.h"  
 
    
 
    
 
   #define PI 3.14159265
 
   #define PI 3.14159265
−
   #define L1 104 //longitud antebrazo (mm)
+
   #define L1 104
−
   #define L2 104 //longitud brazo (mm)
+
   #define L2 104
−
   #define Lp 60 //longitud de la mano
+
   #define Lp 60   
−
  #define Lb 60 //longitud desde la base de la articulación A1  hasta la articulación A2  
+
 
−
 
+
geometry_msgs::Point directa(brazo_fer::Servos posicion_servos_0, int inclinacion_pinza)
−
  geometry_msgs::Point punto_0,punto_1;
+
   {
−
 
 
−
  int cont = 0;
 
−
  int cont_1 = 0;
 
−
  int cont_2 = 0;
 
−
  int error_coordenadas = 0;
 
−
   
 
−
  brazo_fer::Servos pg;
 
−
  brazo_fer::Servos move, move_1, vel;
 
−
  brazo_fer::Servos pin;
 
−
 
 
−
  brazo_fer::Servos on;
 
−
 
−
  brazo_fer::Servos off;
 
−
 
 
−
 
 
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino)  
 
−
   {
 
−
 
 
−
double x = destino.x;
 
−
  double y = destino.y;
 
−
double z = destino.z;
 
 
 
−
y = y + z*tan(atan2(45,250)); //compensación de la pérdida de altura con la distancia, par resistente en los servomotores
+
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p1, delta_p2, epsilon;
−
 
 
−
int coordenadas_correctas = 1; //para determinar si la posición se puede alcanzar
 
−
 
 
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
 
 
double z_p;
 
double z_p;
 
double L_a, L;
 
double L_a, L;
−
+
double a, b;
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
double x, y, z;
−
+
−
alfa = (atan2(x,z)*180)/PI;
+
alfa = (((posicion_servos_0.base*300)/1023)-150)*PI/180;
−
+
beta = (((posicion_servos_0.arti1*300)/1023)-60)*PI/180;
−
z_p = sqrt(pow(z,2)+pow(x,2));
+
gamma = (((posicion_servos_0.arti2*300)/1023)+30)*PI/180;
−
+
delta = (((posicion_servos_0.arti3*300)/1023)+30)*PI/180;
−
L = sqrt(pow(z_p,2)+pow(y,2));
+
−
+
epsilon = (inclinacion_pinza*PI)/180;
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
−
+
L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
 +
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 +
 +
beta_p = beta - beta_a;
 +
 +
delta_a = PI - (beta_a + gamma);
 +
 +
delta_p1 = delta - delta_a;
 +
 +
delta_p2 = 2*PI - (delta - delta_a);
 
 
−
beta_pp = atan2(y,z_p);
+
if (delta_p1 < delta_p2)  
−
+
{
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p1));
−
+
}
−
beta = ((beta_p+beta_a)*180)/PI;
+
else
−
+
{
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p2));
−
+
}
−
delta_a = PI-(beta_a+gamma);
+
−
+
z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
−
gamma = (gamma*180)/PI;
+
−
+
beta_pp = acos(z_p/L);
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
 
−
 
−
if (beta_pp > beta_p) {
 
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
 
−
}
 
−
else {
 
−
delta = ((delta_p+delta_a)*180)/PI;
 
−
 
−
if (isnan(delta)) {
 
−
delta = ((PI+delta_a)*180)/PI;
 
−
}
 
−
}
 
−
 
−
if (isnan(gamma)) //si no hay solución
 
−
{
 
−
coordenadas_correctas = 0;
 
−
}
 
−
 
−
brazo_fer::Servos brazo;
 
 
 
−
brazo.base = ((alfa+150)*1023)/300;
+
a = L1*sin(beta);
−
brazo.arti1 = ((beta+60)*1023)/300;
 
−
brazo.arti2 = ((gamma-30)*1023)/300;
 
−
brazo.arti3 = ((delta-30)*1023)/300;
 
 
 
−
::vel.base = abs(brazo.base - ::pg.base)/5;
+
b = L2*sin(beta+gamma)+Lp*sin(epsilon);
−
::vel.arti1 = abs(brazo.arti1 - ::pg.arti1)/5;
 
−
::vel.arti2 = abs(brazo.arti2 - ::pg.arti2)/5;
 
−
::vel.arti3 = abs(brazo.arti3 - ::pg.arti3)/5;
 
 
 
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
+
if (a >= b)  
−
return brazo;
+
{
 +
y = L*sin(beta_pp);
 
}
 
}
−
else {
+
else
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
{
−
::error_coordenadas = 1;
+
y = -L*sin(beta_pp);
−
return ::pg;
+
}
−
}  
+
−
   }
+
z = z_p*cos(alfa);
 +
 +
x = z_p*sin(alfa);
 +
 +
geometry_msgs::Point punto;
 +
 +
punto.x = x;
 +
punto.y = y;
 +
punto.z = z;
 +
 +
return punto;
 +
   }  
 
    
 
    
−
  void home()
+
brazo_fer::WriteServos inversa(geometry_msgs::Point destino, int inclinacion_pinza, brazo_fer::Servos posicion_servos_0, int velocidad)  
−
   {  
+
   {
 
   
 
   
−
  ros::NodeHandle n;
+
double x = destino.x;
−
+
   double y = destino.y;
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
double z = destino.z;
−
  
+
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
+
int coordenadas_correctas = 1;
−
 
+
 
−
   
+
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
−
if (::cont == 0) {
+
double z_p;
−
+
double L_a, L;
−
::punto_0.x = 0;
+
−
::punto_0.y = 75;
+
epsilon = (inclinacion_pinza*PI)/180;
−
::punto_0.z = 50;
+
−
+
alfa = (atan2(x,z)*180)/PI;
−
::on.base = 1;
+
−
::on.arti1 = 1;
+
z_p = sqrt(pow(z,2)+pow(x,2));
−
::on.arti2 = 1;
+
−
::on.arti3 = 1;
+
L = sqrt(pow(z_p,2)+pow(y,2));
−
::on.pinza = 1;
+
−
+
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
−
::off.base = 0;
+
−
::off.arti1 = 0;
+
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
−
::off.arti2 = 0;
 
−
::off.arti3 = 0;
 
−
::off.pinza = 0;  
 
−
 
−
::punto_1 = ::punto_0;
 
−
 
−
        ::error_coordenadas = 0;
 
−
 
−
brazo_fer::Servos inicio = inversa(::punto_0);
 
−
 
−
brazo_fer::WriteServos ini;
 
−
 
−
ini.posicion = inicio;
 
−
 
−
ini.posicion.pinza = 511;
 
−
 
−
ini.velocidad = ::vel;
 
−
 
−
ini.velocidad.pinza = abs(511 - ::pg.pinza);
 
−
 
−
ini.par = ::on;
 
−
 
−
move_pub_.publish(ini);
 
−
 
−
hand_pub_.publish(ini);
 
−
 
−
}
 
 
 
−
  }
+
beta_pp = atan2(y,z_p);
−
 
+
−
  brazo_fer::Servos acercar(geometry_msgs::Point cerca)
+
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
−
  {
+
−
  double x = cerca.x;
+
beta = ((beta_p+beta_a)*180)/PI;
−
  double z = cerca.z;  
+
−
 
+
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
−
  cerca.z = cerca.z - 70*cos(atan2(x,z));
+
−
  cerca.x = cerca.x - 70*sin(atan2(x,z));
+
delta_a = PI-(beta_a+gamma);
−
 
+
−
  brazo_fer::Servos move = inversa(cerca);
+
gamma = (gamma*180)/PI;
−
 
+
−
  return move;
+
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
−
 
+
−
  }
+
if (beta_pp > beta_p) {
−
 
+
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
−
    brazo_fer::Servos levantar_acercar(geometry_msgs::Point la)
+
}
−
  {
+
else {
−
 
+
delta = ((delta_p+delta_a)*180)/PI;
−
  la.x = 0;
+
−
  la.y = la.y + 50;
+
if (isnan(delta)) {
−
  la.z = 60;
+
delta = ((PI+delta_a)*180)/PI;
−
 
+
}
−
  if (la.y < 75) {
 
−
la.y = 75;
 
 
}
 
}
−
 
 
−
  brazo_fer::Servos move = inversa(la);
 
−
 
 
−
  ::punto_0 = la;
 
−
 
 
−
  return move;
 
−
 
−
  }
 
−
 
 
−
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  //Función de llamda con cada dato recibido
 
−
  {
 
−
 
 
−
ros::NodeHandle n;
 
 
   
 
   
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
+
−
 
+
if (isnan(gamma))
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm" 
+
{
−
   
+
coordenadas_correctas = 0;  
−
brazo_fer::Servos p = pec.posicion;  //Posición del servomotor
+
}
  
−
::pg = pec.posicion;
+
brazo_fer::Servos posicion_servos_1;
 +
brazo_fer::Servos velocidad_servos;
 +
brazo_fer::Servos par_servos;
 
 
−
brazo_fer::Servos e = pec.estado;   //Estado del servomotor
+
posicion_servos_1.base = ((alfa+150)*1023)/300;
 +
posicion_servos_1.arti1 = ((beta+60)*1023)/300;
 +
posicion_servos_1.arti2 = ((gamma-30)*1023)/300;
 +
posicion_servos_1.arti3 = ((delta-30)*1023)/300;
 
 
−
brazo_fer::Servos c = pec.corriente;   //Posición del servomotor
+
if (velocidad == 0)
 +
{
 +
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)/5;
 +
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
 +
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
 +
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)/5;
 +
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
 +
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
 +
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)/5;
 +
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
 +
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
 +
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)/5;
 +
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
 +
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}
 +
 +
}
 +
else
 +
{
 +
velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)*(velocidad/10);
 +
if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
 +
else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
 +
velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)*(velocidad/10);
 +
if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
 +
else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
 +
velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)*(velocidad/10);
 +
if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
 +
else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
 +
velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)*(velocidad/10);
 +
if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
 +
else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}
 +
 +
}
 
 
−
home();
+
brazo_fer::Servos velocidad_servos_0;
 
 
−
::cont = ::cont+1;
+
velocidad_servos_0.base = 0;
 +
velocidad_servos_0.arti1 = 0;
 +
velocidad_servos_0.arti2 = 0;
 +
velocidad_servos_0.arti3 = 0;
 +
 +
par_servos.base = 1;
 +
par_servos.arti1 = 1;
 +
par_servos.arti2 = 1;
 +
par_servos.arti3 = 1;
 
 
−
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) && ::error_coordenadas != 1) {
+
brazo_fer::WriteServos move_arm;
−
 
−
::move_1 = acercar(::punto_0);
 
−
::move_1 = inversa(::punto_0);
 
 
 
−
if (::error_coordenadas == 1) {
+
if (coordenadas_correctas == 1 && (205 <= posicion_servos_1.base && posicion_servos_1.base <= 818) && (120 <= posicion_servos_1.arti1 && posicion_servos_1.arti1 <= 920) && posicion_servos_1.arti2 >= 50  && (posicion_servos_1.arti3 <= 828 && posicion_servos_1.arti3 >= 195)) {
−
+
move_arm.posicion = posicion_servos_1;
−
::cont = 0;
+
move_arm.velocidad = velocidad_servos;
−
home();
+
move_arm.par = par_servos;
−
::cont = ::cont+1;
+
return move_arm;
−
::punto_1 = ::punto_0;
 
 
}
 
}
−
else {
+
else {
 +
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
 +
move_arm.posicion = posicion_servos_0;
 +
move_arm.velocidad = velocidad_servos_0;
 +
move_arm.par = par_servos;
 +
return move_arm;
 +
}  
 +
  }
 +
 
 +
brazo_fer::WriteServos  control_pinza(brazo_fer::Servos posicion_servos_1, brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
 +
  {
 +
 +
brazo_fer::Servos velocidad_servos;  
 +
brazo_fer::Servos par_servos;
 +
 +
velocidad_servos.pinza = 50;
 +
   
 +
par_servos.pinza = 1;
 +
 +
brazo_fer::WriteServos hand_arm;
 +
 +
if ((posicion_servos_1.pinza >= 480 && posicion_servos_1.pinza <= 680 && corriente_servos.pinza <= 300) || (posicion_servos_0.pinza > posicion_servos_1.pinza && posicion_servos_1.pinza >= 480))
 +
{
 +
hand_arm.posicion = posicion_servos_1;
 +
hand_arm.velocidad = velocidad_servos;
 +
hand_arm.par = par_servos;
 +
return hand_arm;
 +
}
 +
else
 +
{
 +
std::cout<<"Alcanzado límite de la pinza"<<std::endl;
 +
hand_arm.posicion = posicion_servos_0;
 +
hand_arm.velocidad = velocidad_servos;
 +
hand_arm.par = par_servos;
 +
return hand_arm;
 +
}
 +
  } 
 +
 
 +
geometry_msgs::Point home(brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
 +
  {
 +
 
 +
  ros::NodeHandle n;
 +
 +
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
 +
 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); 
 +
 +
geometry_msgs::Point punto_0;
 
 
−
  if (::cont_1 == 0) {
+
punto_0.x = 0;
−
 
+
punto_0.y = 80;
−
::move = acercar(::punto_0);
+
punto_0.z = 50;
−
 
+
−
brazo_fer::WriteServos uno;
+
int inclinacion_pinza = 0;
−
 
+
−
uno.posicion = ::move;
+
brazo_fer::WriteServos inicio_brazo = inversa(punto_0, inclinacion_pinza, posicion_servos_0, 0);
−
 
+
−
uno.velocidad = ::vel;
+
brazo_fer::Servos posicion_servos_1;
−
 
+
−
uno.par = ::on;
+
posicion_servos_1.pinza = 511;
−
 
+
−
move_pub_.publish(uno);
+
brazo_fer::WriteServos inicio_pinza = control_pinza(posicion_servos_1, posicion_servos_0, corriente_servos);
−
 
 
−
::cont_1 = ::cont_1+1;
 
−
 
 
−
}
 
−
 
 
−
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
 
 
 
−
if (::cont_2 == 0) {
+
move_pub_.publish(inicio_brazo);
 +
 +
hand_pub_.publish(inicio_pinza);
 +
 
 
−
::move = inversa(::punto_0);
+
return punto_0;
−
+
 
−
brazo_fer::WriteServos dos;  
+
  }
−
 
+
   
−
dos.posicion = ::move;
+
#endif
−
+
 
−
dos.velocidad = ::vel;
+
</syntaxhighlight>
−
 
+
 
−
dos.par = ::on;
+
===Programa===
−
 
+
 
−
move_pub_.publish(dos);
+
Este programa ejecuta la secuencia para coger una botella pequeña de plástico (vacía, ya que la potencia de los servomotores es insuficiente para una llena, deberían emplearse más servomotores por articulación). Los pasos que realiza son los siguientes:
−
+
 
−
::cont_2 = ::cont_2+1;
+
# Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).
−
+
# Posicionamiento en el punto indicado.
−
+
# Cierre de la pinza (hasta tener agarrada la botella).
−
}
+
# Levantamiento y acercamiento (levanta y acerca la botella al MYRAbot)
−
else {
+
 
−
+
Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v02.cpp" con el siguiente contenido:  
−
+
 
−
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
+
<syntaxhighlight lang="cpp" enclose="div">
−
+
 
−
int pinza;
+
  #include "ros/ros.h"
−
+
  #include "brazo_fer/Servos.h"
−
pinza = p.pinza;
+
  #include "brazo_fer/WriteServos.h" 
−
+
  #include "brazo_fer/ReadServos.h"
−
if(c.pinza < 300) {
+
  #include "brazo_fer/brazo_fer.h"   
−
+
  #include "std_msgs/Int16.h"
−
std::cout<<c.pinza<<std::endl;
+
  #include "std_msgs/String.h" 
−
+
  #include "geometry_msgs/Point.h"
−
pinza = pinza + 20;
+
  #include "math.h"  
−
+
 
−
::pin.pinza = pinza;
+
  geometry_msgs::Point punto_0,punto_1;
−
+
 
−
brazo_fer::WriteServos tres;  
+
  int cont = 0;
−
 
+
  int cont_1 = 0;
−
tres.posicion = ::pin;
+
  int cont_2 = 0;
−
+
  int inclinacion_pinza = 0;
−
tres.velocidad.pinza = abs(::pin.pinza - p.pinza);
+
  int start = 0;
−
 
+
   
−
tres.par = ::on;
+
  brazo_fer::Servos pg, cg;
−
+
  brazo_fer::WriteServos move;
−
hand_pub_.publish(tres);
+
  brazo_fer::Servos pinza;
−
+
  brazo_fer::WriteServos pin;
−
}
+
  brazo_fer::WriteServos error_coordenadas;
−
else {
+
    
−
+
   brazo_fer::WriteServos acercar(geometry_msgs::Point cerca)
−
std::cout<<"Agarrado"<<std::endl;
 
−
 
−
::move = levantar_acercar(::punto_0);
 
−
 
−
brazo_fer::WriteServos cuatro;  
 
−
 
 
−
cuatro.posicion = ::move;
 
−
 
−
cuatro.velocidad = ::vel;
 
−
 
−
cuatro.par = ::on;
 
−
 
−
move_pub_.publish(cuatro);
 
−
 
−
::punto_1 = ::punto_0;
 
−
 
−
::cont_1 = 0;
 
−
::cont_2 = 0;
 
−
}
 
−
}
 
−
}
 
−
 
−
}
 
−
else {
 
−
 
−
std::cout<<"En movimiento"<<std::endl;
 
−
}
 
−
}
 
−
}
 
−
 
−
 
 
−
   }
 
−
 
−
   void punto(const geometry_msgs::Point& point)   //Función de llamda con cada dato recibido
 
 
   {
 
   {
−
+
  double x = cerca.x;
−
::punto_0 = point;
+
  double z = cerca.z;
−
 
+
 
−
   }  
+
  cerca.z = cerca.z - 70*cos(atan2(x,z));
−
+
  cerca.x = cerca.x - 70*sin(atan2(x,z));
−
+
 
−
  int main(int argc, char **argv)
+
  brazo_fer::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);
 +
 
 +
  return move;
 +
 
 +
   }
 +
 
 +
    brazo_fer::WriteServos levantar_acercar(geometry_msgs::Point la)
 
   {
 
   {
−
+
 
−
ros::init(argc, argv, "control_recogida");   //Creación del nodo "control_brazo"
+
  la.x = 0;
−
+
  la.y = la.y + 50;
−
ros::NodeHandle n;
+
  la.z = 60;
−
 
 
  
 
  
−
   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente); //Suscripción del topic "pose_arm"
+
   if (la.y < 75) {
 +
la.y = 75;
 +
}
 +
 
 +
  brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);
 
  
 
  
−
   ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto); //Suscripción del topic "point"
+
   ::punto_0 = la;
 
  
 
  
−
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
  return move;
−
 
 
−
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm"
 
  
 +
  }
 +
 
 +
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec) 
 +
  {
 +
 
 +
ros::NodeHandle n;
 +
 +
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 +
 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); 
 +
   
 +
brazo_fer::Servos p = pec.posicion; 
  
 +
::pg = pec.posicion;
 +
::cg = pec.corriente;
 +
 +
brazo_fer::Servos e = pec.estado; 
 +
 +
brazo_fer::Servos c = pec.corriente; 
 
 
−
ros::spin();  //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"
 
−
 
−
        return 0;
 
−
  }
 
  
 +
if (::cont == 0)
 +
{
 +
::punto_0 = home(p, c);
 +
 +
::punto_1 = ::punto_0;
 +
 +
::cont = ::cont+1;
 +
 +
}
 +
 +
if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {
  
−
</syntaxhighlight>
 
  
−
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
+
−
 
+
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
+
  if (::cont_1 == 0) {
−
 
+
 
−
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
+
::move = acercar(::punto_0);
−
 
+
 
−
<syntaxhighlight>roscd brazo_fer
+
move_pub_.publish(::move);
−
make</syntaxhighlight>
+
 
−
 
+
::cont_1 = ::cont_1+1;
−
====Ejecutando el programa====
+
 
−
 
+
}
−
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
+
   
−
 
+
 
−
<syntaxhighlight>roscore</syntaxhighlight>
+
  if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
−
 
+
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
+
if (::cont_2 == 0) {
−
 
+
−
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
−
 
+
 
−
Para ejecutar el programa de control llamado "control_v02", en un terminal nuevo ejecutaremos el siguiente comando:
+
move_pub_.publish(::move);
−
 
+
−
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
+
::cont_2 = ::cont_2+1;
−
 
+
−
Para poder publicar las coordenadas del punto donde se encuentra la botella (ya que no disponemos de otros medios para calcular la posición, se colocará la botella en una posición conocida), debemos publicar en el topic "pick_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella):
+
−
 
+
}
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>
+
else {
−
 
+
−
En el siguiente vídeo se puede ver la ejecución de la secuencia para la recogida de una botella:
+
−
 
+
  if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
−
<center><videoflash>0FcB6-i23cU</videoflash></center>
+
−
 
+
−
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Ver vídeo en YouTube]</center>
+
::pinza.pinza = p.pinza;
−
 
+
−
===Quinto programa (posa la botella)===
+
if(::pin.posicion.pinza != p.pinza) {
−
 
+
−
Este programa ejecuta la secuencia para posar la botella que hemos cogido. Los pasos que realiza son los siguientes:
+
std::cout<<c.pinza<<std::endl;
−
 
+
−
# Posicionamiento en el punto indicado.
+
::pinza.pinza = ::pinza.pinza + 20;
−
# Apertura de la pinza (hasta soltar la botella).
+
−
# Separación (sitúa el brazo a unos 70 mm del punto indicado).
+
::pin = control_pinza(::pinza, p, c);
−
# Retorno a la posición de inicial.
+
−
 
+
hand_pub_.publish(::pin);
−
Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v03.cpp" con el siguiente contenido:
+
−
 
+
}
−
<syntaxhighlight lang="cpp" enclose="div">
+
else {
 +
 +
std::cout<<"Agarrado"<<std::endl;
 +
 +
::move = levantar_acercar(::punto_0);
 +
 +
move_pub_.publish(::move);
 +
 +
::punto_1 = ::punto_0;
 +
 +
::cont_1 = 0;
 +
::cont_2 = 0;
 +
}
 +
}
 +
}
 +
 +
}
 +
else {
 +
 +
std::cout<<"En movimiento"<<std::endl;
 +
}
 +
 +
}
 +
 +
 
 +
  }
 +
 +
  void punto(const geometry_msgs::Point& point)  
 +
  {
 +
ros::NodeHandle n;
 +
 +
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
 +
 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm"
 +
 +
::punto_0 = point;
  
−
   #include "ros/ros.h" 
+
   }
−
  #include "brazo_fer/Servos.h"
+
   
−
  #include "brazo_fer/WriteServos.h"  
+
   int main(int argc, char **argv)
−
   #include "brazo_fer/ReadServos.h" 
 
−
  #include "std_msgs/Int16.h"
 
−
  #include "geometry_msgs/Point.h"
 
−
  #include "math.h" 
 
−
 
 
−
  #define PI 3.14159265
 
−
  #define L1 104 // longitud antebrazo (mm)
 
−
  #define L2 104 // longitud brazo (mm)
 
−
  #define Lp 60 //longitud de la mano
 
−
  #define Lb 60 //longitud desde la base de la articulación A1  hasta la articulación A2 
 
−
 
 
−
  geometry_msgs::Point punto_0,punto_1;
 
−
 
 
−
  int cont = 0;
 
−
  int cont_1 = 0;
 
−
  int cont_2 = 0;
 
−
  int cont_3 = 0;
 
−
  int error_coordenadas = 0;
 
−
   
 
−
  brazo_fer::Servos pg;
 
−
  brazo_fer::Servos move, move_1, vel;
 
−
  brazo_fer::Servos pin;
 
−
 
 
−
  brazo_fer::Servos on;
 
−
 
−
  brazo_fer::Servos off;
 
−
 
 
−
 
 
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino)  
 
 
   {
 
   {
−
 
+
−
double x = destino.x;
+
ros::init(argc, argv, "control_recogida");
−
  double y = destino.y;
+
−
double z = destino.z;
+
ros::NodeHandle n;
 
 
−
y = y + z*tan(atan2(45,250));
+
 
 +
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
 +
 
 +
  ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);
 +
 
 +
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
  
−
int coordenadas_correctas = 1;// para determinar si la posición se puede alcanzar
+
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
  
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
 
−
double z_p;
 
−
double L_a, L;
 
−
 
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
 
−
 
−
alfa = (atan2(x,z)*180)/PI;
 
−
 
−
z_p = sqrt(pow(z,2)+pow(x,2));
 
−
 
−
L = sqrt(pow(z_p,2)+pow(y,2));
 
−
 
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
 
−
 
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
 
 
 
−
beta_pp = atan2(y,z_p);
+
ros::spin();
 
   
 
   
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
        return 0;
−
+
  }
−
beta = ((beta_p+beta_a)*180)/PI;
+
 
−
+
 
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
</syntaxhighlight>
−
+
 
−
delta_a = PI-(beta_a+gamma);
+
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
−
+
 
−
gamma = (gamma*180)/PI;
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
−
+
 
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
+
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
−
+
 
−
if (beta_pp > beta_p) {
+
<syntaxhighlight>roscd brazo_fer
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
make</syntaxhighlight>
−
}
+
 
−
else {
+
===Ejecutando el programa===
−
delta = ((delta_p+delta_a)*180)/PI;
+
 
−
+
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
−
if (isnan(delta)) {
+
 
−
delta = ((PI+delta_a)*180)/PI;
+
<syntaxhighlight>roscore</syntaxhighlight>
−
}
+
 
−
}
+
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
−
+
 
−
if (isnan(gamma)) // si no hay solución
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
−
{
+
 
−
coordenadas_correctas = 0;
+
Para ejecutar el programa de control llamado "control_v02", en un terminal nuevo ejecutaremos el siguiente comando:
−
}
+
 
−
+
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
−
brazo_fer::Servos brazo;
+
 
−
+
Para poder publicar las coordenadas del punto donde se encuentra la botella (ya que no disponemos de otros medios para calcular la posición, se colocará la botella en una posición conocida), debemos publicar en el topic "pick_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella):
−
brazo.base = ((alfa+150)*1023)/300;
+
 
−
brazo.arti1 = ((beta+60)*1023)/300;
+
<syntaxhighlight>rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>
−
brazo.arti2 = ((gamma-30)*1023)/300;
+
 
−
brazo.arti3 = ((delta-30)*1023)/300;
+
En el siguiente vídeo se puede ver la ejecución de la secuencia para la recogida de una botella:
−
+
 
−
::vel.base = abs(brazo.base - ::pg.base)/5;
+
<center><videoflash>0FcB6-i23cU</videoflash></center>
−
::vel.arti1 = abs(brazo.arti1 - ::pg.arti1)/5;
+
 
−
::vel.arti2 = abs(brazo.arti2 - ::pg.arti2)/5;
+
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Ver vídeo en YouTube]</center>
−
::vel.arti3 = abs(brazo.arti3 - ::pg.arti3)/5;
+
 
−
+
==Quinto programa (posa la botella)==
−
+
 
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50 && brazo.arti3 <= 828) {
+
Este programa ejecuta la secuencia para posar la botella que hemos cogido. Los pasos que realiza son los siguientes:
−
return brazo;
+
 
−
}
+
# Posicionamiento en el punto indicado.
−
else {
+
# Apertura de la pinza (hasta soltar la botella).
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
# Separación (sitúa el brazo a unos 70 mm del punto indicado).
−
::error_coordenadas = 1;
+
# Retorno a la posición de inicial.
−
return ::pg;
+
 
−
}    
+
Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v03.cpp" con el siguiente contenido:
−
   }
+
 
−
    
+
<syntaxhighlight lang="cpp" enclose="div">
−
   void home()
+
 
−
   {  
+
  #include "ros/ros.h"
−
 
+
  #include "brazo_fer/Servos.h"
−
   ros::NodeHandle n;
+
  #include "brazo_fer/WriteServos.h"  
−
+
  #include "brazo_fer/ReadServos.h"
−
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
  #include "brazo_fer/brazo_fer.h"   
−
  
+
  #include "std_msgs/Int16.h"
−
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
+
  #include "std_msgs/String.h" 
 +
  #include "geometry_msgs/Point.h"
 +
  #include "math.h"
 +
 
 +
  geometry_msgs::Point punto_0, punto_1;
 +
 
 +
  int cont = 0;
 +
  int cont_1 = 0;
 +
  int cont_2 = 0;
 +
  int cont_3 = 0;
 +
  int inclinacion_pinza = 0;
 +
   
 +
  brazo_fer::Servos pg, cg, pinza;
 +
  brazo_fer::WriteServos move;
 +
   brazo_fer::WriteServos pin;
 +
   brazo_fer::WriteServos error_coordenadas; 
 +
    
 +
   brazo_fer::WriteServos separar(geometry_msgs::Point lejos)
 +
   {
 +
  double x = lejos.x;
 +
   double z = lejos.z;  
 +
 
 +
   lejos.z = lejos.z - 70*cos(atan2(x,z));
 +
   lejos.x = lejos.x - 70*sin(atan2(x,z));
 +
 
 +
   brazo_fer::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);
 
  
 
  
−
   
+
  return move;
−
if (::cont == 0) {
+
 
−
+
  }
−
::punto_0.x = 0;
+
 
−
::punto_0.y = 75;
+
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
−
::punto_0.z = 50;
+
  {  
−
+
 
−
::on.base = 1;
+
ros::NodeHandle n;
−
::on.arti1 = 1;
+
−
::on.arti2 = 1;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
−
::on.arti3 = 1;
+
 
−
::on.pinza = 1;
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
−
+
   
−
::off.base = 0;
+
brazo_fer::Servos p = pec.posicion;  
−
::off.arti1 = 0;
+
 
−
::off.arti2 = 0;
+
::pg = pec.posicion;
−
::off.arti3 = 0;
+
::cg = pec.corriente;
−
::off.pinza = 0;  
+
 +
brazo_fer::Servos e = pec.estado;  
 +
 +
brazo_fer::Servos c = pec.corriente;
 +
 +
if (::cont == 0)
 +
{
 +
::punto_0 = directa(p, inclinacion_pinza);
 
 
 
::punto_1 = ::punto_0;
 
::punto_1 = ::punto_0;
 +
 +
::cont = ::cont+1;
 +
}
 +
 +
if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {
 +
 +
 +
 
 
−
::error_coordenadas = 0;
 
 
 
−
if (::cont_3 > 0) {
+
  if (::cont_1 == 0) {
−
+
 
−
brazo_fer::Servos inicio = inversa(::punto_0);
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
−
+
 
−
brazo_fer::WriteServos ini;
+
move_pub_.publish(::move);
 +
 
 +
::cont_1 = ::cont_1+1;
 +
 
 +
} 
 +
 
 +
  if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
 
 
−
ini.posicion = inicio;
+
if (::cont_2 == 0) {
 +
 +
::pinza.pinza = 470;
 +
 +
::pin = control_pinza(::pinza, p, c);
 +
 +
hand_pub_.publish(::pin);
 +
 +
::cont_2 = ::cont_2+1;
 
 
−
ini.posicion.pinza = 511;
+
}
 +
else {
 
 
−
ini.velocidad = ::vel;
 
 
 
−
ini.velocidad.pinza = abs(511 - ::pg.pinza);
+
  if (p.pinza <= 495) {
−
 
−
ini.par = ::on;
 
 
 
−
move_pub_.publish(ini);
+
if (::cont_3 == 0) {
−
+
−
hand_pub_.publish(ini);
+
::move = separar(::punto_0);
−
+
−
}
+
move_pub_.publish(::move);
−
}
+
−
  }
+
::cont_3 = ::cont_3+1;
−
 
+
−
  brazo_fer::Servos separar(geometry_msgs::Point lejos)
+
}
−
  {
+
−
  double x = lejos.x;
+
else if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
−
  double z = lejos.z;
+
−
 
+
std::cout<<"Suelto"<<std::endl;
−
  lejos.z = lejos.z - 70*cos(atan2(x,z));
+
−
  lejos.x = lejos.x - 70*sin(atan2(x,z));
+
::cont = 0;
−
 
+
−
  brazo_fer::Servos move = inversa(lejos);
+
::cont_1 = 0;
−
 
+
::cont_2 = 0;
−
  return move;
+
::cont_3 = 0;
−
 
+
}
−
  }
+
}
−
 
+
}
−
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  //Función de llamda con cada dato recibido
+
−
  {
+
}
 +
else {
 +
 +
std::cout<<"En movimiento"<<std::endl;
 +
}
 +
}
 +
 +
 
   
 
   
−
ros::NodeHandle n;
+
  }
 
   
 
   
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
  void punto(const geometry_msgs::Point& point)   
−
 
+
   {
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm" 
+
−
   
+
::punto_0 = point;
−
brazo_fer::Servos p = pec.posicion;   //Posición del servomotor
 
  
−
::pg = pec.posicion;
+
  } 
 +
 +
  int main(int argc, char **argv)
 +
  {
 +
 +
ros::init(argc, argv, "control_entrega");  
 +
 +
ros::NodeHandle n;
 
 
−
brazo_fer::Servos e = pec.estado;   //Estado del servomotor
+
 
−
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
−
brazo_fer::Servos c = pec.corriente;   //Posición del servomotor
+
 
−
+
  ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);
−
home();
+
−
+
 
−
::cont = cont+1;
+
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 +
 
 +
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
  
−
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) && ::error_coordenadas != 1) {
 
−
 
−
::move_1 = separar(::punto_0);
 
−
::move_1 = inversa(::punto_0);
 
 
 
−
if (::error_coordenadas == 1) {
+
ros::spin();  
−
+
−
::punto_1 = ::punto_0;
+
        return 0;
−
}
+
  }
−
else {
+
 
−
+
</syntaxhighlight>
−
  if (::cont_1 == 0) {
+
 
−
 
+
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
−
::move = inversa(::punto_0);
+
 
−
 
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>
−
brazo_fer::WriteServos uno;
+
 
−
 
+
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
−
uno.posicion = ::move;
+
 
−
 
+
<syntaxhighlight>roscd brazo_fer
−
uno.velocidad = ::vel;
+
make</syntaxhighlight>
−
 
+
 
−
uno.par = ::on;
+
===Ejecutando el programa===
−
 
+
 
−
move_pub_.publish(uno);
+
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
−
 
+
 
−
::cont_1 = ::cont_1+1;
+
<syntaxhighlight>roscore</syntaxhighlight>
−
 
 
−
}
 
−
 
 
−
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
 
−
 
−
if (::cont_2 == 0) {
 
−
 
−
::pin.pinza = 511;
 
−
 
−
brazo_fer::WriteServos dos;  
 
−
 
 
−
dos.posicion = ::pin;
 
−
 
−
dos.velocidad.pinza = abs(::pin.pinza - p.pinza);
 
−
 
 
−
dos.par = ::on;
 
−
 
−
hand_pub_.publish(dos);
 
−
 
−
::cont_2 = ::cont_2+1;
 
−
 
−
}
 
−
else {
 
−
 
−
 
−
  if (p.pinza <= 516 || p.pinza >= 506) {
 
−
 
−
if (::cont_3 == 0) {
 
  
−
std::cout<<"Suelto"<<std::endl;
+
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
−
+
 
−
                                                ::move = separar(::punto_0);
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
−
+
 
−
brazo_fer::WriteServos tres;  
+
Para ejecutar el programa de control llamado "control_v03", en un terminal nuevo ejecutaremos el siguiente comando:
−
+
 
−
tres.posicion = ::move;
+
<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>
−
+
 
−
tres.velocidad = ::vel;
+
Para poder publicar las coordenadas del punto donde queremos posar la botella, debemos publicar en el ''topic'' "place_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
−
+
 
−
tres.par = ::on;
+
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
−
 
−
move_pub_.publish(tres);
 
−
 
−
::cont_3 = ::cont_3+1;
 
−
 
−
}
 
−
 
−
else if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
 
−
std::cout<<"Suelto"<<std::endl;
 
  
−
::cont = 0;
+
==Sexto programa (Sigue el camino recto)==
−
+
 
−
home();
+
Se ha creado un programa que desplaza el brazo desde el punto en el que se encuentre al que le indiquemos, siguiendo la línea recta que une los puntos. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "control_v04.cpp" con el siguiente contenido:
−
+
 
−
::cont = ::cont + 1;
+
<syntaxhighlight lang="cpp" enclose="div">
−
                                               
 
−
::cont_1 = 0;
 
−
::cont_2 = 0;
 
−
}
 
−
}
 
−
}
 
−
 
−
}
 
−
else {
 
−
 
−
std::cout<<"En movimiento"<<std::endl;
 
−
}
 
−
}
 
−
}
 
−
 
−
 
 
−
  }
 
−
 
−
  void punto(const geometry_msgs::Point& point)  //Función de llamda con cada dato recibido
 
−
  {
 
−
 
−
::punto_0 = point;
 
  
−
   }
+
   #include "ros/ros.h"
 +
  #include "brazo_fer/Servos.h"
 +
  #include "brazo_fer/WriteServos.h" 
 +
  #include "brazo_fer/ReadServos.h"
 +
  #include "brazo_fer/brazo_fer.h" 
 +
  #include "geometry_msgs/Point.h"
 +
  #include "math.h"
 
   
 
   
 +
  brazo_fer::Servos  p, e, c;
 +
  brazo_fer::WriteServos destino, mover; 
 +
  geometry_msgs::Point punto_0, punto_1, punto_i;
 +
  double lambda = 0;
 +
  int inclinacion_pinza = 0;
 +
  int cont = 0;
 
   
 
   
−
   int main(int argc, char **argv)
+
   void posicion(const brazo_fer::ReadServos& pose)
 
   {
 
   {
 +
ros::NodeHandle n;
 
   
 
   
−
ros::init(argc, argv, "control_entrega");   //Creación del nodo "control_brazo"
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
   
 
   
−
ros::NodeHandle n;
+
::p = pose.posicion;
−
+
        ::e = pose.estado;
−
 
+
        ::c = pose.corriente;
−
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente); //Suscripción del topic "pose_arm"
+
   
−
 
+
    if (cont == 0)
−
  ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto); //Suscripción del topic "point"
+
    {
−
 
+
::punto_0 = home(::p, ::c);
−
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
   
−
 
+
::punto_1 = ::punto_0;
−
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
+
−
 
+
cont = 1;
−
 
+
}
−
+
   
−
ros::spin();   //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"
+
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::mover.posicion.base && ::mover.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::mover.posicion.arti1 && ::mover.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::mover.posicion.arti2 && ::mover.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::mover.posicion.arti3 && ::mover.posicion.arti3 < (::p.arti3+15)))
−
+
{
−
        return 0;
+
   
−
  }
+
if (((::p.base-15) < ::destino.posicion.base && ::destino.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.posicion.arti1 && ::destino.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.posicion.arti2 && ::destino.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.posicion.arti3 && ::destino.posicion.arti3 < (::p.arti3+15)))  
−
 
+
{
−
</syntaxhighlight>
+
::punto_0 = ::punto_1;
 +
::lambda = 0;
 +
}
 +
else
 +
{   
 +
geometry_msgs::Point u;
 +
 +
//ecuación de la recta p1 = p0+lambda.u
 +
 +
u.x = ::punto_1.x - ::punto_0.x;
 +
u.y = ::punto_1.y - ::punto_0.y;
 +
u.z = ::punto_1.z - ::punto_0.z;       
 +
 +
double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
 +
 
 +
::lambda = ::lambda + 1/paso;
 +
 +
::punto_i.x = ::punto_0.x + (::lambda)*u.x;
 +
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
 +
::punto_i.z = ::punto_0.z + (::lambda)*u.z;
 +
 +
::mover = inversa(::punto_i, ::inclinacion_pinza, ::p, 0);
 +
 +
move_pub_.publish(::mover);
 +
 +
}
  
−
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
+
}
−
 
+
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>
+
  }
−
 
+
−
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
+
  void punto(const geometry_msgs::Point& point)
−
 
+
  {
−
<syntaxhighlight>roscd brazo_fer
+
ros::NodeHandle n;
−
make</syntaxhighlight>
+
−
 
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
−
====Ejecutando el programa====
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
−
 
+
 
−
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
+
::lambda = 0;   
−
 
+
   
−
<syntaxhighlight>roscore</syntaxhighlight>
+
        ::punto_0 = directa(::p, ::inclinacion_pinza); 
 +
 +
::punto_1 = point;
 +
 +
::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);
 +
 +
  }
 +
 +
 +
  int main(int argc, char **argv)
 +
  {
 +
 +
ros::init(argc, argv, "control_brazo"); 
 +
 +
ros::NodeHandle n;
 +
 +
 +
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
 +
 +
  ros::Subscriber point_sub_= n.subscribe("point", 1, punto);
 +
 +
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 +
   
 +
 +
ros::spin(); 
 +
 +
    return 0;
 +
  }
  
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
+
</syntaxhighlight>
 +
 
 +
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
 +
 
 +
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
 +
 
 +
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
 +
 
 +
<syntaxhighlight>roscd brazo_fer
 +
make</syntaxhighlight>
 +
 
 +
===Ejecutando el programa===
 +
 
 +
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>roscore</syntaxhighlight>
 +
 
 +
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
  
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
  
−
Para ejecutar el programa de control llamado "control_v03", en un terminal nuevo ejecutaremos el siguiente comando:
+
Para ejecutar el programa de control llamado "control_v04", en un terminal nuevo ejecutaremos el siguiente comando:
  
−
<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>
+
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
  
−
Para poder publicar las coordenadas del punto donde queremos posar la botella, debemos publicar en el ''topic'' "place_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella):
+
Para poder publicar las coordenadas del punto donde queremos que se deplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
  
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
+
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
  
−
===Sexto programa (Sigue el camino recto)===
+
==Séptimo programa (Teleoperador)==
  
−
Se ha creado un programa que desplaza el brazo desde el punto en el que se encuentre al que le indiquemos, siguiendo la línea recta que une los puntos. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "control_v04.cpp" con el siguiente contenido:
+
Se ha desarrollado un programa tele-operador para poder manejar el brazo en modo manual usando el teclado del PC. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "teleoperador_teclado.cpp" con el siguiente contenido:  
  
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
Line 1,381: Line 1,498:
 
   #include "brazo_fer/WriteServos.h"   
 
   #include "brazo_fer/WriteServos.h"   
 
   #include "brazo_fer/ReadServos.h"
 
   #include "brazo_fer/ReadServos.h"
 +
  #include "brazo_fer/brazo_fer.h" 
 
   #include "geometry_msgs/Point.h"
 
   #include "geometry_msgs/Point.h"
−
   #include "math.h"
+
   #include "std_msgs/Int16.h"
−
+
   #include "std_msgs/String.h" 
−
   #define PI 3.14159265
+
   #include <signal.h>
−
   #define L1 104
+
   #include <termios.h>
−
   #define L2 104
+
   #include <stdio.h>
−
   #define Lp 60
 
−
 
−
 
−
  brazo_fer::Servos move, vel, p, e, c, destino, on, off;
 
−
  geometry_msgs::Point punto_0, punto_1, punto_i;
 
−
  double lambda = 0;
 
−
  int cont = 0;
 
 
    
 
    
−
   geometry_msgs::Point directa(brazo_fer::Servos origen)
+
   #define KEYCODE_Xmas 0x43
−
   {
+
   #define KEYCODE_Xmenos 0x44
−
+
  #define KEYCODE_Ymas 0x41
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, epsilon;
+
  #define KEYCODE_Ymenos 0x42
−
double z_p;
+
  #define KEYCODE_Zmas 0x77
−
double L_a, L;
+
  #define KEYCODE_Zmenos 0x73
−
double x, y, z;
+
  #define KEYCODE_Pabrir 0x61
−
+
  #define KEYCODE_Pcerrar 0x64
−
alfa = (((origen.base*300)/1023)-150)*PI/180;
+
  #define KEYCODE_PinclinarMas 0x65
−
beta = (((origen.arti1*300)/1023)-60)*PI/180;
+
  #define KEYCODE_PinclinarMenos 0x71 
−
gamma = (((origen.arti2*300)/1023)+30)*PI/180;
+
 
−
delta = (((origen.arti3*300)/1023)+30)*PI/180;
+
struct termios cooked, raw;  
−
 
+
 
−
        epsilon = 0; /Ángulo de inclinación de la pinza respecto a la horizontal
+
int cont = 0;
−
 
−
L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
 
−
 
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
  
−
        beta_p = beta - beta_a;
+
double retardo = 0.11;
  
−
delta_a = PI - (beta_a + gamma);
+
  geometry_msgs::Point punto;
 +
  brazo_fer::Servos vel;
 +
  brazo_fer::Servos pinza, p, c;
 +
 
 +
  int pinza_incli = 0;
 +
 
 
 
−
        L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta - delta_a));
+
void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  
−
+
  {
−
z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
+
   
 +
::p = pec.posicion; 
 +
::c = pec.corriente;
  
−
        beta_pp = acos(z_p/L);
+
  }  
−
+
−
if (L1*sin(beta) >= L2*sin(beta + gamma) + Lp*sin(epsilon))  
+
void quit(int sig)
−
{
+
{
−
y = L*sin(beta_pp);
+
  tcsetattr(0, TCSANOW, &cooked);
−
}
+
  ros::shutdown();
−
else
+
  exit(0);
−
{
+
}
−
y = -L*sin(beta_pp);
+
 
−
}
+
void callback(const ros::TimerEvent&)
 +
{
 +
 +
ros::NodeHandle n; 
 +
 
 +
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 +
 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
 +
 
 +
ros::Time anterior;
 
 
−
z = z_p*cos(alfa);
+
brazo_fer::WriteServos teleop;
−
+
brazo_fer::WriteServos teleop_pinza;
−
x = z_p*sin(alfa);
+
 
 +
if (::cont == 0)
 +
    {
 +
anterior = ros::Time::now();
 +
 +
::punto = home(::p, ::c);
 +
 +
::cont = 1;
 +
 +
::pinza.pinza = 511;
 +
 +
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
 +
teleop_pinza = control_pinza(::pinza, ::p, ::c);
 +
 +
//std::cout<<teleop<<"-"<<teleop_pinza<<std::endl;
 +
}
 
 
−
geometry_msgs::Point punto;
+
   char c;
−
 
−
punto.x = x;
 
−
punto.y = y;
 
−
punto.z = z;
 
−
 
−
return punto;
 
−
   }
 
−
 
 
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino)
 
−
  {
 
−
 
 
−
double x = destino.x;
 
−
  double y = destino.y;
 
−
double z = destino.z;
 
  
−
int coordenadas_correctas = 1;
+
  // get the console in raw mode                                                             
−
 
+
  tcgetattr(0, &cooked);
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
  memcpy(&raw, &cooked, sizeof(struct termios));
−
double z_p;
+
  raw.c_lflag &=~ (ICANON | ECHO);
−
double L_a, L;
+
  // Setting a new line, then end of file                       
−
+
  raw.c_cc[VEOL] = 1;
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
  raw.c_cc[VEOF] = 2;
−
+
  tcsetattr(0, TCSANOW, &raw);
−
alfa = (atan2(x,z)*180)/PI;
+
 
−
+
  puts("");
−
z_p = sqrt(pow(z,2)+pow(x,2));
+
  puts("#####################################################");
−
   
+
  puts("                  CONTROLES BRAZO");
−
L = sqrt(pow(z_p,2)+pow(y,2));
+
  puts("#####################################################");
−
+
  puts("");  
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
  puts("Flecha arriba:________ Subir pinza");
−
   
+
  puts("Flecha abajo:_________ Bajar pinza");
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
  puts("Flecha izquierda:_____ Desplazar pinza a la izquierda");
 +
  puts("Flecha derecha:_______ Desplazar pinza a la derecha");
 +
  puts("Tecla W:______________ Desplazar pinza hacia delante");
 +
  puts("Tecla S:______________ Desplazar pinza hacia atrás");
 +
  puts("Tecla A:______________ Abrir pinza");
 +
  puts("Tecla D:______________ Cerrar pinza");
 +
  puts("Tecla Q:______________ Inclinar pinza hacia arriba");
 +
  puts("Tecla E:______________ Inclinar pinza hacia abajo");               
 +
 
 +
       
 +
    while (ros::ok())
 +
    {
 +
 +
    // get the next event from the keyboard  
 +
    if(read(0, &c, 1) < 0)
 +
    {
 +
      perror("read():");
 +
      exit(-1);
 +
    }
 
 
−
beta_pp = atan2(y,z_p);
+
      if (c == KEYCODE_Xmas)
−
+
      {
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{
−
beta = ((beta_p+beta_a)*180)/PI;
+
::punto.x = ::punto.x - 5;
−
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
+
{::punto.x = ::punto.x + 5;}
−
delta_a = PI-(beta_a+gamma);
+
anterior = ros::Time::now();
−
+
}
−
gamma = (gamma*180)/PI;
+
      }
−
+
      if (c == KEYCODE_Xmenos)
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));      
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
if (beta_pp > beta_p) {
+
{     
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
::punto.x = ::punto.x + 5;
−
}
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
else {
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
delta = ((delta_p+delta_a)*180)/PI;
+
{::punto.x = ::punto.x - 5;}
−
+
anterior = ros::Time::now();
−
if (isnan(delta)) {
+
}
−
delta = ((PI+delta_a)*180)/PI;
+
      }
−
}
+
      if (c == KEYCODE_Ymas)
−
}
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
if (isnan(gamma)) // si no hay solución
+
{    
−
{
+
::punto.y = ::punto.y + 5;
−
coordenadas_correctas = 0;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
}
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
−
 
+
{::punto.y = ::punto.y - 5;}
−
brazo_fer::Servos brazo;
+
anterior = ros::Time::now();
−
+
}    
−
brazo.base = ((alfa+150)*1023)/300;
+
      }
−
brazo.arti1 = ((beta+60)*1023)/300;
+
      if (c == KEYCODE_Ymenos)
−
brazo.arti2 = ((gamma-30)*1023)/300;
+
      {
−
brazo.arti3 = ((delta-30)*1023)/300;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
brazo.pinza = 511;
+
{     
−
+
::punto.y = ::punto.y - 5;
−
::vel.base = 400;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
::vel.arti1 = 400;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
−
::vel.arti2 = 400;
+
{::punto.y = ::punto.y + 5;}
−
::vel.arti3 = 400;
+
anterior = ros::Time::now();
−
::vel.pinza = abs(::move.pinza - ::p.pinza);
+
}
−
 
+
      }
−
+
      if (c == KEYCODE_Zmas)
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
+
      {
−
return brazo;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
}
+
{    
−
else {
+
::punto.z = ::punto.z + 5;
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
::punto_0 = ::punto_1;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
−
::lambda = 0;
+
{::punto.z = ::punto.z - 5;}
−
return ::p;
+
anterior = ros::Time::now();
−
}  
+
}
−
  }
+
      }
−
 
+
      if (c == KEYCODE_Zmenos)
−
  void home()
+
      {
−
  {
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
ros::NodeHandle n;
+
{     
−
+
::punto.z = ::punto.z - 5;
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
+
{::punto.z = ::punto.z + 5;}
−
if (::cont == 0)
+
anterior = ros::Time::now();
−
{
+
}
−
+
      }
−
::punto_0.x = 0;
+
      if (c == KEYCODE_Pabrir)
−
::punto_0.y = 0;
+
      {
−
::punto_0.z = 0;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{    
−
::punto_1 = ::punto_0;
+
::pinza.pinza = ::pinza.pinza - 5;
−
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
::on.base = 1;
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
−
::on.arti1 = 1;
+
anterior = ros::Time::now();
−
::on.arti2 = 1;
+
}
−
::on.arti3 = 1;
+
      }
−
::on.pinza = 1;
+
      if (c == KEYCODE_Pcerrar)
−
+
      {
−
::off.base = 0;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
::off.arti1 = 0;
+
{     
−
::off.arti2 = 0;
+
::pinza.pinza = ::pinza.pinza + 5;
−
::off.arti3 = 0;
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
::off.pinza = 0;
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
−
+
anterior = ros::Time::now();
−
  brazo_fer::Servos mover;
 
−
 
−
geometry_msgs::Point home;
 
−
 
−
home.x = 0;
 
−
home.y = 85;
 
−
home.z = 50;
 
−
 
−
::move = inversa(home);
 
−
 
−
brazo_fer::WriteServos inicio;
 
−
 
−
inicio.posicion = ::move;
 
−
 
−
inicio.velocidad = ::vel;
 
−
 
−
inicio.par = ::on;
 
−
 
−
move_pub_.publish(inicio);
 
−
hand_pub_.publish(inicio);
 
−
 
−
::cont = ::cont + 1;
 
−
 
−
}
 
−
 
 
−
  } 
 
−
 
−
  void posicion(const brazo_fer::ReadServos& pose)
 
−
  {
 
−
ros::NodeHandle n;
 
−
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
−
 
−
::p = pose.posicion;
 
−
        ::e = pose.estado;
 
−
        ::c = pose.corriente;
 
−
   
 
−
        home();
 
−
   
 
−
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::move.base && ::move.base < (::p.base+15)) && ((::p.arti1-15) < ::move.arti1 && ::move.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::move.arti2 && ::move.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::move.arti3 && ::move.arti3 < (::p.arti3+15)))
 
−
{
 
−
   
 
−
if (((::p.base-15) < ::destino.base && ::destino.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.arti1 && ::destino.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.arti2 && ::destino.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.arti3 && ::destino.arti3 < (::p.arti3+15)))
 
−
{
 
−
::punto_0 = ::punto_1;
 
−
::lambda = 0;
 
 
}
 
}
−
else
+
      }
−
{   
+
      if (c == KEYCODE_PinclinarMas)
−
geometry_msgs::Point u;
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
//ecuación de la recta p1 = p0+lambda.u
+
{     
−
+
::pinza_incli = ::pinza_incli + 5;
−
u.x = ::punto_1.x - ::punto_0.x;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
u.y = ::punto_1.y - ::punto_0.y;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
u.z = ::punto_1.z - ::punto_0.z;       
+
{::pinza_incli = ::pinza_incli - 5;}
−
+
anterior = ros::Time::now();
−
double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
 
−
 
 
−
::lambda = ::lambda + 1/paso;
 
−
 
−
::punto_i.x = ::punto_0.x + (::lambda)*u.x;
 
−
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
 
−
::punto_i.z = ::punto_0.z + (::lambda)*u.z;
 
−
 
−
//std::cout<<punto_0<<std::endl;
 
−
std::cout<<punto_i<<" : "<<lambda<<std::endl;
 
−
 
−
::move = inversa(::punto_i);
 
−
 
−
brazo_fer::WriteServos mover;
 
−
 
−
mover.posicion = ::move;
 
−
 
−
mover.velocidad = ::vel;
 
−
 
−
mover.par = ::on;
 
−
 
−
move_pub_.publish(mover);
 
−
 
 
}
 
}
−
}
+
      }
 +
      if (c == KEYCODE_PinclinarMenos)
 +
      {
 +
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 +
{     
 +
::pinza_incli = ::pinza_incli - 5;
 +
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
 +
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 +
{::pinza_incli = ::pinza_incli + 5;}
 +
anterior = ros::Time::now();
 +
}
 +
      }                                   
 +
 +
if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
 +
{
 +
move_pub_.publish(teleop);
 +
}
 +
if (teleop_pinza.posicion.pinza != ::p.pinza)
 +
{
 +
hand_pub_.publish(teleop_pinza);
 +
}    
 +
 
 +
}
 +
}
 
   
 
   
−
   }
+
   int main(int argc, char **argv)
 +
  {
 +
 +
ros::init(argc, argv, "teleoperador_teclado"); 
 
   
 
   
−
  void punto(const geometry_msgs::Point& point)
 
−
  {
 
 
ros::NodeHandle n;
 
ros::NodeHandle n;
−
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 
 
  
 
  
−
::lambda = 0;  
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
−
   
+
 
−
        ::punto_0 = directa(::p);
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
 +
 
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   
 +
 +
 
 +
signal(SIGINT,quit);  
 
 
−
::punto_1 = point;
+
ros::Timer timer = n.createTimer(ros::Duration(1), callback);
−
+
   
−
::destino = inversa(::punto_1);
 
 
   
 
   
−
  }
+
ros::spin(); 
 
   
 
   
−
+
        return 0;
−
   int main(int argc, char **argv)
+
   }
−
  {
+
 
−
+
</syntaxhighlight>
−
ros::init(argc, argv, "control_brazo"); 
+
 
−
+
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
−
ros::NodeHandle n;
 
−
 
−
 
−
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
 
−
 
−
  ros::Subscriber point_sub_= n.subscribe("point", 1, punto);
 
−
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 
−
   
 
−
 
−
ros::spin(); 
 
−
 
−
        return 0;
 
−
  }
 
  
 +
<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>
  
−
</syntaxhighlight>
+
Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
−
 
 
−
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
 
−
 
 
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
 
−
 
 
−
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
 
  
 
<syntaxhighlight>roscd brazo_fer
 
<syntaxhighlight>roscd brazo_fer
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
====Ejecutando el programa====
+
===Ejecutando el programa===
  
 
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
 
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
Line 1,701: Line 1,779:
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
  
−
Para ejecutar el programa de control llamado "control_v04", en un terminal nuevo ejecutaremos el siguiente comando:
+
Para ejecutar el programa de control llamado "teleoperador_teclado", en un terminal nuevo ejecutaremos el siguiente comando:
  
−
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
+
<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</syntaxhighlight>
−
 
 
−
Para poder publicar las coordenadas del punto donde queremos que se deplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
 
−
 
 
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
 
−
 
 
−
===Séptimo programa (Teleoperador)===
 
−
 
 
−
Se ha desarrollado un programa tele-operador para poder manejar el brazo en modo manual usando el teclado del PC. Dentro del ''package'' creado, en el directorio "src" crearemos un fichero llamado "teleoperador_teclado.cpp" con el siguiente contenido:
 
−
 
 
−
<syntaxhighlight lang="cpp" enclose="div">
 
−
 
 
−
 
 
−
  #include "ros/ros.h"
 
−
  #include "brazo_fer/Servos.h"
 
−
  #include "brazo_fer/WriteServos.h" 
 
−
  #include "brazo_fer/ReadServos.h"
 
−
  #include "geometry_msgs/Point.h"
 
−
  #include "std_msgs/Int16.h"
 
−
  #include <signal.h>
 
−
  #include <termios.h>
 
−
  #include <stdio.h>
 
−
 
−
  #define PI 3.14159265
 
−
  #define L1 104
 
−
  #define L2 104
 
−
  #define Lp 60
 
−
 
 
−
  #define KEYCODE_Xmas 0x43
 
−
  #define KEYCODE_Xmenos 0x44
 
−
  #define KEYCODE_Ymas 0x41
 
−
  #define KEYCODE_Ymenos 0x42
 
−
  #define KEYCODE_Zmas 0x77
 
−
  #define KEYCODE_Zmenos 0x73
 
−
  #define KEYCODE_Pabrir 0x61
 
−
  #define KEYCODE_Pcerrar 0x64
 
−
  #define KEYCODE_PinclinarMas 0x65
 
−
  #define KEYCODE_PinclinarMenos 0x71 
 
−
 
 
−
struct termios cooked, raw;
 
−
 
 
−
int cont = 0;
 
−
 
 
−
double retardo = 0.1;
 
−
 
 
−
  geometry_msgs::Point punto;
 
−
  brazo_fer::Servos vel;
 
−
  brazo_fer::Servos pinza, p, c;
 
−
 
 
−
  int pinza_incli;
 
−
 
 
−
  brazo_fer::Servos on;
 
−
 
−
  brazo_fer::Servos off;
 
−
 
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino, int inclinacion_pinza)
 
−
  {
 
−
 
 
−
double x = destino.x;
 
−
  double y = destino.y;
 
−
double z = destino.z;
 
−
 
−
y = y + z*tan(atan2(45,250));
 
−
 
 
−
int coordenadas_correctas = 1;
 
−
 
 
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
 
−
double z_p;
 
−
double L_a, L;
 
−
 
−
epsilon = (inclinacion_pinza*PI)/180;
 
−
 
−
alfa = (atan2(x,z)*180)/PI;
 
−
 
−
z_p = sqrt(pow(z,2)+pow(x,2));
 
−
 
−
L = sqrt(pow(z_p,2)+pow(y,2));
 
−
 
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
 
−
 
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
 
−
 
−
beta_pp = atan2(y,z_p);
 
−
 
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
−
 
−
beta = ((beta_p+beta_a)*180)/PI;
 
−
 
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
 
−
 
−
delta_a = PI-(beta_a+gamma);
 
−
 
−
gamma = (gamma*180)/PI;
 
−
 
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));   
 
−
 
−
if (beta_pp > beta_p) {
 
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
 
−
}
 
−
else {
 
−
delta = ((delta_p+delta_a)*180)/PI;
 
−
 
−
if (isnan(delta)) {
 
−
delta = ((PI+delta_a)*180)/PI;
 
−
}
 
−
}
 
−
 
−
 
−
if (isnan(gamma)) // si no hay solución
 
−
{
 
−
coordenadas_correctas = 0;
 
−
}
 
−
 
−
brazo_fer::Servos brazo;
 
−
 
−
brazo.base = ((alfa+150)*1023)/300;
 
−
brazo.arti1 = ((beta+60)*1023)/300;
 
−
brazo.arti2 = ((gamma-30)*1023)/300;
 
−
brazo.arti3 = ((delta-30)*1023)/300;
 
−
 
−
::vel.base = abs(brazo.base - ::p.base)/5;
 
−
::vel.arti1 = abs(brazo.arti1 - ::p.arti1)/5;
 
−
::vel.arti2 = abs(brazo.arti2 - ::p.arti2)/5;
 
−
::vel.arti3 = abs(brazo.arti3 - ::p.arti3)/5;
 
−
 
−
 
−
std::cout<<brazo<<std::endl;
 
−
 
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && (153 && brazo.arti3 <= 870)) {
 
−
return brazo;
 
−
}
 
−
else {
 
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
 
−
 
 
−
return ::p;
 
−
}  
 
−
  }
 
−
 
−
  brazo_fer::Servos  control_pinza(brazo_fer::Servos abrir_cerrar)
 
−
  {
 
−
 
−
if (abrir_cerrar.pinza >= 480 && abrir_cerrar.pinza <= 680 && ::c.pinza <= 300)
 
−
{
 
−
return abrir_cerrar;
 
−
}
 
−
else
 
−
{
 
−
std::cout<<"Alcanzado límite de la pinza"<<std::endl;
 
−
 
−
return ::p;
 
−
}
 
−
  }
 
−
 
−
  void home()
 
−
  {
 
−
 
 
−
  ros::NodeHandle n;
 
−
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
 
−
 
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm"
 
−
 
 
−
 
−
::punto.x = 0;
 
−
::punto.y = 80;
 
−
::punto.z = 50;
 
−
 
−
::on.base = 1;
 
−
::on.arti1 = 1;
 
−
::on.arti2 = 1;
 
−
::on.arti3 = 1;
 
−
::on.pinza = 1;
 
−
 
−
::off.base = 0;
 
−
::off.arti1 = 0;
 
−
::off.arti2 = 0;
 
−
::off.arti3 = 0;
 
−
::off.pinza = 0;
 
−
 
−
::pinza_incli = 0;
 
−
 
−
brazo_fer::Servos inicio = inversa(::punto, ::pinza_incli);
 
−
 
−
brazo_fer::WriteServos ini;
 
−
 
−
ini.posicion = inicio;
 
−
 
−
ini.posicion.pinza = 511;
 
−
 
−
::pinza.pinza = 511;
 
−
 
−
ini.velocidad = ::vel;
 
−
 
−
ini.velocidad.pinza = abs(511 - ::p.pinza);
 
−
 
−
ini.par = ::on;
 
−
 
−
move_pub_.publish(ini);
 
−
 
−
hand_pub_.publish(ini);
 
−
 
−
  }
 
−
 
−
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  //Función de llamda con cada dato recibido
 
−
  {
 
−
   
 
−
::p = pec.posicion;  //Posición del servomotor
 
−
::c = pec.corriente;
 
−
 
 
−
  }  
 
−
 
−
void quit(int sig)
 
−
{
 
−
  tcsetattr(0, TCSANOW, &cooked);
 
−
  ros::shutdown();
 
−
  exit(0);
 
−
}
 
−
 
 
−
void callback(const ros::TimerEvent&)
 
−
{
 
−
 
−
ros::NodeHandle n; 
 
−
 
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
 
−
 
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm" 
 
−
 
 
−
        ros::Time anterior;
 
−
 
 
−
        brazo_fer::WriteServos teleop;
 
−
        brazo_fer::WriteServos teleop_pinza;
 
−
 
 
−
if (::cont == 0)
 
−
        {
 
−
anterior = ros::Time::now();
 
−
home();
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
teleop_pinza.posicion = control_pinza(::pinza);
 
−
::cont = 1;
 
−
}
 
−
 
−
  char c;
 
−
 
 
−
  // get the console in raw mode                                                             
 
−
  tcgetattr(0, &cooked);
 
−
  memcpy(&raw, &cooked, sizeof(struct termios));
 
−
  raw.c_lflag &=~ (ICANON | ECHO);
 
−
  // Setting a new line, then end of file                       
 
−
  raw.c_cc[VEOL] = 1;
 
−
  raw.c_cc[VEOF] = 2;
 
−
  tcsetattr(0, TCSANOW, &raw);
 
−
 
 
−
  puts("");
 
−
  puts("#####################################################");
 
−
  puts("                  CONTROLES BRAZO");
 
−
  puts("#####################################################");
 
−
  puts(""); 
 
−
  puts("Flecha arriba:________ Subir pinza");
 
−
  puts("Flecha abajo:_________ Bajar pinza");
 
−
  puts("Flecha izquierda:_____ Desplazar pinza a la izquierda");
 
−
  puts("Flecha derecha:_______ Desplazar pinza a la derecha");
 
−
  puts("Tecla W:______________ Desplazar pinza hacia delante");
 
−
  puts("Tecla S:______________ Desplazar pinza hacia atrás");
 
−
  puts("Tecla A:______________ Abrir pinza");
 
−
  puts("Tecla D:______________ Cerrar pinza");
 
−
  puts("Tecla Q:______________ Inclinar pinza hacia arriba");
 
−
  puts("Tecla E:______________ Inclinar pinza hacia abajo");               
 
−
 
 
−
       
 
−
    while (ros::ok())
 
−
    {
 
−
 
−
 
−
    // get the next event from the keyboard 
 
−
    if(read(0, &c, 1) < 0)
 
−
    {
 
−
      perror("read():");
 
−
      exit(-1);
 
−
    }
 
−
 
−
      if (c == KEYCODE_Xmas)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{
 
−
::punto.x = ::punto.x - 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::punto.x = ::punto.x + 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Xmenos)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::punto.x = ::punto.x + 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::punto.x = ::punto.x - 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Ymas)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::punto.y = ::punto.y + 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::punto.y = ::punto.y - 5;}
 
−
anterior = ros::Time::now();
 
−
}    
 
−
      }
 
−
      if (c == KEYCODE_Ymenos)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::punto.y = ::punto.y - 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::punto.y = ::punto.y + 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Zmas)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::punto.z = ::punto.z + 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
 
−
{::punto.z = ::punto.z - 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Zmenos)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::punto.z = ::punto.z - 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::punto.z = ::punto.z + 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Pabrir)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::pinza.pinza = ::pinza.pinza - 5;
 
−
teleop_pinza.posicion = control_pinza(::pinza);
 
−
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_Pcerrar)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::pinza.pinza = ::pinza.pinza + 5;
 
−
teleop_pinza.posicion = control_pinza(::pinza);
 
−
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_PinclinarMas)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::pinza_incli = ::pinza_incli + 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::pinza_incli = ::pinza_incli - 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }
 
−
      if (c == KEYCODE_PinclinarMenos)
 
−
      {
 
−
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
−
{     
 
−
::pinza_incli = ::pinza_incli - 5;
 
−
teleop.posicion = inversa(::punto, ::pinza_incli);
 
−
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
−
{::pinza_incli = ::pinza_incli + 5;}
 
−
anterior = ros::Time::now();
 
−
}
 
−
      }                                   
 
−
 
−
if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
 
−
{
 
−
teleop.par = ::on;
 
−
teleop.velocidad = ::vel;
 
−
move_pub_.publish(teleop);
 
−
}
 
−
if (teleop_pinza.posicion.pinza != ::p.pinza)
 
−
{
 
−
teleop_pinza.par = ::on;
 
−
teleop_pinza.velocidad.pinza = 200;
 
−
hand_pub_.publish(teleop_pinza);
 
−
}    
 
−
 
 
−
}
 
−
}
 
−
 
−
  int main(int argc, char **argv)
 
−
  {
 
−
 
−
ros::init(argc, argv, "teleoperador_teclado"); 
 
−
 
−
ros::NodeHandle n;
 
−
 
 
−
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);  //Suscripción del topic "pose_arm" 
 
−
 
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
 
−
 
 
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm" 
 
−
 
 
−
 
 
−
 
 
−
signal(SIGINT,quit);
 
−
 
−
ros::Timer timer = n.createTimer(ros::Duration(1), callback);
 
−
   
 
−
   
 
−
 
−
ros::spin(); 
 
−
 
−
        return 0;
 
−
  }
 
  
−
</syntaxhighlight>
 
  
−
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
+
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Artículos realcionados
 +
----
 +
!Otros artículos
 +
----
 +
|-
 +
| valign="top" width="50%" |
  
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>
+
[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
 +
[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
 +
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
 +
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
 +
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
  
−
Para compilar el programa hay que situarse en el directorio del ''package''. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
+
| valign="top" |
  
−
<syntaxhighlight>roscd brazo_fer
+
[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
−
make</syntaxhighlight>
+
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
−
 
+
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
−
====Ejecutando el programa====
+
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
−
 
+
----
−
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
+
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
  
−
<syntaxhighlight>roscore</syntaxhighlight>
+
|}
−
 
+
----
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
 
−
 
 
−
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
−
 
 
−
Para ejecutar el programa de control llamado "teleoperador_teclado", en un terminal nuevo ejecutaremos el siguiente comando:
 
−
 
 
−
<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</syntaxhighlight>
 
  
  
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]

Latest revision as of 11:01, 29 September 2014

< volver a principal


Artículos realcionados
Otros artículos

Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación brazo MYRAbot (urdf+gazebo)
Modelo para simulación MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)
Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

Programas de control para el robot Turtlebot sobre ROS


Brazo Bioloid

El brazo en el que se basa este proyecto es el desarrollado en el proyecto MYRA Robot: Hardware Update , realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:

Fotografía del Brazo Bioloid.
  • Placa arduino mega 2560.
  • Circuito integrado 74LS241 (Bufer tri-estado).
  • Circuito regulador de tensión.
  • 5 servomotores serie Dynamixel AX-12A.
  • Piezas de montaje de kit Bioloid.
  • Piezas realizadas para pinza y fijación al MYRAbot.

Arduino IDE y rosserial

Para la comunicación e intercambio de información entre arduino y ROS es necesario instalar arduino IDE y rosserial (stack de ROS que contiene el package rosserial_arduino con las librerias para arduino). Comenzaremos instalando el arduino IDE, para lo que ejecutaremos los siguientes comandos en un terminal: 

sudo apt-get update
sudo apt-get install arduino arduino-core

Una vez realizada la instalación del software de arduino se procedera a la instalación del package de ROS ejecutando en un terminal el siguiente comando: 

sudo apt-get install ros-NUESTRA_VERSIÓN_ROS-rosserial

Instalados arduino IDE y el package rosserial debemos copiar las librerias del package rosserial_arduino al sketchbook de arduino, carpeta que se encuentra habitualmente en la carpeta personal, para lo que ejecutaremos en un terminal los siguientes comandos: 

roscd rosserial_arduino/libraries
cp -r ros_lib /home/”nombre_sesion”/sketchbook/libraries/ros_lib

Creación de un package para nuestros programas

Deberemos tener previamente creado un espacio de trabajo para nuestra versión de ROS. Para poder compilar y enviar nuestros programas a la placa arduino sin tener que pasar por arduino IDE vamos a crear un package llamado “arduino_fer” con las dependencias necesarias para nuestros programas, para ello ejecutaremos los siguientes comandos en un terminal: 

cd ~/ros_workspace
roscreate-pkg arduino_fer rosserial_arduino std_msgs

Deberemos sustituir el contenido del fichero "CMakeLists.txt", situado en la carpeta del package creado, por el siguiente: 

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_find_ros_package(rosserial_arduino)
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)

Para finalizar la cración del package ejecutaremos los siguientes comandos: 

roscd arduino_fer
cmake .

Primer programa (A toda potencia)

El primer programa que vamos a realizar crea un nodo llamado “potencia” que publica un topic llamado “cifra” y está suscrito a un topic llamado “resultado”. Cuando se publique un número en el topic “cifra” lo multiplicará por si mismo y publicará el resultado en el topic “resultado”. El código del programa es el siguiente: 

#include <ros.h> 
#include <std_msgs/Int16.h> 

ros::NodeHandle nh; 
int pot; 

void potencia( const std_msgs::Int16& cifra){ 

::pot = cifra.data*cifra.data; 
} 

ros::Subscriber<std_msgs::Int16> sub("cifra", &potencia ); 
std_msgs::Int16 res; 

ros::Publisher pub("resultado", &res); 

void setup() 
{ 
  nh.initNode(); 
  nh.subscribe(sub);  
  nh.advertise(pub); 
} 

void loop() 
{ 
  res.data = ::pot; 
  pub.publish( &res ); 
  nh.spinOnce(); 
  delay(1000); 
}

Para su compilación y envio a la placa arduino es necesario añadir al archivo "CMakeLists.txt" las siguientes líneas: 

set(FIRMWARE_NAME potencia)

set(${FIRMWARE_NAME}_BOARD MODELO_NUESTRA_PLACA)   # Modelo placa arduino
set(${FIRMWARE_NAME}_SRCS src/potencia.cpp )
set(${FIRMWARE_NAME}_PORT /dev/ttyACM0)            # Puerto serie para carga
generate_ros_firmware(${FIRMWARE_NAME})

Donde se indica el nombre del programa, tipo de placa arduino (uno, atmega328 o mega2560), nombre y ubicación del archivo y puerto serie del PC empleado para la comunicación con la placa. Deberemos ejecutar los siguientes comandos en un terminal: 

roscd arduino_fer
make potencia-upload

Para la ejecución del programa se debe lanzar en un terminal el núcleo de ROS, si no se encuentra ya en marcha, ejecutando el siguiente comando: 

roscore

En otro terminal se ejecutará el nodo que comunica a ROS con la placa arduino, indicándole el puerto empleado para la comunicación. Para saber el puerto que se está empleando, con la placa arduino conectada al pc a través de su puerto USB, podemos verlo en el menú "Herramientas">"Puerto serie" del software arduino IDE. En nuestro caso es el "ttyACM0": 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para la publicación de un número en el topic “cifra” se ejecutará en otro terminal el siguiente comando: 

rostopic pub cifra std_msgs/Int16 NUMERO_DESEADO --once

Para comprobar que realmente el programa calcula el cuadrado del número publicado en el topic “cifra” podemos ejecutar en otro terminal el siguiente comando para visualizar el topic “resultado”: 

rostopic echo resultado

Arduino y servomotores Dinamixel

En Savage Electronics se encuentran las adaptaciones hardware realizadas a la placa arduino mega 2560 para la correcta comunicación con los servomotores Dynamixel AX-12A de ROBOTICS, así como las librerías utilizadas para la programación (DynamixelSerial).

Segundo programa (Muévete)

En este programa simplemente se realiza un prueba de la comunicación entre la placa arduino mega 2560 con un servomotor Dynamixel AX-12A. El programa publica el topic “angulo” con la posición del motor (entero entre 0 y 1023) y está suscrito al topic “giro” del que recibe la posición a la que debe moverse (entero entre 0 y 1023), con una velocidad constante de 128 (entero entre 0 y 1023). El código del programa es el siguiente: 

#include <ros.h>
#include <std_msgs/Int16.h>
#include <DynamixelSerial1.h>

ros::NodeHandle nh;


void mover( const std_msgs::Int16& giro){

  Dynamixel.moveSpeed(1,giro.data,128);

}

ros::Subscriber<std_msgs::Int16> sub("giro", &mover );

std_msgs::Int16 ang;
ros::Publisher pub("angulo", &ang);

void setup()
{
  nh.initNode();
  nh.subscribe(sub);  
  nh.advertise(pub);
  Dynamixel.begin(1000000,2);
  delay(1000);
}

void loop()
{
  int posicion = Dynamixel.readPosition(1);
  ang.data = posicion;
  pub.publish( &ang );
  nh.spinOnce();
  delay(10);
}

Para poder compilar y enviar el programa a la placa arduino deberemos emplear el software arduino IDE, ya que empleamos una librería externa al sistema ROS y no nos permite hacerlo usando nuestro package. Una vez transferido el programa a la placa, para probar el programa debemos ejecutar en un terminal el siguiente comando, que inicia el nucleo de ROS: 

roscore

También debemos lanzar en otro terminal el nodo de comunicación ROS-arduino, ejecutando el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

En un nuevo terminal publicaremos en el topic "giro" la posición de destino del servomotor, ejecutando el siguiente comando: 

rostopic pub giro std_msgs/Int16 POSICION_DESEADA --once

El servomotor de inmediato comenzará a moverse hasta la posición indicada. Podemos visualizar el valor de posición del servomotor en todo momento ejecutando en otro terminal el siguiente comando: 

rostopic echo angulo

Modelo cinemático del brazo

Para el correcto posicionamiento del brazo es necesario obtener las transformaciones geométricas para determinar el ángulo que deben girar los servomotores para alcanzar un punto (cinemática inversa). Al tratarse de un brazo que trabaja en un solo plano con giro (4 articulaciones) es un proceso sencillo que se ha realizado de forma directa, sino se puede recurrir al algoritmo de parametrización Denavit- Hartenberg que permite obtener la cinemática directa y a partir de esta obtener la inversa.

Diagramas para el estudio de la cinemática

Cinemática inversa

Siguiendo los diagramas planteados y considerando x,y y z como las coordenadas del punto destino, y ε el ángulo que forma la pinza respecto al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:

Brazo ecuaciones.jpg

Teniendo en cuenta los ángulos límite de los servomotores Dynamixel AX-12A y el rango de valores que debemos usar, los valores que debemos pasar a los servomotores de cada articulación son los siguientes:

Diagrama de ángulos servomotor Dynamixel AX-12A

Brazo ecuacion base.jpg Se suman 150º al ángulo para situar el 0º coincidente con el eje z.

Brazo ecuacion arti1.jpg Se suman 60º al ángulo para situar el 0º coincidente con el eje z.

Brazo ecuacion arti2.jpg Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.

Brazo ecuacion arti3.jpg Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.

Para enviar a los servomotores valores dentro del rango de trabajo, se multiplica por 1023 (0x3FF), valor máximo del rango de posiciones del servo, y se divide por 300 (300º), el ángulo total de giro del servomotor.

Cinemática directa

A partir de las ecuaciones anteriores podemos plantear también el proceso inverso, la obtención del punto en que se encuentra el brazo a partir de las posiciones de los servomotores. Las ecuaciones obtenidas son las siguientes:

Brazo ecuaciones directa.jpg

Espacio de trabajo y limitaciones

Para evitar colisiones del brazo con las diferentes partes del MYRAbot y con sus propios componentes, es necesario fijar unos límites de giro a los servomotores y restringir el acceso a regiones del espacio. Para esta tarea se ha realizado una hoja de calculo con las formulas de la cinemática inversa y se ha representado en unos gráficos las posiciones de los ejes y las articulaciones del brazo en el espacio.

Hoja de cálculo cinemática inversa y directa, descargar

Programas de control

Tercer programa (Ven aquí)

Primero crearemos un programa que actuará de intermediario entre el programa de control y los servomotores del brazo, que situaremos en la placa arduino. Este programa se encargará de enviar las ordenes recibidas a los servomotores y enviar los datos más relevantes de los servomotores. Como se van a publicar y suscribir los datos de 5 servomotores, vamos a comenzar creando nuestros mensajes personalizados.

Creación de mensajes personalizados en ROS

Previamente crearemos un package con las dependencias necesarias para nuestros programas (std_msgs geometry_msgs roscpp). Dentro de este package crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre de campo).

Comenzaremos creando un mensaje base llamado "servos", que contendrá los datos de cada servomotor. Crearemos un archivo llamado "Servos.msg" con el siguiente contenido: 

int16 base
int16 arti1
int16 arti2
int16 arti3
int16 pinza

Vamos a crear un mensaje que contenga los datos que enviaremos a los servomotores (posición, velocidad y par). Ahora el tipo de dato que vamos a manejar es el que hemos creado anteriormente. Creamos un archivo llamado "WriteServos.msg" con el siguiente contenido: 

Servos posicion
Servos velocidad
Servos par

Para la recepción de datos de los servomotores (posición, estado y corriente) crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido: 

Servos posicion
Servos estado
Servos corriente

Para permitir la creación de los mensajes en el sistema ROS debemos editar el archivo "CMakeLists.txt" eliminando el símbolo "#" para que deje de estar comentada la siguiente línea: 

# rosbuild_gensrv()

Para la compilación y creación de los mensaje ejecutaremos la siguiente secuencia de comandos en un terminal, donde "brazo_fer" es el package creado para nuestros programas: 

roscd brazo_fer
make

Como vamos a usar estos mensajes en arduino, necesitamos añadirlos al package "rosserial", ejecutando en un terminal el siguiente comando: 

rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer

Programa arduino

El programa para la placa arduino está suscrito al topic "move_arm", por el cual recibe los movimientos de los servomotores, y publica el topic "pose_arm", con la información de posición, par y corriente de los servomotores. El código del programa es el siguiente: 

#include <ros.h>
#include <brazo_fer/Servos.h>
#include <brazo_fer/WriteServos.h>
#include <brazo_fer/ReadServos.h>
#include <std_msgs/Bool.h>
#include <math.h>
 
#include <DynamixelSerial1.h>
 
ros::NodeHandle nh;
 
void mover(const brazo_fer::WriteServos& brazo){
 
  brazo_fer::Servos par = brazo.par;
  
  brazo_fer::Servos vel = brazo.velocidad;
 
  brazo_fer::Servos move = brazo.posicion;
 
  int posicion[4];
  
  if (par.base == 0){
    Dynamixel.torqueStatus(1,OFF);
  }
  else {
    Dynamixel.torqueStatus(1,ON);
 
    Dynamixel.moveSpeed(1,move.base,vel.base);  
  }
 
  if (par.arti1 == 0){
    Dynamixel.torqueStatus(2,OFF);
  }
  else {
    Dynamixel.torqueStatus(2,ON);
 
    Dynamixel.moveSpeed(2,move.arti1,vel.arti1);  
  }
 
  if (par.arti2 == 0){
    Dynamixel.torqueStatus(3,OFF);
  }
  else {
    Dynamixel.torqueStatus(3,ON);
 
    Dynamixel.moveSpeed(3,move.arti2,vel.arti2);  
  }
 
  if (par.arti3 == 0){
    Dynamixel.torqueStatus(4,OFF);
  }
  else {
    Dynamixel.torqueStatus(4,ON);    
 
    Dynamixel.moveSpeed(4,move.arti3,vel.arti3);  
  }
 
}

void pinza(const brazo_fer::WriteServos& pinza){
 
  brazo_fer::Servos par = pinza.par;
  
  brazo_fer::Servos vel = pinza.velocidad;
 
  brazo_fer::Servos move = pinza.posicion;
  
  int posicion;
  
  if (par.pinza == 0){
    Dynamixel.torqueStatus(5,OFF);
  }
  else {
    Dynamixel.torqueStatus(5,ON);    
 
    Dynamixel.moveSpeed(5,move.pinza,vel.pinza);  
  }
  
}
 
ros::Subscriber<brazo_fer::WriteServos> move_sub("move_arm", &mover );
ros::Subscriber<brazo_fer::WriteServos> hand_sub("hand_arm", &pinza );
 
brazo_fer::ReadServos pec;
std_msgs::Bool pulsador;
 
ros::Publisher pose_pub("pose_arm", &pec);
 
 
void setup()
{
  nh.initNode();
  nh.subscribe(move_sub);
  nh.subscribe(hand_sub);  
  nh.advertise(pose_pub);
  
  Dynamixel.begin(1000000,2);

}
 
void loop()
{
  brazo_fer::Servos pos;
  brazo_fer::Servos est;
  brazo_fer::Servos cor; 
 
  pos.base = Dynamixel.readPosition(1);
  pos.arti1 = Dynamixel.readPosition(2);
  pos.arti2 = Dynamixel.readPosition(3);
  pos.arti3 = Dynamixel.readPosition(4);
  pos.pinza = Dynamixel.readPosition(5);
 
  est.base = Dynamixel.moving(1);
  est.arti1 = Dynamixel.moving(2);
  est.arti2 = Dynamixel.moving(3);
  est.arti3 = Dynamixel.moving(4);
  est.pinza = Dynamixel.moving(5);      
 
  cor.base = Dynamixel.readLoad(1);
  cor.arti1 = Dynamixel.readLoad(2);
  cor.arti2 = Dynamixel.readLoad(3);
  cor.arti3 = Dynamixel.readLoad(4);
  cor.pinza = Dynamixel.readLoad(5);    
 
  pec.posicion = pos;
  pec.estado = est;
  pec.corriente = cor;  
 
  pose_pub.publish( &pec );
  
  nh.spinOnce();

}

Para compilar y cargar el programa en la placa emplearemos el programa arduino IDE. Este programa nos servirá para otros programas de control que desarrollemos, ya que se trata de un mero intermediario.

Programa ROS

Usando las ecuaciones de la cinemática inversa este programa indica a los servomotores la posición de giro para alcanzar el punto de destino que se encuentre dentro del espacio de trabajo. Está suscrito al topic "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al topic "pose_arm", donde recibe la información de los servomotores que publica el programa de arduino, y publica el topic "move_arm", donde indica al programa de arduino la posición, velocidad y par de los servomotores. Dentro del pakage creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "geometry_msgs/Point.h"
  #include "math.h"
 
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60
 
 
  brazo_fer::Servos move, vel, p, e, c;
 
  void posicion(const brazo_fer::ReadServos& pose)
  {
 
    ::p = pose.posicion;
    ::e = pose.estado;
    ::c = pose.corriente;
 
  }
 
  void punto(const geometry_msgs::Point& point)
  {	
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
 
  	double x = point.x;
  	double y = point.y;
	double z = point.z;
	
	y = y + z*tan(atan2(45,250));
 
	int coordenadas_correctas = 1;
 
	double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
	double z_p;
	double L_a, L;
 
	epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
 
	alfa = (atan2(x,z)*180)/PI;
 
	z_p = sqrt(pow(z,2)+pow(x,2));
 
	L = sqrt(pow(z_p,2)+pow(y,2));
 
	L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
 
	beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
	
	beta_pp = atan2(y,z_p);
 
	beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
	beta = ((beta_p+beta_a)*180)/PI;
 
	gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
 
	delta_a = PI-(beta_a+gamma);
 
	gamma = (gamma*180)/PI;
 
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));       
 
	if (beta_pp > beta_p) {
		delta = ((2*PI-(delta_p-delta_a))*180)/PI;
	}
	else {
		delta = ((delta_p+delta_a)*180)/PI;
 
		if (isnan(delta)) {
			delta = ((PI+delta_a)*180)/PI;
		}
	}
 
	if (isnan(gamma)) // si no hay solución
	{
		coordenadas_correctas = 0; 
	}
 
	::move.base = ((alfa+150)*1023)/300;
	::move.arti1 = ((beta+60)*1023)/300;
	::move.arti2 = ((gamma-30)*1023)/300;
	::move.arti3 = ((delta-30)*1023)/300;
	::move.pinza = 511;

	::vel.base = abs(::move.base - ::p.base)/5;
	::vel.arti1 = abs(::move.arti1 - ::p.arti1)/5;
	::vel.arti2 = abs(::move.arti2 - ::p.arti2)/5;
	::vel.arti3 = abs(::move.arti3 - ::p.arti3)/5;
	::vel.pinza = abs(::move.pinza - ::p.pinza);
 
	if (coordenadas_correctas == 1 && (205 <= ::move.base && ::move.base <= 818) && (120 <= ::move.arti1 && ::move.arti1 <= 920) && ::move.arti2 >= 50  && ::move.arti3 <= 828) {
 
                brazo_fer::WriteServos mover;
 
                mover.posicion = ::move;                
                 
                mover.velocidad = ::vel;
				
		mover.par.base = 1;
		mover.par.arti1 = 1;
		mover.par.arti2 = 1;
		mover.par.arti3 = 1;
		mover.par.pinza = 1;
                
                move_pub_.publish(mover);
	}
	else {
		std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;				
	}
 
  } 
 
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "control_brazo");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
 
  	ros::Subscriber point_sub_= n.subscribe("point", 1, punto); 
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
 
 
	ros::spin();  
 
        return 0;
  }

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar: 

rosbuild_add_executable(control_v01 src/control_v01.cpp)

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores: 

roscd brazo_fer
make

Ejecutando el programa

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando: 

roscore

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para ejecutar el programa de control llamado "control_v01", en un terminal nuevo ejecutaremos el siguiente comando: 

rosrun brazo_fer control_v01

Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando: 

rostopic pub place_point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once

El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde uno de partida:

<videoflash>iwAcutO3C8g</videoflash>
Ver vídeo en YouTube

Cuarto programa (Agarra la botella)

Archivo include con funciones

Con el objetivo de simplificar y hacer más legibles los programas se ha creado este archivo que contiene las funciones de control del brazo. Crearemos un archivo llamado "brazo_fer.h" dentro del directorio "include/brazo_fer" del package creado con el siguiente contenido: 

#ifndef brazo_fer_H
#define brazo_fer_H

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"  
  #include "std_msgs/Int16.h"
  #include "geometry_msgs/Point.h"
  #include "math.h" 
  
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60  

geometry_msgs::Point directa(brazo_fer::Servos posicion_servos_0, int inclinacion_pinza)
  {
	
	double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p1, delta_p2, epsilon;
	double z_p;
	double L_a, L;
	double a, b;
	double x, y, z;
	
	alfa = (((posicion_servos_0.base*300)/1023)-150)*PI/180;
	beta = (((posicion_servos_0.arti1*300)/1023)-60)*PI/180;
	gamma = (((posicion_servos_0.arti2*300)/1023)+30)*PI/180;
	delta = (((posicion_servos_0.arti3*300)/1023)+30)*PI/180;
	
	epsilon = (inclinacion_pinza*PI)/180;	
	
	L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
	
	beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
	
	beta_p = beta - beta_a;
	
	delta_a = PI - (beta_a + gamma);
	
	delta_p1 = delta - delta_a;
	
	delta_p2 = 2*PI - (delta - delta_a);
	
	if (delta_p1 < delta_p2) 
	{
		L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p1));
	}
	else
	{
		L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p2));
	}
	
	z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
	
	beta_pp = acos(z_p/L);
	
	a = L1*sin(beta);
	
	b = L2*sin(beta+gamma)+Lp*sin(epsilon);
	
	if (a >= b) 
	{
		y = L*sin(beta_pp);
	}
	else
	{
		y = -L*sin(beta_pp);
	}	
	
	z = z_p*cos(alfa);
	
	x = z_p*sin(alfa);
	
	geometry_msgs::Point punto;
	
	punto.x = x;
	punto.y = y;
	punto.z = z;
	
	return punto; 
  } 
  
brazo_fer::WriteServos inversa(geometry_msgs::Point destino, int inclinacion_pinza, brazo_fer::Servos posicion_servos_0, int velocidad) 
  {
	  
	double x = destino.x;
  	double y = destino.y;
	double z = destino.z;

	int coordenadas_correctas = 1;

	double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
	double z_p;
	double L_a, L;
 
	epsilon = (inclinacion_pinza*PI)/180;
 
	alfa = (atan2(x,z)*180)/PI;
 
	z_p = sqrt(pow(z,2)+pow(x,2));
 
	L = sqrt(pow(z_p,2)+pow(y,2));
 
	L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
 
	beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
	
	beta_pp = atan2(y,z_p);
 
	beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
	beta = ((beta_p+beta_a)*180)/PI;
 
	gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
 
	delta_a = PI-(beta_a+gamma);
 
	gamma = (gamma*180)/PI;
 
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));       
 
	if (beta_pp > beta_p) {
		delta = ((2*PI-(delta_p-delta_a))*180)/PI;
	}
	else {
		delta = ((delta_p+delta_a)*180)/PI;
 
		if (isnan(delta)) {
			delta = ((PI+delta_a)*180)/PI;
		}
	}
 
 
	if (isnan(gamma))
	{
		coordenadas_correctas = 0; 
	}

	brazo_fer::Servos posicion_servos_1;
	brazo_fer::Servos velocidad_servos;
	brazo_fer::Servos par_servos;	
	
	posicion_servos_1.base = ((alfa+150)*1023)/300;
	posicion_servos_1.arti1 = ((beta+60)*1023)/300;
	posicion_servos_1.arti2 = ((gamma-30)*1023)/300;
	posicion_servos_1.arti3 = ((delta-30)*1023)/300;	
	
	if (velocidad == 0)
	{
		velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)/5;
		if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
		else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
		velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)/5;
		if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
		else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
		velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)/5;
		if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
		else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
		velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)/5;
		if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
		else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}
		
	}
	else
	{
		velocidad_servos.base = abs(posicion_servos_1.base - posicion_servos_0.base)*(velocidad/10);
		if (velocidad_servos.base > 1023){velocidad_servos.base = 1023;}
		else if (velocidad_servos.base < 10){velocidad_servos.base = 10;}
		velocidad_servos.arti1 = abs(posicion_servos_1.arti1 - posicion_servos_0.arti1)*(velocidad/10);
		if (velocidad_servos.arti1 > 1023){velocidad_servos.arti1 = 1023;}
		else if (velocidad_servos.arti1 < 10){velocidad_servos.arti1 = 10;}
		velocidad_servos.arti2 = abs(posicion_servos_1.arti2 - posicion_servos_0.arti2)*(velocidad/10);
		if (velocidad_servos.arti2 > 1023){velocidad_servos.arti2 = 1023;}
		else if (velocidad_servos.arti2 < 10){velocidad_servos.arti2 = 10;}
		velocidad_servos.arti3 = abs(posicion_servos_1.arti3 - posicion_servos_0.arti3)*(velocidad/10);
		if (velocidad_servos.arti3 > 1023){velocidad_servos.arti3 = 1023;}
		else if (velocidad_servos.arti3 < 10){velocidad_servos.arti3 = 10;}
		
	}
	
	brazo_fer::Servos velocidad_servos_0;
	
	velocidad_servos_0.base = 0;
	velocidad_servos_0.arti1 = 0;
	velocidad_servos_0.arti2 = 0;
	velocidad_servos_0.arti3 = 0;
		
	par_servos.base = 1;
	par_servos.arti1 = 1;
	par_servos.arti2 = 1;
	par_servos.arti3 = 1;
	
	brazo_fer::WriteServos move_arm;			
	
	if (coordenadas_correctas == 1 && (205 <= posicion_servos_1.base && posicion_servos_1.base <= 818) && (120 <= posicion_servos_1.arti1 && posicion_servos_1.arti1 <= 920) && posicion_servos_1.arti2 >= 50  && (posicion_servos_1.arti3 <= 828 && posicion_servos_1.arti3 >= 195)) {
		move_arm.posicion = posicion_servos_1;
		move_arm.velocidad = velocidad_servos;
		move_arm.par = par_servos;
		return move_arm;
	}
	else {
		std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
		move_arm.posicion = posicion_servos_0;
		move_arm.velocidad = velocidad_servos_0;
		move_arm.par = par_servos;
		return move_arm;				
	}	  
  }
  
brazo_fer::WriteServos  control_pinza(brazo_fer::Servos posicion_servos_1, brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
  {
	
	brazo_fer::Servos velocidad_servos;	  
	brazo_fer::Servos par_servos;
	
	velocidad_servos.pinza = 50;
	    
	par_servos.pinza = 1;
	
	brazo_fer::WriteServos hand_arm;		
	
	if ((posicion_servos_1.pinza >= 480 && posicion_servos_1.pinza <= 680 && corriente_servos.pinza <= 300) || (posicion_servos_0.pinza > posicion_servos_1.pinza && posicion_servos_1.pinza >= 480))
	{
		hand_arm.posicion = posicion_servos_1;
		hand_arm.velocidad = velocidad_servos;
		hand_arm.par = par_servos;
		return hand_arm;
	}
	else
	{
		std::cout<<"Alcanzado límite de la pinza"<<std::endl;		
		hand_arm.posicion = posicion_servos_0;
		hand_arm.velocidad = velocidad_servos;
		hand_arm.par = par_servos;
		return hand_arm;
	}
  }  
  
geometry_msgs::Point home(brazo_fer::Servos posicion_servos_0, brazo_fer::Servos corriente_servos)
  { 
	  
  	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
		
		geometry_msgs::Point punto_0;
		
		punto_0.x = 0;
		punto_0.y = 80;
		punto_0.z = 50;
		
		int inclinacion_pinza = 0;		
				
		brazo_fer::WriteServos inicio_brazo = inversa(punto_0, inclinacion_pinza, posicion_servos_0, 0);
	
		brazo_fer::Servos posicion_servos_1;
		
		posicion_servos_1.pinza = 511;
	
		brazo_fer::WriteServos inicio_pinza = control_pinza(posicion_servos_1, posicion_servos_0, corriente_servos);
			
		move_pub_.publish(inicio_brazo);
		
		hand_pub_.publish(inicio_pinza);
		
				
		return punto_0;

  }
    
#endif

Programa

Este programa ejecuta la secuencia para coger una botella pequeña de plástico (vacía, ya que la potencia de los servomotores es insuficiente para una llena, deberían emplearse más servomotores por articulación). Los pasos que realiza son los siguientes:

  1. Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).
  2. Posicionamiento en el punto indicado.
  3. Cierre de la pinza (hasta tener agarrada la botella).
  4. Levantamiento y acercamiento (levanta y acerca la botella al MYRAbot)

Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v02.cpp" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"     
  #include "std_msgs/Int16.h"
  #include "std_msgs/String.h"  
  #include "geometry_msgs/Point.h"
  #include "math.h" 
  
  geometry_msgs::Point punto_0,punto_1;
  
  int cont = 0;
  int cont_1 = 0;
  int cont_2 = 0;
  int inclinacion_pinza = 0;
  int start = 0;
    
  brazo_fer::Servos pg, cg;
  brazo_fer::WriteServos move;
  brazo_fer::Servos pinza;
  brazo_fer::WriteServos pin;  
  brazo_fer::WriteServos error_coordenadas;
  
  brazo_fer::WriteServos acercar(geometry_msgs::Point cerca)
  {
  	double x = cerca.x;
  	double z = cerca.z; 
  	 	
  	cerca.z = cerca.z - 70*cos(atan2(x,z));
  	cerca.x = cerca.x - 70*sin(atan2(x,z));
  	
  	brazo_fer::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);
  	
  	return move;

  }
  
    brazo_fer::WriteServos levantar_acercar(geometry_msgs::Point la)
  {
  	 	
  	la.x = 0;
  	la.y = la.y + 50;
   	la.z = 60;
  	
  	if (la.y < 75) {
		la.y = 75;
	}
  	 	
  	brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);
  	
  	::punto_0 = la;
  	
  	return move;

  }
  
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)   
  { 
	  
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   
  	   
	brazo_fer::Servos p = pec.posicion;  

	::pg = pec.posicion;
	::cg = pec.corriente; 	
	
	brazo_fer::Servos e = pec.estado;   
	
	brazo_fer::Servos c = pec.corriente;   
	

	if (::cont == 0)
	{
		::punto_0 = home(p, c);
		
		::punto_1 = ::punto_0;
	
		::cont = ::cont+1;
	
	}
	
	if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {


		
		
  	if (::cont_1 == 0) {
  	
		::move = acercar(::punto_0); 	
  	
		move_pub_.publish(::move);
  	
		::cont_1 = ::cont_1+1;
  	
	}
  	  	
  	
  	if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {	
			
			if (::cont_2 == 0) {
				
				::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
  	
				move_pub_.publish(::move);
			 
				::cont_2 = ::cont_2+1;
			
			
			}
			else {	 
			
			
			  	if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
				
				
				::pinza.pinza = p.pinza;
				
					if(::pin.posicion.pinza != p.pinza) {
					
						std::cout<<c.pinza<<std::endl;
				
						::pinza.pinza = ::pinza.pinza + 20;
					
						::pin = control_pinza(::pinza, p, c);
					
						hand_pub_.publish(::pin);
						
					}
					else {
					
						std::cout<<"Agarrado"<<std::endl;
				
						::move = levantar_acercar(::punto_0);				
			
						move_pub_.publish(::move);
					
						::punto_1 = ::punto_0;
					
						::cont_1 = 0;
						::cont_2 = 0;
				}
				}
			}
			
	}
	else {
			
			std::cout<<"En movimiento"<<std::endl;
	}
	
	}
		
	  
  }
 
  void punto(const geometry_msgs::Point& point)   
  {	
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
	
	::punto_0 = point;

  } 
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "control_recogida");
 
	ros::NodeHandle n;
	
  	
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
  	
  	ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);
  	
	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);	

	
	ros::spin();
 
        return 0;
  }

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar: 

rosbuild_add_executable(contro_v02 src/control_v02.cpp)

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores: 

roscd brazo_fer
make

Ejecutando el programa

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando: 

roscore

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para ejecutar el programa de control llamado "control_v02", en un terminal nuevo ejecutaremos el siguiente comando: 

rosrun brazo_fer control_v02

Para poder publicar las coordenadas del punto donde se encuentra la botella (ya que no disponemos de otros medios para calcular la posición, se colocará la botella en una posición conocida), debemos publicar en el topic "pick_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella): 

rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once

En el siguiente vídeo se puede ver la ejecución de la secuencia para la recogida de una botella:

<videoflash>0FcB6-i23cU</videoflash>
Ver vídeo en YouTube

Quinto programa (posa la botella)

Este programa ejecuta la secuencia para posar la botella que hemos cogido. Los pasos que realiza son los siguientes:

  1. Posicionamiento en el punto indicado.
  2. Apertura de la pinza (hasta soltar la botella).
  3. Separación (sitúa el brazo a unos 70 mm del punto indicado).
  4. Retorno a la posición de inicial.

Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v03.cpp" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"     
  #include "std_msgs/Int16.h"
  #include "std_msgs/String.h"   
  #include "geometry_msgs/Point.h"
  #include "math.h"
  
  geometry_msgs::Point punto_0, punto_1;
  
  int cont = 0;
  int cont_1 = 0;
  int cont_2 = 0;
  int cont_3 = 0;
  int inclinacion_pinza = 0;
    
  brazo_fer::Servos pg, cg, pinza;
  brazo_fer::WriteServos move;
  brazo_fer::WriteServos pin;
  brazo_fer::WriteServos error_coordenadas;  
  
  brazo_fer::WriteServos separar(geometry_msgs::Point lejos)
  {
  	double x = lejos.x;
  	double z = lejos.z; 
  	 	
  	lejos.z = lejos.z - 70*cos(atan2(x,z));
  	lejos.x = lejos.x - 70*sin(atan2(x,z));
  	 	
  	brazo_fer::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);
  	
  	return move;

  }
  
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  
  { 
	  
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
  	   
	brazo_fer::Servos p = pec.posicion;   

	::pg = pec.posicion;
	::cg = pec.corriente; 	
	
	brazo_fer::Servos e = pec.estado;   
	
	brazo_fer::Servos c = pec.corriente;  
	
	if (::cont == 0)
	{
		::punto_0 = directa(p, inclinacion_pinza);
		
		::punto_1 = ::punto_0;
	
		::cont = ::cont+1;
	}
	
	if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {



		
		
  	if (::cont_1 == 0) {
  	
		::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0); 	
  	
		move_pub_.publish(::move);
  	
		::cont_1 = ::cont_1+1;
  	
	}  	
  	
  	if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {	
			
			if (::cont_2 == 0) {
				
				::pinza.pinza = 470;
				
				::pin = control_pinza(::pinza, p, c);
					
				hand_pub_.publish(::pin);
				
				::cont_2 = ::cont_2+1;
			
			}
			else {	 
			
			
			  	if (p.pinza <= 495) {
				
					if (::cont_3 == 0) {
					
						::move = separar(::punto_0);
		
						move_pub_.publish(::move);
						
						::cont_3 = ::cont_3+1;		
										
					}	
					
					else if (((p.base-15) < ::move.posicion.base && ::move.posicion.base < (p.base+15)) && ((p.arti1-15) < ::move.posicion.arti1 && ::move.posicion.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.posicion.arti2 && ::move.posicion.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.posicion.arti3 && ::move.posicion.arti3 < (p.arti3+15))) {
					
						std::cout<<"Suelto"<<std::endl;
						
						::cont = 0;
						
						::cont_1 = 0;
						::cont_2 = 0;
						::cont_3 = 0;
					}
				}
			}
			
	}
	else {
			
			std::cout<<"En movimiento"<<std::endl;
	}
	}
	
		
	  
  }
 
  void punto(const geometry_msgs::Point& point)   
  {	
	
	::punto_0 = point;		

  }  
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "control_entrega");   
 
	ros::NodeHandle n;
	
  	
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);  
  	
  	ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);  
 	
  	
	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   

	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  

	
	ros::spin(); 
 
        return 0;
  }

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar: 

rosbuild_add_executable(contro_v03 src/control_v03.cpp)

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores: 

roscd brazo_fer
make

Ejecutando el programa

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando: 

roscore

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para ejecutar el programa de control llamado "control_v03", en un terminal nuevo ejecutaremos el siguiente comando: 

rosrun brazo_fer control_v03

Para poder publicar las coordenadas del punto donde queremos posar la botella, debemos publicar en el topic "place_point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando: 

rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once

Sexto programa (Sigue el camino recto)

Se ha creado un programa que desplaza el brazo desde el punto en el que se encuentre al que le indiquemos, siguiendo la línea recta que une los puntos. Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v04.cpp" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"   
  #include "geometry_msgs/Point.h"
  #include "math.h" 
 
  brazo_fer::Servos  p, e, c;
  brazo_fer::WriteServos destino, mover;  
  geometry_msgs::Point punto_0, punto_1, punto_i;
  double lambda = 0;
  int inclinacion_pinza = 0;
  int cont = 0; 
 
  void posicion(const brazo_fer::ReadServos& pose)
  {
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 
	::p = pose.posicion;
        ::e = pose.estado;
        ::c = pose.corriente;
    
    if (cont == 0)
    {
		::punto_0 = home(::p, ::c);
    
		::punto_1 = ::punto_0;
		
		cont = 1;
	}
    
	if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::mover.posicion.base && ::mover.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::mover.posicion.arti1 && ::mover.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::mover.posicion.arti2 && ::mover.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::mover.posicion.arti3 && ::mover.posicion.arti3 < (::p.arti3+15)))
	{	
    
		if (((::p.base-15) < ::destino.posicion.base && ::destino.posicion.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.posicion.arti1 && ::destino.posicion.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.posicion.arti2 && ::destino.posicion.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.posicion.arti3 && ::destino.posicion.arti3 < (::p.arti3+15))) 
		{
			::punto_0 = ::punto_1;
			::lambda = 0;
		}
		else
		{    
			geometry_msgs::Point u;
			
			//ecuación de la recta p1 = p0+lambda.u
			
			u.x = ::punto_1.x - ::punto_0.x;
			u.y = ::punto_1.y - ::punto_0.y;
			u.z = ::punto_1.z - ::punto_0.z;        
			
			double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
			   
			::lambda = ::lambda + 1/paso;
			
			::punto_i.x = ::punto_0.x + (::lambda)*u.x;
			::punto_i.y = ::punto_0.y + (::lambda)*u.y;
			::punto_i.z = ::punto_0.z + (::lambda)*u.z;
			
			::mover = inversa(::punto_i, ::inclinacion_pinza, ::p, 0);
			
			move_pub_.publish(::mover);
			 	
		}

	}
 
  }
 
  void punto(const geometry_msgs::Point& point)
  {	
	ros::NodeHandle n;
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
  	
	::lambda = 0;    
    
        ::punto_0 = directa(::p, ::inclinacion_pinza);  	
	
	::punto_1 = point;
	
	::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);
 
  } 
 
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "control_brazo");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
 
  	ros::Subscriber point_sub_= n.subscribe("point", 1, punto); 
 
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); 
  	   
 
	ros::spin();  
 
    return 0;
  }

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar: 

rosbuild_add_executable(contro_v04 src/control_v04.cpp)

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores: 

roscd brazo_fer
make

Ejecutando el programa

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando: 

roscore

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para ejecutar el programa de control llamado "control_v04", en un terminal nuevo ejecutaremos el siguiente comando: 

rosrun brazo_fer control_v04

Para poder publicar las coordenadas del punto donde queremos que se deplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando: 

rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once

Séptimo programa (Teleoperador)

Se ha desarrollado un programa tele-operador para poder manejar el brazo en modo manual usando el teclado del PC. Dentro del package creado, en el directorio "src" crearemos un fichero llamado "teleoperador_teclado.cpp" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"  
  #include "geometry_msgs/Point.h"
  #include "std_msgs/Int16.h"
  #include "std_msgs/String.h"  
  #include <signal.h>
  #include <termios.h>
  #include <stdio.h> 
  
  #define KEYCODE_Xmas 0x43 
  #define KEYCODE_Xmenos 0x44
  #define KEYCODE_Ymas 0x41
  #define KEYCODE_Ymenos 0x42
  #define KEYCODE_Zmas 0x77
  #define KEYCODE_Zmenos 0x73
  #define KEYCODE_Pabrir 0x61
  #define KEYCODE_Pcerrar 0x64
  #define KEYCODE_PinclinarMas 0x65
  #define KEYCODE_PinclinarMenos 0x71  
  
struct termios cooked, raw; 

int cont = 0;

double retardo = 0.11;

  geometry_msgs::Point punto;
  brazo_fer::Servos vel;
  brazo_fer::Servos pinza, p, c;
  
  int pinza_incli = 0;
  
	
void posicion_estado_corriente(const brazo_fer::ReadServos& pec) 
  { 
	     
	::p = pec.posicion;  
	::c = pec.corriente;

  }	  
 
void quit(int sig)
{
  tcsetattr(0, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
} 

void callback(const ros::TimerEvent&)
{
 
	ros::NodeHandle n;   	
  	
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  	  	

	ros::Time anterior;
	
	brazo_fer::WriteServos teleop;
	brazo_fer::WriteServos teleop_pinza;	

	if (::cont == 0)
    {
		anterior = ros::Time::now();
		
		::punto = home(::p, ::c);
		
		::cont = 1;
		
		::pinza.pinza = 511;
		
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		teleop_pinza = control_pinza(::pinza, ::p, ::c);
		
		//std::cout<<teleop<<"-"<<teleop_pinza<<std::endl;
	}
	
  char c;

  // get the console in raw mode                                                              
  tcgetattr(0, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(0, TCSANOW, &raw);

  puts(""); 
  puts("#####################################################");
  puts("                   CONTROLES BRAZO");
  puts("#####################################################");
  puts("");  
  puts("Flecha arriba:________ Subir pinza");
  puts("Flecha abajo:_________ Bajar pinza");
  puts("Flecha izquierda:_____ Desplazar pinza a la izquierda"); 
  puts("Flecha derecha:_______ Desplazar pinza a la derecha");
  puts("Tecla W:______________ Desplazar pinza hacia delante");
  puts("Tecla S:______________ Desplazar pinza hacia atrás");
  puts("Tecla A:______________ Abrir pinza");
  puts("Tecla D:______________ Cerrar pinza");
  puts("Tecla Q:______________ Inclinar pinza hacia arriba");
  puts("Tecla E:______________ Inclinar pinza hacia abajo");                 

        
    while (ros::ok())
    {
				
    // get the next event from the keyboard  
    if(read(0, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
	
      if (c == KEYCODE_Xmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
		::punto.x = ::punto.x - 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::punto.x = ::punto.x + 5;}
		anterior = ros::Time::now();
		}			
      }
      if (c == KEYCODE_Xmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::punto.x = ::punto.x + 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::punto.x = ::punto.x - 5;}
		anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_Ymas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::punto.y = ::punto.y + 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::punto.y = ::punto.y - 5;}
		anterior = ros::Time::now();
		}					    
      }
      if (c == KEYCODE_Ymenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::punto.y = ::punto.y - 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::punto.y = ::punto.y + 5;}		
		anterior = ros::Time::now();
		}						
      }
      if (c == KEYCODE_Zmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::punto.z = ::punto.z + 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
		{::punto.z = ::punto.z - 5;}		
		anterior = ros::Time::now();
		}					
      }
      if (c == KEYCODE_Zmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::punto.z = ::punto.z - 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::punto.z = ::punto.z + 5;}	
		anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_Pabrir)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::pinza.pinza = ::pinza.pinza - 5;
		teleop_pinza = control_pinza(::pinza, ::p, ::c);
		if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}		
		anterior = ros::Time::now();
		}
      }
      if (c == KEYCODE_Pcerrar)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::pinza.pinza = ::pinza.pinza + 5;
		teleop_pinza = control_pinza(::pinza, ::p, ::c);
		if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
		anterior = ros::Time::now();
		}
      }
      if (c == KEYCODE_PinclinarMas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::pinza_incli = ::pinza_incli + 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::pinza_incli = ::pinza_incli - 5;}
		anterior = ros::Time::now();
		}
      }
      if (c == KEYCODE_PinclinarMenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{      
		::pinza_incli = ::pinza_incli - 5;
		teleop = inversa(::punto, ::pinza_incli, ::p, 0);
		if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
		{::pinza_incli = ::pinza_incli + 5;}
		anterior = ros::Time::now();
		}
      }                                     
		
		if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
		{
			move_pub_.publish(teleop);			
		}
		if (teleop_pinza.posicion.pinza != ::p.pinza)
		{				
			hand_pub_.publish(teleop_pinza);
		}						    	

	}	
}
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "teleoperador_teclado");   
 
	ros::NodeHandle n;
  	
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);  	
  	
  	ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
  	
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);    
		 	 

	signal(SIGINT,quit); 
	
	ros::Timer timer = n.createTimer(ros::Duration(1), callback);  
   	   
 
	ros::spin();  
 
        return 0;
  }

Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar: 

rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)

Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores: 

roscd brazo_fer
make

Ejecutando el programa

Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando: 

roscore

Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

Para ejecutar el programa de control llamado "teleoperador_teclado", en un terminal nuevo ejecutaremos el siguiente comando: 

rosrun brazo_fer teleoperador_teclado


Artículos realcionados
Otros artículos

Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación brazo MYRAbot (urdf+gazebo)
Modelo para simulación MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)

Programas de control para el robot Turtlebot sobre ROS


< volver a principal

Retrieved from "https://robotica.unileon.es/index.php?title=Control_brazo_MYRAbot_(bioloid%2barduino)&oldid=4207"