Design, Simulation and Testing of a Controller

And

Software Framework for Automated Construction by a Robotic Manipulator

By

Sushrut Jigneshbhai Gandhi

A Thesis Presented in Partial Fulfillment

of the Requirements for the Degree
Master of Science

Approved April 2019 by the

Graduate Supervisory Committee:

Spring Berman, Chair

Hamidreza Marvi
Sze Zheng Yong

ARIZONA STATE UNIVERSITY

May 2019
 ABSTRACT

The construction industry is very mundane and tiring for workers without the assistance

of machines. This challenge has changed the trend of construction industry tremendously

by motivating the development of robots that can replace human workers. This thesis

presents a computed torque controller that is designed to produce movements by a small-

scale, 5 degree-of-freedom (DOF) robotic arm that are useful for construction operations,

specifically bricklaying. A software framework for the robotic arm with motion and path

planning features and different control capabilities has also been developed using the Robot

Operating System (ROS).

First, a literature review of bricklaying construction activity and existing robots’

performance is discussed. After describing an overview of the required robot structure, a

mathematical model is presented for the 5-DOF robotic arm. A model-based computed

torque controller is designed for the nonlinear dynamic robotic arm, taking into

consideration the dynamic and kinematic properties of the arm. For sustainable growth of

this technology so that it is affordable to the masses, it is important that the energy

consumption by the robot is optimized. In this thesis, the trajectory of the robotic arm is

optimized using sequential quadratic programming. The results of the energy optimization

procedure are also analyzed for different possible trajectories.

A construction test bed setup is simulated in the ROS platform to validate the designed

controllers and optimized robot trajectories on different experimental scenarios. A

commercially available 5-DOF robotic arm is modeled in the ROS simulators Gazebo and

Rviz. The path and motion planning are performed using the MoveIt-ROS interface, and

i
are also implemented on a physical small-scale robotic arm. A Matlab-ROS framework for

execution of different controllers on the physical robot is described. Finally, the results of

the controller simulation and experiments are discussed in detail.

ii
 ACKNOWLEDGMENTS

I would like to thank my advisor, Dr. Spring Berman for believing in me, and for giving

me this wonderful opportunity to work in her lab. I have been extremely lucky to have an

advisor who motivated me to learn and work harder. I will forever be grateful to her

guidance during my journey in ASU.

I would like to thank Dr. Marvi for showing me the field of robotics and teaching me

the modeling strategies for manipulation. I will miss his lectures and interesting videos of

new unique robots.

I would also like to thank Dr. Yong for introducing me to concepts of controls, which

helped me throughout my thesis work. I hope the long discussions on control with Dr. Yong

will guide me throughout my career.

I would like to thank all my ACS lab members and specially Hamed Farivarnejad for

guiding me in design of controllers. I am also thankful to Shreyans and Subhrajyoti for

helping me in my research and thesis documentation.

Finally, I would like to express profound gratitude to my family and friends for

providing me with support and encouragement throughout the process of researching and

writing this thesis. This journey is difficult to imagine without them.

iii
 TABLE OF CONTENTS

Page

LIST OF TABLES .................................................................................................................... v

LIST OF FIGURES .................................................................................................................vi

CHAPTER

1 INTRODUCTION ................................................................................................ 1

1.1 Literature Review .............................................................................................. 1

1.2 Outline of the Thesis ......................................................................................... 2

1.3 Contributions of the Thesis ............................................................................... 2

2 MATHEMATICAL MODELING OF ROBOTIC ARM..................................... 3

2.1 Rotational Matrices ........................................................................................... 3

2.2 Homogeneous Transformation .......................................................................... 5

2.3 Forward Kinematics .......................................................................................... 5

2.4 Denavit - Hartenberg Convention (D-H) .......................................................... 6

2.5 PhantomX Reactor Robotic Arm ...................................................................... 8

3 DESIGN AND SIMULATION OF CONTROLLER ........................................ 12

3.1 Jacobian ........................................................................................................... 12

3.2 Singularity........................................................................................................ 12

3.3 Dynamics ......................................................................................................... 13

3.3.1 Lagrange Method .................................................................................. 13

3.4 Control of Manipulators .................................................................................. 17

iv
 CHAPTER Page

3.4.1 Computed Torque Control .................................................................... 17

4 TRAJECTORY PLANNING WITH OPTIMIZED ENERGY

CONSUMPTION ................................................................................................. 21

4.1 Mathematical Modeling of Objective Function ............................................. 21

4.2 Trajectory & Constraints ................................................................................. 22

4.3 Optimization Results ....................................................................................... 24

4.4 Results and Discussions .................................................................................. 33

5 SOFTWARE FRAMEWORK FOR CONSTRUCTION TESTBED ................ 37

5.1 Robotic Arm Compatible with ROS ............................................................... 37

5.2 ROS Structure ................................................................................................. 38

5.3 Modeling of Robotic Arm in ROS .................................................................. 39

5.4 Motion Planning with MoveIt! ....................................................................... 40

6 SIMULATION AND EXPERIMENTAL RESULTS ........................................ 43

6.1 Simulation of Computed Torque Controller ................................................... 43

6.2 Experiment Setup ............................................................................................ 46

6.3 Matlab-ROS Interface ..................................................................................... 46

7 CONCLUSION AND FUTURE WORK ........................................................... 54

7.1 Conclusion ....................................................................................................... 54

7.2 Future Work ..................................................................................................... 55

REFERENCES ...................................................................................................................... 56

v
CHAPTER Page

APPENDIX

A MATLAB - GAZEBO INTERFACE .................................................................... 59

vi
 LIST OF TABLES

Table Page

1. Values of Robot Parameters ..................................................................................... 8

2. DH Parameters ......................................................................................................... 9

3. Initial and Final Conditions .................................................................................... 20

4. Initial Conditions .................................................................................................... 21

5. Initial Guesses of the Variables.............................................................................. 21

6. Optimization Results .............................................................................................. 22

vii
 LIST OF FIGURES

Figure Page

2.1 Rotation of Frame about Z-axis ............................................................................ 3

2.2 Rotation of Frame about X-axis and Y-axis ......................................................... 4

2.3 Representation of Point P in Different Frame ....................................................... 5

2.4 Denavit-Hartenberg Parameters ............................................................................. 6

2.5 PhantomX Reactor Robotic Arm ........................................................................... 9

3.1 Block Diagram of Computed Torque Control ...................................................... 17

4.1 Joint Position for T = 5 Seconds ......................................................................... 22

4.2 Joint Velocity for T = 5 Seconds ......................................................................... 23

4.3 Joint Acceleration for T = 5 Seconds................................................................... 23

4.4 End Effector Trajectory for T = 5 Seconds ......................................................... 24

4.5 Joint Position for T = 3 Seconds .......................................................................... 24

4.6 Joint Velocity for T = 3 Seconds ......................................................................... 25

4.7 Joint Acceleration for T = 3 Seconds................................................................... 25

4.8 End Effector Trajectory for T = 3 Seconds ......................................................... 26

4.9 Joint Position for T = 2 Seconds .......................................................................... 26

4.10 Joint Velocity for T = 2 Seconds ....................................................................... 27

4.11 Joint Acceleration for T = 2 Seconds................................................................. 27

4.12 End Effector Trajectory for T = 2 Seconds ....................................................... 28

4.13 Joint Position for T = 1 Seconds ........................................................................ 28

4.14 Joint Velocity for T = 1 Seconds ....................................................................... 29

viii
Figure Page

4.15 Joint Acceleration for T = 1 Seconds................................................................. 29

4.16 End Effector Trajectory for T = 1 Seconds ....................................................... 30

4.17 Joint Position for T = 2.5 Seconds ..................................................................... 31

4.18 Joint Velocity for T = 2.5 Seconds .................................................................... 31

4.19 Joint Acceleration for T = 2.5 Seconds .............................................................. 32

4.20 End Effector Trajectory for T = 2.5 Seconds ..................................................... 32

4.21 Comparison of End-Eff. Trajectories for Same Initial and Final Position ........ 33

4.22 Power Vs Time .................................................................................................... 33

5.1 MoveIt-architecture Diagram ................................................................................ 38

6.1 Position-time Graph for Kp1 and Kd1 .................................................................. 41

6.2 Velocity-time Graph for Kp1 and Kd1 ................................................................. 42

6.3 Error-time Graph for Kp1 and Kd1 ...................................................................... 42

6.4 Position-time Graph for Kp2 and Kd2 .................................................................. 43

