Mobile manipulation
- Project Name: Brazo Bioloid controlado por ROS con interfaz Arduino
- Authors: Fernando Casado García
- Web of Authors: http://www.fernando.casadogarcia.es
- Dates: September 2013 -
- Degree: Ph.D.
- Tags: MYRAbot, manipulation, arm, Bioloid
- Technology: ROS, c++, Dynamixel, Arduino
- State: WIP
Contents
Brazo Bioloid
El brazo en el que se basa este proyecto es el desarrollado en el proyecto MYRA Robot: Hardware Update , realizado por Carlos Rodríguez Hernández. Los componentes principales de este son:
- Placa arduino mega 2560.
- Circuito integrado 74LS241 (Bufer tri-estado).
- Circuito regulador de tensión.
- 5 servomotores serie 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 arduino y ROS es necesario instalar arduino IDE y rosserial (package de ROS que contiene el stack rosserial_arduino con las librerias para arduino). Comenzaremos instalando el 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 arduino se procedera a la instalación del package de ROS ejecutando en un terminal el siguiente comando:
sudo apt-get install ros-NUESTRA_VERSIÓN_ROS-rosserial
Instalados arduino IDE y el package rosserial debemos copiar las librerial de stack rosserial_arduino a el sketchbook de 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 espacio de trabajo para nuestra versión de ROS. Para poder compilar y enviar nuestros programas a la placa arduino sin tener que pasar por 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:
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)
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:
#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);
}
Para su compilación y envio a la placa arduino es necesario añadir al archivo CmakeLists.txt las siguientes líneas:
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})
Donde se indica el nombre del programa, tipo de placa 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 ROS, si no se encuentra ya en marcha, ejecutando el siguiente comando:
roscore
En otro terminal se ejecutará el nodo que comunica a ROS con la placa arduino, indicándole el puerto empleado para la comunicación. Para saber el puerto que se está empleando, con la placa arduino conectada al pc a través de su puerto USB, podemos verlo en el menú "Herramientas">"Puerto serie" del software 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 Savage Electronics se encuentran las adaptaciones hardware realizadas a la placa arduino mega 2560 para la correcta comunicación con los servomotores Dynamixel AX-12A de ROBOTICS, así como las librerías utilizadas para la programación (DynamixelSerial).
Segundo programa (Muévete)
En este programa simplemente se realiza un prueba de la comunicación entre la placa arduino mega 2560 con un servomotor 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:
#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);
}
Para poder compilar y enviar el programa a la placa arduino deberemos emplear el software arduino IDE, ya que empleamos una librería externa al sistema ROS y no nos permite hacerlo usando 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 ROS:
roscore
También debemos lanzar en otro terminal el nodo de comunicación ROS-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 parametrización Denavit- Hartenberg que permite obtener la cinemática directa y a partir de esta obtener la inversa.
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:
Teniendo en cuenta los ángulos límite de los servomotores 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:
Se suman 150º al ángulo para situar el 0º coincidente con el eje z.
Se suman 60º al ángulo para situar el 0º coincidente con el eje z.
Se restan 30º al ángulo para situar el 0º coincidente con el eje longitudinal del servomotor.
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:
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.
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 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 crearemos un package con las dependencias necesarias para nuestros programas. Dentro de este packagecrearemos 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 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 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 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:
#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;
void mover(const brazo_fer::WriteServos& brazo){
brazo_fer::Servos par = brazo.par;
brazo_fer::Servos move = brazo.posicion;
int posicion[4];
if (par.base == 0){
Dynamixel.torqueStatus(1,OFF);
}
else {
Dynamixel.torqueStatus(1,ON);
posicion[0] = Dynamixel.readPosition(1);
Dynamixel.moveSpeed(1,move.base,abs(move.base - posicion[0])/5);
}
if (par.arti1 == 0){
Dynamixel.torqueStatus(2,OFF);
}
else {
Dynamixel.torqueStatus(2,ON);
posicion[1] = Dynamixel.readPosition(2);
Dynamixel.moveSpeed(2,move.arti1,abs(move.arti1 - posicion[1])/5);
}
if (par.arti2 == 0){
Dynamixel.torqueStatus(3,OFF);
}
else {
Dynamixel.torqueStatus(3,ON);
posicion[2] = Dynamixel.readPosition(3);
Dynamixel.moveSpeed(3,move.arti2,abs(move.arti2 - posicion[2])/5);
}
if (par.arti3 == 0){
Dynamixel.torqueStatus(4,OFF);
}
else {
Dynamixel.torqueStatus(4,ON);
posicion[3] = Dynamixel.readPosition(4);
Dynamixel.moveSpeed(4,move.arti3,abs(move.arti3 - posicion[3])/5);
}
}
void pinza(const brazo_fer::WriteServos& pinza){
brazo_fer::Servos par = pinza.par;
brazo_fer::Servos move = pinza.posicion;
int posicion;
if (par.pinza == 0){
Dynamixel.torqueStatus(5,OFF);
}
else {
Dynamixel.torqueStatus(5,ON);
posicion = Dynamixel.readPosition(5);
Dynamixel.moveSpeed(5,move.pinza,abs(move.pinza - posicion)/5);
}
}
ros::Subscriber<brazo_fer::WriteServos> move_sub("move_arm", &mover );
ros::Subscriber<brazo_fer::WriteServos> hand_sub("hand_arm", &pinza );
brazo_fer::ReadServos pec;
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);
delay(1000);
}
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();
delay(10);
}
Para compilar y cargar el programa en la placa emplearemos el programa arduino IDE. Este programa nos servirá para otros programas de control que desarrollemos, ya que se trata de un mero intermediario.
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 arduino, y publica el topic "move_arm", donde indica al programa 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:
#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, 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, gamma, delta, delta_a, delta_p;
double z_p;
double L_a, L;
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,2)+pow(z_p-Lp,2));
beta_p = atan2(y,z_p-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) {
brazo_fer::WriteServos mover;
mover.posicion = ::move;
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 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;
}
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:
rosbuild_add_executable(contro_v01 src/control_v01.cpp)
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:
roscd brazo_fer
make
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:
roscore
Ahora debemos iniciar el nodo de la placa arduino ejecutando en un nuevo terminal el siguiente comando:
rosrun rosserial_python serial_node.py /dev/ttyACM0
Para ejecutar el programa de control llamado "control_v01", en un terminal nuevo ejecutaremos el siguiente comando:
rosrun brazo_fer control_v01
Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once
El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde uno de partida:
Cuarto programa (Agarra la botella)
Este programa ejecuta la secuencia para coger una botella pequeña de plástico (vacía, ya que la potencia de los servomotores es insuficiente para una llena, deberían emplearse más servomotores por articulación). Los pasos que realiza son los siguientes:
- Primera aproximación (sitúa el brazo a unos 60 mm del punto indicado).
- Posicionamiento en el punto indicado.
- Cierre de la pinza (hasta tener agarrada la botella).
- Levantamiento y acercamiento (levanta y acerca la botella al MYRAbot)
En el siguiente vídeo se puede ver el procedimiento para la recogida de una botella:
