Difference between revisions of "MYRAbot's arm control (bioloid+arduino)"

From robotica.unileon.es
Jump to: navigation, search
(→‎Arduino and servo motors Dinamixel)
 
(83 intermediate revisions by 2 users not shown)
Line 1: Line 1:
 
[[Mobile manipulation | < go back to main]]
 
[[Mobile manipulation | < go back to main]]
  
−
==Bioloid arm==
+
 
 +
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Related articles
 +
----
 +
!Other articles
 +
----
 +
|-
 +
| valign="top" width="50%" |
 +
 
 +
[[Objects recognition and position calculation (webcam)]]<br/>
 +
[[MYRAbot's arm model for simulation (urdf+gazebo)]]<br/>
 +
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Voice control (sphinx+festival)]]<br/>
 +
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 +
 
 +
| valign="top" |
 +
 
 +
[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
 +
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
 +
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 +
 
 +
|}
 +
----
 +
 
 +
 
 +
=Bioloid arm=
  
 
The arm that we will use is that Carlos Rodríguez Hernández developed in the project [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]]. The main components are:
 
The arm that we will use is that Carlos Rodríguez Hernández developed in the project [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]]. The main components are:
Line 13: Line 42:
 
* Parts made for the gripper and to fix to MYRAbot.
 
* Parts made for the gripper and to fix to MYRAbot.
  
−
==Arduino IDE and rosserial==
+
=Arduino IDE and rosserial=
  
 
For the communication among  [http://www.arduino.cc arduino] and [http://www.ros.org ROS], we install [http://www.arduino.cc arduino] IDE and rosserial (''package'' de [http://www.ros.org ROS] which include the ''package'' rosserial_arduino with the libraries for [http://www.arduino.cc arduino]. We will start to installing [http://www.arduino.cc arduino] IDE, for this, we will execute the next commands in a terminal:
 
For the communication among  [http://www.arduino.cc arduino] and [http://www.ros.org ROS], we install [http://www.arduino.cc arduino] IDE and rosserial (''package'' de [http://www.ros.org ROS] which include the ''package'' rosserial_arduino with the libraries for [http://www.arduino.cc arduino]. We will start to installing [http://www.arduino.cc arduino] IDE, for this, we will execute the next commands in a terminal:
Line 29: Line 58:
 
cp -r ros_lib /home/”SESSION_NAME”/sketchbook/libraries/ros_lib</syntaxhighlight>
 
cp -r ros_lib /home/”SESSION_NAME”/sketchbook/libraries/ros_lib</syntaxhighlight>
  
−
===Creating a package for our programs===
+
==Creating a package for our programs==
  
 
Previously, we must have created a [[Fernando-TFM-ROS02#Creando un espacio de trabajo|workspace]] for our [http://www.ros.org ROS] distribution. We will create a ''package'' in order to compile and send our programs to [http://www.arduino.cc arduino] board. We will make a ''package'' named "arduino_fer" with the the necessary dependences for our programs, for this, we will execute the next commands in a terminal:
 
Previously, we must have created a [[Fernando-TFM-ROS02#Creando un espacio de trabajo|workspace]] for our [http://www.ros.org ROS] distribution. We will create a ''package'' in order to compile and send our programs to [http://www.arduino.cc arduino] board. We will make a ''package'' named "arduino_fer" with the the necessary dependences for our programs, for this, we will execute the next commands in a terminal:
Line 44: Line 73:
 
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)</syntaxhighlight>
 
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)</syntaxhighlight>
  
−
We will execute the next commands in a terminal to finalize the creation of the package:
+
We will execute the next commands in a terminal in order to finalize the creation of the package:
  
 
<syntaxhighlight>roscd arduino_fer
 
<syntaxhighlight>roscd arduino_fer
 
cmake .</syntaxhighlight>
 
cmake .</syntaxhighlight>
  
−
===First program (To whole power)===
+
==First program (To whole power)==
  
 
We will make the first program that publishes a ''topic'' named "cifra" and subscribe a ''topic'' named "resultado". When we publish a number in the ''topic'' "cifra" the program multiplies the number by itself and publishes the result in the ''topic'' "resultado". The code of the program is shown below:
 
We will make the first program that publishes a ''topic'' named "cifra" and subscribe a ''topic'' named "resultado". When we publish a number in the ''topic'' "cifra" the program multiplies the number by itself and publishes the result in the ''topic'' "resultado". The code of the program is shown below:
Line 86: Line 115:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
We must add to the file "CMakeLists.txt" the lines indicated below in order to compile the program and send to [http://www.arduino.cc arduino] board:
+
We must add to the file "CMakeLists.txt" the lines indicated below in order in order to compile the program and send to [http://www.arduino.cc arduino] board:
  
 
<syntaxhighlight lang=cmake>set(FIRMWARE_NAME potencia)
 
<syntaxhighlight lang=cmake>set(FIRMWARE_NAME potencia)
Line 100: Line 129:
 
make potencia-upload</syntaxhighlight>
 
make potencia-upload</syntaxhighlight>
  
−
We must launch the [http://www.ros.org ROS] core in a terminal to allow the execution of the program, if it is not already launched. We will execute the next command in a terminal:
+
We must launch the [http://www.ros.org ROS] core in a terminal in order to allow the execution of the program, if it is not already launched. We will execute the next command in a terminal:
  
 
<syntaxhighlight>roscore</syntaxhighlight>
 
<syntaxhighlight>roscore</syntaxhighlight>
Line 108: Line 137:
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
  
−
We will execute in other terminal the next command to publish a number in the ''topic'' "cifra":
+
We will execute in other terminal the next command in order to publish a number in the ''topic'' "cifra":
  
 
<syntaxhighlight>rostopic pub cifra std_msgs/Int16 NUMBER --once</syntaxhighlight>
 
<syntaxhighlight>rostopic pub cifra std_msgs/Int16 NUMBER --once</syntaxhighlight>
  
−
We will execute in other terminal the next command to test that the program calculates the square of the published number in the ''topic'' "cifra" and publishes the result in the ''topic'' "resultado":
+
We will execute in other terminal the next command in order to test that the program calculates the square of the published number in the ''topic'' "cifra" and publishes the result in the ''topic'' "resultado":
  
 
<syntaxhighlight>rostopic echo resultado</syntaxhighlight>
 
<syntaxhighlight>rostopic echo resultado</syntaxhighlight>
  
−
==Arduino and servo motors Dinamixel==
+
=Arduino and servo motors Dinamixel=
  
 
In [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] we can find the hardware adaptations to communicate the [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] board with the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] of [http://www.robotis.com/xe/ ROBOTICS]. Here also we can find the libraries for programming ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).
 
In [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] we can find the hardware adaptations to communicate the [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] board with the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] of [http://www.robotis.com/xe/ ROBOTICS]. Here also we can find the libraries for programming ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).
  
−
===Second program (Move)===
+
==Second program (Move)==
−
 
 
−
 
 
−
 
 
−
<!--
 
  
−
===Segundo programa (Muévete)===
+
This program is a test of the communications between [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] board and servo motor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. The program publishes the ''topic'' "angulo" which contains the position of the servo motor (between 0 and 1023) and subscribes the ''topic'' "giro" which receives the goal position (between 0 and 1023). The servo motor will move with a constant velocity of 128 (Integer between 0 and 1023). The code of the program is shown below:
−
 
 
−
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:
 
  
 
<syntaxhighlight lang=cpp>
 
<syntaxhighlight lang=cpp>
Line 167: Line 190:
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>
−
Para poder compilar y enviar el programa a la placa [http://www.arduino.cc arduino] deberemos emplear el software [http://www.arduino.cc arduino] IDE, ya que empleamos una librería externa al sistema [http://www.ros.org ROS] y no nos permite hacerlo usando [[#Creación de un package para nuestros programas|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 [http://www.ros.org ROS]:
+
We can't use the [[#Creating a package for our programs|previously made ''package'']] in order to compile and send the program to the [http://www.arduino.cc arduino] board. We must use the [http://www.arduino.cc arduino] IDE software due to that we use a external library to [http://www.ros.org ROS] system. When the program was uploaded we will execute the next command in a terminal, to launch the core system:
  
 
<syntaxhighlight>roscore</syntaxhighlight>
 
<syntaxhighlight>roscore</syntaxhighlight>
  
−
También debemos lanzar en otro terminal el nodo de comunicación [http://www.ros.org ROS]-[http://www.arduino.cc arduino], ejecutando el siguiente comando:
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
  
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
  
−
En un nuevo terminal publicaremos en el ''topic'' "giro" la posición de destino del servomotor, ejecutando el siguiente comando:  
+
We will execute the next command in other terminal in order to publish the goal position of the servo motor in the ''topic'' "giro":
  
−
<syntaxhighlight>rostopic pub giro std_msgs/Int16 POSICION_DESEADA --once</syntaxhighlight>
+
<syntaxhighlight>rostopic pub giro std_msgs/Int16 GOAL_POSITION --once</syntaxhighlight>
  
−
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:
+
The servo motor will immediately move to the goal position. We can execute the next command in other terminal in order to see the position value of the servo motor at any time:
  
 
<syntaxhighlight>rostopic echo angulo</syntaxhighlight>
 
<syntaxhighlight>rostopic echo angulo</syntaxhighlight>
  
−
==Modelo cinemático del brazo==
+
=Arm's kinematics model=
  
−
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.
+
We need to know the spatial position of the arm for the correct placement of the gripper. The position depends on the position angle of the servo motors (direct kinematics). We know the goal point and we must obtain the position angle of the servo motors to achieve it (inverse kinematics). The arm have 5 joints (1 to open-close gripper) and all work in the same plane, for this reason we don't need use the [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters Denavit-Hartenberg parameters].
  
−
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagramas para el estudio de la cinemática]]
+
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagrams for kinematics study]]
  
−
===Cinemática inversa===
+
==Inverse kinematic==
  
−
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:
+
We use the diagram shown above to obtain the inverse kinematics. Where ''x'', ''y'' and ''z'' are the goal point coordinates, and &epsilon; is the angle that the gripper forms with x-z plane. The obtained equations are shown below:
  
 
[[file:brazo_ecuaciones.jpg]]
 
[[file:brazo_ecuaciones.jpg]]
  
−
Teniendo en cuenta los ángulos límite de los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm 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:
+
We have to calculate the value of position angle for the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] according its turn limits (1023 positions throughout 300º):
  
 
[[file:brazo_dynamixel_AX-12.jpg|thumb|250px|Diagrama de ángulos servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]]]
 
[[file:brazo_dynamixel_AX-12.jpg|thumb|250px|Diagrama de ángulos servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]]]
  
−
[[file:brazo_ecuacion_base.jpg]] Se suman 150º al ángulo para situar el 0º coincidente con el eje z.
+
[[file:brazo_ecuacion_base.jpg]] We add 150º from the position angle in order to set the 0º coincident with z axis.
  
−
[[file:brazo_ecuacion_arti1.jpg]] Se suman 60º al ángulo para situar el 0º coincidente con el eje z.
+
[[file:brazo_ecuacion_arti1.jpg]] We add 60º from the position angle in order to set the 0º coincident with z axis.
  
−
[[file:brazo_ecuacion_arti2.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
+
[[file:brazo_ecuacion_arti2.jpg]] We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.
  
−
[[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]] We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.
−
 
  
−
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.
+
==Direct kinematics==
  
−
===Cinemática directa===
+
We can use the previous equations to obtain the placement point through the position angle of the servo motors. The equations are shown below:
  
−
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:
+
[[file:brazo_ecuaciones_directa.jpg]]
  
−
[[file:brazo_ecuaciones_directa.jpg]]
+
==Arm workspace and boundaries==
 +
 
 +
We need to set the boundaries of the position angles of the servo motors to avoid self collisions and collision with other parts of MYRAbot. We have made a spreadsheet using the equations for inverse and direct kinematics. The spreadsheet shows the spacial placement of the arm in two dispersion graphs.
  
−
===Espacio de trabajo y limitaciones===
+
[[file:brazo_cinematica_excel.jpg|thumb|750px|center|Spreadsheet of inverse and direct kinematics, [http://www.fernando.casadogarcia.es/files/cinematica_brazo_MYRAbot.xlsx download]]]
  
−
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.
+
=Control programs=
  
−
[[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]]]
+
==Third program (Come here)==
  
−
==Programas de control==
+
We place a interface program in [http://www.arduino.cc arduino] board that communicates the servo motor with the control programs in [http://www.ros.org ROS]. We need publish and subscribe the data of 5 servo motors so we will create own messages.
  
−
===Tercer programa (Ven aquí)===
+
===Creating custom messages in ROS===
  
−
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''.
+
First, we will create a new ''package'' with the necessary dependences for our programs (std_msgs geometry_msgs roscpp). We will execute the next commands in a terminal:
  
−
====Creación de mensajes personalizados en ROS====
+
<syntaxhighlight>
 +
cd ~/ros_workspace
 +
roscreate-pkg brazo_fer std_msgs geometry_msgs roscpp</syntaxhighlight>
  
−
Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas. 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 e campo).  
+
We will create a folder named "msg" within the created ''package'', here we place the next description files for our messages (data type and field name).
  
−
Comenzaremos creando un mensaje base llamado "servos", que contendrá los datos de cada servomotor. Crearemos un archivo llamado "Servos.msg" con el siguiente contenido:
+
We will create the basis message that contains the data for each servo motor. We will create a file named "Servos.msg" with the content that is shown below:
  
 
<syntaxhighlight>int16 base
 
<syntaxhighlight>int16 base
Line 241: Line 267:
 
int16 pinza</syntaxhighlight>
 
int16 pinza</syntaxhighlight>
  
−
Vamos a crear un mensaje que contenga los datos que enviaremos a los servomotores (posición 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:
+
We will use the previous message in order to define the data type. We will create a file named "WriteServos.msg" to send data to the servo motors (position, velocity and torque) with the content that is shown below:
  
 
<syntaxhighlight>Servos posicion
 
<syntaxhighlight>Servos posicion
Line 247: Line 273:
 
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:
+
We will create a file named "ReadServos.msg" in order to receive data to the servo motors (position, status and current) with the content that is shown below:
  
 
<syntaxhighlight>Servos posicion
 
<syntaxhighlight>Servos posicion
Line 253: Line 279:
 
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:
+
We have to edit the file "CMakeLists.tx" in our ''package'' in order to allow the creation the [http://www.ros.org ROS] messages. We will delete the symbol "#" in the line that is shown below:
  
 
<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>
 
<syntaxhighlight># rosbuild_gensrv()</syntaxhighlight>
  
−
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:
+
We will execute the next commands in a terminal in order to compile and create the messages, where "brazo_fer" is the name of the created ''package'':
  
 
<syntaxhighlight>roscd brazo_fer
 
<syntaxhighlight>roscd brazo_fer
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
Como vamos a usar estos mensajes en [http://www.arduino.cc arduino], necesitamos añadirlos al ''package'' "rosserial", ejecutando en un terminal el siguiente comando:
+
We will execute the next command in a terminal in order to add this messages to the [http://www.arduino.cc arduino] libraries in "rosserial":  
  
 
<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====
+
===Arduino program===
  
−
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:
+
The program for [http://www.arduino.cc arduino] board subscribes the ''topic'' "move_arm" which receives the orders for the servo motors and publishes the "topic" "pose_arm" which sends the servo motors information. The code of the program is shown below:
  
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
Line 408: Line 434:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
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.
+
We will use [http://www.arduino.cc arduino] IDE software to compile and send the program to the board. We will use this program for the next control programs since it is a interface between the servo motors and [http://www.ros.org ROS] system.
  
−
====Programa ROS====
+
===ROS program===
  
−
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:
+
This program uses the equations of the inverse kinematics to set the position angle of the servo motors in order to reach the goal point that is within the workspace. The program subscribe the ''topic'' "point" which receives the goal point, also it subscribes the ''topic'' "pose_arm" which receives the information of servo motors, and publish the ''topic' "move_arm" which indicates to the [http://www.arduino.cc arduino] program the position, velocity and torque of for the servo motors. We will create a file named "control_v01.cpp" with the content that is shown below within the folder "src" of our ''package'':
  
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
Line 458: Line 484:
 
double L_a, L;
 
double L_a, L;
 
   
 
   
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
epsilon = 0;  
 
   
 
   
 
alfa = (atan2(x,z)*180)/PI;
 
alfa = (atan2(x,z)*180)/PI;
Line 529: Line 555:
 
}
 
}
 
else {
 
else {
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
 
}
 
}
 
   
 
   
Line 557: Line 583:
 
</syntaxhighlight>
 
</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:  
+
We have to add the next line to the file "CMakeLists.txt" of our ''package'' in order to compile and create the executable file:
  
 
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v01 src/control_v01.cpp)</syntaxhighlight>
 
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_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:
+
We will execute the next commands in a terminal in order to compile and create the executable file:
  
 
<syntaxhighlight>roscd brazo_fer
 
<syntaxhighlight>roscd brazo_fer
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
====Ejecutando el programa====
+
===Running the program===
  
−
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:
+
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
  
 
<syntaxhighlight>roscore</syntaxhighlight>
 
<syntaxhighlight>roscore</syntaxhighlight>
  
−
Ahora debemos iniciar el nodo de la placa [http://www.arduino.cc arduino] ejecutando en un nuevo terminal el siguiente comando:
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
  
 
<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_v01", en un terminal nuevo ejecutaremos el siguiente comando:
+
We will execute the next command in a terminal in order to launch the control program:
  
 
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
 
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
  
−
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:
+
We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal point:
  
 
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>
 
<syntaxhighlight>rostopic pub 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:
+
The arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:
  
 
<center><videoflash>iwAcutO3C8g</videoflash></center>
 
<center><videoflash>iwAcutO3C8g</videoflash></center>
  
−
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Ver vídeo en YouTube]</center>
+
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Watch in YouTube]</center>
  
−
===Cuarto programa (Agarra la botella)===
+
==Fourth program (Grasp the bottle)==
  
−
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:
+
===Include file with functions===
  
−
# Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).
+
We have created a file with the control fuctions for the arm in orther to simplify and make more legible the programs. We will create a file named "brazo_fer.h" within the folder "include/brazo_fer" of the created ''package'' with the content that is shown below:
−
# 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 609: Line 633:
 
   #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_pp = atan2(y,z_p);
 
−
 
 
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
−
+
−
beta = ((beta_p+beta_a)*180)/PI;
+
beta_p = beta - beta_a;
−
+
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
delta_a = PI - (beta_a + gamma);
−
+
−
delta_a = PI-(beta_a+gamma);
+
delta_p1 = delta - delta_a;
−
+
−
gamma = (gamma*180)/PI;
+
delta_p2 = 2*PI - (delta - delta_a);
−
+
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
+
if (delta_p1 < delta_p2)  
−
+
{
−
if (beta_pp > beta_p) {
+
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p1));
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
 
 
}
 
}
−
else {
+
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;  
+
L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta_p2));
 
}
 
}
−
 
−
brazo_fer::Servos brazo;
 
 
 
−
brazo.base = ((alfa+150)*1023)/300;
+
z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
−
brazo.arti1 = ((beta+60)*1023)/300;
+
−
brazo.arti2 = ((gamma-30)*1023)/300;
+
beta_pp = acos(z_p/L);
−
brazo.arti3 = ((delta-30)*1023)/300;
+
 +
a = L1*sin(beta);
 
 
−
::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);
−
    
+
−
  void home()
+
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)  
 +
   {
 
   
 
   
−
   ros::NodeHandle n;
+
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;
 
   
 
   
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"
+
alfa = (atan2(x,z)*180)/PI;
−
 
+
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
+
z_p = sqrt(pow(z,2)+pow(x,2));
−
 
+
−
   
+
L = sqrt(pow(z_p,2)+pow(y,2));
−
if (::cont == 0) {
+
−
+
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
−
::punto_0.x = 0;
+
−
::punto_0.y = 75;
+
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
−
::punto_0.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;
 
−
 
−
::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;
−
  if (::cont_1 == 0) {
+
move_arm.posicion = posicion_servos_0;
−
 
+
move_arm.velocidad = velocidad_servos_0;
−
::move = acercar(::punto_0);
+
move_arm.par = par_servos;
−
  
+
return move_arm;
−
brazo_fer::WriteServos uno;
+
}  
 +
  }
 +
 
 +
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);
 
  
 
  
−
uno.posicion = ::move;
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
−
 
+
−
uno.velocidad = ::vel;
+
geometry_msgs::Point punto_0;
−
 
+
−
uno.par = ::on;
+
punto_0.x = 0;
−
 
+
punto_0.y = 80;
−
move_pub_.publish(uno);
+
punto_0.z = 50;
−
 
+
−
::cont_1 = ::cont_1+1;
+
int inclinacion_pinza = 0;
−
 
+
−
}
+
brazo_fer::WriteServos inicio_brazo = inversa(punto_0, inclinacion_pinza, posicion_servos_0, 0);
−
 
+
−
  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))) {
+
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);
 
 
−
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;
+
===Program===
−
 