6.5 Velocity-time Graph for Kp2 and Kd2 ................................................................. 43

6.6 Error-time Graph for Kp2 and Kd2 ...................................................................... 44

6.7 MoveIt Setup Assistant ......................................................................................... 45

6.8 MoveIt Motion Planning in Rviz .......................................................................... 46

6.9 Matlab-ROS Workflow ......................................................................................... 46

6.10 Flowchart of Torque Controller with Matlab-ROS Interface ............................ 47

6.11 Setup of PhantomX Reactor Arm ....................................................................... 48

6.12 Pick and Place Action and Gazebo Model of PhantomX Robot ....................... 49

ix
 CHAPTER 1

1.1 Introduction

Over the past decade, a major focus in technological innovations has been to make

industrial processes autonomous. In recent years, there has been a tremendous increase in

demand and use of robots to replace humans for performing mundane and tiring jobs. As

the construction industry has been booming over the last few years, the labor shortage is a

major concern for efficient productivity. According to a survey by the National Association

of Homebuilders (QUOCTRUNG BUI 2018), sixty percent of contractors are finding it

difficult to recruit skilled laborers for their projects. As trained labors are aging out of the

industry, robots equipped with effective autonomous technologies will increase

productivity and efficiency. Currently, extensive research is being conducted to make the

bricklaying process independent of humans. Robots like Hadrian X (Pivac 2016) and

SAM100 (PODKAMINER 2016) have been demonstrated to be five times faster than

human workers. According to (PODKAMINER 2016), SAM100 can lay 300-400 bricks

per hour compared to a human bricklayer’s 60-75 bricks. There are currently different types

of robots producing houses and industrial structures. Hadrian X uses pre-fabricated walls

to construct a house in 3 days (Quirke 2018), and SAM100 uses bricks and mortar for

building walls. Bricklaying is difficult to automate, due to the uncertain environment of

the construction sites. Describing the scenario of automation in bricklaying, one of the

participants of the Las Vegas Bricklaying competition said that it is difficult for a robot to

judge how much mortar is enough for particular brick or how to build a round wall with

square bricks (QUOCTRUNG BUI 2018). The current robots are also not flexible and

1
mobile enough to build walls higher than 2 meters. These issues are preventing robots from

being widely used in the construction industry.

1.2 Outline of the thesis

This thesis aims to achieve better movement of robotic arms that are used in bricklaying

operations by designing a model-based controller for such robots and considering the

nonlinearity of the robot dynamics. It is useful to have a software framework that can be

used to simulate the robot operating with different controllers before implementing them

on a real robot.

This thesis describes the controller design, simulation and software implementation for

a 5 degree-of-freedom (DOF) PhantomX Reactor robot arm (Rick 2017). From a

Solidworks design, the link and joint properties were considered for the controller design.

The required design concepts are discussed in Chapter 2. The computed torque controller

design and simulation are described in Chapter 3. In industry, when the robot is used for

mass production, it is important to minimize the energy consumption of the robot along its

trajectory. This optimization procedure is described in Chapter 4. Chapter 5 explains the

software requirements and software setup for path planning and controller implementation.

The simulation results, experimental setup and results are illustrated in Chapter 6. The

conclusion and future possible modifications are explained in Chapter 7.

2
 1.3 Contributions of the thesis

In this thesis, a computed torque controller has been designed for a nonlinear 5 degree-

of-freedom robotic arm to perform movements that are useful for bricklaying operations.

A new method of trajectory optimization is implemented to make the robot energy

efficient. A software framework is developed to integrate modeling, path planning,

controller implementation and simulation of the robotic arm. This setup can be

implemented on any robotic arm by just changing its Unified Robot Description Format

(URDF) files.

3
 CHAPTER 2

MATHEMATICAL MODELING OF ROBOTIC ARM

2.1 Rotational Matrices

The rotation matrix R is very important in modeling a robotic arm. It transforms the

current coordinate frame to a reference frame and vice-versa. The column vectors of the

rotation matrix are orthogonal to each other, as they are unit vectors of an orthonormal

frame. The rotation matrix is in the Special Orthonormal group SO(n) for m × m real

matrices, because of these special properties:

• Rotation matrices have a determinant of 1.

• Rotation matrices are orthogonal matrices: RT = R-1

Figure 2.1 Rotation of coordinate frame about its z-axis

The rotation matrix for rotating a coordinate frame about its z-axis by angle φ is:

𝑐𝑜𝑠φ −𝑠𝑖𝑛φ 0
𝑅𝑧(φ) = [ 𝑠𝑖𝑛φ 𝑐𝑜𝑠φ 0]
0 0 1

4
Figure 2.2 Rotation of frame about x-axis and y-axis, respectively

The rotation matrix for rotating a frame about its x-axis by angle ψ and y-axis by angle ϕ,

respectively, is:

1 0 0
𝑅𝑥(ψ) = [0 𝑐𝑜𝑠ψ −𝑠𝑖𝑛ψ]
0 𝑠𝑖𝑛ψ 𝑐𝑜𝑠ψ

𝑐𝑜𝑠ϕ 0 𝑠𝑖𝑛ϕ
𝑅𝑦(ϕ) = [ 0 1 0 ]
−𝑠𝑖𝑛ϕ 0 𝑐𝑜𝑠ϕ

These matrices are useful to describe rotation about any arbitrary axis.

2.2 Homogeneous Transformation

The configuration of a rigid body is expressed by the translation of the body from a

reference frame and the body’s orientation relative to this frame.

5
Figure 2.3 Representation of a point P in two different frames

Consider an arbitrary point P in three-dimensional space (Figure 2.3). Here, pB contains

the vector coordinates of point P with respect to the origin of reference frame B. The vector

from the origin of frame B to the origin of frame A is represented by pAB. Let BrA be the

rotation matrix of frame A with respect to frame B. Then pB will be,

pB = pAB + BrApA

The rigid transformation (pAB, BrA) is in the special Euclidean group SE(3) and can be

represented by a 4×4 matrix which is called the homogeneous transformation matrix.

2.3 Forward Kinematics

A robotic arm consists of a series of links which are connected by joints. The joints

are usually of two types: revolute and prismatic. The whole setup is called a kinematic

chain, where one end of the robotic arm is attached to a fixed base and the other end is

attached to an end-effector.

6
 The structure of a robotic arm can be described by the number of degrees-of-freedom

(DOFs) associated with the arm. The configuration space of the arm is comprised of all

possible values of the joint variables, which are angles for revolute joints and distances for

prismatic joints. The configuration space of an arm with r revolute joints and p prismatic

joints has (r + p) DOFs. The forward kinematics can be used to compute the pose of the

end-effector with respect to base of the robot. This computation is done using particular

functions of joint variables, described in the next section.

2.4 Denavit-Hartenberg (D-H) Convention

The most commonly used method to calculate forward kinematics for an open-chain

robotic arm is the Denavit-Hartenberg (D-H) method. The D-H method derives the position

and orientation of a link with respect to an adjacent link. The DH parameters of the

manipulator denote the geometric relations between its joints and links. These parameters

are used for the dynamic modelling of the robot.

Figure 2.4 Denavit-Hartenberg (D-H) parameters (Siciliano et al. 2009, p.62)

7
 There are specific rules for the D-H convention (Siciliano et al. 2009, p.62), outlined

below (see Figure 2.4 for notation):

• Select axis Zi along the axis of joint i+1.

• Find the common normal to the Zi-1 and Zi axes and intersect it with axis Zi. The

intersection point is considered to be the origin point Oi .

• Select axis Xi with the same direction as the common normal to the axes Zi-1 and

Zi. This axis points from joint i to joint i+1.

• Select the axis Yi to make the coordinate frame a right-handed frame.

The convention has some exceptions and assumptions.

• For the frame at joint 1, the Z0 axis direction is specified by the joint orientation.

The X0 axis and the origin of the frame are defined arbitrarily.

• If two continuous frames are parallel, then the common normal between them is

not defined uniquely.

• When two Z axes intersect each other, then the X axis direction can be chosen

arbitrarily.

• If joint i is prismatic, then the Zi-1 axis direction is chosen arbitrarily.

After establishing these frame conventions, the DH parameters can be used to find the

positions and orientations of the robotic arm frames. The DH parameters are defined as:

ai = Distance between the frame origins Oi and Oi’

di = Offset of Oi’ from Oi-1

𝛼𝑖 = Angle between axes Zi-1 and Zi about axis Xi

𝜃𝑖 = Angle between axes Xi-1 and Xi about axis Zi-1

