Robotics Lab Project

Submitted to: Engr. Shehbaz Khan

Group Members:
Abdul Moiz Qarni
Kashan Ahmad Khan
Ahmed Rauf Qureshi
Registration #:
17PWMCT0564
17PWMCT0576
17PWMCT0588

University of Engineering & Technology

Peshawar

1
Contents
Outline............................................................................................................................................. 4
Introduction .................................................................................................................................... 4
Problem Statement: ........................................................................................................................ 4
Problem Solution: ........................................................................................................................... 4
The Robot: ....................................................................................................................................... 4
Braccio Robotic Arm: .................................................................................................................. 5
Our Robotic Arm: ........................................................................................................................ 5
Requirements:................................................................................................................................. 6
Hardware: ................................................................................................................................... 6
Software: ..................................................................................................................................... 6
Methodology:.................................................................................................................................. 6
1) Construct a Hardware Arm .................................................................................................. 6
2) Test the Arm with Arduino .................................................................................................. 6
3) Configure ROS with Arduino ................................................................................................ 6
4) Perform the Robotic Manipulation with ROS ...................................................................... 7
Forward Kinematics of the Braccio Robotic Arm: ........................................................................... 7
Developing the DH Table: ........................................................................................................... 7
Step 0: Number of Joints......................................................................................................... 7
Step 1: Assigning coordinate frame 0 to the robot base. ....................................................... 7
Step 2: Align zi with the axis of joint i+1. ................................................................................ 8
Step 3: Locate the origin oi at the intersection of zi and zi-1 ................................................... 8
Step 4: Assigning the x coordinates ........................................................................................ 9
Step 5: Assigning the y coordinate ....................................................................................... 10
Step 6: Set origin on at the tool tip ........................................................................................ 10
Step 7: Finding the bi............................................................................................................. 10
Step 8: di = distance from origin oi-1 to bi measured along zi-1 ............................................. 11
Step 9: ai-1 = distance from bi to oi measured along xi .......................................................... 11
Step 10: θi = angle of rotation from xi-1 to xi measured along zi-1 ......................................... 11
Step 11: αi = angle of rotation from zi-1 to zi measured along xi............................................ 12
DH Table: ............................................................................................................................... 12

2
 Finding Forward Kinematics: .................................................................................................... 12
Matlab Simulation:........................................................................................................................ 14
Conclusion ..................................................................................................................................... 15
Appendix: ...................................................................................................................................... 15
Matlab program used: .............................................................................................................. 15
Arduino Program used: ............................................................................................................. 15
References .................................................................................................................................... 17

3
 Robotic Arm Manipulation via ROS
Outline
In this project we will use ROS (Robotic Operating System) to communicate with an Arduino
board to manipulate a simple 6-DOF robotic arm to simulate a simple pick and drop operation.

Introduction
 This project focuses on basic use of ROS and how to interface it with other (Prototyping
and Industrial) standards PLCs and Automation Modules.
 In this specific attempt, we will learn how to interface it with an Arduino Board for
prototyping and performing basic operations.
 By doing so we will learn the applications of robotics and automation in practical life.

Problem Statement:
Robot arms are ideal for operations, in the places which needs maximum accuracy, efficiency
and nonstop operation, human operation can possibly cause errors and the production, or the
stage of production is limited by the work load and speed of human operator, and all the above
mentioned facts.

Problem Solution:
Robotic ARM! Robotic arms can be programmed or taught to operate in a sequence nonstop
and with max efficiency without any possible error (keeping the Conditions and requirements
optimum for the system’s functionality). It can decrease time required to do an operation and
increase the amount of work done and set of operations per unit time. Hence eliminating
human error and speeding up the work.

The Robot:
The robot used in this project is a Braccio Robotic Arm. Now the mathematics and the ROS
manipulation is done based on the parameters of an actual Braccio robotic arm. However for
the hardware a similar attempt of a DIY Braccio Arm is used.

4
Braccio Robotic Arm:
The TinkerKit Braccio is a fully operational robotic arm, controlled via Arduino. It can be
assembled in several ways for multiple tasks such as moving objects. You can also attach a
camera or solar panel. There are so many ways in which the Braccio can extend the reach of
your devices. [1]