+
 
−
move_pub_.publish(dos);
+
This program execute the sequence to grasp a little plastic bottle (the bottle is empty since the torque of the servo motors is low, we need more servo motors on each joint in order to pick more load). The sequence steps are shown below:
−
+
 
−
::cont_2 = ::cont_2+1;
+
# Approach (place the gripper at 70 mm of the bottle position).
−
+
# Placement in the bottle position.
−
+
# To close the gripper (until to hold the bottle).
−
}
+
# Lifting and approach.
−
else {
+
 
−
+
We will create a file named "control_v02.cpp" with the content that is shown below within the folder "src" of our ''package'':
−
+
 
−
  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;
+
  {
−
+
  double x = cerca.x;
−
::move = levantar_acercar(::punto_0);
+
  double z = cerca.z;  
−
+
 
−
brazo_fer::WriteServos cuatro;  
+
  cerca.z = cerca.z - 70*cos(atan2(x,z));
−
 
+
  cerca.x = cerca.x - 70*sin(atan2(x,z));
−
cuatro.posicion = ::move;
+
 
−
+
  brazo_fer::WriteServos move = inversa(cerca, ::inclinacion_pinza, ::pg, 0);
−
cuatro.velocidad = ::vel;
+
 
−
+
  return move;
−
cuatro.par = ::on;
+
 
−
+
  }
−
move_pub_.publish(cuatro);
+
 
−
+
    brazo_fer::WriteServos levantar_acercar(geometry_msgs::Point la)
−
::punto_1 = ::punto_0;
+
  {
−
+
 
−
::cont_1 = 0;
+
  la.x = 0;
−
::cont_2 = 0;
+
  la.y = la.y + 50;
−
}
+
  la.z = 60;
−
}
+
 
