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

From robotica.unileon.es
Jump to: navigation, search
(→‎Running the program)
 
(45 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)==
  
 
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:
 
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:
Line 166: Line 195:
 
<syntaxhighlight>roscore</syntaxhighlight>
 
<syntaxhighlight>roscore</syntaxhighlight>
  
−
We will execute the next command in other terminal to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
+
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>
  
−
We will execute the next command in other terminal to publish the goal position of the servo motor in the ''topic'' "giro":
+
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 GOAL_POSITION --once</syntaxhighlight>
 
<syntaxhighlight>rostopic pub giro std_msgs/Int16 GOAL_POSITION --once</syntaxhighlight>
  
−
The servo motor will immediately move to the goal position. We can execute the next command in other terminal to see the position value of the servo motor at any time:
+
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>
  
−
==Arm's kinematics model==
+
=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 [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters Denavit-Hartenberg parameters].
 
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].
Line 184: Line 213:
 
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagrams for kinematics study]]
 
[[file:brazo_cinematica.jpg|thumb|500px|center|Diagrams for kinematics study]]
  
−
===Inverse kinematic===  
+
==Inverse kinematic==
  
 
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:
 
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:
Line 202: Line 231:
 
[[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.
 
[[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.
  
−
===Direct kinematics===
+
==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:
 
We can use the previous equations to obtain the placement point through the position angle of the servo motors. The equations are shown below:
Line 208: Line 237:
 
[[file:brazo_ecuaciones_directa.jpg]]
 
[[file:brazo_ecuaciones_directa.jpg]]
  
−
===Arm workspace and boundaries===
+
==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.
 
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.
Line 214: Line 243:
 
[[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]]]
 
[[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]]]
  
−
==Control programs==
+
=Control programs=
  
−
===Third program (Came here)===
+
==Third program (Come here)==
  
 
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.
 
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.
  
−
====Creating custom messages in ROS====
+
===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:
 +
 
 +
<syntaxhighlight>
 +
cd ~/ros_workspace
 +
roscreate-pkg brazo_fer std_msgs geometry_msgs roscpp</syntaxhighlight>
  
−
First, we will [[Fernando-TFM-ROS02#Creando un package|create a new ''package'']] with the necessary dependences for our programs (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 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:
 
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:
Line 232: Line 267:
 
int16 pinza</syntaxhighlight>
 
int16 pinza</syntaxhighlight>
  
−
We will use the previous message 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:
+
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 238: Line 273:
 
Servos par</syntaxhighlight>
 
Servos par</syntaxhighlight>
  
−
We will create a file named "ReadServos.msg" to receive data to the servo motors (position, status and current) with the content that is shown below:
+
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 244: Line 279:
 
Servos corriente</syntaxhighlight>
 
Servos corriente</syntaxhighlight>
  
−
We have to edit the file "CMakeLists.tx" in our ''package'' to allow the creation the [http://www.ros.org ROS] messages. We will delete the symbol "#" in the line that is shown below:
+
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>
  
−
We will execute the next commands in a terminal to compile and create the messages, where "brazo_fer" is the name of the created ''package'':
+
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
Line 257: Line 292:
 
<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>
 
<syntaxhighlight>rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer</syntaxhighlight>
  
−
====Arduino program====
+
===Arduino program===
  
 
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:
 
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:
Line 401: Line 436:
 
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.
 
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.
  
−
====ROS program====
+
===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 [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'':
 
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'':
Line 548: Line 583:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
We have to add the next line to the file "CMakeLists.txt" of our ''package'' to compile and create the executable file:
+
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>
  
−
We will execute the next commands in a terminal to compile and create the executable file:
+
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>
  
−
====Running the program====
+
===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.
 
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.
Line 571: Line 606:
 
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
 
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
  
−
We will execute the next command in a terminal in order to publish in the ''topic'' point the goal point:
+
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>
−
<!--
 
  
−
 
+
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:
−
Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el ''topic'' "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
 
−
 
 
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>
 
−
 
 
−
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:
 
  
 
<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 606: 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;
+
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p1, delta_p2, epsilon;
−
  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
 
−
 
 
−
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;
 
    
 
    
−
   void home()
+
   brazo_fer::WriteServos separar(geometry_msgs::Point lejos)
−
  {
+
   {
−
 
 
−
  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"
 
−
 
 
−
   
 
−
if (::cont == 0) {
 
−
 
−
::punto_0.x = 0;
 
−
::punto_0.y = 75;
 
−
::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;
 
−
 
−
if (::cont_3 > 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);
 
−
 
−
}
 
−
}
 
−
  }
 
−
 
 
−
  brazo_fer::Servos separar(geometry_msgs::Point lejos)
 
−
   {
 
 
   double x = lejos.x;
 
   double x = lejos.x;
 
   double z = lejos.z;  
 
   double z = lejos.z;  
Line 1,187: Line 1,187:
 
   lejos.x = lejos.x - 70*sin(atan2(x,z));
 
   lejos.x = lejos.x - 70*sin(atan2(x,z));
 
  
 
  
−
   brazo_fer::Servos move = inversa(lejos);
+
   brazo_fer::WriteServos move = inversa(lejos, ::inclinacion_pinza, ::pg, 0);
 
  
 
  
 
   return move;
 
   return move;
Line 1,193: Line 1,193:
 
   }
 
   }
 
    
 
    
−
   void posicion_estado_corriente(const brazo_fer::ReadServos& pec)   //Función de llamda con cada dato recibido
+
   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;  //Posición del servomotor
+
brazo_fer::Servos p = pec.posicion;   
  
−
::pg = pec.posicion;
+
::pg = pec.posicion;
 +
::cg = pec.corriente;
 
 
−
brazo_fer::Servos e = pec.estado;  //Estado del servomotor
+
brazo_fer::Servos e = pec.estado;   
 
 
−
brazo_fer::Servos c = pec.corriente;   //Posición del servomotor
+
brazo_fer::Servos c = pec.corriente;
 
 
−
home();
+
if (::cont == 0)
−
+
{
−
::cont = cont+1;
+
::punto_0 = directa(p, inclinacion_pinza);
−
 
 
−
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);
+
::punto_1 = ::punto_0;
−
::move_1 = inversa(::punto_0);
 
 
 
−
if (::error_coordenadas == 1) {
+
::cont = ::cont+1;
−
 
−
::punto_1 = ::punto_0;
 
 
}
 
}
−
else {
+
 +
if (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {
 +
 
 +
 
 +
 
 +
 
 
 
   if (::cont_1 == 0) {
 
   if (::cont_1 == 0) {
 
  
 
  
−
::move = inversa(::punto_0);
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
−
 
 
−
brazo_fer::WriteServos uno;
 
 
  
 
  
−
uno.posicion = ::move;
+
move_pub_.publish(::move);
−
 
 
−
uno.velocidad = ::vel;
 
−
 
 
−
uno.par = ::on;
 
−
 
 
−
move_pub_.publish(uno);
 
 
  
 
  
 
::cont_1 = ::cont_1+1;
 
::cont_1 = ::cont_1+1;
 
  
 
  
−
}
+
}
 
  
 
  
−
   if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
+
   if (((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) {
 
if (::cont_2 == 0) {
 
 
−
::pin.pinza = 511;
+
::pinza.pinza = 470;
 +
 +
::pin = control_pinza(::pinza, p, c);
 
 
−
brazo_fer::WriteServos dos;  
+
hand_pub_.publish(::pin);
−
 
 
−
dos.posicion = ::pin;
 
−
 
−
dos.velocidad.pinza = abs(::pin.pinza - p.pinza);
 
−
 
 
−
dos.par = ::on;
 
−
 
−
hand_pub_.publish(dos);
 
 
 
 
::cont_2 = ::cont_2+1;
 
::cont_2 = ::cont_2+1;
Line 1,265: Line 1,252:
 
 
 
 
−
  if (p.pinza <= 516 || p.pinza >= 506) {
+
  if (p.pinza <= 495) {
 
 
 
if (::cont_3 == 0) {
 
if (::cont_3 == 0) {
−
 
+
−
std::cout<<"Suelto"<<std::endl;
+
::move = separar(::punto_0);
−
 
−
                                                ::move = separar(::punto_0);
 
−
 
−
brazo_fer::WriteServos tres;  
 
−
 
−
tres.posicion = ::move;
 
−
 
−
tres.velocidad = ::vel;
 
−
 
−
tres.par = ::on;
 
 
 
−
move_pub_.publish(tres);
+
move_pub_.publish(::move);
 
 
 
::cont_3 = ::cont_3+1;
 
::cont_3 = ::cont_3+1;
Line 1,287: Line 1,264:
 
}
 
}
 
 
−
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))) {
+
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;
 
std::cout<<"Suelto"<<std::endl;
−
 
+
 
::cont = 0;
 
::cont = 0;
 
 
−
home();
 
−
 
−
::cont = ::cont + 1;
 
−
                                               
 
 
::cont_1 = 0;
 
::cont_1 = 0;
 
::cont_2 = 0;
 
::cont_2 = 0;
 +
::cont_3 = 0;
 
}
 
}
 
}
 
}
Line 1,308: Line 1,283:
 
}
 
}
 
}
 
}
−
}
+
 
 
 
   
 
   
 
   }
 
   }
 
   
 
   
−
   void punto(const geometry_msgs::Point& point)  //Función de llamda con cada dato recibido
+
   void punto(const geometry_msgs::Point& point)   
 
   {
 
   {
 
 
 
::punto_0 = point;
 
::punto_0 = point;
  
−
   }  
+
   }   
−
   
 
 
   
 
   
 
   int main(int argc, char **argv)
 
   int main(int argc, char **argv)
 
   {
 
   {
 
   
 
   
−
ros::init(argc, argv, "control_entrega");  //Creación del nodo "control_brazo"
+
ros::init(argc, argv, "control_entrega");   
 
   
 
   
 
ros::NodeHandle n;
 
ros::NodeHandle n;
 
 
 
  
 
  
−
   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);  //Suscripción del topic "pose_arm"
+
   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);   
 
  
 
  
−
   ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);  //Suscripción del topic "point"
+
   ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto);   
 +
 
  
 
  
−
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); 
  
 
 
−
ros::spin();   //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"
+
ros::spin();  
 
   
 
   
 
         return 0;
 
         return 0;
Line 1,346: Line 1,320:
 
</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_v03 src/control_v03.cpp)</syntaxhighlight>
 
<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:
+
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_v03", 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_v03</syntaxhighlight>
 
<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):
+
We will execute the next command in a terminal in order to publish in the ''topic'' "place_point" the goal position:
  
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
+
<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
  