8
 (An angle is considered positive if the rotation is made counter-clockwise.)

If a joint is revolute, then its joint variable is 𝜃𝑖 , and if a joint is prismatic, then its

joint variable is di.

The relationship between two adjacent frames is determined by a 4×4

homogeneous transformation matrix. For example, translate the frame with origin Oi-1 by

distance di along axis Zi-1 and rotate is about Zi-1 by an angle 𝜃𝑖 . The associated

transformation matrix is:

𝑐𝜃𝑖 −𝑠𝜃𝑖 0 0
𝑠𝜃 𝑐𝜃𝑖 0 0
Ai’ = [ 𝑖 ]
0 0 1 𝑑𝑖
0 0 0 1

(Here, 𝑐𝜃 = cos 𝜃 and 𝑠𝜃 = sin 𝜃.)

Now translate the frame by distance ai along axis Xi’ and rotate it about Xi’ by an

angle 𝛼𝑖 . The associated transformation matrix is:

1 0 0 𝑎𝑖
0 𝑐𝛼𝑖 −𝑠𝛼𝑖 0
Ai = [ ]
0 𝑠𝛼𝑖 𝑐𝛼𝑖 0
0 0 0 1

The resulting homogeneous transformation matrix after post-multiplication is:

𝑐𝜃𝑖 −𝑠𝜃𝑖 𝑐𝛼𝑖 𝑠𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑐𝜃𝑖

𝑠𝜃 𝑐𝜃𝑖 𝑐𝛼𝑖 −𝑐𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑠𝜃𝑖
A = Ai’Ai= [ 𝑖 ]
0 𝑠𝛼𝑖 𝑐𝛼𝑖 𝑑𝑖
0 0 0 1

Following this method, it is possible to find the position and orientation of the end-

effector with respect to the base of the robotic arm. For a 5-DOF robotic arm, this

configuration can be derived by post-multiplication of 5 rotation matrices. Here, the

forward kinematics of a 5-DOF robotic arm is derived using the DH parameters.

9
 2.5 PhantomX Reactor Robotic Arm

In this section, the DH parameters of a PhantomX Reactor robotic arm (Figure 2.5) are

computed, and the kinematics of the robot are modeled. The robot has 5 DOFs, and all five

of its joints are revolute joints.

Table 1. Values of Robot Parameters

𝑎0 = 0 𝑐𝑚 𝑑0 = 0 𝑐𝑚

𝑎1 = 0 𝑐𝑚 𝑑1 = 4 𝑐𝑚

𝑎2 = 14.611 𝑐𝑚 𝑑2 = 3.97 𝑐𝑚

𝑎3 = 14.551 𝑐𝑚 𝑑3 = 0 𝑐𝑚

𝑎4 = 0 𝑐𝑚 𝑑4 = 0 𝑐𝑚

𝑎5 = 0 𝑐𝑚 𝑑5 = 4.6 𝑐𝑚

Figure 2.5 PhantomX Reactor robotic arm (Rick 2017) with axes superimposed.
10
 Table 2. Robot DH Parameters

Link 𝑎 𝑑 𝛼 𝜃

1 0 𝑑1 90 𝑞1

2 𝑎2 𝑑2 0 𝑞2

3 𝑎3 0 0 𝑞3

4 0 0 90 𝑞4

5 0 𝑑5 0 𝑞5

Let Aji represent the transformation of frame i to frame j, and define c1 = cos 𝜃1 , c1c2

= cos 𝜃1 * cos 𝜃2, c12 = cos (𝜃1+𝜃2), and so on.

For the PhantomX Reactor robotic arm,

c1 0 s1 0
s1 0 −c1 0
A01 = [ ]
0 1 0 𝑑1
0 0 0 1

c1c2 −c1s2 s1 4 ∗ s1 + 15 ∗ c1c2

c2s1 −s1s2 −c1 15 ∗ c2s1 − 4 ∗ c1
A02 = [ ];
s2 c2 0 15 ∗ s2 + 3
0 0 0 1

c1c23 −c1s23 s1 4 ∗ s1 + 15 ∗ c1c2 + 15 ∗ c1c2c3 − 15 ∗ c1s2s3

c23s1 −s1s23 −c1 15 ∗ c2s1 − 4 ∗ c1 − 15 ∗ s1s2s3 + 15 ∗ c2c3s1
A03 = [ ];
s23 c23 0 15 ∗ s23 + 15 ∗ s2 + 3
0 0 0 1

c1c234 s1 c1s234 4 ∗ s1 + 15 ∗ c1c2 + 15 ∗ c1c2c3 − 15 ∗ c1s2s3

0 c234s1 −c1 s234 ∗ s1 15 ∗ c2s1 − 4 ∗ c1 − 15 ∗ s1s2s3 + 15 ∗ c2c3s1
A 4= [ ];
s234 0 −c234 15 ∗ s23 + 15 ∗ s2 + 3
0 0 0 1

11
 s1s5 + c1c5c234 c5s1 − c234c1s5 c1s234 Px
c234s1 −c1 s234 ∗ s1 Py
A05 = [ ];
s234 0 −c234 Pz
0 0 0 1

where,

• Px = 4 ∗ 𝑠1 + 15 ∗ 𝑐1𝑐2 + 8 ∗ 𝑐4(𝑐1𝑐2𝑠3 + 𝑐1𝑐3𝑠2) + 8 ∗ 𝑠4(𝑐1𝑐2𝑐3 −

𝑐1𝑠2𝑠3) + 15 ∗ 𝑐1𝑐2𝑐3 − 15 ∗ 𝑐1𝑠2𝑠3

• Py = −4 ∗ 𝑐1 + 15 ∗ 𝑠1𝑐2 + 8 ∗ 𝑐4(𝑠1𝑐2𝑠3 + 𝑠1𝑐3𝑠2) + 8 ∗ 𝑠4(𝑠1𝑠2𝑠3 −

𝑠1𝑐2𝑠3) + 15 ∗ 𝑐1𝑐2𝑐3 − 15 ∗ 𝑐1𝑠2𝑠3

• Pz = 15*s23 – 8*c234 +15*s2+3

12
 CHAPTER 3

DESIGN AND SIMULATION OF CONTROLLER

3.1 Jacobian

Differential kinematics are used to find the relationships between the robotic arm’s

joint velocities and its end-effector linear and angular velocities. Defining the end-effector

linear velocity as 𝑝̇ e, the end-effector angular velocity as we, and the vector of joint

velocities as 𝑞̇ , then

𝑝̇ e = Jp(q) 𝑞̇ (3.1)

we = Jo(q) 𝑞̇ (3.2)

Here Jp is a (3 × n) matrix that relates the end-effector linear velocity to the joint velocities.

Jo is a (3 × n) matrix that relates the end-effector angular velocity to the joint velocities.

These equations can also be written in the form,

𝑝𝑒̇
ve = [ ] = J(q)𝑞̇ (3.3)
𝑤𝑒

Here J is known as the Geometric Jacobian of the 5-DOF manipulator. It is a function

of the manipulator’s joint variables. The Geometric Jacobian is a very useful property for

the robotic arm, as it is used to its find singularities, inverse kinematics and dynamics.

3.2 Singularities

During the motion of the robotic arm, there are some configurations at which the

instantaneous Jacobian matrix is not a full rank matrix. Those configurations of the

manipulator are called the singularities of the robotic arm.

13
 At singular configurations, the robotic arm loses its one degree of freedom , which

shows that it cannot achieve any arbitrary motion. The Jacobian matrix is used to find the

inverse kinematics, and if a singularity exists, then there will be infinite possible

configurations. Near a singularity, small velocities of the end-effector cause large

velocities in the joints of the arm, making the motion jerky and hazardous for motors.

Singularities are usually of two types:

1) Boundary singularities: This singularity arises when the robotic arm is fully

stretched or retracted; i.e., when the arm reaches the boundaries of its workspace.

It can be avoided by assigning minimum and maximum values for each joint angle.

2) Internal singularities: This singularity arises during particular motions of the end-

effector in its workspace. This can be hazardous for joint motors, as it can occur

during the planned motion of the manipulator. It can be avoided by considering

internal singularities during the trajectory planning.

3.3 Dynamics

In the manipulator design, the major concept is its dynamic modeling. The dynamic

modeling reduces the costs of actual experiments, as simulations are possible if the

dynamic model is accurate. Dynamic models are often derived using the Lagrange method,

which takes into account the kinetic energy and potential energy of the robot.

3.3.1 Lagrange Method

The equation of motion of a manipulator can be derived using the Lagrange method.