−
}
+
  if (la.y < 75) {
−
+
la.y = 75;
 
}
 
}
−
else {
+
 
−
+
  brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);
−
std::cout<<"En movimiento"<<std::endl;
+
 
−
}
+
  ::punto_0 = la;
−
}
+
 
−
}
+
  return move;
−
+
 
−
 
 
 
   }
 
   }
 +
 
 +
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec) 
 +
  {
 +
 
 +
ros::NodeHandle n;
 
   
 
   
−
  void punto(const geometry_msgs::Point& point)  //Función de llamda con cada dato recibido
+
  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;    
 
 
−
::punto_0 = point;
+
brazo_fer::Servos c = pec.corriente;   
−
 
 
−
   }
 
−
 
−
 
−
  int main(int argc, char **argv)
 
−
  {
 
−
 
−
ros::init(argc, argv, "control_recogida");  //Creación del nodo "control_brazo"
 
−
 
−
ros::NodeHandle n;
 
 
 
−
 
 
−
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);  //Suscripción del topic "pose_arm"
 
−
 
 
−
  ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);  //Suscripción del topic "point"
 
−
 
 
−
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"
 
−
 
  
 +
if (::cont == 0)
 +
{
 +
::punto_0 = home(p, c);
 +
 +
::punto_1 = ::punto_0;
 +
 +
::cont = ::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 ) {
−
 
−
        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:
+
  if (::cont_1 == 0) {
−
 
+
 
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
+
::move = acercar(::punto_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_pub_.publish(::move);
−
 
+
 
−
<syntaxhighlight>roscd brazo_fer
+
::cont_1 = ::cont_1+1;
−
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:
+
  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))) {
−
 
+
−
<syntaxhighlight>roscore</syntaxhighlight>
+
if (::cont_2 == 0) {
−
 
+
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
−
 
+
 
−
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
move_pub_.publish(::move);
−
 
+
−
Para ejecutar el programa de control llamado "control_v02", en un terminal nuevo ejecutaremos el siguiente comando:
+
::cont_2 = ::cont_2+1;
−
 
+
−
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
+
−
 
+
}
−
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):
+
else {
−
 
+
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</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))) {
−
En el siguiente vídeo se puede ver la ejecución de la secuencia para la recogida de una botella:
+
−
 
+
−
<center><videoflash>0FcB6-i23cU</videoflash></center>
+
::pinza.pinza = p.pinza;
−
 
+
−
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Ver vídeo en YouTube]</center>
+
if(::pin.posicion.pinza != p.pinza) {
−
 
+
−
===Quinto programa (posa la botella)===
+
std::cout<<c.pinza<<std::endl;
−
 
+
−
Este programa ejecuta la secuencia para posar la botella que hemos cogido. Los pasos que realiza son los siguientes:
+
::pinza.pinza = ::pinza.pinza + 20;
−
 
+
−
# Posicionamiento en el punto indicado.
+
::pin = control_pinza(::pinza, p, c);
−
# Apertura de la pinza (hasta soltar la botella).
+
−
# Separación (sitúa el brazo a unos 70 mm del punto indicado).
+
hand_pub_.publish(::pin);
−
# 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:
+
else {
−
 
+
−
<syntaxhighlight lang="cpp" enclose="div">
+
std::cout<<"Agarrado"<<std::endl;
−
 
+
−
  #include "ros/ros.h" 
+
::move = levantar_acercar(::punto_0);
−
  #include "brazo_fer/Servos.h"
+
−
  #include "brazo_fer/WriteServos.h" 
+
move_pub_.publish(::move);
−
  #include "brazo_fer/ReadServos.h" 
+
−
  #include "std_msgs/Int16.h"
+
::punto_1 = ::punto_0;
−
  #include "geometry_msgs/Point.h"
+
−
  #include "math.h" 
+
::cont_1 = 0;
−
 
+
::cont_2 = 0;
−
  #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 
+
}
−
 
+
else {
−
  geometry_msgs::Point punto_0,punto_1;
+
−
 
+
std::cout<<"En movimiento"<<std::endl;
−
  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;
+
  }
−
   double y = destino.y;
+
−
double z = destino.z;
+
  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"
 
 
−
y = y + z*tan(atan2(45,250));
+
::punto_0 = point;
  
−
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 L_a, L;
 
 
   
 
   
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
  int main(int argc, char **argv)
 +
  {
 
   
 
   
−
alfa = (atan2(x,z)*180)/PI;
+
ros::init(argc, argv, "control_recogida");
 
   
 
   
−
z_p = sqrt(pow(z,2)+pow(x,2));
+
ros::NodeHandle n;
−
+
−
L = sqrt(pow(z_p,2)+pow(y,2));
+
 
−
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
 
−
+
  ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
 
 +
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 +
 
 +
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 +
 
 
 
−
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;
+
 
−
+
</syntaxhighlight>
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
 
−
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:
−
delta_a = PI-(beta_a+gamma);
+
 
−
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
−
gamma = (gamma*180)/PI;
+
 
−
+
We will execute the next commands in a terminal in order to compile and create the executable file:
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
+
 
−
+
<syntaxhighlight>roscd brazo_fer
−
if (beta_pp > beta_p) {
+
make</syntaxhighlight>
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
 
−
}
+
===Running the program===
−
else {
+
 
−
delta = ((delta_p+delta_a)*180)/PI;
+
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
−
+
 
−
if (isnan(delta)) {
+
<syntaxhighlight>roscore</syntaxhighlight>
−
delta = ((PI+delta_a)*180)/PI;
+
 
−
}
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
−
}
+
 
−
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
−
if (isnan(gamma)) // si no hay solución
+
 
−
{
+
We will execute the next command in a terminal in order to launch the control program:
−
coordenadas_correctas = 0;
+
 
−
}
+
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
−
+
 
−
brazo_fer::Servos brazo;
+
We will execute the next command in a terminal in order to publish in the ''topic'' "pick_point" the bottle position (We have placed the bottle in a known position but we can use a vision system to set the bottle position):
−
+
 
−
brazo.base = ((alfa+150)*1023)/300;
+
<syntaxhighlight>rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>
−
brazo.arti1 = ((beta+60)*1023)/300;
+
 
−
brazo.arti2 = ((gamma-30)*1023)/300;
+
The arm will execute the pick sequence. We can see the arm grasping the bottle in the next video:
−
brazo.arti3 = ((delta-30)*1023)/300;
+
 
−
+
<center><videoflash>0FcB6-i23cU</videoflash></center>
−
::vel.base = abs(brazo.base - ::pg.base)/5;
+
 
−
::vel.arti1 = abs(brazo.arti1 - ::pg.arti1)/5;
+
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Watch in YouTube]</center>
−
::vel.arti2 = abs(brazo.arti2 - ::pg.arti2)/5;
+
 
−
::vel.arti3 = abs(brazo.arti3 - ::pg.arti3)/5;
+
==Fifth program (Place the bottle)==
−
+
 
−
+
This program execute the sequence to place the bottle. The sequence steps are shown below:
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
+
   
−
return brazo;
+
# Placement in the goal position.
−
}
+
# To open the gripper (until to free the bottle).
−
else {
+
# Distance (place the gripper at 70 mm of the goal position).
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
# Return to initial position.
−
::error_coordenadas = 1;
+
 
−
return ::pg;
+
We will create a file named "control_v03.cpp" with the content that is shown below within the folder "src" of our package:
−
}  
+
 
 +