−
===Sexto programa (Sigue el camino recto)===
+
==Sixth program (Follow the straight way)==
  
−
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:
+
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:  
  
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
Line 1,383: Line 1,357:
 
   #include "brazo_fer/WriteServos.h"   
 
   #include "brazo_fer/WriteServos.h"   
 
   #include "brazo_fer/ReadServos.h"
 
   #include "brazo_fer/ReadServos.h"
 +
  #include "brazo_fer/brazo_fer.h" 
 
   #include "geometry_msgs/Point.h"
 
   #include "geometry_msgs/Point.h"
−
   #include "math.h"
+
   #include "math.h"  
 
   
 
   
−
  #define PI 3.14159265
+
   brazo_fer::Servos p, e, c;
−
  #define L1 104
+
  brazo_fer::WriteServos destino, mover;
−
  #define L2 104
 
−
  #define Lp 60
 
−
 
−
 
−
   brazo_fer::Servos move, vel, p, e, c, destino, on, off;
 
 
   geometry_msgs::Point punto_0, punto_1, punto_i;
 
   geometry_msgs::Point punto_0, punto_1, punto_i;
 
   double lambda = 0;
 
   double lambda = 0;
−
   int cont = 0;
+
  int inclinacion_pinza = 0;
−
 
+
   int cont = 0;  
−
   geometry_msgs::Point directa(brazo_fer::Servos origen)
+
 +
   void posicion(const brazo_fer::ReadServos& pose)
 
   {
 
   {
−
+
ros::NodeHandle n;
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, epsilon;
+
−
double z_p;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
−
double L_a, L;
+
−
double x, y, z;
+
::p = pose.posicion;
−
+
        ::e = pose.estado;
−
alfa = (((origen.base*300)/1023)-150)*PI/180;
+
         ::c = pose.corriente;
−
beta = (((origen.arti1*300)/1023)-60)*PI/180;
+
   
−
gamma = (((origen.arti2*300)/1023)+30)*PI/180;
+
    if (cont == 0)
−
delta = (((origen.arti3*300)/1023)+30)*PI/180;
+
    {
−
 
+
::punto_0 = home(::p, ::c);
−
         epsilon = 0; /Ángulo de inclinación de la pinza respecto a la horizontal
+
   
−
+
::punto_1 = ::punto_0;
−
L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
+
−
+
cont = 1;
−
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);
 
−
 
−
        L = sqrt(pow(L_a,2)+pow(Lp,2)-2*L_a*Lp*cos(delta - delta_a));
 
−
 
−
z_p = L_a*cos(beta_p) + Lp*cos(epsilon);
 
−
 
 
−
        beta_pp = acos(z_p/L);
 
−
 
−
if (L1*sin(beta) >= L2*sin(beta + gamma) + Lp*sin(epsilon))
 
−
{
 
−
y = L*sin(beta_pp);
 
 
}
 
}
−
else
+
   
−
{
+
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)))
−
y = -L*sin(beta_pp);
+
{
−
}
+
   
−
+
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)))  
−
z = z_p*cos(alfa);
+
{
−
+
::punto_0 = ::punto_1;
−
x = z_p*sin(alfa);
+
::lambda = 0;
−
+
}
−
geometry_msgs::Point punto;
+
else
−
+
{   
−
punto.x = x;
+
geometry_msgs::Point u;
−
punto.y = y;
+
−
punto.z = z;
+
//ecuación de la recta p1 = p0+lambda.u
−
+
−
return punto;  
+
u.x = ::punto_1.x - ::punto_0.x;
−
  }
+
u.y = ::punto_1.y - ::punto_0.y;
−
 
+
u.z = ::punto_1.z - ::punto_0.z;      
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino)
+
−
  {
+
double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
−
 
+
 
−
double x = destino.x;
+
::lambda = ::lambda + 1/paso;
−
  double y = destino.y;
+
−
double z = destino.z;
+
::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);
 +
 +
}
  
−
int coordenadas_correctas = 1;
+
}
−
 
+
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
  }
−
double z_p;
 
−
double L_a, L;
 
 
   
 
   
−
epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal
+
  void punto(const geometry_msgs::Point& point)
 +
  {
 +
ros::NodeHandle n;
 
   
 
   
−
alfa = (atan2(x,z)*180)/PI;
+
  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);
 
   
 
   
−
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));
+
  int main(int argc, char **argv)
 +
  {
 
   
 
   
−
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
ros::init(argc, argv, "control_brazo");  
−
 
−
beta_pp = atan2(y,z_p);
 
 
   
 
   
−
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
ros::NodeHandle n;
 
   
 
   
−
beta = ((beta_p+beta_a)*180)/PI;
 
 
   
 
   
−
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);  
 
   
 
   
−
delta_a = PI-(beta_a+gamma);
+
  ros::Subscriber point_sub_= n.subscribe("point", 1, punto);  
 
   
 
   
−
gamma = (gamma*180)/PI;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 +
   
 
   
 
   