The Lagrangian of the robotic arm is defined as:

14
 L(𝑞, 𝑞̇ ) = K(𝑞, 𝑞̇ ) – U(𝑞) , (3.4)

where K is kinetic energy and U is total potential energy of the robotic arm. From the given

Lagrangian, the Lagrange equations are written as

∂ ∂L ∂L
− = f𝑖 , (3.5)
∂t ∂𝑞̇ 𝑖 ∂𝑞𝑖

i = 1,…,n, where f[n×1] is a vector of forces associated with the joint angles qi and n is the

number of joints.

As Lagrange equations are combination of kinetic energy and potential energy, the

formulas of each are shown here.

The kinetic energy of a link i is defined by (Bruno Siciliano, et al. 2009),

1 1
𝐾𝑙𝑖 = 𝑚𝑙𝑖 𝑝̇ 𝑙𝑇𝑖 𝑝̇ 𝑙𝑖 + 𝑤𝑖𝑇 𝑅𝑖 𝐼𝑙𝑖𝑖 𝑅𝑖𝑇 𝑤𝑖 (3.6)
2 2

where 𝑝̇𝑙𝑖 = the linear velocity of the center of mass of link i

𝑤𝑖 = the angular velocity of link i

𝑅𝑖 = the rotation matrix of link i with respect to the base frame

𝐼𝑙𝑖𝑖 = the inertia matrix relative to the center of mass of link i

𝑚𝑙𝑖 = the mass of link i

Here the linear and angular velocities can be written in generalized form, which is a

function of Jacobian matrices:

𝑙 𝑙 𝑙
𝑝̇ 𝑙𝑖 = 𝐽𝑃𝑖1 𝑞̇ 1 + ⋯ + 𝐽𝑃𝑖𝑖 𝑞̇ 𝑖 = 𝐽𝑃𝑖 𝑞̇ (3.7)

𝑙 𝑙 𝑙
𝑤𝑖 = 𝐽𝑂𝑖1 𝑞̇ 1 + ⋯ + 𝐽𝑂𝑖𝑖 𝑞̇ 𝑖 = 𝐽𝑂𝑖 𝑞̇ (3.8)

where Jacobians can be derived as,

𝑙 𝑙 𝑙
𝐽𝑃𝑖 = [ 𝐽𝑃𝑖1 … 𝐽𝑃𝑖𝑖 0] (3.9)
15
 𝑙 𝑙 𝑙
𝐽𝑂𝑖 = [ 𝐽𝑂𝑖1 … 𝐽𝑂𝑖𝑖 0] (3.10)

The Jacobian matrices in these definitions are computed as (Marvi 2018),