<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 "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 home()
+
   void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
 
   {  
 
   {  
 
   
 
   
−
  ros::NodeHandle n;
+
ros::NodeHandle n;
 
   
 
   
−
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  //Publicación del topic "move_arm"
+
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
 
  
 
  
−
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
+
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
−
 
+
   
−
   
+
brazo_fer::Servos p = pec.posicion; 
−
if (::cont == 0) {
+
 
−
+
::pg = pec.posicion;
−
::punto_0.x = 0;
+
::cg = pec.corriente;
−
::punto_0.y = 75;
+
−
::punto_0.z = 50;
+
brazo_fer::Servos e = pec.estado;  
−
+
−
::on.base = 1;
+
brazo_fer::Servos c = pec.corriente;
−
::on.arti1 = 1;
+
−
::on.arti2 = 1;
+
if (::cont == 0)
−
::on.arti3 = 1;
+
{
−
::on.pinza = 1;
+
::punto_0 = directa(p, inclinacion_pinza);
−
 
−
::off.base = 0;
 
−
::off.arti1 = 0;
 
−
::off.arti2 = 0;
 
−
::off.arti3 = 0;
 
−
::off.pinza = 0;  
 
 
 
 
::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);
−
+
 
−
ini.posicion = inicio;
+
::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.pinza = 511;
+
if (::cont_2 == 0) {
 +
 +
::pinza.pinza = 470;
 +
 +
::pin = control_pinza(::pinza, p, c);
 +
 +
hand_pub_.publish(::pin);
 +
 +
::cont_2 = ::cont_2+1;
 
 
−
ini.velocidad = ::vel;
+
}
 +
else {
 
 
−
ini.velocidad.pinza = abs(511 - ::pg.pinza);
 
 
 
−
ini.par = ::on;
+
  if (p.pinza <= 495) {
 
 
−
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 {
−
ros::NodeHandle n;
+
 +
std::cout<<"En movimiento"<<std::endl;
 +
}
 +
}
 +
 +
 +
 
 +
  }
 
   
 
   
−
  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) {
+
 
−
 
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:  
−
::move = inversa(::punto_0);
+
 
−
 
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>
−
brazo_fer::WriteServos uno;
+
 
−
 
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
−
uno.posicion = ::move;
+
 
−
 
+
<syntaxhighlight>roscd brazo_fer
−
uno.velocidad = ::vel;
+
make</syntaxhighlight>
−
 
+
 
−
uno.par = ::on;
+
===Running the program===
−
 
+
 
−
move_pub_.publish(uno);
+
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
−
 
+
 
−
::cont_1 = ::cont_1+1;
+
<syntaxhighlight>roscore</syntaxhighlight>
−
 
+
 
−
}
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
−
 
 
−
  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;
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
−
+
 
−
                                                ::move = separar(::punto_0);
+
We will execute the next command in a terminal in order to launch the control program:
−
+
 
−
brazo_fer::WriteServos tres;  
+
<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>
−
+
 
−
tres.posicion = ::move;
+
We will execute the next command in a terminal in order to publish in the ''topic'' "place_point" the goal position:
−
+
 
−
tres.velocidad = ::vel;
+
<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
−
+
 
−
tres.par = ::on;
+
==Sixth program (Follow the straight way)==
−
+
 
−
move_pub_.publish(tres);
+
This program move the arm following the line between the initial position and the goal position. We will create a file named "control_v04.cpp" with the content that is shown below within the folder "src" of our package:  
−
+
 
−
::cont_3 = ::cont_3+1;
+
<syntaxhighlight lang="cpp" enclose="div">
−
 
−
}
 
−
 
−
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;
+
  #include "ros/ros.h"
−
+
  #include "brazo_fer/Servos.h"
−
home();
+
  #include "brazo_fer/WriteServos.h" 
−
+
  #include "brazo_fer/ReadServos.h"
−
::cont = ::cont + 1;
+
  #include "brazo_fer/brazo_fer.h" 
−
                                               
+
  #include "geometry_msgs/Point.h"
−
::cont_1 = 0;
+
   #include "math.h"
−
::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
+
   brazo_fer::Servos  p, e, c;
−
   {
+
   brazo_fer::WriteServos destino, mover; 
−
+
   geometry_msgs::Point punto_0, punto_1, punto_i;
−
::punto_0 = point;
+
  double lambda = 0;
−
 
+
  int inclinacion_pinza = 0;
−
   }
+
   int cont = 0;
 
   
 
   
−
+
   void posicion(const brazo_fer::ReadServos& pose)
−
   int main(int argc, char **argv)
 
 
   {
 
   {
 +
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:
 
−
 
 
−
<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>
 
−
 
 
−
Para ejecutar el programa de control llamado "control_v03", en un terminal nuevo ejecutaremos el siguiente comando:
 
−
 
 
−
<syntaxhighlight>rosrun brazo_fer control_v03</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):
 
−
 
 
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
 
−
 
 
−
===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:
 
−
 
 
−
<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 "math.h"
 
 
   
 
   
−
   #define PI 3.14159265
+
   void punto(const geometry_msgs::Point& point)
−
   #define L1 104
+
   {
−
  #define L2 104
+
ros::NodeHandle n;
−
  #define Lp 60
 
 
   
 
   
−
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
−
  brazo_fer::Servos move, vel, p, e, c, destino, on, off;
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
−
  geometry_msgs::Point punto_0, punto_1, punto_i;
+
 
−
  double lambda = 0;
+
::lambda = 0;  
−
  int cont = 0;
+
   
−
 
+
        ::punto_0 = directa(::p, ::inclinacion_pinza); 
−
  geometry_msgs::Point directa(brazo_fer::Servos origen)
 
−
  {
 
 
 
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, epsilon;
+
::punto_1 = point;
−
double z_p;
 
−
double L_a, L;
 
−
double x, y, z;
 
 
 
−
alfa = (((origen.base*300)/1023)-150)*PI/180;
+
::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);
−
beta = (((origen.arti1*300)/1023)-60)*PI/180;
+
−
gamma = (((origen.arti2*300)/1023)+30)*PI/180;
+
  }
−
delta = (((origen.arti3*300)/1023)+30)*PI/180;
+
 +
 +
  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;
 +
  }
  
−
        epsilon = 0; /Ángulo de inclinación de la pinza respecto a la horizontal
+
</syntaxhighlight>
−
+
 
−
L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:
−
 
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
  
−
        beta_p = beta - beta_a;
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
  
−
delta_a = PI - (beta_a + gamma);
+
We will execute the next commands in a terminal in order to compile and create the executable file:
−
+
 
−
        L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta - delta_a));
+
<syntaxhighlight>roscd brazo_fer
−
+
make</syntaxhighlight>
−
z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
+
 
 +
===Running the program===
 +
 
 +
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
 +
 
 +
<syntaxhighlight>roscore</syntaxhighlight>
 +
 
 +
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
 +
 
 +
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 +
 
 +
We will execute the next command in a terminal in order to launch the control program:
 +
 
 +
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
  
−
        beta_pp = acos(z_p/L);
+
We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal position:
−
+
 
−
if (L1*sin(beta) >= L2*sin(beta + gamma) + Lp*sin(epsilon))
+
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
−
{
+
 
−
y = L*sin(beta_pp);
+
==Seventh program (Teleoperator)==
−
}
+
 
