Select Page

## Program a robot to draw

A good exercise for programmation is to make a robotic arm reproduce a trajectory, such as drawing on a piece of paper. Once you have this knowledge, a robotic arm can be programmed to do anything you want.

# Material

• Robotic arm Handy
• 4x Servomotor SMS1012
• Servodriver
• 5V power supply
• USB cable
• Arduino UNO

# Objectif

Obj: Make a robot draw

To realize this project, we need to:

• Mount the robotic arm (Structure)
• Link the electronics to the motor and power to allow the control of the arm (Hardware)
• Write the program to make the robot follow a trajectory (Software)

# Structure

Obj: The structure should be able to carry a pen in three dimensions of space.

We used a desktop robotic arm Handy for this project. The parts are optimized for a tiny servomotor and were printed from base to wrist. An aditional pen holder was designed and printed.
that you can download here. The arm is mounted with 4 micro servomotors and the base is attached to a wooden plate.

# Hardware

Obj: The board should be linked to servomotor and power supply in order to control the servomotor with a PWM signal.

• arduino UNO

Arduino board was picked, they are easy to use and there is huge community to ask for help. As the program ask for a bit of memory we chose a Arduino UNO.

• Servo driver

The arduino cannot power so much servomotors. A driver need to be add to distribute the power from another source.

This tutorial was followed to make the driver.

# Software

## Objectives

;
The objective of the program can be divided in smaller tasks:

• Describe the trajectory coordinates in space
• Transform the coordintates of the trajectory into the desired angles of the robot joints.
• Control the robot position
• Convert robot angles into servomotor command value
• Send command to servomotor

## Describe the trajectory coordinates in space

The trajectory chosen is a circle for its simplicity. The parametric equations are :

For the z coordinates, we take the pen height with respect to the wrist: 20mm.

We write a function that output the desired position with respect to the time step:

```void circleTraj(double R,double x,double y,double z,int i,double Te, float Xd[6]){
Xd[0]=x+R*cos(2*PI*Te*i);
Xd[1]=y+R*sin(2*PI*Te*i);
Xd[2]=z;
Xd[3]=0;
Xd[4]=0;
Xd[5]=0;
}

```

## Transform the coordintates of the trajectory into the desired angles of the robot joints

The robotic arm can be described with the Denavit-Hartenberg mehtod as a robotic chain with 4 articulated links. This method is interesting to controle articulated projects with more than 3 articulations.

The method defines a cartesian frame for each joint following the same logic which result in the definition of parameters that can be used to describe mathematically the movement of the links.

Definition of the parameters with respect to geometry (DH parameters)

To compute the desired angles from the desired position we use the inverse of the jacobian matrix defined with the denavit-hartenberg parameters. To compute matrices we use MatrixMath.h library

```void computeAngle(){
double detJ;
//    compute jacobian

// avoid singularity

// invert jacobian
if(detJ>=1){
}

qu[i]=0;
for(int j=0;j<6;j++){
qu[i]+=invJ[i][j]*Xu[j];
}
}
}

```

## Convert joint angle into servomotor command

In order to convert angles to PWM command, servomotor position with respect to the joint and PWM nominal values should be known. This conversion also depends on the cartesian frames used to describe the mechanical link with DH method.

```int jointToImp(double x,int i) {
int imp=(x - joint_min[i]) * (imp_max[i]-imp_min[i]) / (joint_max[i]-joint_min[i]) + imp_min[i];
imp=max(imp,imp_min[i]);
imp=min(imp,imp_max[i]);
return imp;
}

```

## Send command to servomotor

To drive the servomotors, we use the basic Servo.h library available in the Arduino IDE. We write a function that sends the PWM command to each servomotor according to the requested angle.

```void servoToImp(double q[nbLinks],int velocity){
for (int i=0; i<nbLinks; i++) {
servo[i].writeMicroseconds(jointToImp(q[i],i));
delay(velocity);
}
}

```

## Control the robot position

To control servomotors, desired and actual position must be compared in real time

Robot position is compute with the angle of the joints using the transformtaion matrices defined with the denavit-hartenberg method.

```        forwardKinematics(sig,a,al,d,th,q,nbLinks,X);

```

Controle is applied to compute the next desired position

```        circleTraj(20,165,0,20,k,Te,Xd);
circleTraj(20,165,0,20,k+1,Te,Xk);
for(int i=0;i<6;i++) Xu[i]=(Xk[i]-Xd[i]+gain*Te*(Xd[i]-X[i]));

```

The desired servomotor angles are computed to reache the next desired position

```        for(int i=0;i<nbLinks;i++) qN1[i]=q[i];
computeAngle();

```

PWM command is apply for every servo.

```        servoToImp(q,10);

```

## Complete code

Customized library based on MatrixMath.h , MatrixMathExt.h

```/*****************************************************************\
* Project : Handy
* Author : X. Wiedmer
* Version : V01
* Date : 08/06/2017
* Revision :
* Summary :
\*****************************************************************/
// Library
#include <SoftwareSerial.h>
#include <Servo.h>
#include <MatrixMathExt.h>

// Pin assignement
#define BASE 2
#define SHOULDER 3
#define ARM 4
#define FOREARM 5
#define WRIST 6

//paramètres Denavit-Hartenberg
const double baseLength = 45;//mm
const double shoulderLength = 10;//mm
const double armLength = 75;//mm
const double forearmLength = 75;//mm
const double wristLength = 40;//mm

//paramètres servomoteur

//paramètre du contrôle
double Te=0.01; // pas de temps du contrôleur
double gain=30; // gain du contrôle
int N=(int)2/Te+1; // nombre d'étape pour parcourir 2 fois le cercle

// Variables
float X[6],Xd[6],Xk[6],Xu[6];

int firstShot = 0; // Pour que le programme ne s'exécute qu'une fois

/******************************************************************\
* PRIVATE FUNCTION: setup
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initiate inputs/outputs
*
\******************************************************************/
void setup(){
Serial.begin(9600); // Initialise le port série

for (int i=0; i<nbLinks; i++) { // Initiatlise et positionne les servomoteurs au démarrage
servo[i].writeMicroseconds(jointToImp(0,i));
servo[i].attach(servoPin[i]);
delay(500);
}
}

/******************************************************************\
* PRIVATE FUNCTION: loop
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Main Function of the code
\******************************************************************/
void loop(){
if (firstShot == 0){
for(int k=0;k<N;k++){

//Calcule la position du robot

//Calcule la prochaine position
circleTraj(20,165,0,20,k,Te,Xd);
circleTraj(20,165,0,20,k+1,Te,Xk);
for(int i=0;i<6;i++) Xu[i]=(Xk[i]-Xd[i]+gain*Te*(Xd[i]-X[i]));

//Calcule les angles moteurs pour atteindre la prochaine position
computeAngle();

//Envoie la commande au servomoteur
servoToImp(q,10);

// Affiche les angles moteur à chaque itération
Serial.print("-----------------Iteration ");
Serial.println(k);
Serial.print("angle base = ");
Serial.println(q[0]);
Serial.print("angle bras = ");
Serial.println(q[1]);
Serial.print("angle avant-bras = ");
Serial.println(q[2]);
Serial.print("angle poignet = ");
Serial.println(q[3]);
}
firstShot=1;
}
if(firstShot==1){
Serial.println("robot au repos");
restR();
Serial.println("robot stop");
stopR();
firstShot=2;
}

}

// Decrit la trajectoire dans l'espace en focntion du temps
void circleTraj(double R,double x,double y,double z,int i,double Te, float Xd[6]){
Xd[0]=x+R*cos(2*PI*Te*i);
Xd[1]=y+R*sin(2*PI*Te*i);
Xd[2]=z;
Xd[3]=0;
Xd[4]=0;
Xd[5]=0;
}

// Calcule la matrice de transformation d'un lien robotique
void computeLinkTransform(double a,double al,double d, double th, float T[][4]){
float trans[4][4] = {{cos(th),  -sin(th)*cos(al),  sin(th)*sin(al), a*cos(th)},
{sin(th),   cos(th)*cos(al), -cos(th)*sin(al), a*sin(th)},
{     0,            sin(al),         cos(al),        d      },
{     0,                 0,               0,            1      } };
Matrix.Copy((float*)trans,4,4,(float*)T);
}

// Calcule la position de la chaine robotique
void forwardKinematics(double* sig,double* a,double* al,double* d,double* th,double* q, int nbLinks, float X[6]){
float A[4][4]={{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}};
float R[4][4];
float Ti[4][4];

// Compute transformation matrix
if (sig[i]==0){
th[i]=q[i];
}else{
d[i]=q[i];
}
//A=A*Ti
Matrix.Multiply((float*)A, (float*)Ti,4, 4,4, (float*)R);
Matrix.Copy((float*)R,4,4,(float*)A);
}

X[0]=A[0][3];
X[1]=A[1][3];
X[2]=A[2][3];
X[3]=0;
X[4]=0;
X[5]=0;
}

// Calcule un produit vectoriel
void CrossProduct1D(float* a, float* b, float* r){
/*Ex: double v1[3]={1,0,0};
double v2[3]={0,0,1};
double w[3];
CrossProduct1D(v1,v2,w);*/

r[0] = a[1]*b[2]-a[2]*b[1];
r[1] = a[2]*b[0]-a[0]*b[2];
r[2] = a[0]*b[1]-a[1]*b[0];

}

// Calcule la matrice jacobienne d'une chaine robotique
void jacobian(double* sig,double* a,double* al,double* d,double* th,double* q, int nbLinks, float* J){
float Ti[4][4];
float A[4][4]={{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}};
float R[4][4];
float v[4];
O[0][0]=0;
O[0][1]=0;
O[0][2]=0;

z[0][0]=0;
z[0][1]=0;
z[0][2]=1;

float v0[4]={0,0,1,1};

// Compute transformation matrix
if (sig[i]==0){
th[i]=q[i];
}else{
d[i]=q[i];
}
// A=A*T[j];
Matrix.Multiply((float*)A, (float*)Ti,4, 4,4, (float*)R);
Matrix.Copy((float*)R,4,4,(float*)A);
//Compute joint center and orientation vector
O[i+1][0]=A[0][3];
O[i+1][1]=A[1][3];
O[i+1][2]=A[2][3];

Matrix.Multiply((float*)A, (float*)v0,4, 4,1, (float*)v);
//z{i}=[v(1); v(2); v(3)]-O{i};
z[i+1][0]=v[0]-O[i+1][0];
z[i+1][1]=v[1]-O[i+1][1];
z[i+1][2]=v[2]-O[i+1][2];

}

//Jacobian
float tempv[3];
float cprod[3];

if (sig[i]==0){
// Ji=[z(i-1)xON-O(i-1); z(i-1)]
CrossProduct1D(z[i],tempv,cprod);
}else{
}
}
}

// Calcul le déterminant d'une matrice
{
int i,j,m,n,c;
if(k==1)
{
return(a[0][0]);
}
else
{
det=0;
for(c=0;c<k;c++)
{
m=0;
n=0;
for(i=0;i<k;i++)
{
for(j=0;j<k;j++)
{
b[i][j]=0;
if(i!=0&&j!=c)
{
b[m][n]=a[i][j];
if(n<(k-2))
n++;
else
{
n=0;
m++;
}
}
}
}
det=det+s*(a[0][c]*detrm(b,k-1));
s=-1*s;
}
}
return(det);
}
// Calcule les angles moteur en fonction de la position désirée
void computeAngle(){
double detJ;
//    compute jacobian

// avoid singularity

// invert jacobian
if(detJ>=1){
}

qu[i]=0;
for(int j=0;j<6;j++){
qu[i]+=invJ[i][j]*Xu[j];
}
}
}

// Convertie l'angle moteur en commande PWM
int jointToImp(double x,int i) {
int imp=(x - joint_min[i]) * (imp_max[i]-imp_min[i]) / (joint_max[i]-joint_min[i]) + imp_min[i];
imp=max(imp,imp_min[i]);
imp=min(imp,imp_max[i]);
return imp;
}

// Intitialise servomoteur
void servoAttach(){
for (int i=0; i<nbLinks; i++) {
servo[i].attach(servoPin[i]);
}
}

// Envoie la commande PWM à chaque servomoteur
for (int i=0; i<nbLinks; i++) {
servo[i].writeMicroseconds(jointToImp(q[i],i));
delay(velocity);
}
}

// Met le robot en position de repos
void restR(){
servoToImp(restPos,500);
}

// Déconnecte les servomoteurs
void stopR(){
servo[j].detach();
}
}

```

# Result

The robot follow the trajectory and is able to draw a circle on the paper