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

From robotica.unileon.es
Jump to: navigation, search
(→‎Creating custom messages in ROS)
(→‎ROS program)
Line 406: Line 406:
  
 
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.
 +
 +
====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:
 +
 +
<syntaxhighlight lang="cpp" enclose="div">
 +
 +
#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
 +
 +
</syntaxhighlight>
  
 
====ROS program====
 
====ROS program====

Revision as of 15:52, 26 April 2014

< go back to main

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 (Came 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.

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

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)

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 "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 
  #define Lb 60  
  
  geometry_msgs::Point punto_0,punto_1;
  
  int cont = 0;
  int cont_1 = 0;
  int cont_2 = 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;
  
		  
  brazo_fer::Servos inversa(geometry_msgs::Point destino) 
  {
	  
	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 = 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; 
	}
 
	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()
  { 
	  
  	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 (::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);
		
		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;
   	la.z = 60;
  	
  	if (la.y < 75) {
		la.y = 75;
	}
  	 	
  	brazo_fer::Servos move = inversa(la);
  	
  	::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;   //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);
			
				brazo_fer::WriteServos dos;		  	
		  	
				dos.posicion = ::move;
					
				dos.velocidad = ::vel;
		  	
				dos.par = ::on;
  	
				move_pub_.publish(dos);
			 
				::cont_2 = ::cont_2+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))) {
				
				int pinza;
				
				pinza = p.pinza;
				
					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);
						
					}
					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;

  } 
 
 
  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 "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 
  #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;
  
		  
  brazo_fer::Servos inversa(geometry_msgs::Point destino) 
  {
	  
	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 = 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)) 
	{
		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()
  { 
	  
  	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 (::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;
		
		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; 
  	 	
  	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::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; 	
	
	brazo_fer::Servos e = pec.estado;   
	
	brazo_fer::Servos c = pec.corriente;   
	
	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 = separar(::punto_0);
		::move_1 = inversa(::punto_0);
	
	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;					
						
                                                ::move = separar(::punto_0);
				
						brazo_fer::WriteServos tres;		  	
				
						tres.posicion = ::move;
						
						tres.velocidad = ::vel;
				
						tres.par = ::on;
		
						move_pub_.publish(tres);
						
						::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;
						
						home();
						
						::cont = ::cont + 1; 
                                                				
						::cont_1 = 0;
						::cont_2 = 0;
					}
				}
			}
			
	}
	else {
			
			std::cout<<"Moving"<<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 "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;
	double z_p;
	double L_a, L;
	double x, y, z;
	
	alfa = (((origen.base*300)/1023)-150)*PI/180;
	beta = (((origen.arti1*300)/1023)-60)*PI/180;
	gamma = (((origen.arti2*300)/1023)+30)*PI/180;
	delta = (((origen.arti3*300)/1023)+30)*PI/180;

        epsilon = 0;
	
	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);
	
        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);

        beta_pp = acos(z_p/L);
	
	if (L1*sin(beta) >= L2*sin(beta + gamma) + Lp*sin(epsilon)) 
	{
		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::Servos inversa(geometry_msgs::Point destino) 
  {
	  
	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 = 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))
	{
		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;
	brazo.pinza = 511;	
	
	::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::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1); 
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);	
	
	if (::cont == 0)
	{
	
		::punto_0.x = 0;
		::punto_0.y = 0;
		::punto_0.z = 0;
		
		::punto_1 = ::punto_0;		
		
		::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;
		
	  	brazo_fer::Servos mover;
		
		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 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 "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); 
	
	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

< go back to main

Retrieved from "https://robotica.unileon.es/index.php?title=MYRAbot%27s_arm_control_(bioloid%2barduino)&oldid=3561"