==Brazo Bioloid==

 

 

 

==Arduino IDE y rosserial==

===Creación de un package para nuestros programas===

===Primer programa (A toda potencia)===

==Arduino y servomotores Dinamixel==

===Segundo programa (Muévete)===

==Modelo cinemático del brazo==

===Cinemática inversa===

===Cinemática directa===

===Espacio de trabajo y limitaciones===

==Programas de control==

===Tercer programa (Ven aquí)===

====Creación de mensajes personalizados en ROS====

Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas (std_msgs geometry_msgs roscpp). Dentro de este ''package'' crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre e campo).  

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

Para la recepción de datos de los servomotores crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:

Para la creación de los mensajes en el sistema [http://www.ros.org ROS] debemos editar el archivo "CMakeLists.txt" y eliminar el símbolo "#" para que deje de estar comentada la siguiente línea:

====Programa arduino====

====Programa ROS====

Este es el programa que indica la posición de los servomotores para alcanzar un punto del espacio de trabajo. Está suscrito al ''topic'' "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al ''topic'' "pose_arm", donde recibe la información de los servomotores que publica el programa [http://www.arduino.cc arduino], y publica el ''topic'' "move_arm", donde indica al programa [http://www.arduino.cc arduino] la posición y par de los servomotores. Dentro del ''pakage'' creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido:

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v01 src/control_v01.cpp)</syntaxhighlight>

====Ejecutando el programa====

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

===Cuarto programa (Agarra la botella)===

# Primera aproximación (sitúa el brazo a unos 70 mm del punto indicado).

# Posicionamiento en el punto indicado.

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

   #include "ros/ros.h"   //Librería de funciones del sistema ROS

   #include "math.h" //Librería de funciones matemáticas

   #define L1 104 //longitud antebrazo (mm)

   #define L2 104 //longitud brazo (mm)

   #define Lp 60 //longitud de la mano

  #define Lb 60 //longitud desde la base de la articulación A1  hasta la articulación A2  

  geometry_msgs::Point punto_0,punto_1;

  brazo_fer::Servos pg;

  brazo_fer::Servos move, move_1, vel;

  brazo_fer::Servos inversa(geometry_msgs::Point destino)  

   {

 

 

double alfa, beta, beta_a, beta_p, beta_pp, gamma, delta, delta_a, delta_p, epsilon;

epsilon = 0; //Ángulo de inclinación de la pinza respecto a la horizontal

alfa = (atan2(x,z)*180)/PI;

z_p = sqrt(pow(z,2)+pow(x,2));

L = sqrt(pow(z_p,2)+pow(y,2));

L_a = sqrt(pow(y+(Lp*sin(epsilon)),2)+pow(z_p-(Lp*cos(epsilon)),2));

beta_p = atan2(y+(Lp*sin(epsilon)),z_p-(Lp*cos(epsilon)));

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

beta = ((beta_p+beta_a)*180)/PI;

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

delta_a = PI-(beta_a+gamma);

gamma = (gamma*180)/PI;

        delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));     

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

brazo_fer::Servos brazo;

brazo.base = ((alfa+150)*1023)/300;

brazo.arti1 = ((beta+60)*1023)/300;

brazo.arti3 = ((delta-30)*1023)/300;

::vel.base = abs(brazo.base - ::pg.base)/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 coordenadas no validas o punto fuera del alcance"<<std::endl;

::error_coordenadas = 1;