−
else
+
This program allows to control the arm using the keyboard of the remote PC. We will create a file named "teleoperador_teclado.cpp" with the content that is shown below within the folder "src" of our package:
−
{
 
−
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::Servos inversa(geometry_msgs::Point destino)
 
−
  {
 
−
 
 
−
double x = destino.x;
 
−
  double y = destino.y;
 
−
double z = destino.z;
 
  
−
int coordenadas_correctas = 1;
+
<syntaxhighlight lang="cpp" enclose="div">
  
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
  #include "ros/ros.h"
−
double z_p;
+
  #include "brazo_fer/Servos.h"
−
double L_a, L;
+
  #include "brazo_fer/WriteServos.h" 
−
   
+
  #include "brazo_fer/ReadServos.h"
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
  #include "brazo_fer/brazo_fer.h"  
−
   
+
  #include "geometry_msgs/Point.h"
−
alfa = (atan2(x,z)*180)/PI;
+
  #include "std_msgs/Int16.h"
−
+
  #include "std_msgs/String.h" 
−
z_p = sqrt(pow(z,2)+pow(x,2));
+
  #include <signal.h>
−
+
  #include <termios.h>
−
L = sqrt(pow(z_p,2)+pow(y,2));
+
  #include <stdio.h>
−
+
 
−
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
  #define KEYCODE_Xmas 0x43
−
+
  #define KEYCODE_Xmenos 0x44
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
  #define KEYCODE_Ymas 0x41
−
+
  #define KEYCODE_Ymenos 0x42
−
beta_pp = atan2(y,z_p);
+
  #define KEYCODE_Zmas 0x77
−
   
+
  #define KEYCODE_Zmenos 0x73
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
  #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;
 +
 
 +
  }  
 
   
 
   
−
beta = ((beta_p+beta_a)*180)/PI;
+
void quit(int sig)
−
+
{
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
  tcsetattr(0, TCSANOW, &cooked);
 +
  ros::shutdown();
 +
  exit(0);
 +
}
 +
 
 +
void callback(const ros::TimerEvent&)
 +
{
 
   
 
   
−
delta_a = PI-(beta_a+gamma);
+
ros::NodeHandle n;  
−
+
 
−
gamma = (gamma*180)/PI;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
−
+
 
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));      
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
−
+
 
−
if (beta_pp > beta_p) {
+
ros::Time anterior;
−
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
−
}
+
brazo_fer::WriteServos teleop;
−
else {
+
brazo_fer::WriteServos teleop_pinza;
−
delta = ((delta_p+delta_a)*180)/PI;
+
 
−
+
if (::cont == 0)
−
if (isnan(delta)) {
+
    {
−
delta = ((PI+delta_a)*180)/PI;
+
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;
 
}
 
}
−
 
−
if (isnan(gamma)) // si no hay solución
 
−
{
 
−
coordenadas_correctas = 0;
 
−
}
 
−
 
−
brazo_fer::Servos brazo;
 
 
 
−
brazo.base = ((alfa+150)*1023)/300;
+
  char c;
−
brazo.arti1 = ((beta+60)*1023)/300;
 
−
brazo.arti2 = ((gamma-30)*1023)/300;
 
−
brazo.arti3 = ((delta-30)*1023)/300;
 
−
brazo.pinza = 511;
 
−
 
−
::vel.base = 400;
 
−
::vel.arti1 = 400;
 
−
::vel.arti2 = 400;
 
−
::vel.arti3 = 400;
 
−
::vel.pinza = abs(::move.pinza - ::p.pinza);
 
  
 +
  // 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("                      ARM CONTROLS");
 +
  puts("#####################################################");
 +
  puts(""); 
 +
  puts("Up arrow:___________ Move up gripper");
 +
  puts("Down arrow:_________ Move down gripper");
 +
  puts("Left arrow:_________ Move left gripper");
 +
  puts("Right arrow:________ Move right gripper");
 +
  puts("W key:______________ Move forward gripper");
 +
  puts("S key:______________ Move backward gripper");
 +
  puts("A key:______________ Open gripper");
 +
  puts("D key:______________ Close gripper");
 +
  puts("Q key:______________ Tilt up gripper");
 +
  puts("E key:______________ Tilt down gripper");               
 +
             
 +
 +
       
 +
    while (ros::ok())
 +
    {
 +
 +
    // get the next event from the keyboard 
 +
    if(read(0, &c, 1) < 0)
 +
    {
 +
      perror("read():");
 +
      exit(-1);
 +
    }
 
 
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
+
      if (c == KEYCODE_Xmas)
−
return brazo;
+
      {
−
}
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
else {
+
{
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
::punto.x = ::punto.x - 5;
−
::punto_0 = ::punto_1;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
::lambda = 0;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
return ::p;
+
{::punto.x = ::punto.x + 5;}
−
}  
+
anterior = ros::Time::now();
−
  }
+
}
−
 
+
      }
−
  void home()
+
      if (c == KEYCODE_Xmenos)
−
  {
+
      {
−
ros::NodeHandle n;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{    
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+
::punto.x = ::punto.x + 5;
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+
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)
−
if (::cont == 0)
+
{::punto.x = ::punto.x - 5;}
−
{
+
anterior = ros::Time::now();
−
+
}
−
::punto_0.x = 0;
+
      }
−
::punto_0.y = 0;
+
      if (c == KEYCODE_Ymas)
−
::punto_0.z = 0;
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
::punto_1 = ::punto_0;
+
{     
−
+
::punto.y = ::punto.y + 5;
−
::on.base = 1;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
::on.arti1 = 1;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
−
::on.arti2 = 1;
+
{::punto.y = ::punto.y - 5;}
−
::on.arti3 = 1;
+
anterior = ros::Time::now();
−
::on.pinza = 1;
+
}    
−
+
      }
−
::off.base = 0;
+
      if (c == KEYCODE_Ymenos)
−
::off.arti1 = 0;
+
      {
−
::off.arti2 = 0;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
::off.arti3 = 0;
+
{     
−
::off.pinza = 0;
+
::punto.y = ::punto.y - 5;
−
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
  brazo_fer::Servos mover;
+
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;}
−
geometry_msgs::Point home;
+
anterior = ros::Time::now();
−
+
}
−
home.x = 0;
+
      }
−
home.y = 85;
+
      if (c == KEYCODE_Zmas)
−
home.z = 50;
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
::move = inversa(home);
+
{    
−
+
::punto.z = ::punto.z + 5;
−
brazo_fer::WriteServos inicio;
+
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)
−
inicio.posicion = ::move;
+
{::punto.z = ::punto.z - 5;}
−
+
anterior = ros::Time::now();
−
inicio.velocidad = ::vel;
+
}
−
+
      }
−
inicio.par = ::on;
+
      if (c == KEYCODE_Zmenos)
−
+
      {
−
move_pub_.publish(inicio);
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
hand_pub_.publish(inicio);
+
{     
−
+
::punto.z = ::punto.z - 5;
−
::cont = ::cont + 1;
+
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();
−
  }
+
}
−
+
      }
−
  void posicion(const brazo_fer::ReadServos& pose)
+
      if (c == KEYCODE_Pabrir)
−
  {
+
      {
−
ros::NodeHandle n;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{     
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
+
::pinza.pinza = ::pinza.pinza - 5;
−
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
::p = pose.posicion;
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
−
        ::e = pose.estado;
+
anterior = ros::Time::now();
−
        ::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
 
−
{  
 
−
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;
 
−
 
−
//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_Pcerrar)
−
  }
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
  void punto(const geometry_msgs::Point& point)
+
{     
−
  {
+
::pinza.pinza = ::pinza.pinza + 5;
−
ros::NodeHandle n;
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
+
anterior = ros::Time::now();
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+
}
−
 
+
      }
−
::lambda = 0;   
+
      if (c == KEYCODE_PinclinarMas)
−
   
+
      {
−
        ::punto_0 = directa(::p);
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{     
−
::punto_1 = point;
+
::pinza_incli = ::pinza_incli + 5;
−
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
::destino = inversa(::punto_1);
+
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)
−
 
−
 
−
   int main(int argc, char **argv)
 
 
   {
 
   {
 
   
 
   
−
ros::init(argc, argv, "control_brazo");   
+
ros::init(argc, argv, "teleoperador_teclado");   
 
   
 
   
 
ros::NodeHandle n;
 
ros::NodeHandle n;
−
+
 
−
+
   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
−
   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 move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
−
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_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();   
 
ros::spin();   
Line 1,683: Line 1,761:
 
         return 0;
 
         return 0;
 
   }
 
   }
−
 
  
 
</syntaxhighlight>
 
</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:
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:  
  
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.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:
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
  
 
<syntaxhighlight>roscd brazo_fer
 
<syntaxhighlight>roscd brazo_fer
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
====Ejecutando el programa====
+
===Running the program===
  
−
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
+
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
  
 
<syntaxhighlight>roscore</syntaxhighlight>
 
<syntaxhighlight>roscore</syntaxhighlight>
  
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
  
 
<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:
+
We will execute the next command in a terminal in order to launch the control program:
  
−
<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);
+
----
−
+
{| style="border: solid 0px white; width: 100%"
−
ros::Timer timer = n.createTimer(ros::Duration(1), callback);
+
!Related articles
−
   
+
----
−
   
+
!Other articles
−
+
----
−
ros::spin(); 
+
|-
−
+
| valign="top" width="50%" |
−
        return 0;
 
−
  }
 
  
−
</syntaxhighlight>
+
[[Objects recognition and position calculation (webcam)]]<br/>
 +
[[MYRAbot's arm model for simulation (urdf+gazebo)]]<br/>
 +
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Voice control (sphinx+festival)]]<br/>
 +
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
  
−
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:
+
| valign="top" |
  
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>
+
[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
−
 
+
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
−
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:
+
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
  
−
<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>
 
−
 
−
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 | < go back to main]]
 
[[Mobile manipulation | < go back to main]]

Latest revision as of 11:07, 29 September 2014

< go back to main


Related articles
Other articles

Objects recognition and position calculation (webcam)
MYRAbot's arm model for simulation (urdf+gazebo)
MYRAbot model for simulation (urdf+gazebo)
Integration of MYRAbot in moveIt! (gazebo+moveIt!)
Voice control (sphinx+festival)
MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

CeRVaNTeS' arm and base control (maxon+epos2)
CeRVaNTeS' arm model for simulation (urdf+gazebo)
CeRVaNTeS model for simulation (urdf+gazebo)
Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)
CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)


Bioloid arm

The arm that we will use is that Carlos Rodríguez Hernández developed in the project MYRA Robot: Hardware Update . The main components are:

Photograph of Bioloid arm.
  • Arduino mega 2560 board.
  • 74LS241 chip (tree-state Buffer).
  • Voltage regulator circuit.
  • 5 serial servo motors Dynamixel AX-12A.
  • Mounting components of Bioloid kit.
  • Parts made for the gripper and to fix to MYRAbot.

Arduino IDE and rosserial

For the communication among arduino and ROS, we install arduino IDE and rosserial (package de ROS which include the package rosserial_arduino with the libraries for arduino. We will start to installing arduino IDE, for this, we will execute the next commands in a terminal: 

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

When the installation of arduino software ends, we will install the ROS package. For this, we will execute the next command in a terminal: 

sudo apt-get install ros-ROS_DISTRO-rosserial

When we will have installed arduino IDE and the stack rosserial, we must copy the libraries for the package rosserial_arduino to the sketchbook of arduino. For this, we will execute the next commands in a terminal: 

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

Creating a package for our programs

Previously, we must have created a workspace for our ROS distribution. We will create a package in order to compile and send our programs to arduino board. We will make a package named "arduino_fer" with the the necessary dependences for our programs, for this, we will execute the next commands in a terminal: 

cd ~/ros_workspace
roscreate-pkg arduino_fer rosserial_arduino std_msgs

We must change the content of the file "CMakeLists.txt" of our package for the next lines: 

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)

We will execute the next commands in a terminal in order to finalize the creation of the package: 

roscd arduino_fer
cmake .

First program (To whole power)

We will make the first program that publishes a topic named "cifra" and subscribe a topic named "resultado". When we publish a number in the topic "cifra" the program multiplies the number by itself and publishes the result in the topic "resultado". The code of the program is shown below: 

#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); 
}

We must add to the file "CMakeLists.txt" the lines indicated below in order in order to compile the program and send to arduino board: 

set(FIRMWARE_NAME potencia)

set(${FIRMWARE_NAME}_BOARD MODELO_NUESTRA_PLACA)   # Model of arduino board
set(${FIRMWARE_NAME}_SRCS src/potencia.cpp )
set(${FIRMWARE_NAME}_PORT /dev/ttyACM0)            # Serial port to upload
generate_ros_firmware(${FIRMWARE_NAME})

Here is set the name for the program, type of arduino board (uno, atmega328 o mega2560), name and address of the file, and serial port of the PC used to communicate with the board. We have to execute the next commands in a terminal: 

roscd arduino_fer
make potencia-upload

We must launch the ROS core in a terminal in order to allow the execution of the program, if it is not already launched. We will execute the next command in a terminal: 

roscore

In other terminal, we will execute the node that communicates ROS with the arduino board, we set the port that is used to communication. In order to know the port that is used, we can see the port used in the menu "Tools">"Serial port" of the software arduino IDE, the board must be plugged. In this case the port is "ttyACM0". 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute in other terminal the next command in order to publish a number in the topic "cifra": 

rostopic pub cifra std_msgs/Int16 NUMBER --once

We will execute in other terminal the next command in order to test that the program calculates the square of the published number in the topic "cifra" and publishes the result in the topic "resultado": 

rostopic echo resultado

Arduino and servo motors Dinamixel

In Savage Electronics we can find the hardware adaptations to communicate the arduino mega 2560 board with the servo motors Dynamixel AX-12A of ROBOTICS. Here also we can find the libraries for programming (DynamixelSerial).

Second program (Move)

This program is a test of the communications between arduino mega 2560 board and servo motor Dynamixel AX-12A. The program publishes the topic "angulo" which contains the position of the servo motor (between 0 and 1023) and subscribes the topic "giro" which receives the goal position (between 0 and 1023). The servo motor will move with a constant velocity of 128 (Integer between 0 and 1023). The code of the program is shown below: 

#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);
}

We can't use the previously made package in order to compile and send the program to the arduino board. We must use the arduino IDE software due to that we use a external library to ROS system. When the program was uploaded we will execute the next command in a terminal, to launch the core system: 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in other terminal in order to publish the goal position of the servo motor in the topic "giro": 

rostopic pub giro std_msgs/Int16 GOAL_POSITION --once

The servo motor will immediately move to the goal position. We can execute the next command in other terminal in order to see the position value of the servo motor at any time: 

rostopic echo angulo

Arm's kinematics model

We need to know the spatial position of the arm for the correct placement of the gripper. The position depends on the position angle of the servo motors (direct kinematics). We know the goal point and we must obtain the position angle of the servo motors to achieve it (inverse kinematics). The arm have 5 joints (1 to open-close gripper) and all work in the same plane, for this reason we don't need use the Denavit-Hartenberg parameters.

Diagrams for kinematics study

Inverse kinematic

We use the diagram shown above to obtain the inverse kinematics. Where x, y and z are the goal point coordinates, and ε is the angle that the gripper forms with x-z plane. The obtained equations are shown below:

Brazo ecuaciones.jpg

We have to calculate the value of position angle for the servo motors Dynamixel AX-12A according its turn limits (1023 positions throughout 300º):

Diagrama de ángulos servomotor Dynamixel AX-12A

Brazo ecuacion base.jpg We add 150º from the position angle in order to set the 0º coincident with z axis.

