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

From robotica.unileon.es
Jump to: navigation, search
(Fourth program (Grasp the bottle))
 
(19 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 49: Line 78:
 
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 116: Line 145:
 
<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 178: Line 207:
 
<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:
 
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:
Line 263: 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 407: 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 563: Line 592:
 
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 587: Line 616:
 
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Watch in YouTube]</center>
 
<center>[http://www.youtube.com/watch?v=iwAcutO3C8g Watch in YouTube]</center>
  
===Fourth program (Grasp the bottle)===
+
==Fourth program (Grasp the bottle)==
  
====Include file with functions====
+
===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:
 
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:
Line 872: Line 901:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
====Program====
+
===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:
 
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:
Line 885: Line 914:
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
  
   #include "ros/ros.h"  
+
   #include "ros/ros.h"
 
   #include "brazo_fer/Servos.h"
 
   #include "brazo_fer/Servos.h"
 
   #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 "std_msgs/Int16.h"
 
   #include "std_msgs/Int16.h"
 +
  #include "std_msgs/String.h" 
 
   #include "geometry_msgs/Point.h"
 
   #include "geometry_msgs/Point.h"
   #include "math.h"
+
   #include "math.h"  
 
 
  #define PI 3.14159265
 
  #define L1 104
 
  #define L2 104
 
  #define Lp 60
 
  #define Lb 60 
 
 
    
 
    
 
   geometry_msgs::Point punto_0,punto_1;
 
   geometry_msgs::Point punto_0,punto_1;
Line 904: Line 929:
 
   int cont_1 = 0;
 
   int cont_1 = 0;
 
   int cont_2 = 0;
 
   int cont_2 = 0;
   int error_coordenadas = 0;
+
   int inclinacion_pinza = 0;
 +
  int start = 0;
 
      
 
      
   brazo_fer::Servos pg;
+
   brazo_fer::Servos pg, cg;
   brazo_fer::Servos move, move_1, vel;
+
   brazo_fer::WriteServos move;
   brazo_fer::Servos pin;
+
   brazo_fer::Servos pinza;
 +
  brazo_fer::WriteServos pin
 +
  brazo_fer::WriteServos error_coordenadas;
 
    
 
    
   brazo_fer::Servos on;
+
   brazo_fer::WriteServos acercar(geometry_msgs::Point cerca)
+
  {
  brazo_fer::Servos off;
+
  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)
  brazo_fer::Servos inversa(geometry_msgs::Point destino)  
 
 
   {
 
   {
 
+
 
double x = destino.x;
+
  la.x = 0;
   double y = destino.y;
+
   la.y = la.y + 50;
double z = destino.z;
+
  la.z = 60;
+
 
y = y + z*tan(atan2(45,250));
+
  if (la.y < 75) {
 +
la.y = 75;
 +
}
 +
 
 +
  brazo_fer::WriteServos move = inversa(la, ::inclinacion_pinza, ::pg, 0);
 +
 
 +
  ::punto_0 = la;
 +
 
 +
  return move;
  
int coordenadas_correctas = 1;
+
  }
 
+
 
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec) 
double z_p;
+
  {
double L_a, L;
+
 
 +
ros::NodeHandle n;
 
   
 
   
epsilon = 0;  
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
+
 
alfa = (atan2(x,z)*180)/PI;
+
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
   
+
   
z_p = sqrt(pow(z,2)+pow(x,2));
+
brazo_fer::Servos p = pec.posicion;  
+
 
L = sqrt(pow(z_p,2)+pow(y,2));
+
::pg = pec.posicion;
+
::cg = pec.corriente;
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
+
brazo_fer::Servos e = pec.estado;  
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
 +
brazo_fer::Servos c = pec.corriente;  
 
 
beta_pp = atan2(y,z_p);
+
 
+
if (::cont == 0)
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
{
+
::punto_0 = home(p, c);
beta = ((beta_p+beta_a)*180)/PI;
+
+
::punto_1 = ::punto_0;
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;
+
::cont = ::cont+1;
brazo.arti1 = ((beta+60)*1023)/300;
 
brazo.arti2 = ((gamma-30)*1023)/300;
 
brazo.arti3 = ((delta-30)*1023)/300;
 
 
 
::vel.base = abs(brazo.base - ::pg.base)/5;
+
}
::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 (::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) {
return brazo;
+
 
}
+
 
else {
+
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
+
::error_coordenadas = 1;
+
  if (::cont_1 == 0) {
return ::pg;
+
 
}  
+
::move = acercar(::punto_0);
  }
+
 
 
+
move_pub_.publish(::move);
  void home()
+
 
  {
+
::cont_1 = ::cont_1+1;
 
 
  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); 
+
}
 +
   
 
  
 
  
   
+
  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 == 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;
 
 
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);
+
if (::cont_2 == 0) {
+
hand_pub_.publish(ini);
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
 
}
 
 
  }
 
 
 
  brazo_fer::Servos 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::Servos move = inversa(cerca);
+
move_pub_.publish(::move);
 
+
  return move;
+
::cont_2 = ::cont_2+1;
 
+
  }
+
 
+
}
    brazo_fer::Servos levantar_acercar(geometry_msgs::Point la)
+
else {
  {
+
 
+
  la.x = 0;
+
  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))) {
  la.y = la.y + 50;
+
  la.z = 60;
+
 
+
::pinza.pinza = p.pinza;
  if (la.y < 75) {
+
la.y = 75;
+
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 {
  brazo_fer::Servos move = inversa(la);
+
 
+
std::cout<<"En movimiento"<<std::endl;
  ::punto_0 = la;
+
}
 
+
  return move;
+
}
 
+
 +
 
 
   }
 
   }
 
+
   void posicion_estado_corriente(const brazo_fer::ReadServos& pec)   
+
   void punto(const geometry_msgs::Point& point)   
   {  
+
   {
 
 
 
ros::NodeHandle n;
 
ros::NodeHandle n;
 
   
 
   
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
+
   ros::Publisher 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);    
+
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"
   
+
brazo_fer::Servos p = pec.posicion;   //Posición del servomotor
+
::punto_0 = point;
  
::pg = pec.posicion;
+
  }
 +
 +
  int main(int argc, char **argv)
 +
  {
 +
 +
ros::init(argc, argv, "control_recogida");
 +
 +
ros::NodeHandle n;
 
 
brazo_fer::Servos e = pec.estado;  //Estado del servomotor
 
 
brazo_fer::Servos c = pec.corriente;  //Posición del servomotor
 
 
home();
 
 
::cont = ::cont+1;
 
 
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) && ::error_coordenadas != 1) {
 
 
::move_1 = acercar(::punto_0);
 
::move_1 = inversa(::punto_0);
 
 
if (::error_coordenadas == 1) {
 
 
::cont = 0;
 
home();
 
::cont = ::cont+1;
 
::punto_1 = ::punto_0;
 
}
 
else {
 
 
  if (::cont_1 == 0) {
 
 
  
 
  
::move = acercar(::punto_0);
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
 
  
 
  
brazo_fer::WriteServos uno;
+
  ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto);
 
  
 
  
uno.posicion = ::move;
+
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
+
 
uno.velocidad = ::vel;
+
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 
+
 
uno.par = ::on;
+
 
+
ros::spin();
move_pub_.publish(uno);
+
 
+
        return 0;
::cont_1 = ::cont_1+1;
+
  }
 
+
 
}
+
</syntaxhighlight>
 
+
 
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
+
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:  
+
 
if (::cont_2 == 0) {
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
+
 
::move = inversa(::punto_0);
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
+
 
brazo_fer::WriteServos dos;  
+
<syntaxhighlight>roscd brazo_fer
 
+
make</syntaxhighlight>
dos.posicion = ::move;
+
 
+
===Running the program===
dos.velocidad = ::vel;
+
 
 
+
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.
dos.par = ::on;
+
 
 
+
<syntaxhighlight>roscore</syntaxhighlight>
move_pub_.publish(dos);
+
 
+
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]:
::cont_2 = ::cont_2+1;
+
 
+
<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:
else {
+
 
+
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
+
 
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
+
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):
+
 
int pinza;
+
<syntaxhighlight>rostopic pub pick_point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>
+
 
pinza = p.pinza;
+
The arm will execute the pick sequence. We can see the arm grasping the bottle in the next video:
+
 
if(c.pinza < 300) {
+
<center><videoflash>0FcB6-i23cU</videoflash></center>
+
 
std::cout<<c.pinza<<std::endl;
+
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Watch in YouTube]</center>
+
 
pinza = pinza + 20;
+
==Fifth program (Place the bottle)==
 
::pin.pinza = pinza;
 
 
brazo_fer::WriteServos tres;  
 
 
 
tres.posicion = ::pin;
 
 
tres.velocidad.pinza = abs(::pin.pinza - p.pinza);
 
 
 
tres.par = ::on;
 
 
hand_pub_.publish(tres);
 
 
}
 
else {
 
 
std::cout<<"Hold"<<std::endl;
 
 
::move = levantar_acercar(::punto_0);
 
 
brazo_fer::WriteServos cuatro;  
 
 
 
cuatro.posicion = ::move;
 
 
cuatro.velocidad = ::vel;
 
 
cuatro.par = ::on;
 
 
move_pub_.publish(cuatro);
 
 
::punto_1 = ::punto_0;
 
 
::cont_1 = 0;
 
::cont_2 = 0;
 
}
 
}
 
}
 
 
}
 
else {
 
 
std::cout<<"Moving"<<std::endl;
 
}
 
}
 
}
 
 
 
 
  }
 
 
  void punto(const geometry_msgs::Point& point)  
 
  {
 
 
::punto_0 = point;
 
  
  }
+
This program execute the sequence to place the bottle. The sequence steps are shown below:
+
   
+
# Placement in the goal position.
  int main(int argc, char **argv)
+
# To open the gripper (until to free the bottle).
  {
+
# Distance (place the gripper at 70 mm of the goal position).
+
# Return to initial position.
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); 
+
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"
ros::spin();  
+
  #include "brazo_fer/Servos.h"
+
  #include "brazo_fer/WriteServos.h" 
        return 0;
+
  #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; 
  
</syntaxhighlight>
+
::pg = pec.posicion;
 
+
::cg = pec.corriente;
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_fer::Servos e = pec.estado; 
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>
+
 
+
brazo_fer::Servos c = pec.corriente; 
We will execute the next commands in a terminal in order to compile and create the executable file:  
+
 
+
if (::cont == 0)
<syntaxhighlight>roscd brazo_fer
+
{
make</syntaxhighlight>
+
::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 ) {
  
====Running the program====
 
  
First we will execute the next command in a terminal in order to launch the [http://www.ros.org ROS] core, if it has not launched previously.
 
  
<syntaxhighlight>roscore</syntaxhighlight>
+
 
+
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
+
  if (::cont_1 == 0) {
 
+
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
::move = inversa(::punto_0, ::inclinacion_pinza, ::pg, 0);
 
+
 
We will execute the next command in a terminal in order to launch the control program:
+
move_pub_.publish(::move);
 
+
 
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
+
::cont_1 = ::cont_1+1;
 
+
 
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):
+
 
+
 
<syntaxhighlight>rostopic pub pick_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))) {
 
+
The arm will execute the pick sequence. We can see the arm grasping the bottle in the next video:
+
if (::cont_2 == 0) {
 
+
<center><videoflash>0FcB6-i23cU</videoflash></center>
+
::pinza.pinza = 470;
 
+
<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Watch in YouTube]</center>
+
::pin = control_pinza(::pinza, p, c);
 
+
===Fifth program (Place the bottle)===
+
hand_pub_.publish(::pin);
 
+
This program execute the sequence to place the bottle. The sequence steps are shown below:
+
::cont_2 = ::cont_2+1;
   
+
# 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).
+
# Return to initial position.
+
 
+
  if (p.pinza <= 495) {
We will create a file named "control_v03.cpp" with the content that is shown below within the folder "src" of our package:  
+
 +
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;
  
<syntaxhighlight lang="cpp" enclose="div">
+
   }  
 
+
   
   #include "ros/ros.h" 
+
   int main(int argc, char **argv)
  #include "brazo_fer/Servos.h"
+
   {
  #include "brazo_fer/WriteServos.h" 
+
  #include "brazo_fer/ReadServos.h"  
+
ros::init(argc, argv, "control_entrega");   
  #include "std_msgs/Int16.h"
+
  #include "geometry_msgs/Point.h"
+
ros::NodeHandle n;
  #include "math.h"  
 
 
 
  #define PI 3.14159265
 
  #define L1 104
 
  #define L2 104
 
  #define Lp 60
 
  #define Lb 60 
 
 
 
  geometry_msgs::Point punto_0,punto_1;
 
 
 
   int cont = 0;
 
  int cont_1 = 0;
 
   int cont_2 = 0;
 
  int cont_3 = 0;
 
  int error_coordenadas = 0;
 
   
 
  brazo_fer::Servos pg;
 
  brazo_fer::Servos move, move_1, vel;
 
   brazo_fer::Servos pin;
 
 
 
  brazo_fer::Servos on;
 
 
 
  brazo_fer::Servos off;
+
 
 
+
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);
 
+
 
  brazo_fer::Servos inversa(geometry_msgs::Point destino)  
+
  ros::Subscriber point_sub_= n.subscribe("place_point", 1, punto)
  {
+
 
+
 
double x = destino.x;
+
ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
  double y = destino.y;
+
 
double z = destino.z;
+
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 +
 
 
 
y = y + z*tan(atan2(45,250));
+
ros::spin();
 +
 +
        return 0;
 +
  }
 +
 
 +
</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(contro_v03 src/control_v03.cpp)</syntaxhighlight>
 +
 
 +
We will execute the next commands in a terminal in order to compile and create the executable file:
  
int coordenadas_correctas = 1;
+
<syntaxhighlight>roscd brazo_fer
 +
make</syntaxhighlight>
  
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
===Running the program===
double z_p;
+
 
double L_a, L;
+
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.
+
 
epsilon = 0;
+
<syntaxhighlight>roscore</syntaxhighlight>
+
 
alfa = (atan2(x,z)*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]:
+
 
z_p = sqrt(pow(z,2)+pow(x,2));
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
 
L = sqrt(pow(z_p,2)+pow(y,2));
+
We will execute the next command in a terminal in order to launch the control program:
   
+
 
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>
 +
 
 +
We will execute the next command in a terminal in order to publish in the ''topic'' "place_point" the goal position:
 +
 
 +
<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
 +
 
 +
==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:
 +
 
 +
<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 "geometry_msgs/Point.h"
 +
  #include "math.h"
 
   
 
   
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
  brazo_fer::Servos  p, e, c;
+
  brazo_fer::WriteServos destino, mover; 
beta_pp = atan2(y,z_p);
+
  geometry_msgs::Point punto_0, punto_1, punto_i;
 +
  double lambda = 0;
 +
  int inclinacion_pinza = 0;
 +
  int cont = 0;  
 
   
 
   
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
  void posicion(const brazo_fer::ReadServos& pose)
 +
  {
 +
ros::NodeHandle n;
 
   
 
   
beta = ((beta_p+beta_a)*180)/PI;
+
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
 
   
 
   
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
::p = pose.posicion;
+
        ::e = pose.estado;
delta_a = PI-(beta_a+gamma);
+
        ::c = pose.corriente;
+
   
gamma = (gamma*180)/PI;
+
    if (cont == 0)
+
    {
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));      
+
::punto_0 = home(::p, ::c);
+
   
if (beta_pp > beta_p) {
+
::punto_1 = ::punto_0;
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
 +
cont = 1;
 
}
 
}
else {
+
   
delta = ((delta_p+delta_a)*180)/PI;
+
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 (isnan(delta)) {
+
   
delta = ((PI+delta_a)*180)/PI;
+
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);
 +
 +
}
 +
 
}
 
}
   
+
   
if (isnan(gamma))
 
{
 
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 - ::pg.base)/5;
 
::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) {
 
return brazo;
 
}
 
else {
 
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
 
::error_coordenadas = 1;
 
return ::pg;
 
}  
 
 
   }
 
   }
 
+
   void home()
+
   void punto(const geometry_msgs::Point& point)
   {  
+
   {
 
+
ros::NodeHandle n;
  ros::NodeHandle n;
 
 
   
 
   
 
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 
   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
 +
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
 
  
 
  
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1); 
+
::lambda = 0;  
 
+
   
   
+
        ::punto_0 = directa(::p, ::inclinacion_pinza);
if (::cont == 0) {
+
+
::punto_1 = point;
::punto_0.x = 0;
+
::punto_0.y = 75;
+
::destino = inversa(::punto_1, ::inclinacion_pinza, ::p, 0);
::punto_0.z = 50;
+
+
  }  
::on.base = 1;
+
::on.arti1 = 1;
+
::on.arti2 = 1;
+
   int main(int argc, char **argv)
::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 z = lejos.z;
+
ros::init(argc, argv, "control_brazo");   
 
+
  lejos.z = lejos.z - 70*cos(atan2(x,z));
 
  lejos.x = lejos.x - 70*sin(atan2(x,z));
 
 
 
  brazo_fer::Servos move = inversa(lejos);
 
 
 
  return move;
 
 
 
  }
 
 
 
  void posicion_estado_corriente(const brazo_fer::ReadServos& pec) 
 
   {
 
 
 
 
ros::NodeHandle n;
 
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 move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
 
+
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  
   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);    
 
 
      
 
      
brazo_fer::Servos p = pec.posicion;   
+
 +
ros::spin();
 +
 +
    return 0;
 +
   }
  
::pg = pec.posicion;
+
</syntaxhighlight>
+
 
brazo_fer::Servos e = pec.estado; 
+
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_fer::Servos c = pec.corriente; 
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v04 src/control_v04.cpp)</syntaxhighlight>
+
 
home();
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
+
 
::cont = cont+1;
+
<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.
  
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z ) && ::error_coordenadas != 1) {
+
<syntaxhighlight>roscore</syntaxhighlight>
+
 
::move_1 = separar(::punto_0);
+
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]:
::move_1 = inversa(::punto_0);
+
 
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
if (::error_coordenadas == 1) {
 
 
::punto_1 = ::punto_0;
 
}
 
else {
 
 
  if (::cont_1 == 0) {
 
 
 
::move = inversa(::punto_0);
 
 
 
brazo_fer::WriteServos uno;
 
 
 
uno.posicion = ::move;
 
 
 
uno.velocidad = ::vel;
 
 
 
uno.par = ::on;
 
 
 
move_pub_.publish(uno);
 
 
 
::cont_1 = ::cont_1+1;
 
 
 
}
 
 
 
  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
 
 
if (::cont_2 == 0) {
 
 
::pin.pinza = 511;
 
 
brazo_fer::WriteServos dos;  
 
 
 
dos.posicion = ::pin;
 
 
dos.velocidad.pinza = abs(::pin.pinza - p.pinza);
 
 
 
dos.par = ::on;
 
 
hand_pub_.publish(dos);
 
 
::cont_2 = ::cont_2+1;
 
 
}
 
else {
 
 
 
  if (p.pinza <= 516 || p.pinza >= 506) {
 
 
if (::cont_3 == 0) {
 
  
std::cout<<"Free"<<std::endl;
+
We will execute the next command in a terminal in order to launch the control program:
+
 
                                                ::move = separar(::punto_0);
+
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
+
 
brazo_fer::WriteServos tres;  
+
We will execute the next command in a terminal in order to publish in the ''topic'' "point" the goal position:
+
 
tres.posicion = ::move;
+
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 150, y: 100, z: 0}' --once</syntaxhighlight>
+
 
tres.velocidad = ::vel;
+
==Seventh program (Teleoperator)==
+
 
tres.par = ::on;
+
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:  
+
 
move_pub_.publish(tres);
+
<syntaxhighlight lang="cpp" enclose="div">
 
::cont_3 = ::cont_3+1;
 
 
}
 
 
else if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {
 
std::cout<<"Free"<<std::endl;
 
  
::cont = 0;
+
  #include "ros/ros.h"
+
  #include "brazo_fer/Servos.h"
home();
+
  #include "brazo_fer/WriteServos.h" 
+
  #include "brazo_fer/ReadServos.h"
::cont = ::cont + 1;
+
   #include "brazo_fer/brazo_fer.h"  
                                               
+
   #include "geometry_msgs/Point.h"
::cont_1 = 0;
+
   #include "std_msgs/Int16.h"
::cont_2 = 0;
+
   #include "std_msgs/String.h" 
}
+
  #include <signal.h>
}
+
  #include <termios.h>
}
+
  #include <stdio.h>
+
    
}
+
  #define KEYCODE_Xmas 0x43
else {
+
  #define KEYCODE_Xmenos 0x44
+
   #define KEYCODE_Ymas 0x41
std::cout<<"Moving"<<std::endl;
+
   #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  
   
+
 
   void punto(const geometry_msgs::Point& point)    
+
struct termios cooked, raw;  
   {
 
 
::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);  
+
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;
 +
 
 
 
ros::spin();   
+
void posicion_estado_corriente(const brazo_fer::ReadServos& pec)
 +
  {
 +
   
 +
::p = pec.posicion; 
 +
::c = pec.corriente;
 +
 
 +
  }    
 
   
 
   
        return 0;
+
void quit(int sig)
   }
+
{
 +
  tcsetattr(0, TCSANOW, &cooked);
 +
   ros::shutdown();
 +
  exit(0);
 +
}  
  
</syntaxhighlight>
+
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);   
  
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:  
+
ros::Time anterior;
 +
 +
brazo_fer::WriteServos teleop;
 +
brazo_fer::WriteServos teleop_pinza;
  
<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v03 src/control_v03.cpp)</syntaxhighlight>
+
if (::cont == 0)
 
+
    {
We will execute the next commands in a terminal in order to compile and create the executable file:
+
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;
  
<syntaxhighlight>roscd brazo_fer
+
  // get the console in raw mode                                                             
make</syntaxhighlight>
+
  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);
  
====Running the program====
+
  puts("");
 
+
  puts("#####################################################");
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.
+
  puts("                      ARM CONTROLS");
 
+
  puts("#####################################################");
<syntaxhighlight>roscore</syntaxhighlight>
+
  puts(""); 
 
+
  puts("Up arrow:___________ Move up gripper");
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]:
+
  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");               
 +
             
  
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
+
       
 
+
    while (ros::ok())
We will execute the next command in a terminal in order to launch the control program:
+
    {
 
+
<syntaxhighlight>rosrun brazo_fer control_v03</syntaxhighlight>
+
    // get the next event from the keyboard 
 
+
    if(read(0, &c, 1) < 0)
We will execute the next command in a terminal in order to publish in the ''topic'' "place_point" the goal position:
+
    {
 
+
      perror("read():");
<syntaxhighlight>rostopic pub place_point geometry_msgs/Point '{x: 150, y: 0, z: 150}' --once</syntaxhighlight>
+
      exit(-1);
 
+
    }
===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:  
 
 
 
<syntaxhighlight lang="cpp" enclose="div">
 
 
 
  #include "ros/ros.h"
 
  #include "brazo_fer/Servos.h"
 
  #include "brazo_fer/WriteServos.h" 
 
  #include "brazo_fer/ReadServos.h"
 
  #include "geometry_msgs/Point.h"
 
  #include "math.h"
 
 
  #define PI 3.14159265
 
  #define L1 104
 
  #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;
 
  double lambda = 0;
 
  int cont = 0;
 
 
 
  geometry_msgs::Point directa(brazo_fer::Servos origen)
 
  {
 
 
 
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, epsilon;
+
      if (c == KEYCODE_Xmas)
double z_p;
+
      {
double L_a, L;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
double x, y, z;
+
{
+
::punto.x = ::punto.x - 5;
alfa = (((origen.base*300)/1023)-150)*PI/180;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
beta = (((origen.arti1*300)/1023)-60)*PI/180;
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
gamma = (((origen.arti2*300)/1023)+30)*PI/180;
+
{::punto.x = ::punto.x + 5;}
delta = (((origen.arti3*300)/1023)+30)*PI/180;
+
anterior = ros::Time::now();
 
+
}
        epsilon = 0;
+
      }
+
      if (c == KEYCODE_Xmenos)
L_a = sqrt(pow(L1,2)+pow(L2,2)-2*L1*L2*cos(gamma));
+
      {
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
{     
 
+
::punto.x = ::punto.x + 5;
        beta_p = beta - beta_a;
+
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)  
delta_a = PI - (beta_a + gamma);
+
{::punto.x = ::punto.x - 5;}
+
anterior = ros::Time::now();
        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);
+
      if (c == KEYCODE_Ymas)
 
+
      {
        beta_pp = acos(z_p/L);
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
+
{     
if (L1*sin(beta) >= L2*sin(beta + gamma) + Lp*sin(epsilon))  
+
::punto.y = ::punto.y + 5;
{
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
y = L*sin(beta_pp);
+
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;}
else
+
anterior = ros::Time::now();
{
+
}    
y = -L*sin(beta_pp);
+
      }
}
+
      if (c == KEYCODE_Ymenos)
+
      {
z = z_p*cos(alfa);
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
+
{    
x = z_p*sin(alfa);
+
::punto.y = ::punto.y - 5;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
geometry_msgs::Point punto;
+
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;}
punto.x = x;
+
anterior = ros::Time::now();
punto.y = y;
+
}
punto.z = z;
+
      }
+
      if (c == KEYCODE_Zmas)
return punto;  
+
      {
  }  
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
+
{     
  brazo_fer::Servos inversa(geometry_msgs::Point destino)  
+
::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) 
double x = destino.x;
+
{::punto.z = ::punto.z - 5;}
  double y = destino.y;
+
anterior = ros::Time::now();
double z = destino.z;
+
}
 
+
      }
int coordenadas_correctas = 1;
+
      if (c == KEYCODE_Zmenos)
 
+
      {
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
double z_p;
+
{    
double L_a, L;
+
::punto.z = ::punto.z - 5;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
epsilon = 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;}
alfa = (atan2(x,z)*180)/PI;
+
anterior = ros::Time::now();
+
}
z_p = sqrt(pow(z,2)+pow(x,2));
+
      }
+
      if (c == KEYCODE_Pabrir)
L = sqrt(pow(z_p,2)+pow(y,2));
+
      {
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
+
{     
+
::pinza.pinza = ::pinza.pinza - 5;
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
beta_pp = atan2(y,z_p);
+
anterior = ros::Time::now();
+
}
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
+
      }
+
      if (c == KEYCODE_Pcerrar)
beta = ((beta_p+beta_a)*180)/PI;
+
      {
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
+
{     
+
::pinza.pinza = ::pinza.pinza + 5;
delta_a = PI-(beta_a+gamma);
+
teleop_pinza = control_pinza(::pinza, ::p, ::c);
+
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
gamma = (gamma*180)/PI;
+
anterior = ros::Time::now();
+
}
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     
+
      }
+
      if (c == KEYCODE_PinclinarMas)
if (beta_pp > beta_p) {
+
      {
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
}
+
{    
else {
+
::pinza_incli = ::pinza_incli + 5;
delta = ((delta_p+delta_a)*180)/PI;
+
teleop = inversa(::punto, ::pinza_incli, ::p, 0);
+
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)  
if (isnan(delta)) {
+
{::pinza_incli = ::pinza_incli - 5;}
delta = ((PI+delta_a)*180)/PI;
+
anterior = ros::Time::now();
 
}
 
}
}
+
      }
+
      if (c == KEYCODE_PinclinarMenos)
if (isnan(gamma))
+
      {
{
+
if (ros::Time::now() > anterior + ros::Duration(::retardo))
coordenadas_correctas = 0;  
+
{    
}
+
::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);
 +
}    
  
brazo_fer::Servos brazo;
+
}
+
}
brazo.base = ((alfa+150)*1023)/300;
+
brazo.arti1 = ((beta+60)*1023)/300;
+
  int main(int argc, char **argv)
brazo.arti2 = ((gamma-30)*1023)/300;
+
  {
brazo.arti3 = ((delta-30)*1023)/300;
+
brazo.pinza = 511;
+
ros::init(argc, argv, "teleoperador_teclado");   
+
::vel.base = 400;
 
::vel.arti1 = 400;
 
::vel.arti2 = 400;
 
::vel.arti3 = 400;
 
::vel.pinza = abs(::move.pinza - ::p.pinza);
 
 
 
 
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {
 
return brazo;
 
}
 
else {
 
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
 
::punto_0 = ::punto_1;
 
::lambda = 0;
 
return ::p;
 
}    
 
  }
 
 
 
  void home()
 
  {
 
 
ros::NodeHandle n;
 
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::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);  
+
ros::spin();
  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+
+
        return 0;
if (::cont == 0)
+
  }
{
+
 
+
</syntaxhighlight>
::punto_0.x = 0;
+
 
::punto_0.y = 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:  
::punto_0.z = 0;
+
 
+
<syntaxhighlight lang=cmake>rosbuild_add_executable(teleoperador_teclado src/teleoperador_teclado.cpp)</syntaxhighlight>
::punto_1 = ::punto_0;
+
 
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
::on.base = 1;
+
 
::on.arti1 = 1;
+
<syntaxhighlight>roscd brazo_fer
::on.arti2 = 1;
+
make</syntaxhighlight>
::on.arti3 = 1;
+
 
::on.pinza = 1;
+
===Running the program===
+
 
::off.base = 0;
+
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.
::off.arti1 = 0;
+
 
::off.arti2 = 0;
+
<syntaxhighlight>roscore</syntaxhighlight>
::off.arti3 = 0;
+
 
::off.pinza = 0;
+
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]:
+
 
  brazo_fer::Servos mover;
+
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
geometry_msgs::Point home;
 
 
home.x = 0;
 
home.y = 85;
 
home.z = 50;
 
 
::move = inversa(home);
 
 
brazo_fer::WriteServos inicio;
 
 
inicio.posicion = ::move;
 
 
inicio.velocidad = ::vel;
 
 
inicio.par = ::on;
 
 
move_pub_.publish(inicio);
 
hand_pub_.publish(inicio);
 
 
::cont = ::cont + 1;
 
 
}
 
 
 
  } 
 
 
  void posicion(const brazo_fer::ReadServos& pose)
 
  {
 
ros::NodeHandle n;
 
 
  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
 
 
::p = pose.posicion;
 
        ::e = pose.estado;
 
        ::c = pose.corriente;
 
   
 
        home();
 
   
 
if ((::punto_0.x != ::punto_1.x || ::punto_0.y != ::punto_1.y || ::punto_0.z != ::punto_1.z) && ((::p.base-15) < ::move.base && ::move.base < (::p.base+15)) && ((::p.arti1-15) < ::move.arti1 && ::move.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::move.arti2 && ::move.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::move.arti3 && ::move.arti3 < (::p.arti3+15)))
 
{
 
   
 
if (((::p.base-15) < ::destino.base && ::destino.base < (::p.base+15)) && ((::p.arti1-15) < ::destino.arti1 && ::destino.arti1 < (::p.arti1+15)) && ((::p.arti2-15) < ::destino.arti2 && ::destino.arti2 < (::p.arti2+15)) && ((::p.arti3-15) < ::destino.arti3 && ::destino.arti3 < (::p.arti3+15)))
 
{
 
::punto_0 = ::punto_1;
 
::lambda = 0;
 
}
 
else
 
{   
 
geometry_msgs::Point 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;
 
 
 
::move = inversa(::punto_i);
 
 
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;
 
 
::destino = inversa(::punto_1);
 
 
  }
 
 
 
  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 will execute the next command in a terminal in order to launch the control program:
  
</syntaxhighlight>
+
<syntaxhighlight>rosrun brazo_fer teleoperador_teclado</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(contro_v04 src/control_v04.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.
 
 
 
<syntaxhighlight>roscore</syntaxhighlight>
 
 
 
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
 
 
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
 
 
We will execute the next command in a terminal in order to launch the control program:
 
 
 
<syntaxhighlight>rosrun brazo_fer control_v04</syntaxhighlight>
 
 
 
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>
 
 
 
===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:
 
 
 
<syntaxhighlight lang="cpp" enclose="div">
 
 
 
 
 
  #include "ros/ros.h"
 
  #include "brazo_fer/Servos.h"
 
  #include "brazo_fer/WriteServos.h" 
 
  #include "brazo_fer/ReadServos.h"
 
  #include "geometry_msgs/Point.h"
 
  #include "std_msgs/Int16.h"
 
  #include <signal.h>
 
  #include <termios.h>
 
  #include <stdio.h>
 
 
  #define PI 3.14159265
 
  #define L1 104
 
  #define L2 104
 
  #define Lp 60
 
 
 
  #define KEYCODE_Xmas 0x43
 
  #define KEYCODE_Xmenos 0x44
 
  #define KEYCODE_Ymas 0x41
 
  #define KEYCODE_Ymenos 0x42
 
  #define KEYCODE_Zmas 0x77
 
  #define KEYCODE_Zmenos 0x73
 
  #define KEYCODE_Pabrir 0x61
 
  #define KEYCODE_Pcerrar 0x64
 
  #define KEYCODE_PinclinarMas 0x65
 
  #define KEYCODE_PinclinarMenos 0x71 
 
 
 
struct termios cooked, raw;
 
 
 
int cont = 0;
 
 
 
double retardo = 0.1;
 
 
 
  geometry_msgs::Point punto;
 
  brazo_fer::Servos vel;
 
  brazo_fer::Servos pinza, p, c;
 
 
 
  int pinza_incli;
 
 
 
  brazo_fer::Servos on;
 
 
  brazo_fer::Servos off;
 
 
  brazo_fer::Servos inversa(geometry_msgs::Point destino, int inclinacion_pinza)
 
  {
 
 
 
double x = destino.x;
 
  double y = destino.y;
 
double z = destino.z;
 
 
y = y + z*tan(atan2(45,250));
 
 
 
int coordenadas_correctas = 1;
 
 
 
double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;
 
double z_p;
 
double L_a, L;
 
 
epsilon = (inclinacion_pinza*PI)/180;
 
 
alfa = (atan2(x,z)*180)/PI;
 
 
z_p = sqrt(pow(z,2)+pow(x,2));
 
 
L = sqrt(pow(z_p,2)+pow(y,2));
 
 
L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));
 
 
beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));
 
 
beta_pp = atan2(y,z_p);
 
 
beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));
 
 
beta = ((beta_p+beta_a)*180)/PI;
 
 
gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));
 
 
delta_a = PI-(beta_a+gamma);
 
 
gamma = (gamma*180)/PI;
 
 
        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));   
 
 
if (beta_pp > beta_p) {
 
delta = ((2*PI-(delta_p-delta_a))*180)/PI;
 
}
 