𝑙
𝑍𝑗−1 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐 𝑗𝑜𝑖𝑛𝑡
𝐽𝑃𝑖𝑗 = {
𝑍𝑗−1 × (𝑝𝑙𝑖 − 𝑝𝑗−1 ) 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒 𝑗𝑜𝑖𝑛𝑡

𝑙 𝑂 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐 𝑗𝑜𝑖𝑛𝑡
𝐽𝑂𝑖𝑗 = { 𝑍
𝑗−1 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒 𝑗𝑜𝑖𝑛𝑡

Then the kinetic energy formula can be written in terms of the Jacobian matrices:

1 𝑙𝑇 𝑙 1 𝑙𝑇 𝑙
𝐾𝑙𝑖 = 𝑚 𝑞̇ 𝑇 𝐽𝑃𝑖 𝐽𝑃𝑖 𝑞̇ + 𝑞̇ 𝑇 𝐽𝑂𝑖 𝑅𝑖 𝐼𝑙𝑖𝑖 𝑅𝑖𝑇 𝐽𝑂𝑖 𝑞̇ (3.11)
2 𝑙𝑖 2

The motor is also considered in dynamics modelling when the motors are very heavy

compared to links. The kinetic energy of a motor i can be written as,

1 1
𝐾𝑚𝑖 = 𝑚 𝑝̇ 𝑇 𝑝̇ + 𝑇
𝑤𝑚 𝐼 𝑤 (3.12)
2 𝑚𝑖 𝑚𝑖 𝑚𝑖 2 𝑖 𝑚𝑖 𝑚𝑖

where 𝑝̇𝑚𝑖 = the linear velocity of the center of mass of motor i

𝑤𝑚𝑖 = the angular velocity of motor i

𝐼𝑚𝑖 = the inertia matrix relative to the center of mass of motor i

𝑚𝑚𝑖 = the mass of motor i

Here the linear and angular velocities can be written in generalized form, which is a

function of Jacobian matrices:

𝑚
𝑝̇𝑚𝑖 = 𝐽𝑃 𝑖 𝑞̇

𝑚
𝑤𝑚𝑖 = 𝐽𝑂 𝑖 𝑞̇

The Jacobians are calculated as,

𝑚 𝑚 𝑚
𝐽𝑃 𝑖 = [ 𝐽𝑃1𝑖 … 𝑖
𝐽𝑃𝑖−1 0] (3.13)

𝑚 𝑚 𝑚
𝐽𝑂 𝑖 = [ 𝐽𝑂1𝑖 … 𝐽𝑂𝑖𝑖 0] (3.14)
16
The Jacobian matrices in these definitions can be calculated as,

𝑚
𝑍𝑗−1 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐 𝑗𝑜𝑖𝑛𝑡
𝐽𝑃𝑗𝑖 = {
𝑍𝑗−1 × (𝑝𝑚𝑖 − 𝑝𝑗−1 ) 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒 𝑗𝑜𝑖𝑛𝑡

𝑙
𝑚 𝐽𝑂𝑖𝑗 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐 𝑗𝑜𝑖𝑛𝑡
𝐽𝑂𝑗𝑖 = {
𝑘𝑟𝑖 𝑍𝑚𝑖 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒 𝑗𝑜𝑖𝑛𝑡

Then the kinetic energy formula for motors can written in terms of the Jacobian matrices:

1 𝑚𝑇 𝑚 1 𝑚𝑇 𝑚 𝑚
𝐾𝑚𝑖 = 𝑚 𝑞̇ 𝑇 𝐽𝑃 𝑖 𝐽𝑃 𝑖 𝑞̇ + 𝑞̇ 𝑇 𝐽𝑂 𝑖 𝑅𝑖 𝐼𝑚𝑖𝑖 𝑅𝑚
𝑇
𝐽 𝑖 𝑞̇ (3.15)
2 𝑚𝑖 2 𝑖 𝑂

The total kinetic energy is calculated by adding the kinetic energy of the links and the

motors, which is
1
K = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ (3.16)
2

where,
𝑛
𝑙𝑇 𝑙 𝑙𝑇 𝑙 𝑚𝑇 𝑚 𝑚𝑇 𝑚 𝑚
𝑀(𝑞) = ∑( 𝑚𝑙𝑖 𝐽𝑃𝑖 𝐽𝑃𝑖 + 𝐽𝑂𝑖 𝑅𝑖 𝐼𝑙𝑖𝑖 𝑅𝑖𝑇 𝐽𝑂𝑖 + 𝑚𝑚𝑖 𝐽𝑃 𝑖 𝐽𝑃 𝑖 + 𝐽𝑂 𝑖 𝑅𝑖 𝐼𝑚𝑖𝑖 𝑅𝑚
𝑇
𝐽 𝑖)
𝑖 𝑂
𝑖=1

Here, M is an 𝑛 × 𝑛 matrix called the Inertia matrix and is symmetric and positive definite.

Here n is number of degrees of freedom and kr is gear ratio.

This method is also used for finding the total potential energy. The total potential energy

is function of the joint positions, which is derived as,

U = − ∑𝑛𝑖=1( 𝑚𝑙𝑖 𝑔0𝑇 𝑝𝑙𝑖 + 𝑚𝑚𝑖 𝑔0𝑇 𝑝𝑚𝑖 ) (3.17)

where g0 [3×1] is gravitational acceleration with respect to the base frame and total potential

energy is defined as gravity matrix.

17
 After calculating the elements of the Lagrangian, it is written in the form of an inertia

matrix and gravity matrix g(q). Finally, the equations of motion can be derived from the

resulting Lagrange equations as:

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏 (3.18)

where C is an 𝑛 × 𝑛 matrix called the Coriolis matrix. Every element cij at row i and

column j of the matrix C is defined according to the equation,

∑𝑛𝑗=1 𝑐𝑖𝑗 𝑞̇ 𝑗 = ∑𝑛𝑗=1 ∑𝑛𝑘=1 ℎ𝑖𝑗𝑘 𝑞̇ 𝑘 𝑞̇ 𝑗 (3.19)

where ℎ𝑖𝑗𝑘 is,

𝜕𝑀𝑖𝑗 1 𝜕𝑀𝑗𝑘
ℎ𝑖𝑗𝑘 = −
𝜕𝑞𝑘 2 𝜕𝑞𝑖

In this way, equations of motion are derived for a 5-DOF robotic arm.

3.4 Control of Manipulators

3.4.1 Computed Torque Control

Robotic arms have nonlinear behavior, since the dynamic equations of the arms contain

nonlinear functions of the joint positions and velocities. When a controller is included, the

resulting closed-loop equation of the system is therefore nonlinear. So, usually motion

controllers for robotic arms are nonlinear controllers. However, the computed torque

controller (CTC) is an exception to these types of controllers. It makes the closed-loop

control equations linear because of its selection of input and output signals. The computed

torque controller is a model-based controller, unlike PD and PID controllers. In computed

torque control, torque is considered as an input and it is a function of nonlinear terms that

contain the mass matrix, Coriolis matrix and gravity matrix.

18
 The computed torque control law is as follows,

𝜏 = 𝑀(𝑞)[𝑞̈ 𝑑 + 𝐾𝑑 𝑞̃̇ + 𝐾𝑝 𝑞̃] + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) (3.20)

Here 𝑞̃ is the joint angular position error, which is the difference between the desired

joint angles 𝑞𝑑 and the actual joint angles 𝑞 , and Kd and Kp are symmetric positive definite

matrices. The computed torque controller is a model-based controller, since it contains the

desired joint angular acceleration 𝑞̈ 𝑑 and errors in joint angular velocity and angular

position, as well as the mass, Coriolis and gravity matrix.

Figure 3.1 shows the block diagram of a computed torque controller for a robotic arm.

g(q)

𝑞̈ 𝑑 M(q) Robotic
arm

C(𝑞, 𝑞̇ )
Kd
V Kp
𝑞̇ 𝑑

𝑞𝑑

Figure 3.1 Block diagram of computed torque controller

The dynamic equation of a robotic arm is given by equation (3.18). In this equation, 𝜏 can

be replaced by the control inputs defined in equation (3.20), which gives:

𝑀(𝑞)𝑞̈ = 𝑀(𝑞)[𝑞̈ 𝑑 + 𝐾𝑑 𝑞̃̇ + 𝐾𝑝 𝑞̃] (3.21)

19
The mass matrix has to be a positive definite matrix, and therefore equation (3.21) implies

that:

𝑞̃̈ + 𝐾𝑑 𝑞̃̇ + 𝐾𝑝 𝑞̃ = 0 (3.22)

This equation is linear and autonomous. Lyapunov candidate functions exist for this

equation, which means that the closed-loop system is asymptotically stable, i.e.

lim 𝑞̃̇ (𝑡) = 0 and lim 𝑞̃(𝑡) = 0

𝑡→∞ 𝑡→∞

20
 CHAPTER 4

TRAJECTORY PLANNING WITH OPTIMIZED ENERGY CONSUMPTION

This chapter presents a mathematical formulation for energy optimization of a 5

degree-of-freedom robotic manipulator by using a 7th-order time-dependent trajectory

equation. The equation of motion of the robot is modelled from its dynamics, as described

in Chapter 3. The objective is for the manipulator to reach a target position from an initial

position, which are both user-defined, in a specified amount of time. Physical constraints

like joint angle limits and joint velocity limits of the manipulator are implemented. The

energy optimization method outputs a set of joint angles over a sequence of time steps,

which yields an energy-optimized trajectory for the manipulator to follow over the defined

time interval.

4.1 Mathematical Modelling of Objective Function

The power Pi consumed by a servo motor i is the product of the torque 𝜏𝑖 produced by

the motor and the angular velocity 𝑞̇ 𝑖 of the motor,

𝑃𝑖 = 𝜏𝑖 𝑞̇ 𝑖 (4.1)

The equation of motion of a 5-DOF robot arm is given by equation (3.18),

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏

where 𝜏 = [ 𝜏1 𝜏2 𝜏3 𝜏4 𝜏5 ]′, M is the mass matrix, C is the Coriolis matrix, g is the gravity

matrix, and 𝑞, 𝑞̇, 𝑞̈ are the vectors of the 5 joint angular positions, velocities, and

accelerations, respectively.

21
 Since the torques are derived from the mass matrix, Coriolis matrix, and gravity matrix,

which are functions of the joint positions and velocities, the torques are also functions of

𝑞, 𝑞.̇ As power is a function of torque and joint velocity, it is also function of 𝑞, 𝑞̇ .

4.2 Trajectory & Constraints

The path of the robotic manipulator is the locus of joint angles in the joint space that

the manipulator must follow for the execution of an assigned path. A path that is

parameterized by time is called a trajectory. We use a time-varying polynomial equation

to define the trajectory of the end-effector. To ensure smooth movement of the end-

effector, a 7th-order polynomial is considered.

• Path equation for all 5 joints:

q1 = x1t7 + x2t6 + c1t5 + d1t4 + e1t3 + f1t2 + g1t + h1

q2 = x3t7 + x4t6 + c2t5 + d2t4 + e2t3 + f2t2 + g2t + h2

q3 = x5t7 + x6t6 + c3t5 + d3t4 + e3t3 + f3t2 + g3t + h3

q4 = x7t7 + x8t6 + c4t5 + d4t4 + e4t3 + f4t2 + g4t + h4

q5= x9t7 + x10t6 + c5t5 + d5t4 + e5t3 + f5t2 + g5t + h5

• Velocity equation for joint 1:

q̇1 = 7x1t6 + 6x2t5 + 5c1t4 + 4d1t3 + 3e1t2 + 2f1t + g1

• Acceleration equation for joint 1:

q̈1 = 42x1t5 + 30x2t4 + 20c1t3 + 12d1t2 + 6e1t + 2f1

22
Similar velocity and acceleration equations can be written for the remaining 4 joints. We

substitute in the initial and final conditions of position, velocity and acceleration for the

joints, as shown below.

Table 3. Initial and final conditions

At time t = 0 At time t = 𝑡𝑓

𝑞 = 𝑞0,𝑖 where 𝑞0,𝑖 is initial position of joint i 𝑞 = 𝑞𝑓,𝑖 where 𝑞𝑓,𝑖 is final position of joint i

q̇ = 0 for all joints q̇ = 0 for all joints

q̈ = 0 for all joints q̈ = 0 for all joints

Applying the initial conditions to the equation for q1,

f1=0, g1=0 and h1=q0,1.

Hence, the equation of q1 is reduced to

q1 = x1 t 7 + x2 t 6 + c1 t 5 + d1 t 4 + e1 t 3 + q 0,1

Equations for q2, q3, q4 and q5 are developed in a similar manner. By applying the initial and

final conditions to these equations, the corresponding values of ci, di, ei, fi, gi and hi are

found.

Nonlinear constraints on q1 through q5 are imposed as follows. The units are radians.

−3.14 ≤ 𝑞1 ≤ 3.14

−1.57 ≤ 𝑞2 ≤ 1.57

−1.57 ≤ 𝑞3 ≤ 1.57

−1.57 ≤ 𝑞4 ≤ 1.57

−3.14 ≤ 𝑞5 ≤ 3.14

23
Nonlinear constraints on q̇ 1 through q̇ 5 are imposed as follows. The units are radians/sec.

−2.26 ≤ q̇ 1 ≤ 2.26

−2.26 ≤ q̇ 2 ≤ 2.26

−2.26 ≤ q̇ 3 ≤ 2.26

−2.26 ≤ q̇ 4 ≤ 2.26

−2.26 ≤ q̇ 5 ≤ 2.26

In this formulation, we use time as a parameter and not as a variable. The variables x1

through x10 are optimized and the above constraints are imposed on the qi and q̇ 𝑖. The

values of x1 through x10 obtained from the optimization procedure are substituted into the

path equation and are checked to confirm that the constraints are satisfied.

In the table below, 𝑞0,𝑖 is the user-defined initial position of the joint angle 𝑞𝑖 and 𝑞𝑓,𝑖 is

the joint angle 𝑞𝑖 at the desired final position.

Table 4. Initial and Final Conditions (in radians)

q1 q2 q3 q4 q5

𝑞0,𝑖 0.7071 1.7071 1.7071 1.7071 1.7071

𝑞𝑓,𝑖 1.7070 1.2217 1.2217 1.2217 1.2217

4.2 Optimization Results

Total power consumption Σ𝑃𝑖 is minimized in this optimization problem. The

‘fmincon’ optimization function in MATLAB is used with the ‘SQP’ (Ren 2018)

algorithm. To initiate the optimization, the initial guesses of the variables used are:

24
 Table 5. Initial guesses of the variables

x1= 5*1e-5 x3=9*1e-5 x5=5*1e-5 x7=6*1e-5 x9=6*1e-5

x2=2*1e-4 x4=7*1e-4 x6=1.5*1e-4 x8=1*1e-4 x10=1*1e-4

The optimization was performed for 7 different final times tf. The optimized values for x1

through x10 and the power consumption calculated from these variables are shown for each

final time in Table 6. It is evident from the table that the minimum value of power

consumption is achieved for tf = 2.5 s. Figure 4.22 plots the values of power versus the

final time tf.

Table 6. Optimization Results

tf x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 Power
(s) (Watts)
1 1E-05 -0.01 0.104 0.02 1E-05 10 1E-05 10 1E-05 10 1.5543
1.5 0.064 -0.04 0.005 0.004 0.047 7.935 0.036 5.82 1E-05 4.93 1.9
2 1E-05 0.03 1E-04 1E-05 7E-03 1.78 6E-03 1.42 1E-05 1.19 0.7765
2.5 1E-05 -0.12 2E-04 1E-05 3E-04 0.189 3E-04 0.18 1E-05 0.20 0.3198
3 1E-05 0.05 8E-05 0.034 0.005 0.220 0.004 0.17 1E-05 0.16 0.8599
4 2E-05 0.02 3E-05 0.034 3E-05 0.030 3E-05 0.03 1E-05 0.02 2.6165
5 1E-05 0.09 1E-05 1E-05 1E-05 0.012 1E-05 0.01 1E-05 0.01 1.361

Visually, the trajectory of the end-effector over a time span of 1 s looks straight.

However, in that case the power consumption is higher compared to the power for a time

span of 2.5 s. This could be due to the fact that the joint velocities for the case where tf =

1 s are relatively high(Fig.4.12).

➢ For tf = 5 seconds

25
Figure 4.1 Joint Position for t = 5 seconds

Figure 4.2 Joint velocity for t = 5 seconds

26
Figure 4.3 Joint Acceleration for t = 5 seconds

Figure 4.4 End-effector trajectory for t = 5 seconds

27
 ➢ For t = 3 seconds

Figure 4.5 Joint position for t = 3 seconds

Figure 4.6 Joint velocity for t = 3 seconds

28
Figure 4.7 Joint acceleration for t = 3 seconds

Figure 4.8 End-effector trajectory for t = 3 seconds

29
 ➢ For t = 2 seconds

Figure 4.9 Joint position for t = 2 seconds

Figure 4.10 Joint velocity for t = 2 seconds

30
Figure 4.11 Joint acceleration for t = 2 seconds

Figure 4.12 End-effector trajectory for t = 2 seconds

31
 ➢ For t = 1 second

Figure 4.13 Joint position for t = 1 second

Figure 4.11 Joint velocity for t = 1 second

32
Figure 4.15 Joint acceleration for t = 1 second

Figure 4.16 End-effector trajectory for t = 1 second

33
 ➢ For t = 2.5 seconds

Figure 4.17 Joint position for t = 2.5 seconds

Figure 4.18 joint velocity for t = 2.5 seconds

34
Figure 4.19 Joint acceleration for t = 2.5 seconds

Figure 4.20 End-effector trajectory for t = 2.5 seconds

35
Figure 4.21 Comparison of End-effector Trajectories for the same Initial and Final
Positions

Power (Watts)
3

2.5
Power (Watts)

1.5

0.5

0
0 1 2 3 4 5 6
Time (seconds)

Figure 4.22 Power vs. Time

36
 CHAPTER 5

SOFTWARE FRAMEWORK FOR CONSTRUCTION TESTBED

5.1 ROS Compatible Robotic Arm

ROS (Robot Operating System) is a robot software development platform. It has a

huge user community, which makes ROS a preferred platform for roboticists. It has users

from all over the world and because of that, robotics companies are inclining towards using

ROS. Due to the following properties, many users prefer ROS over other robotics

platforms:

• Feature-rich packages: ROS has various high-capability packages for different

applications, including for autonomous navigation of mobile robots and motion

planning of robotic arms. These features and packages have numerous applications,

and code can be modified to create new applications for robots.

• Various functions: ROS has many functionalities for visualization, debugging and

simulation. Rviz and Gazebo are widely used simulators in the ROS community. The

rqt tool is used for visualization of control outputs, joint behaviors and feedback

properties of systems.

• Compatible with sensors and actuators: ROS is compatible with various sensors and

actuators. It is easy to communicate with these components via ROS. The Dynamixel

servos used in the PhantomX Robotic Arm are also compatible with ROS.

• Different coding languages: ROS operations are based on nodes. The nodes can be

programmed in many languages like Python, Matlab, C and C++. Just because of that

no other platform is as varied as ROS.

37
• Modularity: ROS is comparatively more modular than other robotic platforms. The

ROS platform is conceptualized based on node coding for different activities; if one

node crashes, then other nodes and other activities can still work.

• Dynamic community: The ROS community is developing very rapidly, and is very

active and helpful to new ROS users. The innovative applications are shared

throughout the community by users. Problems regarding ROS are solved on a ROS

wiki page.

5.2 ROS Structure

ROS is a highly structured platform, like other operating systems. ROS has 3 main

levels: file system, computational graph and community level. The ROS file system has

the following folders: Meta-Packages, Packages, and Messages-service-action-nodes.

Here, packages are the most basic element of the ROS platform. ROS packages have

configuration files, launch files, scripts, package manifest files and a CMake build file.

The package folder is very important for ROS setup, and some of package sub-folders

are described here.

• Config: All configuration files are stored in this folder. Users can assign parameters

of robot controllers, joints, and joint limits under configuration files.

• Launch: All launch files are stored in this folder. The nodes for different applications

are launched by these files.

• Scripts: The users can store Python scripts in this folder. These executable scripts

help to implement functionalities for nodes.

• Src: All source files are stored in this file. The file must be written in C++.
38
• Msg: All message definitions are stored in this folder.

• Srv: All services are stored in this folder.

• Package.xml: This is the package manifest file of the package.

• CMakeLists.txt: This is the CMake build file of the package.

5.3 Modeling of Robotic Arm in ROS

To validate robot controllers, 3D modeling of robots in software is very important. The

modeling of robots saves a great deal of time and money that would be spent on physical

robot experiments. There are some packages in ROS which assist in modeling of robots.

In this work, modeling of the PhantomX robotic arm is done using a URDF (Unified Robot

Description Format) package, a joint state publisher and a robot state publisher. The URDF

is the XML file format for describing the robot model.

The joint state publisher package is very important for modeling the robot. The joint

state publisher node publishes joint values to nonfixed joints from reading URDF files.

This way, it makes it easy for users to understand URDF files. The robot state publisher is

also a useful package for publishing 3D poses of the robot. It publishes each robot link in

a 3D world using kinematics described in the URDF files. The TF nodes represents

relations between coordinates of robot reference frames.

The URDF file of any robot contains joint and link information. The link tag contains

properties like size, shape, and geometry, and it can also include mesh properties of the

link. The link also has some dynamic properties like inertia and collision of links.

The different types of joints, such as revolute, prismatic, and fixed, are defined under

joint tags. The joint limits, velocity limits, and effort limits are also defined under joint
39
tags. The URDF file creates a 3D model from declarations of joint and link tags. The

interface of the hardware with ROS is also defined in URDF files. Simulators like Gazebo

and Rviz read URDF files and create 3D models for users.

In this work, a 3D model of the 5-DOF PhantomX Reactor robotic arm was created in

the Gazebo and Rviz simulators. The files are on the GitHub page of the Autonomous

Collective Systems (ACS) Laboratory (Lab 2019).

5.4 Motion Planning with Moveit!

MoveIt has different packages for manipulation of a robotic arm. It is possible to solve

problems of motion planning, pick and place, grasping, and so on with different plugins

provided by MoveIt APIs. It has library plugins for manipulation, collision checking,

motion planning, control and perception. The customized robots are also compatible with

MoveIt with the help of the MoveIt Setup Assistant.

The MoveIt platform is centralized around a move-group node. It connects various

services and actions of different applications with robot. The move-group node subscribes

to the joint state and the TF topic of the robot. It gathers information about the robot from

URDF and configuration files provided by the user.

The user can work with MoveIt in 3 different ways. The user can command the move-

group node via C++ APIs, Python APIs or the GUI of MoveIt. The GUI is compatible with

the Rviz simulator, in which the user can add plugins associated with their requirements.

It is also possible to interlink MoveIt-Rviz with a real robot or Gazebo for experiments.

40
 Below, the MoveIt architecture diagram is shown to illustrate Moveit programming.

Figure 5.1 MoveIt architecture diagram (Figure borrowed from MoveIt-documentation)

As shown in Figure 5.1, the move-group node collects values from robot sensors and

controllers. It processes path planning, collision checking, scene planning and grasping. It

operates those functionalities when an action or service is called by the user in the form of

Rviz plugins, Python moveit-commander or move-group-interface.

MoveIt uses the Open Motion Planning Library (OMPL) for path planning of robotic

arms. The OMPL has several algorithmic plugins for motion planning, so users can change

41
algorithms in the MoveIt GUI as per their applications. By default, MoveIt uses RRT

(Rapidly-exploring Random Trees) and RRT* (optimized RRT) plugins for manipulation.

In this work, the PhantomX Reactor Robotic Arm (Phantomx) was made compatible

with MoveIt with the use of the setup assistant and the URDF file of the robot. Motion

planning was also performed to obtain the desired trajectories of the robotic arm that were

computed using the optimization procedure described in Chapter 4. The PhantomX robot

was also inter-linked with Gazebo and physical hardware to test MoveIt package. The files

for MoveIt and Gazebo simulations are on the GitHub page of the Autonomous Collective

Systems (ACS) Laboratory (Lab 2019).

42
 CHAPTER 6

SIMULATION AND EXPERIMENTAL RESULTS

6.1 Simulation of Computed Torque Controller

The computed torque controller is a model-based controller for robotic arms. As

mentioned in Chapter 3, the control law for the CTC is given by:

𝜏 = 𝑀(𝑞 )[𝑞̈ 𝑑 + 𝐾𝑑 𝑞̃̇ + 𝐾𝑝 𝑞̃] + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞 )

This controller makes the nonlinear robot dynamics into a second-order linear system:

𝑞̃̈ + 𝐾𝑑 𝑞̃̇ + 𝐾𝑝 𝑞̃ = 0

Here, this second-order system is solved using the ODE45 command in Matlab

software. To make the system solvable using ODE45, the second-order system can be

converted into two first-order differential equations, written in state-space form as follows:

𝑑 𝑞̃ 𝑞̃̇
[ ]= [ ]
𝑑𝑡 𝑞̃̇ −𝐾𝑑 𝑞̃̇ − 𝐾𝑝 𝑞̃

0 𝐼
𝑑 𝑞̃ 𝑞̃
[ ]= [ ] [ ̇]
𝑑𝑡 𝑞̃̇ −𝐾𝑝 −𝐾𝑑 𝑞̃

The joint positions and velocities are unknown in these equations, and because of that

they are considered as symbolic variables in Matlab. The Matlab code for the computed

torque controller is on the GitHub page of the Autonomous Collective Systems (ACS)

Laboratory (Lab 2019). The proportional and velocity gain matrices are defined as diagonal

matrices to make this centralized system decentralized.

After trial and error we set Kp = Kp1 = diag {1.8, 0.65, 2, 0.7, 2.5} and Kd = Kd1 =

diag {10, 2, 10, 2, 10}. Figures 6.1, 6.2, and 6.3 show the joint positions, velocities, and
43
error values over time, respectively. These are the outputs from the integration of the robot

dynamics with the computed torque controller in Matlab. It is seen in Figure 6.3 that the

error values all converge to zero for the given Kp1 and Kd1 matrices.

Next, we set Kp = Kp2 = diag {14.8, 7.65, 15, 6.7, 20.5} and Kd = Kd2 = diag {10, 2,

10, 2, 10}; i.e., the gains in the Kp matrix are increased. As Figures 6.4-6.6 show, the

manipulator has a faster response but exhibits jerkier motion.

Figure 6.1 Position-Time graph for Kp1 and Kd1(radian-second)

44
Figure 6.2 Velocity-Time graph for Kp1 and Kd1(radian/sec-second)

Figure 6.3 Error-Time graph for Kp1 and Kd1 (radian-second)

45
Figure 6.4 Position-Time graph for Kp2 and Kd2(radian-second)

Figure 6.5 Velocity-Time graph for Kp2 and Kd2(radian/sec-second)

46
 Figure 6.6 Error-Time graph for Kp2 and Kd2(radian-second)

6.2 Experiment Setup

The commercially available 5-DOF PhantomX Reactor Robotic Arm was used here

for experiments. It has a static base, and it is possible to make it mobile by attaching it to

a mobile robotic platform. The Dynamixel AX-12a servo motors are used in the PhantomX

Robotic Arm. These servos have position and velocity feedback, while inputs are only

possible to achieve position control. These servos are compatible with ROS, so it is

possible to command them from an external computer. The PhantomX robot was modeled

in MoveIt using the setup assistant. The setup assistant software is shown in Figure 6.7.

47
Figure 6.7 MoveIt Setup Assistant (Moveit! n.d.)

After attaching the URDF files of the PhantomX robot, the end-effector configurations

and control methods are chosen as per the required applications. Then after setting up the

PhantomX robot in MoveIt, it is possible to plan its path with the RRT algorithm. This

planned trajectory is commanded to the physical hardware, and the Dynamixel servos

attempt to move the robot according to the trajectory generated in MoveIt.

For the MoveIt and physical robot coordination, some files must be modified to achieve

similar controllers in MoveIt and Gazebo / the physical robot. The MoveIt simple manager

has to be called in a launch file for initiation of the MoveIt controllers. The configure file

is required to start controllers in Gazebo. Finally, launching Gazebo and MoveIt will

initiate controllers in the computer as well as on the real hardware. In this way, it is possible

to coordinate Gazebo and the physical robot with MoveIt. The required files are on the

48
GitHub page of the Autonomous Collective Systems (ACS) Laboratory (Lab 2019). The

MoveIt motion planning GUI is shown in Figure 6.8.

Figure 6.8 MoveIt motion planning GUI in Rviz (Initial robot configuration in Orange
and final configuration in white)

6.3 Matlab-ROS interface

Matlab has introduced a robotics toolbox, which can connect Matlab to ROS. The

benefit of the ROS-Matlab interface is that the user does not need to code complicated

controllers and perception algorithms in c++ coding language; they can directly use Matlab

code to send commands and subscribe to the robot.

49
Figure 6.9 Matlab-ROS workflow (Matlab-documentation)

The Matlab-ROS workflow is shown in Figure 6.9. ROS communicates with the robot

hardware and simultaneously subscribes and publishes to Matlab algorithms to obtain

updated outcomes of joint values from Matlab. This interface allows the user to add new

functions to ROS libraries. The user just has to input sensor values from the robot to Matlab

code, and then Matlab performs the calculations and publishes the outcome of joint values

to the robot via ROS.

Matlab can be connected to a Gazebo simulation or a physical robot by assigning the

IP address of the robot to the Matlab node. As per the flowchart in Figure 6.10, Matlab will

connect with ROS using the assigned IP address. It will subscribe to ROS topics associated

with the physical robot’s position and velocity sensors. At every specified time instant,

Matlab will solve the ODEs that describe the robot’s dynamics to obtain the robot’s joint

position and velocity values and find the required motor torques needed for that motion. It

50
will publish the torque values to the robot motor controllers via ROS. The motor controllers

are assumed to be effort controllers.

ROBOT

Dynamixel Servo Dynamixel Servo

Velocity Sensor Position Sensor

TTL
communication

ROS interface

Torque

Computed Torque
Controller (Matlab)

Figure 6.10 Flowchart of computed torque controller with Matlab-ROS interface

51
Figure 6.11 Setup of PhantomX Reactor robotic arm

Here, the PhantomX robotic arm was connected to a computer by TTL communication

and Dynamixel servo motors (Figure 6.11). The Matlab code for the Gazebo and Matlab

interface is presented in Appendix A.

An experiment to implement pick-and-place action, which is essential in bricklaying

operations, was performed on the PhantomX Robot with position controller, and a similar

action was simulated in Gazebo. Figure 6.12 shows a time sequence of snapshots of a

successful pick-and-place action by the physical robot with a cardboard block, as well as a

snapshot of the Gazebo model.

52
 Figure 6.12 Snapshots of pick-and-place action by the PhantomX robotic arm and

Gazebo model of PhantomX robotic arm (center)

53
 CHAPTER 7

CONCLUSION AND FUTURE WORK

7.1 CONCLUSION

This thesis describes the procedure of designing, simulating, and implementing a

controller for a robotic arm to perform movements that are useful for construction

activities. The thesis presents the kinematics and dynamics of a 5-DOF robotic

manipulator, a trajectory planning procedure that can be used to generate bricklaying

motions, controller design for improved performance, a software framework for controlling

the robotic arm, and hardware implementation on a 5-DOF robotic arm.

In this thesis, a computed torque controller was designed for better trajectory

performance of a robotic arm. The computed torque controller produces improved

performance in trajectory following over a conventional PD controller because it accounts

for the dynamics of the arm. Although a 5-DOF arm is a nonlinear dynamic system, the

computed torque controller transforms it into a linear second-order system. Simulation

results of the computed torque controller show that the arm with this controller is

asymptotically stable and follows the desired trajectory.

The purpose of the designing the software framework was to make it easier for new

users to control a robotic arm. The 5-DOF robotic arm was also modeled in ROS to

visualize the actual behavior of the arm in the Gazebo or Rviz simulators. A Matlab-ROS

interface was developed so that the computed torque controller can be implemented easily

on the robot via Matlab. This Matlab-ROS interface allows users to implement different

controllers on this robot just by changing the controller source file. The Moveit-ROS

54
interface developed in this thesis empowers users to use different motion and path planning

algorithms for a robotic arm. The Matlab-ROS and Moveit interfaces were also used to

control an actual PhantomX Reactor robotic arm. The Dynamixel servos AX-12a and

USB2Dynamixel converter were used for implementing the controller on the hardware.

7.2 FUTURE WORK

There are some possible extensions to this thesis as future work:

(1) The current robotic arm has a static base. It is possible to expand the workspace of the

PhantomX robotic arm by attaching it to a mobile robotic platform, such as the TurtleBot3

Waffle robot.

(2) During brick-laying operations, if a brick is of varying or unknown weight, then it is

difficult to obtain satisfactory performance from PD and CTC controllers. As future work,

adaptive and robust controllers can be designed for loads with unknown parameters.

(3) The Dynamixel servos AX-12a have proportional controllers, and they can only sense

joint position and velocity. These servos could be replaced by MX-64 servos, which have

torque controllers that are possible to directly command using the CTC controller’s output.

55
 REFERENCES

A. Balestrino, G. De Maria, L. Sciavicco. An adaptive model following control. ASME

Journal of Dynamic Systems, Measurement and Control , vol. 105, pp. 143–151,
1983.

Alia N, Barakatl, Khaled A.Gouda, Kenz A. Bozed. "Kinematics Analysis and

Simulation of a Robotic Arm." Proceedings of 2016 4th International Conference
on Control Engineering & Information Technology. 2016.

Anil Mahtani, Luis Sánchez, Enrique Fernández, Aaron Martinez. Effective Robotics
Programming with ROS. Packt Publishing, 2016.

Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo. Robotics-
Modelling, Planning and Control. Springer, 2009.

Carol Fairchild, Dr. Thomas L. Harman. ROS Robotics By Example. Packt Publishing,
2016.

Corke, Peter. "Robot Manipulator Capability in MATLAB." IEEE ROBOTICS &

AUTOMATION MAGAZINE , 2017.

—. Robotics,Vision and Control. Springer, 2012.

"Dynamixel_controllers." ros-wiki. n.d.

http://wiki.ros.org/dynamixel_controllers/Tutorials.

"dynamixel_motor joint control." GitHub. n.d.

https://github.com/arebgun/dynamixel_motor/blob/master/dynamixel_controllers/
src/dynamixel_controllers/joint_trajectory_action_controller.py.

Fenton, X. Shi and N.G. "A complete and general solution to the forward kinematics
problem of platform-type robotic manipulators." IEEE International Conference
on Robotics and Automation proceedings. 1994. pp. 3055-3062.

"Gazebo-ROS control." Gazebo. n.d. http://gazebosim.org/tutorials/?tut=ros_control.

"Getting started with ROS-Matlab." Matlab documentaion. n.d.

https://www.mathworks.com/matlabcentral/fileexchange/64534-getting-started-
with-robot-operating-system-ros.

Joseph, Lentin. Learning Robotics Using Python. Packt Publishing Ltd., 2014.

—. Mastering ROS for Robotics Programming. Packt Publishing Ltd., 2015.

56
Lab, ACS. ACS Lab GitHub Page. 03 28, 2019. https://github.com/ACSLaboratory
(accessed 3 28, 2019).

Marvi, Hamidreza. MAE-547 Modeling and Control of Robots Lecture Notes. 2018.

Melchiorri, Claudio. "Control of robot manipulators." Masters Thesis, n.d.

Morgan Quigley, Brian Gerkey, and William D. Smart. Programming Robots with ROS.
2016: O’Reilly Media, Inc., n.d.

"Move-group Python Interface." Moveit. n.d. https://ros-

planning.github.io/moveit_tutorials/doc/move_group_python_interface/move_gro
up_python_interface_tutorial.html.

"Moveit!" Moveit concept documentation. n.d.

https://moveit.ros.org/documentation/concepts/.

Pivac, Mike. X, Hadrian. 2016. https://www.fbr.com.au/view/hadrian-x.

PODKAMINER, NATE. Construction Robotics SAM100. 2016.

https://www.construction-robotics.com/sam100/.

Quirke, Joe. CIOB. november 15, 2018.

http://www.globalconstructionreview.com/news/hadrian-bricklaying-robot-builds-
complete-house-th/.

QUOCTRUNG BUI, ROGER KISBY. Bricklayers Think They’re Safe From Robots.
New York, March 06, 2018.

R. Kelly, V. Santibáñez and A. Loría. Control of Robot Manipulators in Joint Space.

Springer, 2005.

Ren, Yi. Design informatics Lab. 2018. http://designinformaticslab.github.io/index.html.

Rick. Trossen Robotics - Phantomx Robotic Reactor Arm . 2017.

https://www.trossenrobotics.com/p/phantomx-ax-12-reactor-robot-arm.aspx.

Shahab, Mohammad. "2DOF Robotic Manipulator." Report, 2008.

"wiki-ros." ROS actionlib. n.d. http://wiki.ros.org/actionlib.

57
Y. Antonio, S. Victor and M.V. Javier. "Global asymptotic stability of the classical PID
controller by considering saturation effects in industrial robots." International
Journal of Advanced Robotic Systems, Vol. 8,, 2011: pp. 34-42.

58
 APPENDIX A

MATLAB - GAZEBO INTERFACE

59
rosshutdown;
#connects with robot with following ip-address
rosinit('ip-address')

#Declares publisher with trajectory controller

[myPub,pubMsg] =
rospublisher('SG_robot/arm_joint_controller/command');

#Defines msg type associated with trajectory controller

joint_send =
rosmessage('trajectory_msgs/JointTrajectoryPoint');

load('x_pid_2');
joint_send.Positions = zeros(5,1);
joint_send.Velocities = zeros(5,1);
pubMsg.Points = joint_send;
pubMsg.JointNames =
{'shoulder_yaw_joint','shoulder_pitch_joint','elbow_pit
ch_joint','wrist_pitch_joint','wrist_roll_joint','gripp
er_revolute_joint'}

i=1; j=1;
while(i<6)
pubMsg.Points.Positions =
[Q1(4*i,1);Q1(4*i,2);Q1(4*i,3);Q1(4*i,4);Q1(4*i,5)]

if i == 1
pubMsg.Points.TimeFromStart.Sec =
round((0.49*((i-1)/2)),0);
pubMsg.Points.TimeFromStart.Nsec = 240099009
elseif i == 2
pubMsg.Points.TimeFromStart.Sec =
round((0.49*((i-1)/2)),0);
pubMsg.Points.TimeFromStart.Nsec = 490099009
elseif i == 3
pubMsg.Points.TimeFromStart.Sec =
round((0.49*((i-1)/2)),0);
pubMsg.Points.TimeFromStart.Nsec = 740099009
else i == 4
pubMsg.Points.TimeFromStart.Sec =
round((0.49*((i-1)/2)),0);
pubMsg.Points.TimeFromStart.Nsec = 990099009
60
 end

# Publishes pubMsg to actual robot from Matlab

send(myPub,pubMsg);
for t= 0:0.001:1
j = i+t;
end
i = j;
end

61