−
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));      
+
ros::spin();
 
   
 
   
−
if (beta_pp > beta_p) {
+
    return 0;
−
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;
+
</syntaxhighlight>
−
+
 
−
brazo.base = ((alfa+150)*1023)/300;
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:  
−
brazo.arti1 = ((beta+60)*1023)/300;
+
 
−
brazo.arti2 = ((gamma-30)*1023)/300;
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
−
brazo.arti3 = ((delta-30)*1023)/300;
+
 
−
brazo.pinza = 511;
+
We will execute the next commands in a terminal in order to compile and create the executable file:
−
+
 
−
::vel.base = 400;
+
<syntaxhighlight>roscd brazo_fer
−
::vel.arti1 = 400;
+
make</syntaxhighlight>
−
::vel.arti2 = 400;
+
 
−
::vel.arti3 = 400;
+
===Running the program===
−
::vel.pinza = abs(::move.pinza - ::p.pinza);
+
 
 +
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]:
−
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
+
 
−
return brazo;
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
−
}
+
 
−
else {
+
We will execute the next command in a terminal in order to launch the control program:
−
std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
+
 
−
::punto_0 = ::punto_1;
+
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
−
::lambda = 0;
+
 
−
return ::p;
+
We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal position:
−
}  
+
 
−
  }
+
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
−
 
+
 
−
  void home()
+
==Seventh program (Teleoperator)==
−
  {
+
 
−
ros::NodeHandle n;
+
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:  
−
+
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+
<syntaxhighlight lang="cpp" enclose="div">
−
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+
 
−
+
  #include "ros/ros.h"
−
if (::cont == 0)
+
  #include "brazo_fer/Servos.h"
−
{
+
  #include "brazo_fer/WriteServos.h" 
−
+
  #include "brazo_fer/ReadServos.h"
−
::punto_0.x = 0;
+
  #include "brazo_fer/brazo_fer.h" 
−
::punto_0.y = 0;
+
  #include "geometry_msgs/Point.h"
−
::punto_0.z = 0;
+
  #include "std_msgs/Int16.h"
−
+
  #include "std_msgs/String.h" 
−
::punto_1 = ::punto_0;
+
  #include <signal.h>
−
+
  #include <termios.h>
−
::on.base = 1;
+
  #include <stdio.h>
−
::on.arti1 = 1;
+
 
−
::on.arti2 = 1;
+
  #define KEYCODE_Xmas 0x43
−
::on.arti3 = 1;
+
  #define KEYCODE_Xmenos 0x44
−
::on.pinza = 1;
+
  #define KEYCODE_Ymas 0x41
−
+
  #define KEYCODE_Ymenos 0x42
−
::off.base = 0;
+
  #define KEYCODE_Zmas 0x77
−
::off.arti1 = 0;
+
  #define KEYCODE_Zmenos 0x73
−
::off.arti2 = 0;
+
  #define KEYCODE_Pabrir 0x61
−
::off.arti3 = 0;
+
  #define KEYCODE_Pcerrar 0x64
−
::off.pinza = 0;
+
  #define KEYCODE_PinclinarMas 0x65
−
+
  #define KEYCODE_PinclinarMenos 0x71 
−
  brazo_fer::Servos mover;
+
 
−
+
struct termios cooked, raw;  
−
geometry_msgs::Point home;
+
 
−
+
int cont = 0;
−
home.x = 0;
+
 
−
home.y = 85;
+
double retardo = 0.11;
−
home.z = 50;
+
 
−
+
  geometry_msgs::Point punto;
−
::move = inversa(home);
+
  brazo_fer::Servos vel;
−
+
  brazo_fer::Servos pinza, p, c;
−
brazo_fer::WriteServos inicio;
+
 
−
+
  int pinza_incli = 0;
−
inicio.posicion = ::move;
+
 
−
+
−
inicio.velocidad = ::vel;
+
void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  
−
+
   {  
−
inicio.par = ::on;
+
   
−
+
::p = pec.posicion;   
−
move_pub_.publish(inicio);
+
::c = pec.corriente;
−
hand_pub_.publish(inicio);
+
 
−
+
  }  
−
::cont = ::cont + 1;
 
−
 
−
}
 
−
 
 
−
  } 
 
−
 
−
  void posicion(const brazo_fer::ReadServos& pose)
 
−
   {
 
−
ros::NodeHandle n;
 
−
   
 
−
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
 
   
 
   
−
::p = pose.posicion;
+
void quit(int sig)
−
        ::e = pose.estado;
+
{
−
        ::c = pose.corriente;
+
  tcsetattr(0, TCSANOW, &cooked);
−
   
+
  ros::shutdown();
−
        home();
+
  exit(0);
−
   
+
}
−
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)))
+
 
−
{
+
void callback(const ros::TimerEvent&)
−
   
+
{
−
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)))  
+
−
{
+
ros::NodeHandle n; 
−
::punto_0 = ::punto_1;
+
 
−
::lambda = 0;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
−
}
+
 
−
else
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   
−
{   
+
 
−
geometry_msgs::Point u;
+
ros::Time anterior;
−
+
−
//ecuación de la recta p1 = p0+lambda.u
+
brazo_fer::WriteServos teleop;
−
+
brazo_fer::WriteServos teleop_pinza;
−
u.x = ::punto_1.x - ::punto_0.x;
+
 
−
u.y = ::punto_1.y - ::punto_0.y;
+
if (::cont == 0)
−
u.z = ::punto_1.z - ::punto_0.z;      
+
    {
−
+
anterior = ros::Time::now();
−
double paso = abs(sqrt(pow(u.x,2)+pow(u.y,2)+pow(u.z,2))/10);
+
−
 
+
::punto = home(::p, ::c);
−
::lambda = ::lambda + 1/paso;
+
−
+
::cont = 1;
−
::punto_i.x = ::punto_0.x + (::lambda)*u.x;
+
−
::punto_i.y = ::punto_0.y + (::lambda)*u.y;
+
::pinza.pinza = 511;
−
::punto_i.z = ::punto_0.z + (::lambda)*u.z;
+
 +
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())
 +
    {
 
 
−
//std::cout<<punto_0<<std::endl;
+
    // get the next event from the keyboard 
−
std::cout<<punto_i<<" : "<<lambda<<std::endl;
+
    if(read(0, &c, 1) < 0)
−
+
    {
−
::move = inversa(::punto_i);
+
      perror("read():");
−
+
      exit(-1);
−
brazo_fer::WriteServos mover;
+
     }
−
 
−
mover.posicion = ::move;
 
−
 
−
mover.velocidad = ::vel;
 
−
 
−
mover.par = ::on;
 
−
 
−
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); 
 
 
 
−
::punto_1 = point;
+
      if (c == KEYCODE_Xmas)
−
+
      {
−
::destino = inversa(::punto_1);
+
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)
−
  int main(int argc, char **argv)
+
{::punto.x = ::punto.x + 5;}
−
  {
+
anterior = ros::Time::now();
−
+
}
−
ros::init(argc, argv, "control_brazo");  
+
      }
−
+
      if (c == KEYCODE_Xmenos)
−
ros::NodeHandle n;
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
+
{     
−
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);  
+
::punto.x = ::punto.x + 5;
−
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
  ros::Subscriber point_sub_= n.subscribe("point", 1, punto);  
+
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;}
−
  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);  
+
}
−
   
+
      }
−
+
      if (c == KEYCODE_Ymas)
−
ros::spin();
+
      {
−
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
        return 0;
+
{     
−
  }
+
::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)
−
</syntaxhighlight>
+
{::punto.y = ::punto.y - 5;}
−
 
+
anterior = ros::Time::now();
−
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
+
}    
−
 
+
      }
−
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
+
      if (c == KEYCODE_Ymenos)
−
 
+
      {
−
Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
 
+
{     
−
<syntaxhighlight>roscd brazo_fer
+
::punto.y = ::punto.y - 5;
−
make</syntaxhighlight>
+
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)
−
====Ejecutando el programa====
+
{::punto.y = ::punto.y + 5;}
−
 
+
anterior = ros::Time::now();
−
Para ejecutar el programa debemos lanzar el núcleo de ROS, si no se encuentra iniciado, ejecutando en un terminal el siguiente comando:
+
}
−
 
+
      }
−
<syntaxhighlight>roscore</syntaxhighlight>
+
      if (c == KEYCODE_Zmas)
−
 
+
      {
−
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
 
+
{     
−
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
::punto.z = ::punto.z + 5;
−
 
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
−
Para ejecutar el programa de control llamado "control_v04", en un terminal nuevo ejecutaremos el siguiente comando:
+
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;}
−
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
+
anterior = ros::Time::now();
−
 
+
}
−
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:
+
      }
−
 
+
      if (c == KEYCODE_Zmenos)
−
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
+
      {
−
 
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
===Séptimo programa (Teleoperador)===
+
{     
−
 
+
::punto.z = ::punto.z - 5;
−
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:  
+
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)
−
<syntaxhighlight lang="cpp" enclose="div">
+
{::punto.z = ::punto.z + 5;}
−
 
+
anterior = ros::Time::now();
−
 
+
}
−
  #include "ros/ros.h"
+
      }
−
  #include "brazo_fer/Servos.h"
+
      if (c == KEYCODE_Pabrir)
−
  #include "brazo_fer/WriteServos.h" 
+
      {
−
  #include "brazo_fer/ReadServos.h"
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
  #include "geometry_msgs/Point.h"
+
{     
−
  #include "std_msgs/Int16.h"
+
::pinza.pinza = ::pinza.pinza - 5;
−
  #include <signal.h>
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
  #include <termios.h>
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
−
  #include <stdio.h>
+
anterior = ros::Time::now();
−
+
}
−
  #define PI 3.14159265
+
      }
−
  #define L1 104
+
      if (c == KEYCODE_Pcerrar)
−
  #define L2 104
+
      {
−
  #define Lp 60
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
 
+
{     
−
  #define KEYCODE_Xmas 0x43
+
::pinza.pinza = ::pinza.pinza + 5;
−
  #define KEYCODE_Xmenos 0x44
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
−
  #define KEYCODE_Ymas 0x41
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
−
  #define KEYCODE_Ymenos 0x42
+
anterior = ros::Time::now();
−
  #define KEYCODE_Zmas 0x77
+
}
−
  #define KEYCODE_Zmenos 0x73
+
      }
−
  #define KEYCODE_Pabrir 0x61
+
      if (c == KEYCODE_PinclinarMas)
−
  #define KEYCODE_Pcerrar 0x64
+
      {
−
  #define KEYCODE_PinclinarMas 0x65
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
−
  #define KEYCODE_PinclinarMenos 0x71 
+
{    
−
 
+
::pinza_incli = ::pinza_incli + 5;
−
struct termios cooked, raw;
+
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 cont = 0;
+
}
−
 
+
}
−
double retardo = 0.1;
+
−
 
+
  int main(int argc, char **argv)
−
   geometry_msgs::Point punto;
+
   {
−
  brazo_fer::Servos vel;
+
−
  brazo_fer::Servos pinza, p, c;
+
ros::init(argc, argv, "teleoperador_teclado");  
−
 
+
−
   int pinza_incli;
+
ros::NodeHandle n;
−
 
+
 
−
  brazo_fer::Servos on;
+
  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);  
 
 
−
  brazo_fer::Servos off;  
+
ros::Timer timer = n.createTimer(ros::Duration(1), callback); 
 +
   
 +
 +
ros::spin();
 
   
 
   
−
  brazo_fer::Servos inversa(geometry_msgs::Point destino, int inclinacion_pinza)
+
        return 0;
−
   {
+
   }
−
 
 
−
double x = destino.x;
 
−
  double y = destino.y;
 
−
double z = destino.z;
 
−
 
−
y = y + z*tan(atan2(45,250));
 
  
−
int coordenadas_correctas = 1;
+
</syntaxhighlight>
 +
 
 +
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(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>
 +
 
 +
We will execute the next commands in a terminal in order to compile and create the executable file:
 +
 
 +
<syntaxhighlight>roscd brazo_fer
 +
make</syntaxhighlight>
 +
 
 +
===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.
  
−
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
<syntaxhighlight>roscore</syntaxhighlight>
−
double z_p;
+
 
−
double L_a, L;
+
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]:
−
 
−
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;
+
<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:
−
 
−
  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);
 
−
}    
 
  
−
}
+
<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</syntaxhighlight>
−
}
 
−
 
−
  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"