else {
 
delta = ((delta_p+delta_a)*180)/PI;
 
 
if (isnan(delta)) {
 
delta = ((PI+delta_a)*180)/PI;
 
}
 
}
 
 
 
if (isnan(gamma)) // si no hay solución
 
{
 
coordenadas_correctas = 0;
 
}
 
 
brazo_fer::Servos brazo;
 
 
brazo.base = ((alfa+150)*1023)/300;
 
brazo.arti1 = ((beta+60)*1023)/300;
 
brazo.arti2 = ((gamma-30)*1023)/300;
 
brazo.arti3 = ((delta-30)*1023)/300;
 
 
::vel.base = abs(brazo.base - ::p.base)/5;
 
::vel.arti1 = abs(brazo.arti1 - ::p.arti1)/5;
 
::vel.arti2 = abs(brazo.arti2 - ::p.arti2)/5;
 
::vel.arti3 = abs(brazo.arti3 - ::p.arti3)/5;
 
 
 
std::cout<<brazo<<std::endl;
 
 
if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && (153 && brazo.arti3 <= 870)) {
 
return brazo;
 
}
 
else {
 
std::cout<<"Error, invalid coordinates or unattainable point"<<std::endl;
 
 
 
return ::p;
 
}  
 
  }
 
 
  brazo_fer::Servos  control_pinza(brazo_fer::Servos abrir_cerrar)
 
  {
 
 
if (abrir_cerrar.pinza >= 480 && abrir_cerrar.pinza <= 680 && ::c.pinza <= 300)
 
{
 
return abrir_cerrar;
 
}
 
else
 
{
 
std::cout<<"Gripper limit reached"<<std::endl;
 
 
return ::p;
 
}
 
  }
 
 
  void home()
 
  {
 
 
 
  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); 
 
 
 
 
::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) 
 
  {
 
   
 
::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();
 
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("                      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.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::punto.x = ::punto.x + 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Xmenos)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::punto.x = ::punto.x + 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::punto.x = ::punto.x - 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Ymas)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::punto.y = ::punto.y + 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::punto.y = ::punto.y - 5;}
 
anterior = ros::Time::now();
 
}    
 
      }
 
      if (c == KEYCODE_Ymenos)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::punto.y = ::punto.y - 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::punto.y = ::punto.y + 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Zmas)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::punto.z = ::punto.z + 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3) 
 
{::punto.z = ::punto.z - 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Zmenos)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::punto.z = ::punto.z - 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::punto.z = ::punto.z + 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Pabrir)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::pinza.pinza = ::pinza.pinza - 5;
 
teleop_pinza.posicion = control_pinza(::pinza);
 
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza + 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_Pcerrar)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::pinza.pinza = ::pinza.pinza + 5;
 
teleop_pinza.posicion = control_pinza(::pinza);
 
if (teleop_pinza.posicion.pinza == ::p.pinza) {::pinza.pinza = ::pinza.pinza - 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_PinclinarMas)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::pinza_incli = ::pinza_incli + 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::pinza_incli = ::pinza_incli - 5;}
 
anterior = ros::Time::now();
 
}
 
      }
 
      if (c == KEYCODE_PinclinarMenos)
 
      {
 
if (ros::Time::now() > anterior + ros::Duration(::retardo))
 
{     
 
::pinza_incli = ::pinza_incli - 5;
 
teleop.posicion = inversa(::punto, ::pinza_incli);
 
if (teleop.posicion.base == ::p.base && teleop.posicion.arti1 == ::p.arti1 && teleop.posicion.arti2 == ::p.arti2 && teleop.posicion.arti3 == ::p.arti3)
 
{::pinza_incli = ::pinza_incli + 5;}
 
anterior = ros::Time::now();
 
}
 
      }                                   
 
 
if (teleop.posicion.base != ::p.base || teleop.posicion.arti1 != ::p.arti1 || teleop.posicion.arti2 != ::p.arti2 || teleop.posicion.arti3 != ::p.arti3)
 
{
 
teleop.par = ::on;
 
teleop.velocidad = ::vel;
 
move_pub_.publish(teleop);
 
}
 
if (teleop_pinza.posicion.pinza != ::p.pinza)
 
{
 
teleop_pinza.par = ::on;
 
teleop_pinza.velocidad.pinza = 200;
 
hand_pub_.publish(teleop_pinza);
 
}    
 
 
 
}
 
}
 
 
  int main(int argc, char **argv)
 
  {
 
 
ros::init(argc, argv, "teleoperador_teclado"); 
 
 
ros::NodeHandle n;
 
 
 
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);     
 
 
 
  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);
+
----
+
{| 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)]]
  
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:
+
| 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/>
 +
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
  
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.
 
 
<syntaxhighlight>roscore</syntaxhighlight>
 
 
We will execute the next command in other terminal in order to launch the communication node [http://www.ros.org ROS]-[http://www.arduino.cc arduino]:
 
 
<syntaxhighlight>rosrun rosserial_python serial_node.py /dev/ttyACM0</syntaxhighlight>
 
 
We will execute the next command in a terminal in order to launch the control program:
 
 
<syntaxhighlight>rosrun brazo_fer 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