Brazo ecuacion arti1.jpg We add 60º from the position angle in order to set the 0º coincident with z axis.

Brazo ecuacion arti2.jpg We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.

Brazo ecuacion arti3.jpg We subtract 30º from the position angle in order to set the 0º coincident with the longitudinal axis of servo motor.

Direct kinematics

We can use the previous equations to obtain the placement point through the position angle of the servo motors. The equations are shown below:

Brazo ecuaciones directa.jpg

Arm workspace and boundaries

We need to set the boundaries of the position angles of the servo motors to avoid self collisions and collision with other parts of MYRAbot. We have made a spreadsheet using the equations for inverse and direct kinematics. The spreadsheet shows the spacial placement of the arm in two dispersion graphs.

Spreadsheet of inverse and direct kinematics, download

Control programs

Third program (Come here)

We place a interface program in arduino board that communicates the servo motor with the control programs in ROS. We need publish and subscribe the data of 5 servo motors so we will create own messages.

Creating custom messages in ROS

First, we will create a new package with the necessary dependences for our programs (std_msgs geometry_msgs roscpp). We will execute the next commands in a terminal: 

cd ~/ros_workspace
roscreate-pkg brazo_fer std_msgs geometry_msgs roscpp

We will create a folder named "msg" within the created package, here we place the next description files for our messages (data type and field name).

We will create the basis message that contains the data for each servo motor. We will create a file named "Servos.msg" with the content that is shown below: 

int16 base
int16 arti1
int16 arti2
int16 arti3
int16 pinza

We will use the previous message in order to define the data type. We will create a file named "WriteServos.msg" to send data to the servo motors (position, velocity and torque) with the content that is shown below: 

Servos posicion
Servos velocidad
Servos par

We will create a file named "ReadServos.msg" in order to receive data to the servo motors (position, status and current) with the content that is shown below: 

Servos posicion
Servos estado
Servos corriente

We have to edit the file "CMakeLists.tx" in our package in order to allow the creation the ROS messages. We will delete the symbol "#" in the line that is shown below: 

# rosbuild_gensrv()

We will execute the next commands in a terminal in order to compile and create the messages, where "brazo_fer" is the name of the created package: 

roscd brazo_fer
make

We will execute the next command in a terminal in order to add this messages to the arduino libraries in "rosserial": 

rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer

Arduino program

The program for arduino board subscribes the topic "move_arm" which receives the orders for the servo motors and publishes the "topic" "pose_arm" which sends the servo motors information. The code of the program is shown below: 

#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();

}

We will use arduino IDE software to compile and send the program to the board. We will use this program for the next control programs since it is a interface between the servo motors and ROS system.

ROS program

This program uses the equations of the inverse kinematics to set the position angle of the servo motors in order to reach the goal point that is within the workspace. The program subscribe the topic "point" which receives the goal point, also it subscribes the topic "pose_arm" which receives the information of servo motors, and publish the topic' "move_arm" which indicates to the arduino program the position, velocity and torque of for the servo motors. We will create a file named "control_v01.cpp" with the content that is shown below within the folder "src" of our package: 

  #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; 
 
	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, invalid coordinates or unattainable point"<<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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(contro_v01 src/control_v01.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer
make

Running the program

First we will execute the next command in a terminal in order to launch the ROS core, if it has not launched previously. 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in a terminal in order to launch the control program: 

rosrun brazo_fer control_v01

We will execute the next command in a terminal in order to publish in the topic "point" the goal point: 

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

The arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:

<videoflash>iwAcutO3C8g</videoflash>
Watch in YouTube

Fourth program (Grasp the bottle)

Include file with functions

We have created a file with the control fuctions for the arm in orther to simplify and make more legible the programs. We will create a file named "brazo_fer.h" within the folder "include/brazo_fer" of the created package with the content that is shown below: 

#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

Program

This program execute the sequence to grasp a little plastic bottle (the bottle is empty since the torque of the servo motors is low, we need more servo motors on each joint in order to pick more load). The sequence steps are shown below:

  1. Approach (place the gripper at 70 mm of the bottle position).
  2. Placement in the bottle position.
  3. To close the gripper (until to hold the bottle).
  4. Lifting and approach.

We will create a file named "control_v02.cpp" with the content that is shown below within the folder "src" of our package: 

  #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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(contro_v02 src/control_v02.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer
make

Running the program

First we will execute the next command in a terminal in order to launch the ROS core, if it has not launched previously. 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in a terminal in order to launch the control program: 

rosrun brazo_fer control_v02

We will execute the next command in a terminal in order to publish in the topic "pick_point" the bottle position (We have placed the bottle in a known position but we can use a vision system to set the bottle position): 

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

The arm will execute the pick sequence. We can see the arm grasping the bottle in the next video:

<videoflash>0FcB6-i23cU</videoflash>
Watch in YouTube

Fifth program (Place the bottle)

This program execute the sequence to place the bottle. The sequence steps are shown below:

  1. Placement in the goal position.
  2. To open the gripper (until to free the bottle).
  3. Distance (place the gripper at 70 mm of the goal position).
  4. Return to initial position.

We will create a file named "control_v03.cpp" with the content that is shown below within the folder "src" of our package: 

  #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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(contro_v03 src/control_v03.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer
make

Running the program

First we will execute the next command in a terminal in order to launch the ROS core, if it has not launched previously. 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in a terminal in order to launch the control program: 

rosrun brazo_fer control_v03

We will execute the next command in a terminal in order to publish in the topic "place_point" the goal position: 

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

Sixth program (Follow the straight way)

This program move the arm following the line between the initial position and the goal position. We will create a file named "control_v04.cpp" with the content that is shown below within the folder "src" of our package: 

  #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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(contro_v04 src/control_v04.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer
make

Running the program

First we will execute the next command in a terminal in order to launch the ROS core, if it has not launched previously. 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in a terminal in order to launch the control program: 

rosrun brazo_fer control_v04

We will execute the next command in a terminal in order to publish in the topic "point" the goal position: 

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

Seventh program (Teleoperator)

This program allows to control the arm using the keyboard of the remote PC. We will create a file named "teleoperador_teclado.cpp" with the content that is shown below within the folder "src" of our package: 

  #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("                      ARM CONTROLS");
  puts("#####################################################");
  puts("");  
  puts("Up arrow:___________ Move up gripper");
  puts("Down arrow:_________ Move down gripper");
  puts("Left arrow:_________ Move left gripper"); 
  puts("Right arrow:________ Move right gripper");
  puts("W key:______________ Move forward gripper");
  puts("S key:______________ Move backward gripper");
  puts("A key:______________ Open gripper");
  puts("D key:______________ Close gripper");
  puts("Q key:______________ Tilt up gripper");
  puts("E key:______________ Tilt down gripper");                 
              

        
    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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer
make

Running the program

First we will execute the next command in a terminal in order to launch the ROS core, if it has not launched previously. 

roscore

We will execute the next command in other terminal in order to launch the communication node ROS-arduino: 

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in a terminal in order to launch the control program: 

rosrun brazo_fer teleoperador_teclado


Related articles
Other articles

Objects recognition and position calculation (webcam)
MYRAbot's arm model for simulation (urdf+gazebo)
MYRAbot model for simulation (urdf+gazebo)
Integration of MYRAbot in moveIt! (gazebo+moveIt!)
Voice control (sphinx+festival)
MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

CeRVaNTeS' arm and base control (maxon+epos2)
CeRVaNTeS' arm model for simulation (urdf+gazebo)
CeRVaNTeS model for simulation (urdf+gazebo)
Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)
CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)


< go back to main

Retrieved from "https://robotica.unileon.es/index.php?title=MYRAbot%27s_arm_control_(bioloid%2barduino)&oldid=4214"