return ::pg;

}  

   }

  void home()

   {  

  ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"

  

  ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"

if (::cont == 0) {

::punto_0.z = 50;

::on.base = 1;

::on.arti2 = 1;

::on.arti3 = 1;

::on.pinza = 1;

::off.base = 0;

::off.arti2 = 0;

::off.pinza = 0;  

::punto_1 = ::punto_0;

        ::error_coordenadas = 0;

brazo_fer::Servos inicio = inversa(::punto_0);

ini.velocidad.pinza = abs(511 - ::pg.pinza);

ini.par = ::on;

move_pub_.publish(ini);

hand_pub_.publish(ini);

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

  return move;

 

    brazo_fer::Servos levantar_acercar(geometry_msgs::Point la)

  {

  la.x = 0;

  la.y = la.y + 50;

  if (la.y < 75) {

la.y = 75;

  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" 

brazo_fer::Servos p = pec.posicion;  //Posición del servomotor

::pg = pec.posicion;

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

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) {

::move = inversa(::punto_0);

dos.par = ::on;

move_pub_.publish(dos);

::cont_2 = ::cont_2+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(c.pinza < 300) {

std::cout<<c.pinza<<std::endl;

pinza = pinza + 20;

::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);

std::cout<<"Agarrado"<<std::endl;

::move = levantar_acercar(::punto_0);

brazo_fer::WriteServos cuatro;  

cuatro.posicion = ::move;

cuatro.velocidad = ::vel;

cuatro.par = ::on;

move_pub_.publish(cuatro);

::punto_1 = ::punto_0;

::cont_1 = 0;

::cont_2 = 0;

std::cout<<"En movimiento"<<std::endl;

   }

   void punto(const geometry_msgs::Point& point)   //Función de llamda con cada dato recibido

::punto_0 = point;

 

   }  

  int main(int argc, char **argv)

ros::init(argc, argv, "control_recogida");   //Creación del nodo "control_brazo"

ros::NodeHandle n;

   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente); //Suscripción del topic "pose_arm"

   ros::Subscriber point_sub_= n.subscribe("pick_point", 1, punto); //Suscripción del topic "point"

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"

 

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

 

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v02 src/control_v02.cpp)</syntaxhighlight>

 

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

 

<syntaxhighlight>roscd brazo_fer

make</syntaxhighlight>

 

====Ejecutando el programa====

 

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

 

 

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

 

 

 

<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>

 

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

 

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

 

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

 

<center><videoflash>0FcB6-i23cU</videoflash></center>

 

<center>[http://www.youtube.com/watch?v=0FcB6-i23cU Ver vídeo en YouTube]</center>

 

===Quinto programa (posa la botella)===

 

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

 

# Posicionamiento en el punto indicado.

# Apertura de la pinza (hasta soltar la botella).

 

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

 

<syntaxhighlight lang="cpp" enclose="div">

   #include "ros/ros.h" 

  #include "brazo_fer/WriteServos.h"  

   #include "brazo_fer/ReadServos.h" 

  #define L1 104 // longitud antebrazo (mm)

  int cont_3 = 0;

  brazo_fer::Servos move, move_1, vel;

  brazo_fer::Servos inversa(geometry_msgs::Point destino)  

double x = destino.x;

double z = destino.z;

y = y + z*tan(atan2(45,250));

int coordenadas_correctas = 1;// para determinar si la posición se puede alcanzar

beta_pp = atan2(y,z_p);

beta_a = acos((pow(L1,2)+pow(L_a,2)-pow(L2,2))/(2*L1*L_a));

gamma = acos((pow(L1,2)+pow(L2,2)-pow(L_a,2))/(2*L1*L2));

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;

delta = ((PI+delta_a)*180)/PI;

if (isnan(gamma)) // si no hay solución

brazo_fer::Servos brazo;

brazo.base = ((alfa+150)*1023)/300;

brazo.arti1 = ((beta+60)*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) {

std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;

::error_coordenadas = 1;

return ::pg;

}    

   }

    

   void home()

   {  

   ros::NodeHandle n;

   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"

  

   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   //Publicación del topic "hand_arm"

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.pinza = 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.pinza = abs(511 - ::pg.pinza);

move_pub_.publish(ini);

hand_pub_.publish(ini);

  }

  brazo_fer::Servos 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::Servos move = inversa(lejos);

  return move;

 

  }

  void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  //Función de llamda con cada dato recibido

  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" 

brazo_fer::Servos p = pec.posicion;   //Posición del servomotor

::pg = pec.posicion;