Figure 1 Braccio Robotic Arm

Our Robotic Arm:

The physical Robotic arm used was the one made by hand in a DIY fashion and it’s as shown
below:

Figure 2 our robot used

5
Requirements:
Hardware:
 3x MG996R 180◦ Servos
 3x TowerPro MG90S 180◦ Servos
 1x Arduino Nano Board
 Connecting wires
 Batteries
 Servo Brackets

Software:
 Arduino IDE
 ROS containing system
 MoveIt
 Rviz
 Matlab

Methodology:
1) Construct a Hardware Arm
The hardware consists of cardboard box, on which the base servo is mounted and all the rest of
the servos are attached to the base servo using Servo Brackets. The electronics are soldered on
a perfboard and the circuit is placed inside the box. An external power supply is used to power
the 7V servos, while 5V servos are using Arduino Nano’s 5V pin.

2) Test the Arm with Arduino

The system underwent checking and troubleshooting with just Arduino as the robot was made
to undergo various sort of manipulation controlled externally from potentiometers and other
various varying inputs. Once the robot was tested out to be working fine, the next step was
considered.

3) Configure ROS with Arduino

To complete this connection a library called rosserial [2] was installed and the necessary steps
taking to establish the connection were considered. After the installation and interference a
few steps were considered to verify and test the connection, making sure that it was ready for
the next step.

6
 4) Perform the Robotic Manipulation with ROS
For the ROS manipulation, first of all a repository containing the package for the Braccio robot
[3] needs to be cloned, after doing that the catkin workspace needs to be built. Next a
respective code for the robot needs to be uploaded on the Arduino connected and then the
ROS process needs to be initiated. If done correctly, the parse and publisher from the Rviz
program would allow the Arduino to successfully be controlled by the ROS program and the
robot would be manipulated accordingly.

Forward Kinematics of the Braccio Robotic Arm:

With the Robotic arm functioning, the Forward Kinematics of it needs to be calculated, to
develop the equations of them, the DH table needs to be developed first.

Developing the DH Table:

For developing the Forward Kinematics of it, we first to develop the DH table. To develop one,
the following steps need to be followed.

Step 0: Number of Joints

Let’s consider the following diagram of the robotic arm, from the displayed arrows we can see
that there are total of 6 numbers of joints

Figure 3 Step 0: Identifying Joints

Step 1: Assigning coordinate frame 0 to the robot base.

In this the base of the robotic arm is going to be assigned ‘0’ as show in the figure below

7
 Figure 4 Step 1: the 0th frame

Step 2: Align zi with the axis of joint i+1.

Now let’s consider a skeletal diagram of our robotic arm, with this given, we can assign all the z
values to the frame

Figure 5 Assigning z

Step 3: Locate the origin oi at the intersection of zi and zi-1

This is shown in the Figure below

8
 Figure 6 Assigned the Origin

Step 4: Assigning the x coordinates

In the below figure are the xi and xi+1 coordinates

Figure 7 Assigning the x coordinates

9
Step 5: Assigning the y coordinate
In the below figure are the yi and yi+1 coordinates

Figure 8 Assigning y frame

Step 6: Set origin on at the tool tip

This part of the steps is already solved in the steps about with the x6, y6 and z6 having assigned
the directions as shown.

Step 7: Finding the bi

This would be by finding the intersection between xi and zi-1. After doing so, the result given
would be:

Figure 9 Finding the bi

10
Step 8: di = distance from origin oi-1 to bi measured along zi-1
A general measurement of the link length of the robot is used, the result is:
d1 = 0
d2 = 0
d3 = d3
d4 = d4
d5 = 0
d6 = 0

Step 9: ai-1 = distance from bi to oi measured along xi

Likewise the distance used in this one is a generalized distance of the robot. The result is:
a0 = 0
a1 = 0
a2 = a2
a3 = a3
a4 = 0
a5 = 0

Step 10: θi = angle of rotation from xi-1 to xi measured along zi-1

Using general parameters, the information is:
θ1 = θ1
θ2 = θ2
θ3 = θ3
θ4 = θ4
θ5 = θ5
θ6 = θ6

11
Step 11: αi = angle of rotation from zi-1 to zi measured along xi
Again using standardized parameters the result is:
α0 = 0
α1 = -90◦
α2 = 0
α3 = -90◦
α4 = 90◦
α5 = -90◦

DH Table:
Now with all the steps completed, we finally obtain the DH table as:

Axis θi di ai-1 αi-1

1 θ1 0 0 0
2 θ2 0 0 -90◦
3 θ3 d3 a2 0
4 θ4 d4 a3 -90
5 θ5 0 0 90
6 θ6 0 0 -90

Finding Forward Kinematics:

With the DH table developed, we can now find the forward kinematics of our robotic arm as:
𝑐1 −𝑠1 0 0
0 𝑠1 𝑐1 0 0
1𝑇 =
0 0 1 0
0 0 0 1
𝑐2 −𝑠2 0 0
1 0 0 1 0
2𝑇 =
−𝑠2 −𝑐2 0 0
0 0 0 1
𝑐3 −𝑠3 0 𝑎2
2 𝑠3 𝑐3 0 0
3𝑇 =
0 0 1 𝑑3
0 0 0 1
12
 𝑐4 −𝑠4 0 𝑎3
3 0 0 1 𝑑4
4𝑇 =
−𝑠4 −𝑐4 0 0
0 0 0 1
𝑐5 −𝑠5 0 0
4 0 0 −1 0
5𝑇 =
−𝑠5 −𝑐5 0 0
0 0 0 1
𝑐6 −𝑠6 0 0
5 0 0 1 0
6𝑇 =
−𝑠6 −𝑐6 0 0
0 0 0 1
Now to get the final FK equation we multiply all the matrices to get:
𝑟11 𝑟12 𝑟13 𝑝𝑥
0 𝑟21 𝑟22 𝑟23 𝑝𝑦
6𝑇 = 𝑟 𝑟32 𝑟33 𝑝𝑧
31
0 0 0 1
Where:
𝑟11 = 𝑐1 [𝑐23 (𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 ) − 𝑠23 𝑠5 𝑠6 ] + 𝑠1 (𝑠4 𝑐5 𝑐6 + 𝑐4 𝑠6 )
𝑟21 = 𝑠1 [𝑐23 (𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 ) − 𝑠23 𝑠5 𝑠6 ] − 𝑐1 (𝑠4 𝑐5 𝑐6 + 𝑐4 𝑠6 )
𝑟31 = −𝑠23 (𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6 ) − 𝑐23 𝑠5 𝑠6
𝑟12 = 𝑐1 [𝑐23 (−𝑐4 𝑐5 𝑐6 − 𝑠4 𝑐6 ) − 𝑠23 𝑠5 𝑠6 ] + 𝑠1 (−𝑠4 𝑐5 𝑠6 + 𝑐4 𝑠6 )
𝑟22 = 𝑠1 [𝑐23 (−𝑐4 𝑐5 𝑐6 − 𝑠4 𝑐6 ) + 𝑠23 𝑠5 𝑠6 ] + 𝑠1 (−𝑠4 𝑐5 𝑠6 + 𝑐4 𝑠6 )
𝑟32 = −𝑠23 (𝑐4 𝑐5 𝑐6 + 𝑠4 𝑐6 ) + 𝑐23 𝑠5 𝑠6
𝑟13 = −𝑐1 (𝑐23 𝑐4 𝑠5 + 𝑠23 𝑐5 ) − 𝑠1 𝑠4 𝑠5
𝑟23 = −𝑠1 (𝑐23 𝑐4 𝑠5 + 𝑠23 𝑐5 ) + 𝑐1 𝑠4 𝑠5
𝑟33 = 𝑠22 𝑐4 𝑠5 − 𝑐23 𝑐5
𝑝𝑥 = 𝑐1 (𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23 ) − 𝑑3 𝑠1
𝑝𝑦 = 𝑠1 (𝑎2 𝑐2 + 𝑎3 𝑐23 − 𝑑4 𝑠23 ) + 𝑑3 𝑐1

𝑝𝑧 = −𝑎3 𝑠23 − 𝑠2 𝑎2 − 𝑑4 𝑐23

13
Matlab Simulation:
After now having all the calculations done, the Matlab simulation part can take place, this
allows to verify the result and output a plotted instant of the robot with respect to the
information given.
Matlab however does require all the variables to be defined, therefore the link lengths would
be given as:

 L1 = 0.1
 L2 = 0.4
 L3 = 0.1
 L4 = 0.3
With these input parameters given, the program can be ran.
Running the program outputs the following DH table:

Figure 10 Matlab DH table

Which is synonymous to the DH table calculated earlier, and then the following robot instant
would be plotted as an output:

Figure 11 Matlab plotting of robot

14
Conclusion
This project was about controlling a 6DoF robot via ROS, this was achieved by assuming a Robot
first, which was chosen to be a Braccio Robotic Arm, then a physical replica was constructed of
it. Afterwards the packages required for this project were obtained and then ran, which had a
final result of the robot being able to be manipulated via ROS. Afterwards the Forward
Kinematics of the robot were calculated manually and via Matlab.

Appendix:
Matlab program used:
startup_rvc
clc
clear all
L1 = 0.1; L2= 0.4; L3 = 0.1; L4 = 0.3;
L(1) = Link([0, 0, 0, 0, 0])
L(2) = Link([0, 0, 0, -pi/2, 0])
L(3) = Link([0, L1, L2, 0, 0])
L(4) = Link([0, L3, L4, -pi/2, 0])
L(5) = Link([0, 0, 0, pi/2, 0])
L(6) = Link([0, 0, 0, -pi/2, 0])
Rob = SerialLink(L)
Rob.name = 'Braccio'
Rob.plot([0,pi/2,0,pi/2,0,0])

Arduino Program used:

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <WProgram.h>
#endif

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/JointState.h>

ros::NodeHandle nh;
Servo roll;
Servo crap;
Servo gripper;
Servo wrist;
Servo elbow;
Servo shoulder2;
Servo base;

double base_angle=0;
double shoulder_angle=0;
double elbow_angle=0;
double wrist_angle= 0;

15
double gripper_angle=0;
double roll_angle=0;
double roll_angle2=0;

void servo_cb(const sensor_msgs::JointState& cmd_msg){

base_angle=radiansToDegrees(cmd_msg.position[0]);
shoulder_angle=radiansToDegrees(cmd_msg.position[1]);
elbow_angle=radiansToDegrees(cmd_msg.position[2]);
wrist_angle=radiansToDegrees(cmd_msg.position[3]);
gripper_angle=radiansToDegrees(cmd_msg.position[4]);
roll_angle=radiansToDegrees(cmd_msg.position[5]);
roll_angle2=radiansToDegrees(cmd_msg.position[6]);

base.write(base_angle);
shoulder2.write(shoulder_angle);
elbow.write(elbow_angle);
wrist.write(wrist_angle);
gripper.write(gripper_angle);
roll.write(roll_angle2);

ros::Subscriber<sensor_msgs::JointState> sub("joint_states", servo_cb);

void setup(){
nh.getHardware()->setBaud(115200);
nh.initNode();
nh.subscribe(sub);

base.attach(2); //base_joint
elbow.attach(8); //elbow_joint
wrist.attach(6); //wrist_pitch_joint
roll.attach(5);//6 //wrist_roll
shoulder2.attach(9); //shoulder_joint
gripper.attach(11); //gripper_joint

delay(1);
base.write(0);
shoulder2.write(0);
elbow.write(0);
wrist.write(0);
gripper.write(0);
roll.write(0);
}

void loop(){
nh.spinOnce();
}

double radiansToDegrees(float position_radians)

{

position_radians = position_radians + 1.6;

return position_radians * 57.2958;

16
}

References

[1] [Online]. Available: https://store.arduino.cc/usa/tinkerkit-braccio.

[2] [Online]. Available: http://wiki.ros.org/rosserial_arduino/Tutorials.

[3] [Online]. Available: https://github.com/ohlr/braccio_arduino_ros_rviz.

17