|
|
| (74 intermediate revisions by 2 users not shown) |
| Line 1: |
Line 1: |
| | * '''Project Name:''' Brazo Bioloid controlado por ROS con interfaz Arduino | | * '''Project Name:''' Brazo Bioloid controlado por ROS con interfaz Arduino |
| − | | + | * '''Author:''' [http://www.fernando.casadogarcia.es Fernando Casado García] |
| − | * '''Authors:''' Fernando Casado García | |
| − | | |
| − | * '''Web of Authors:''' http://www.fernando.casadogarcia.es
| |
| − | | |
| | * '''Dates:''' September 2013 - | | * '''Dates:''' September 2013 - |
| | + | * '''Degree:''' PhD |
| | + | * '''Tags:''' MYRAbot, manipulation, arm, Bioloid, webcam, recognition, simulation |
| | + | * '''Technologies:''' ROS, c++, Dynamixel, Arduino, find_object_2d, gazebo, URDF, moveIt!, actionlib, maxon, epos2, EposManager (wpi-rover) |
| | + | * '''State:''' Ongoing |
| | | | |
| − | * '''Degree:''' Ph.D.
| |
| − |
| |
| − | * '''Tags:''' MYRAbot, manipulation, arm, Bioloid
| |
| − |
| |
| − | * '''Technology:''' ROS, c++, Dynamixel, Arduino
| |
| − |
| |
| − | * '''State:''' WIP
| |
| − |
| |
| − | ==Brazo Bioloid==
| |
| − |
| |
| − | El brazo en el que se basa este proyecto es el desarrollado en el proyecto [[Carlos-TFM-MYRABot01 | MYRA Robot: Hardware Update ]], realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:
| |
| − | [[file:IMG_4871_b.JPG|thumb|200px|Fotografía del Brazo [http://www.robotis.com/xe/BIOLOID_main_en Bioloid].]]
| |
| − |
| |
| − | * Placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560].
| |
| − | * Circuito integrado [http://www.futurlec.com/74LS/74LS241.shtml 74LS241] (Bufer tri-estado).
| |
| − | * Circuito regulador de tensión.
| |
| − | * 5 servomotores serie [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A].
| |
| − | * Piezas de montaje de kit Bioloid.
| |
| − | * Piezas realizadas para pinza y fijación al MYRAbot.
| |
| − |
| |
| − |
| |
| − |
| |
| − |
| |
| − | ==Arduino IDE y rosserial==
| |
| − |
| |
| − | Para la comunicación e intercambio de información entre [http://www.arduino.cc arduino] y [http://www.ros.org ROS] es necesario instalar [http://www.arduino.cc arduino] IDE y rosserial (''package'' de [http://www.ros.org ROS] que contiene el ''stack'' rosserial_arduino con las librerias para [http://www.arduino.cc arduino]). Comenzaremos instalando el [http://www.arduino.cc arduino] IDE, para lo que ejecutaremos los siguientes comandos en un terminal:
| |
| − |
| |
| − | sudo apt-get update
| |
| − | sudo apt-get install arduino arduino-core
| |
| − | sudo apt-get install arduino
| |
| − |
| |
| − | Una vez realizada la instalación del software de [http://www.arduino.cc arduino] se procedera a la instalación del ''package'' de [http://www.ros.org ROS] ejecutando en un terminal el siguiente comando:
| |
| − |
| |
| − | sudo apt-get install ros-NUESTRA_VERSIÓN_ROS-rosserial
| |
| − |
| |
| − | Instalados [http://www.arduino.cc arduino] IDE y el ''package'' rosserial debemos copiar las librerial de ''stack'' rosserial_arduino a el sketchbook de [http://www.arduino.cc arduino], carpeta que se encuentra habitualmente en la carpeta personal, para lo que ejecutaremos en un terminal los siguientes comandos:
| |
| − |
| |
| − | roscd rosserial_arduino/libraries
| |
| − | cp -r ros_lib home/”nombre_sesion”/sketchbook/libraries/ros_lib
| |
| − |
| |
| − | ===Creación de un package para nuestros programas===
| |
| − |
| |
| − | Deberemos tener previamente creado un [[Fernando-TFM-ROS02#Creando un espacio de trabajo|espacio de trabajo]] para nuestra versión de [http://www.ros.org ROS]. Para poder compilar y enviar nuestros programas a la placa [http://www.arduino.cc arduino] sin tener que pasar por [http://www.arduino.cc arduino] IDE vamos a crear un ''package'' llamado “arduino_fer” con las dependencias necesarias para nuestros programas, para ello ejecutaremos los siguientes comandos en un terminal:
| |
| − | cd ~/ros_workspace
| |
| − | roscreate-pkg arduino_fer rosserial_arduino std_msgs
| |
| − | Deberemos sustituir el contenido del fichero CmakeLists.txt, situado en la carpeta del ''package'' creado, por el siguiente:
| |
| − |
| |
| − | <nowiki>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)</nowiki>
| |
| − |
| |
| − | Para finalizar la cración del ''package'' ejecutaremos los siguientes comandos:
| |
| − | roscd arduino_fer
| |
| − | cmake .
| |
| − |
| |
| − | ===Primer programa (A toda potencia)===
| |
| − |
| |
| − | El primer programa que vamos a realizar crea un nodo llamado “potencia” que publica un ''topic'' llamado “cifra” y está suscrito a un ''topic'' llamado “resultado”. Cuando se publique un número en el ''topic'' “cifra” lo multiplicará por si mismo y publicará el resultado en el ''topic'' “resultado”. El código del programa es el siguiente:
| |
| − | <syntaxhighlight lang=c>
| |
| − | #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);
| |
| − | }
| |
| − | </syntaxhighlight>
| |
| − | Para su compilación y envio a la placa [http://www.arduino.cc arduino] es necesario añadir al archivo CmakeLists.txt las siguientes líneas:
| |
| − |
| |
| − | <nowiki>set(FIRMWARE_NAME potencia)
| |
| − |
| |
| − | set(${FIRMWARE_NAME}_BOARD MODELO_NUESTRA_PLACA) # Modelo placa arduino
| |
| − | set(${FIRMWARE_NAME}_SRCS src/potencia.cpp )
| |
| − | set(${FIRMWARE_NAME}_PORT /dev/ttyACM0) # Puerto serie de subida
| |
| − | generate_ros_firmware(${FIRMWARE_NAME})</nowiki>
| |
| − |
| |
| − | Donde se indica el nombre del programa, tipo de placa [http://www.arduino.cc arduino] (uno, atmega328 o mega2560), nombre y ubicación del archivo y puerto serie del PC empleado para la comunicación con la placa. Deberemos ejecutar los siguientes comandos en un terminal:
| |
| − | roscd arduino_fer
| |
| − | make potencia-upload
| |
| − | Para la ejecución del programa se debe lanzar en un terminal el núcleo de [http://www.ros.org ROS], si no se encuentra ya en marcha, ejecutando el siguiente comando:
| |
| − | roscore
| |
| − | En otro terminal se ejecutará el nodo que comunica a [http://www.ros.org ROS] con la placa [http://www.arduino.cc arduino], indicándole el puerto empleado para la comunicación. Para saber el puerto que se está empleando, con la placa [http://www.arduino.cc arduino] conectada al pc a través de su puerto USB, podemos verlo en el menú "Herramientas">"Puerto serie" del software [http://www.arduino.cc arduino] IDE. En nuestro caso es el "ttyACM0":
| |
| − | rosrun rosserial_python serial_node.py /dev/ttyACM0
| |
| − | Para la publicación de un número en el ''topic'' “cifra” se ejecutará en otro terminal el siguiente comando:
| |
| − | rostopic pub cifra std_msgs/Int16 NUMERO_DESEADO --once
| |
| − | Para comprobar que realmente el programa calcula el cuadrado del número publicado en el ''topic'' “cifra” podemos ejecutar en otro terminal el siguiente comando para visualizar el ''topic'' “resultado”:
| |
| − | rostopic echo resultado
| |
| − |
| |
| − | ==Arduino y servomotores Dinamixel==
| |
| − |
| |
| − | En [http://savageelectronics.blogspot.com.es/2011/01/arduino-y-dynamixel-ax-12.html Savage Electronics] se encuentran las adaptaciones hardware realizadas a la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] para la correcta comunicación con los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] de [http://www.robotis.com/xe/ ROBOTICS], así como las librerías utilizadas para la programación ([http://savageelectronics.blogspot.com.es/2011/08/actualizacion-biblioteca-dynamixel.html DynamixelSerial]).
| |
| − |
| |
| − | ===Segundo programa (Muévete)===
| |
| − |
| |
| − | En este programa simplemente se realiza un prueba de la comunicación entre la placa [http://arduino.cc/en/Main/ArduinoBoardMega2560 arduino mega 2560] con un servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]. El programa publica el ''topic'' “angulo” con la posición del motor (entero entre 0 y 1023) y está suscrito al ''topic'' “giro” del que recibe la posición a la que debe moverse (entero entre 0 y 1023), con una velocidad constante de 128 (entero entre 0 y 1023). El código del programa es el siguiente:
| |
| − |
| |
| − | <syntaxhighlight lang=c>
| |
| − | #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);
| |
| − | }
| |
| − | </syntaxhighlight>
| |
| − | Para poder compilar y enviar el programa a la placa [http://www.arduino.cc arduino] deberemos emplear el software [http://www.arduino.cc arduino] IDE, ya que empleamos una librería externa al sistema [http://www.ros.org ROS] y no nos permite hacerlo usando [[#Creación de un package para nuestros programas|nuestro ''package'']].
| |
| − | Una vez transferido el programa a la placa, para probar el programa debemos ejecutar en un terminal el siguiente comando, que inicia el nucleo de [http://www.ros.org ROS]:
| |
| − |
| |
| − | roscore
| |
| − |
| |
| − | También debemos lanzar en otro terminal el nodo de comunicación [http://www.ros.org ROS]-[http://www.arduino.cc arduino], ejecutando el siguiente comando:
| |
| − |
| |
| − | rosrun rosserial_python serial_node.py /dev/ttyACM0
| |
| − |
| |
| − | En un nuevo terminal publicaremos en el ''topic'' "giro" la posición de destino del servomotor, ejecutando el siguiente comando:
| |
| − |
| |
| − | rostopic pub giro std_msgs/Int16 POSICION_DESEADA --once
| |
| − |
| |
| − | El servomotor de inmediato comenzará a moverse hasta la posición indicada. Podemos visualizar el valor de posición del servomotor en todo momento ejecutando en otro terminal el siguiente comando:
| |
| − |
| |
| − | rostopic echo angulo
| |
| − |
| |
| − | ==Modelo cinemático del brazo==
| |
| − |
| |
| − | Para el correcto posicionamiento del brazo es necesario obtener las transformaciones geométricas para determinar el ángulo que deben girar los servomotores para alcanzar un punto (cinemática inversa). Al tratarse de un brazo que trabaja en un solo plano con giro (4 articulaciones) es un proceso sencillo que se ha realizado de forma directa, sino se puede recurrir al algoritmo de [http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters parametrización Denavit- Hartenberg] que permite obtener la cinemática directa y a partir de esta obtener la inversa.
| |
| − |
| |
| − | [[file:brazo_cinematica.jpg|thumb|500px|center|Diagramas para el estudio de la cinemática]]
| |
| − |
| |
| − | ===Cinemática inversa===
| |
| − |
| |
| − | Siguiendo los diagramas planteados y considerando ''x'',''y'' y ''z'' como las coordenadas del punto destino, y que la pinza siempre se ha de mantener paralela al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:
| |
| − |
| |
| − | [[file:brazo_ecuaciones.jpg]]
| |
| − |
| |
| − | Teniendo en cuenta los ángulos límite de los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] y el rango de valores que debemos usar, los valores que debemos pasar a los servomotores de cada articulación son los siguientes:
| |
| − |
| |
| − | [[file:brazo_dynamixel_AX-12.jpg|thumb|250px|Diagrama de ángulos servomotor [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A]]]
| |
| − |
| |
| − | [[file:brazo_ecuacion_base.jpg]] Se suman 150º al ángulo para situar el 0º coincidente con el eje z.
| |
| − |
| |
| − | [[file:brazo_ecuacion_arti1.jpg]] Se suman 60º al ángulo para situar el 0º coincidente con el eje z.
| |
| − |
| |
| − | [[file:brazo_ecuacion_arti2.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
| |
| − |
| |
| − | [[file:brazo_ecuacion_arti3.jpg]] Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
| |
| − |
| |
| − |
| |
| − | Para enviar a los servomotores valores dentro del rango de trabajo, se multiplica por 1023 (0x3FF), valor máximo del rango de posiciones del servo, y se divide por 300 (300º), el ángulo total de giro del servomotor.
| |
| − |
| |
| − | ===Cinemática directa===
| |
| − |
| |
| − | A partir de las ecuaciones anteriores podemos plantear también el proceso inverso, la obtención del punto en que se encuentra el brazo a partir de las posiciones de los servomotores. Las ecuaciones obtenidas son las siguientes:
| |
| − |
| |
| − | [[file:brazo_ecuaciones_directa.jpg]]
| |
| − |
| |
| − | ===Espacio de trabajo y limitaciones===
| |
| − |
| |
| − | Para evitar colisiones del brazo con las diferentes partes del MYRAbot y con sus propios componentes, es necesario fijar unos límites de giro a los servomotores y restringir el acceso a regiones del espacio. Para esta tarea se ha realizado una hoja de calculo con las formulas de la cinemática inversa y se ha representado en unos gráficos las posiciones de los ejes y las articulaciones del brazo en el espacio.
| |
| − |
| |
| − | [[file:brazo_cinematica_excel.jpg|thumb|750px|center|Hoja de cálculo cinemática inversa y directa, [http://www.fernando.casadogarcia.es/brazo_cinematica.xls descargar]]]
| |
| − |
| |
| − | ==Programas de control==
| |
| − |
| |
| − | ===Tercer programa (Ven aquí)===
| |
| − |
| |
| − | Primero crearemos un programa que actuará de intermediario entre el programa de control y los servomotores del brazo, que situaremos en la placa [http://www.arduino.cc arduino]. Este programa se encargará de enviar las ordenes recibidas a los servomotores y enviar los datos más relevantes de los servomotores. Como se van a ''publicar'' y ''suscribir'' los datos de 5 servomotores, vamos a comenzar creando nuestros ''mensajes personalizados''.
| |
| − |
| |
| − | ====Creación de mensajes personalizados en ROS====
| |
| − |
| |
| − | Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas. Dentro de este ''package''crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre e campo).
| |
| − |
| |
| − | Comenzaremos creando un mensaje base llamado "servos", que contendrá los datos de cada servomotor. Crearemos un archivo llamado "Servos.msg" con el siguiente contenido:
| |
| − |
| |
| − | int16 base
| |
| − | int16 arti1
| |
| − | int16 arti2
| |
| − | int16 arti3
| |
| − | int16 pinza
| |
| − |
| |
| − | Vamos a crear un mensaje que contenga los datos que enviaremos a los servomotores (posición y par). Ahora el tipo de dato que vamos a manejar es el que hemos creado anteriormente. Creamos un archivo llamado "WriteServos.msg" con el siguiente contenido:
| |
| − |
| |
| − | Servos posicion
| |
| − | Servos par
| |
| − |
| |
| − | Para la recepción de datos de los servomotores crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:
| |
| − |
| |
| − | Servos posicion
| |
| − | Servos estado
| |
| − | Servos corriente
| |
| − |
| |
| − | 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:
| |
| − |
| |
| − | # rosbuild_gensrv()
| |
| − |
| |
| − | Para la compilación y creación de los mensaje ejecutaremos la siguiente secuencia de comandos en un terminal, donde "brazo_fer" es el package creado para nuestros programas:
| |
| − |
| |
| − | roscd brazo_fer
| |
| − | make
| |
| − |
| |
| − | Como vamos a usar estos mensajes en [http://www.arduino.cc arduino], necesitamos añadirlos al ''package'' "rosserial", ejecutando en un terminal el siguiente comando:
| |
| − |
| |
| − | rosrun rosserial_client make_library.py ~/sketchbook/libraries brazo_fer
| |
| − |
| |
| − |
| |
| − | ====Programa arduino====
| |
| − |
| |
| − | El programa para la placa [http://www.arduino.cc arduino] está suscrito al ''topic'' "move_arm", por el cual recibe los movimientos de los servomotores, y publica el ''topic'' "pose_arm", con la información de posición, par y corriente de los servomotores. El código del programa es el siguiente:
| |
| − |
| |
| − | <syntaxhighlight lang=c>
| |
| − |
| |
| − | #include <ros.h>
| |
| − | #include <brazo_fer/Servos.h>
| |
| − | #include <brazo_fer/WriteServos.h>
| |
| − | #include <brazo_fer/ReadServos.h>
| |
| − | #include <std_msgs/Int16.h>
| |
| − | #include <math.h>
| |
| − |
| |
| − | #include <DynamixelSerial1.h>
| |
| − |
| |
| − | ros::NodeHandle nh;
| |
| − |
| |
| − | int posicion[5];
| |
| − |
| |
| − | int max_min(int vel){
| |
| − |
| |
| − | if (vel < 10){
| |
| − | vel = 10;
| |
| − | }
| |
| − | else if (vel > 200){
| |
| − | vel = 200;
| |
| − | }
| |
| − | else {
| |
| − | }
| |
| − | return vel;
| |
| − | }
| |
| − |
| |
| − |
| |
| − | void mover(const brazo_fer::WriteServos& giro){
| |
| − |
| |
| − | brazo_fer::Servos par = giro.par;
| |
| − |
| |
| − | brazo_fer::Servos move = giro.posicion;
| |
| − |
| |
| − | if (par.base == 0){
| |
| − | Dynamixel.torqueStatus(1,OFF);
| |
| − | }
| |
| − | else {
| |
| − | ::posicion[0] = Dynamixel.readPosition(1);
| |
| − |
| |
| − | Dynamixel.moveSpeed(1,move.base,max_min((abs(move.base-::posicion[0])/5)));
| |
| − | }
| |
| − |
| |
| − | if (par.arti1 == 0){
| |
| − | Dynamixel.torqueStatus(2,OFF);
| |
| − | }
| |
| − | else {
| |
| − | ::posicion[1] = Dynamixel.readPosition(2);
| |
| − |
| |
| − | Dynamixel.moveSpeed(2,move.arti1,max_min((abs(move.arti1-::posicion[1])/5)));
| |
| − | }
| |
| − |
| |
| − | if (par.arti2 == 0){
| |
| − | Dynamixel.torqueStatus(3,OFF);
| |
| − | }
| |
| − | else {
| |
| − | ::posicion[2] = Dynamixel.readPosition(3);
| |
| − |
| |
| − | Dynamixel.moveSpeed(3,move.arti2,max_min((abs(move.arti2-::posicion[2])/5)));
| |
| − | }
| |
| | | | |
| − | if (par.arti3 == 0){
| + | ---- |
| − | Dynamixel.torqueStatus(4,OFF);
| + | ---- |
| − | }
| + | [[File:myrabot_portada.png|100px|thumb|right|MYRABot]] |
| − | else {
| |
| − | ::posicion[3] = Dynamixel.readPosition(4);
| |
| | | | |
| − | Dynamixel.moveSpeed(4,move.arti3,max_min((abs(move.arti3-::posicion[3])/5)));
| |
| − | }
| |
| | | | |
| − | if (par.pinza == 0){
| + | <center style="font-size: 30px;">MYRABot</center> |
| − | Dynamixel.torqueStatus(5,OFF);
| |
| − | }
| |
| − | else {
| |
| − | ::posicion[4] = Dynamixel.readPosition(5);
| |
| | | | |
| − | Dynamixel.moveSpeed(4,move.pinza,max_min((abs(move.pinza-::posicion[3])/5)));
| |
| − | }
| |
| | | | |
| − | }
| + | {| style="border: solid 0px white; width: 100%" |
| | + | ! Español |
| | + | ! English |
| | + | |- |
| | + | | valign="top" width="50%" |Contenido |
| | + | ---- |
| | | | |
| − | ros::Subscriber<brazo_fer::WriteServos> move_sub("move_arm", &mover );
| + | #[[Control brazo MYRAbot (bioloid+arduino)]] |
| | + | #[[Detección y cálculo de posición de objetos (cámara web)]] |
| | + | #[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]] |
| | + | #[[Modelo para simulación MYRAbot (urdf+gazebo)]] |
| | + | #[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]] |
| | + | #[[Órdenes y confirmación mediante voz (sphinx+festival)]] |
| | + | #[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]] |
| | | | |
| − | brazo_fer::ReadServos pec;
| + | | valign="top" |Content |
| | + | ---- |
| | | | |
| − | ros::Publisher pose_pub("pose_arm", &pec);
| + | #[[MYRAbot's arm control (bioloid+arduino)]] |
| | + | #[[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)]] |
| | + | |} |
| | | | |
| | + | MYRABot ROS packages: |
| | | | |
| − | void setup()
| + | [https://github.com/Robotica-ule/MYRABot MYRABot's GitHub] |
| − | {
| |
| − | nh.initNode();
| |
| − | nh.subscribe(move_sub);
| |
| − | nh.advertise(pose_pub);
| |
| − | Dynamixel.begin(1000000,2);
| |
| − | delay(10);
| |
| − | }
| |
| | | | |
| − | void loop()
| |
| − | {
| |
| − | ::posicion[0] = Dynamixel.readPosition(1);
| |
| − | ::posicion[1] = Dynamixel.readPosition(2);
| |
| − | ::posicion[2] = Dynamixel.readPosition(3);
| |
| − | ::posicion[3] = Dynamixel.readPosition(4);
| |
| − | ::posicion[4] = Dynamixel.readPosition(5);
| |
| − |
| |
| − | int estado[5];
| |
| − |
| |
| − | estado[0] = Dynamixel.moving(1);
| |
| − | estado[1] = Dynamixel.moving(2);
| |
| − | estado[2] = Dynamixel.moving(3);
| |
| − | estado[3] = Dynamixel.moving(4);
| |
| − | estado[4] = Dynamixel.moving(5);
| |
| − |
| |
| − | int corriente[5];
| |
| − |
| |
| − | corriente[0] = Dynamixel.readLoad(1);
| |
| − | corriente[1] = Dynamixel.readLoad(2);
| |
| − | corriente[2] = Dynamixel.readLoad(3);
| |
| − | corriente[3] = Dynamixel.readLoad(4);
| |
| − | corriente[4] = Dynamixel.readLoad(5);
| |
| − |
| |
| − | brazo_fer::mover pos;
| |
| − |
| |
| − | pos.base = ::posicion[0];
| |
| − | pos.arti1 = ::posicion[1];
| |
| − | pos.arti2 = ::posicion[2];
| |
| − | pos.arti3 = ::posicion[3];
| |
| − | pos.pinza = ::posicion[4];
| |
| − |
| |
| − | brazo_fer::mover est;
| |
| − |
| |
| − | est.base = estado[0];
| |
| − | est.arti1 = estado[1];
| |
| − | est.arti2 = estado[2];
| |
| − | est.arti3 = estado[3];
| |
| − | est.pinza = estado[4];
| |
| − |
| |
| − | brazo_fer::mover cor;
| |
| − |
| |
| − | cor.base = corriente[0];
| |
| − | cor.arti1 = corriente[1];
| |
| − | cor.arti2 = corriente[2];
| |
| − | cor.arti3 = corriente[3];
| |
| − | cor.pinza = corriente[4];
| |
| − |
| |
| − | pec.posicion = pos;
| |
| − | pec.estado = est;
| |
| − | pec.corriente = cor;
| |
| − |
| |
| − | pose_pub.publish( &pec );
| |
| − | nh.spinOnce();
| |
| − | delay(10);
| |
| − | }
| |
| | | | |
| | + | ---- |
| | + | ---- |
| | + | [[File:cervantes_portada.png|100px|thumb|right|CeRVaNTeS]] |
| | | | |
| − | </syntaxhighlight>
| |
| | | | |
| − | ====Programa ROS==== | + | <center style="font-size: 30px;">CeRVaNTeS</center> |
| | | | |
| − | 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=c>
| + | {| style="border: solid 0px white; width: 100%" |
| | + | ! Español |
| | + | ! English |
| | + | |- |
| | + | | valign="top" width="50%" |Contenido |
| | + | ---- |
| | | | |
| − | #include "ros/ros.h"
| + | #[[Control brazo y base CeRVaNTeS (maxon+epos2)]] |
| − | #include "brazo_fer/Servos.h"
| + | #[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]] |
| − | #include "std_msgs/WriteServos.h"
| + | #[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]] |
| − | #include "std_msgs/ReadServos.h"
| + | #[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]] |
| − | #include "geometry_msgs/Point.h"
| + | #[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]] |
| − | #include "math.h"
| |
| − |
| |
| − | #define PI 3.14159265
| |
| − | #define L1 104
| |
| − | #define L2 104
| |
| − | #define Lp 60
| |
| − |
| |
| − |
| |
| − | brazo_fer::Servos move, 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;
| |
| | | | |
| − | int coordenadas_correctas = 1;
| + | | valign="top" |Content |
| | + | ---- |
| | | | |
| − | double alfa, beta, beta_a, beta_p, gamma, delta, delta_a, delta_p;
| + | #[[CeRVaNTeS' arm and base control (maxon+epos2)]] |
| − | double z_p;
| + | #[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]] |
| − | double L_a, L;
| + | #[[CeRVaNTeS model for simulation (urdf+gazebo)]] |
| − |
| + | #[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]] |
| − | alfa = (atan2(x,z)*180)/PI;
| + | #[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]] |
| − |
| + | |} |
| − | z_p = sqrt(pow(z,2)+pow(x,2));
| |
| − |
| |
| − | L = sqrt(pow(zp,2)+pow(y,2));
| |
| − |
| |
| − | L_a = sqrt(pow(y,2)+pow(zp-Lp,2));
| |
| − |
| |
| − | beta_p = atan2(y,zp-Lp);
| |
| − |
| |
| − | 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 (y<0) {
| |
| − | delta = ((2*PI-(delta_p-delta_a))*180)/PI;
| |
| − | }
| |
| − | else {
| |
| − | delta = ((delta_p+delta_a)*180)/PI;
| |
| | | | |
| − | if (isnan(delta)) {
| |
| − | delta = ((PI+delta_a)*180)/PI;
| |
| − | }
| |
| − | }
| |
| − |
| |
| − |
| |
| − | if (isnan(gamma)) // si no hay solución
| |
| − | {
| |
| − | coordenadas_correctas = 0;
| |
| − | }
| |
| − |
| |
| − |
| |
| − | ::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;
| |
| − |
| |
| − |
| |
| − | if (coordenadas_correctas == 1 && (205 <= ::move.base && ::move.base <= 818) && (120 <= ::move.arti1 && ::move.arti1 <= 920) && ::move.arti2 >= 50 && ::move.arti3 <= 828 && (350 <= ::move.pinza && ::move.pinza <= 565)) {
| |
| − |
| |
| − | brazo_fer::WriteServos mover;
| |
| | | | |
| − | mover.posicion = ::move;
| + | ---- |
| | + | ---- |
| | + | [[File:rb1_portada.png|100px|thumb|right|OrBiOne]] |
| | | | |
| − | move_pub_.publish(mover);
| |
| − | }
| |
| − | else {
| |
| − | std::cout<<"error coordenadas no validas o punto fuera del alcance"<<std::endl;
| |
| − | }
| |
| − |
| |
| − | }
| |
| − |
| |
| − |
| |
| − | int main(int argc, char **argv)
| |
| − | {
| |
| − |
| |
| − | ros::init(argc, argv, "control_brazo");
| |
| − |
| |
| − | ros::NodeHandle n;
| |
| − |
| |
| − |
| |
| − | ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
| |
| − |
| |
| − | ros::Subscriber point_sub_= n.subscribe("point", 1, punto);
| |
| − |
| |
| − | ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
| |
| − |
| |
| − |
| |
| − | ros::spin();
| |
| − |
| |
| − | return 0;
| |
| − | }
| |
| | | | |
| − | </syntaxhighlight> | + | <center style="font-size: 30px;">OrBiOne</center> |
