MYRAbot's arm control (bioloid+arduino)

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 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 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 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 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 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 to launch the communication node ROS-arduino:

rosrun rosserial_python serial_node.py /dev/ttyACM0

We will execute the next command in other terminal 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 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. 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: