Embedded Factor Graph for Controlling a Low-Cost Robotics Arm … · 2015-07-30 · Embedded Factor...

Embedded Factor Graph forControlling a Low-Cost

Robotics Arm

eingereichteMASTERARBEIT

von

Xinhua Liao

geb. am 14.07.1988wohnhaft in:

Willi-Graf-Str.25/37280805 Munchen

Tel.: 0176 80555639

Lehrstuhl furSTEUERUNGS- und REGELUNGSTECHNIK

Technische Universitat Munchen

Univ.-Prof. Dr.-Ing./Univ. Tokio Martin Buss

Betreuer: Dipl.-Ing./Dr. Indar SugiartoBeginn: 31.03.2014Zwischenbericht: 12.06.2014Abgabe: 29.08.2014

WATECHNISCHE UNIVERSITAT MONCHEN

LEHRSTUHL FUR STEUERUNGS- UND REGELUNGSTECHNIKORDINARIUS: UNIV.-PROF. DR.-INGJUNIV. TOKIO MARTIN BUSS

MASTER’S THESISfor

Xinhua LiaoStudent ID 03649182, Degree El

Embedded Factor Graph for Controlling a Low-Cost Robotics Arm

Problem description:

S

•1~~

24.03.2014

A Factor Graph is a probabilistic graphical model quite popular in the signal processing community,which recently gained favour as an alternative for modelling dynamic control systems. It shows promising potential to span in a uniform manner perception, cognition and action; and thus to ultimately yieldsimpler yet more powerful integrated architectures for intelligent robots and other comparable systems.In this project, the student shall implement such a factor graph in dedicated hardware with intrinsicparallelism of resources, such as SoC (System on Chip). The student will evaluate and optimize theimplementation of Factor Graph using such SoC for controlling a robotic arm in the grasp-and-placescenario. Among several possible methods, one particularly interesting method to be explored is theimitation learning approach. This programming-by-demonstration approach will harness the sequentialstructure of a dynamic factor graph which is proposed to handle uncertainty in the dynamic system.

Tasks:

• Study the existing SoC system and the robots.• Study existing algorithm for Factor Graph and analyse aspects that are vital for a dynamic system.• Evaluate the imitation learning approach for implementing robot control and implement it as a

Factor Graph on the SoC.

Bibliography:

[1] Deisenroth, et.al., “Learning to Control a Low-Cost Manipulator using Data-Efficient Reinforcement Learning”. In International Conference on Robotics: Science & System (R:SS), 2011

[2] Sugiarto, I., Conradt, J. “Discrete Belief Propagation Network Using Population Coding andFactor Graph for Kinematic Control of a Mobile Robot”. In International Conference on Computational Intelligence and Cybernetics, 2013

Supervisor:Start:Intermediate Report:Delivery:

M. Sc. Indar Sugiarto31.03.201412.06.201429.08.2014

(J. Conradt)Professor

Abstract

In probability theory, a Factor Graph is a probabilistic graphical model, which isefficient in performing marginal computation through belief propagation algorithms,such as sum-product-algorithm. Recently it gains favour for modelling dynamic con-trol systems. Meanwhile, it also shows promising potential in the machine learningarea. Thus it may ultimately yields simpler yet more powerful integrated architec-tures for intelligent robots and other comparable systems.

In this thesis, the imitation learning and the inverse kinematics using factor graphare combined together in order to model the dynamic control system, which will beused in the scenario of performing the grasping and placing task with the roboticarm. Then the FPGA design is developed to realize the parallel data processing forthe factor graph computation.

In the thesis design,the imitation learning uses Gaussian Mixture Regression tooptimize the data during the learning process. Then the inverse kinematics usingfactor graph is developed for getting necessary joint space data for controlling therobotic arm.

This programming-by-demonstration approach will harness the sequential structureof a dynamic factor graph which is proposed to handle uncertainty in the dynamicsystem.

The simulation result proves the combination of imitation learning with inverse kine-matics using factor graph. Meanwhile different data parallel processing algorithmsare compared in aspects of accuracy and timing. Then one of them is implementedas parallel computation units in the FPGA.

Zusammenfassung

In der Wahrscheinlichkeitstheorie, ist ein Faktor Graph eine probabilistische graphis-che Modell,das bei der Durchfuehrung der Rand Berechnung durch Belief Propaga-tion Algorithmen wie Summen-Produkt-Algorithmus effizient ist. Kuerzlich gewinnter Gefallen fuer die Modellierung von dynamischen Kontrollsysteme. Unterdessenzeigt sich auch vielversprechende Potenzial in der Maschine Lernbereich. So kann esletztlich ergibt einfacher noch mehr leistungsfaehige, integrierte Architekturen fuerintelligente Roboter und andere vergleichbare Systeme.

In dieser Arbeit werden die Nachahmung Lernen und die inverse Kinematik mitFaktorgraph Modell zusammen kombiniert, um die dynamische zu Steuerung. DieseKombination wird in dem Szenario der des Greifen und Platzieren mit dem RobotikArm durchgefuerht. Dann wird der FPGA-Design entwickelt, um die paralleleDatenverarbeitung fuer die Faktorgraph Berechnung zu realisieren.

In der Dissertation Design, nutzt die Nachahmung Lernen Gaussian Mixture Re-gression, um die Daten waehrend des Lernprozesses zu optimieren. Dann werdendie inverse Kinematik mit Faktor Graph fuer Rechnung der Gelenkraum Daten en-twickelt. Mit diese Daten, man kann die Robotik Arm steueren.

Diese Programmierung-by-Demonstration Ansatz wird die sequentielle Struktur einesdynamischen Faktorgraphen, die vorgeschlagen wird, die Unsicherheit im dynamis-chen System umgehen zu nutzen.

Das Simulationsergebnis beweist die Kombination der Nachahmung Lernen mit in-verser Kinematik von Faktor Graph Modell. Inzwischen verschiedenen Datenver-arbeitungsalgorithmen parallel in Aspekte der Genauigkeit und Timing verglichen.Einer von ihnen wird als Parallel-Recheneinheiten in dem FPGA realisiert.

3

Special Thanks

Here I would like to thank professor. Dr. sc.nat. Joerg Conradt and my advisorM.Sc. Indar Sugiarto, for their kind and patient help and instructions on this

thesis, without which, I can’t progress well in my thesis.

I would also thank the classmates in the lab, which are always willingly for helpingothers.

CONTENTS 5

Contents

1 Introduction 71.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.2 Content overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2 Design idea of combining imitation learning with inverse kinemat-ics 112.1 General overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.1.1 The robot configuration and forward kinematics . . . . . . . . 112.1.2 The imitation learning . . . . . . . . . . . . . . . . . . . . . . 112.1.3 Optimization with Gaussian Mixture Regression model . . . . 122.1.4 Inverse kinematics with factor graph . . . . . . . . . . . . . . 122.1.5 FPGA design . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.1.6 Robot control in grasp-and-place scenario . . . . . . . . . . . 13

2.2 Design Detail . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.2.1 The robot configuration and forward kinematics . . . . . . . . 132.2.2 The imitation learning . . . . . . . . . . . . . . . . . . . . . . 172.2.3 Optimization with Gaussian Mixture Regression model . . . . 192.2.4 Factor Graph model overview . . . . . . . . . . . . . . . . . . 242.2.5 General design with the factor graph model for inverse kine-

matics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252.2.6 Design of factor graph model in this thesis . . . . . . . . . . . 282.2.7 Parallel design model for the inverse kinematics using Factor

Graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.2.8 Limitation of the design from Viktor et al . . . . . . . . . . . 332.2.9 My Design using Factor Graph model . . . . . . . . . . . . . 342.2.10 Limitation of my design . . . . . . . . . . . . . . . . . . . . . 38

3 Implementation 413.1 General introduction for the platform . . . . . . . . . . . . . . . . . . 413.2 Data collection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

3.2.1 Collection of position values . . . . . . . . . . . . . . . . . . . 413.2.2 Collection of angle values . . . . . . . . . . . . . . . . . . . . . 42

3.3 Hardware Implementation . . . . . . . . . . . . . . . . . . . . . . . . 43

6 CONTENTS

3.3.1 Trenz Carrier Board . . . . . . . . . . . . . . . . . . . . . . . 433.3.2 FPGA Design . . . . . . . . . . . . . . . . . . . . . . . . . . . 443.3.3 Hardware Interface . . . . . . . . . . . . . . . . . . . . . . . . 53

3.4 Software Implementation . . . . . . . . . . . . . . . . . . . . . . . . . 533.4.1 Implementation on the Zynq SoC . . . . . . . . . . . . . . . . 543.4.2 Implementation for the robot control . . . . . . . . . . . . . . 55

3.5 Design verification . . . . . . . . . . . . . . . . . . . . . . . . . . . . 563.5.1 Denavit-Hartenberg convention verification . . . . . . . . . . . 563.5.2 GMR verification . . . . . . . . . . . . . . . . . . . . . . . . . 573.5.3 Inverse kinematics model using Factor Graph verification . . . 573.5.4 Compare the result of inverse kinematics and forward kinematics 57

4 Experimental Results 594.1 Result of the Denavit-Hartenberg algorithm . . . . . . . . . . . . . . 594.2 Result of the Gaussian Mixture Model . . . . . . . . . . . . . . . . . 624.3 Result of the Inverse Kinematics using Factor Graph . . . . . . . . . 63

4.3.1 Training Factor Graph . . . . . . . . . . . . . . . . . . . . . . 634.3.2 Getting the joint data from the position data . . . . . . . . . 644.3.3 Regeneration of the trajectory . . . . . . . . . . . . . . . . . . 65

4.4 Result comparison between my design and Viktor’s Design . . . . . . 66

5 Discussion 715.1 Bias of the result for inverse kinematics calculation using Factor Graph 715.2 Uncertainty in the measuring system . . . . . . . . . . . . . . . . . . 725.3 Benefits of my design . . . . . . . . . . . . . . . . . . . . . . . . . . . 725.4 Comparison between Gaussian mixture regression and using factor

graph as regression model . . . . . . . . . . . . . . . . . . . . . . . . 735.5 Future Development . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

5.5.1 FPGA Development . . . . . . . . . . . . . . . . . . . . . . . 735.5.2 Improving my design of inverse kinematics using factor graph 74

6 Conclusion 75

List of Figures 77

Bibliography 79

7

Chapter 1

Introduction

1.1 Problem Statement

In robot control area, the dynamic control modelling is an interesting topic.In orderto model a proper dynamic model for enhancing robot control, different strategieswere developed. One of the strategies which is often referred to is Factor Graph. Inprobability theory and its applications, “algorithms that must deal with complicatedglobal functions of many variables often exploit the manner, in which the givenfunctions factor act as a product of local functions, each of which depends on asubset of the variables. Such a factorization can be visualized with a bipartitegraph we call a factor graph.”[KFL01].

The factor graph can be described as a particular type of probabilistic graphicalmodel, which can be grouped into two classes: the directed graphical models (re-ferred to as Bayesian Network) and the undirected graphical models (referred to asMarkov Random Fields). These models can be used to represent joint probabilityand/or conditional probability of interconnecting variables.[SMJ13].

On the other side, in machine learning area, programming by demonstration is apowerful method to solve complex, high-dimensional tasks in humanoid robots, suchas reaching and grasping objects.[ARAJ10]

This allows the robot to record the execution of certain human demonstration tasksto obtain primitive data of the robot’s motion. In some cases, obtaining the robot’skinematics and dynamics are hard to accomplish. In order to optimize the robotcontrol, the imitation learning is adopted in robot applications. The imitation learn-ing uses the learning-by-demonstration method to train the robot to finish certaintasks. A common use of this strategy with the robot is the task of performinggrasp-and-place scenario.In this case, the motor control that is applied to performthe object manipulation task is integrated with visual understanding capabilities.Several approaches based on this scenario have been developed before, such as the

8 CHAPTER 1. INTRODUCTION

forward kinematics model developed by [EPD13], where a forward kinematics modelof the robot’s dynamics to generate trajectory predictions for a given controller isdeveloped.

However, using the graphical model, such as factor graph, to perform the imitationlearning is not well explored. In factor graph, the nodes and factors in the graphupdate its messages according to the input the network receives. The factor graphcan perform the inverse kinematics, which calculates the joint parameters of therobotic arm, given the positions of the end-effector of the robotic arm. This is quitedifferent from using forward kinematics used by e.g. Peter Englert et al [EPD13].

Meanwhile, formal studies of reasoning with discrete factor graph have alreadybeen explored. They proved the functionality of the inverse kinematics. Indaret al.[SMJ13]present the conceptual principle of factor graph with discrete nodesand how it can be used for reasoning using exact inference mechanism.

Methods such as handling simultaneously constraints in joint space and in task spaceby combining directly the probabilistic representation of the task constraints with asolution to Jacobian-based inverse kinematics have also been developed by Calinon,Set al [AND77].

But strategy to combine the imitation learning and inverse kinematics using FactorGraph have not been explored. In order to harness the movement’s performance ofthe dynamic system in the grasp-and-place scenario, the combination of imitationlearning and the Factor Graph shows possibility to handle problems of data fusionas well as task learning jointly and to take advantage of both strategies, which showsan interesting research value in robotic control area.

Meanwhile by implementing the Factor Graph it often gives rise to the problem ofcomplexity, where belief propagation computation cost increases dramatically as thenumber of states of variables and the node degrees of a graph increase.[Rey09].Inthis sense in order to speed up the calculation of the factor graph, effective way ofupdating the message passing along nodes and factors should be developed. Onepossibility is to introduce the parallel data processing mechanism by using intrinsicresources in FPGA.

The concept of using FPGA for partitioning to gain performance optimization hasbeen proposed by many researchers. For example, from the work of Kalapi Roy-Neogi et al[RNS95], they utilize multiple FPGAs when a single FPGA is not suffi-cient for a design implementation. But their goal was to efficiently and accuratelyhandle timing specifications.[RNS95].Since there has not many works concentratingon the FPGA design for data parallelism, this problem remains worth for studying.

1.2. CONTENT OVERVIEW 9

In previous work of Viktor K. Prasanna et al [NYP11]from University of SouthernCalifornia Los Angeles, they explore a way to perform parallel processing data forbelief propagation in the factor graph on general-purpose multi-core systems. In hiswork, data is shared among the processors (cores). And it gains a 30x speed up inperformance, compared with a serial implementation on a platform with 32 cores.

However such data parallelism mechanism for belief propagation has not been ex-plored on the FPGA. Given the fact that the functionality of signal processing onthe FPGA can be user defined according to the demand, it shows also an interestingaspect of implementing the factor graph and data parallelism mechanism into theFPGA and measure its performance.

The reason to choose FPGA platform is that the FPGA is better in implementingthe data parallelism compared with multi CPU core structure. This is because themulti CPU core structure is fixed and the hardware structure is not flexible. Thoughit is possible to implement parallelism into different cores, but to increase number ofcores in the system, man cannot simply make a new core, but to choose a system withmore cores, which may increase the cost of the implementation. While in FPGA,each block can be user defined, which is easier for structure extension and it is moreflexible to change. Therefore it is more convenient for implementation. Besides,FPGA suppliers such as Xilinx offers good technical support and design tools forFPGA implementation. Furthermore, compared with multi-thread processing, theparallelism using FPGA deals with hardware processing, which proves to be moreefficient than software processing.

Generally speaking, in this thesis, the problem is both theoretical and also practi-cal. Theoretically speaking, the combination of the factor graph and the imitationlearning for robotic control in the same grasp-and place scenario is a new conceptualproblem. Meanwhile, a joint hardware and software environment should also be builtin order to perform the scenario, which is also a challenging task. The remainingquestion is whether this combination can actuality improve the performance of therobot in grasping and placing scenario. Thus such combination leaves interestingaspects worth to be explored.

1.2 Content overview

In this thesis, Chapter 1 introduces the basic background concept and related workin this thesis. Chapter 2 introduces the general design concept of my solution to thethesis tasks. Chapter 3 introduces the implementation platform, the software andhardware environment for implementing my design. Chapter 4 shows the experi-mental results, given the design concept and the implementing methods. Chapter5 introduces some discussion about the experimental results and introduces some

10 CHAPTER 1. INTRODUCTION

aspects of how to improve the thesis result in the future. Chapter 6 conclude thethesis.

11

Chapter 2

Design idea of combining imitationlearning with inverse kinematics

2.1 General overview

The design of my solution to this thesis can be divided mainly into two parts: Theconceptual development for the combination of imitation learning and the inversekinematics using factor graph; and the implementation of such combination intothe FPGA using the data parallelism mechanism. The technical implementationcan also be separated into two tasks: the systematic simulation of the conceptualalgorithm and the design of the FPGA and control of the robotic arm, which canfurther be divided into hardware implementation and software implementation.

The conceptual model developed for combining the Factor Graph and the imitationlearning is divided into several steps:

2.1.1 The robot configuration and forward kinematics

Firstly, before implementing any application on the robotic arm, its physical con-figuration should be known. The configuration of the robotic arm will be useful forother applications in this thesis and for performing the forward kinematics, whichoffers good contrast example compared with inverse kinematics . The forward kine-matics model used in this thesis is the Denavit-Hartenberg model. Using this model,the position coordinate of the end-effector can be directly confirmed, given the jointparameter values of the robotic arm.

2.1.2 The imitation learning

Secondly, in the imitation learning part, the robotic arm is guided by human op-erator from the initial position to the target position several times. At the sametime, the robotic arm automatically record sufficient data necessary for training the

12 CHAPTER 2. DESIGN IDEA OF COMBINING IMITATION LEARNING WITH INVERSE KINEMATICS

factor graph in each guidance scenario. This step is also known as programmingby demonstration. The data collected are mainly the end-effector position valuesof the robotic arm and the corresponding joint angle values to each position in themovement. All the collected data will be formed into one dataset for further usage.

2.1.3 Optimization with Gaussian Mixture Regression model

Thirdly, the dataset collected by the last step are treated as an input for the in-verse kinematics. But before doing inverse kinematics with factor graph,they areoptimized using Gaussian mixture regression algorithm. The Gaussian mixture re-gression algorithm is based on the Gaussian mixture model and the ExpectationMaximization algorithm. The dataset containing position values and joint valuesfrom each scenario are applied to the mixture Gaussian model.

Through the Gaussian mixture regression process, an optimized trajectory for therobotic arm movement can be formed. In the optimization process , the Expecta-tion Maximization algorithm and the K-mean clustering process are introduced toestimate parameters for the Gaussian mixture model.

2.1.4 Inverse kinematics with factor graph

Fourthly, after forming an optimized trajectory for the grasp-and-place scenario, it ispossible to utilize the mechanism of inverse kinematics by using the factor graph todo the inference from the position variables to the joint angel variables. The factorgraph uses the sum-product algorithm to send messages from the nodes represent-ing the position variables to the nodes representing the joint angel variables, andupdates the probabilities of each state of the joint angel variable. Using expectationcalculation, it is easy to get the real value of the angel, which corresponds to theposition variable.

Then with these angle datasets in a sequence, the robotic arm can execute the jointangle datasets to perform the desired movement accordingly. In this sense, the taskfor inverse kinematics is done using factor graph.

The benefits of using the factor graph as an inverse kinematics approach is that itprovides an efficient way to perform inverse kinematics from the task space of therobotic arm to the joint space for the robot dynamic control.

2.1.5 FPGA design

The FPGA is an alternative useful tool for data processing, especially for parallelcomputing. On the other hand, the design with FPGA can not simply migratingthe design from an simulation environment straight forward into the FPGA system.

2.2. DESIGN DETAIL 13

Because the FPGA is hardware oriented, any migration from the simulation en-vironment into the hardware level design may cause syntheses or implementationerror. In this sense,in order to implement the factor graph onto the FPGA board,the design of the FPGA project should carefully consider the resource consumptionson the FPGA chip. At the same time, the signal and data should also be carefullydesigned.

2.1.6 Robot control in grasp-and-place scenario

Finally, through executing commands according to the joint angel datasets corre-sponding to each position in the grasp-and place scenario, the robotic arm can becontrolled to reach the destination. In this sense, the robotic arm performs just thesame movement curve as in the imitation learning demonstration. Then the task ofthe grasping and place scenario is completed.

2.2 Design Detail

In the following section. A deeper view into the design will be presented.

2.2.1 The robot configuration and forward kinematics

A robot manipulator is composed of a set of links connected together by joints. Thejoints can either be very simple, such as a revolute joint or a prismatic joint, or theycan be more complex, such as a ball and socket joint. [M.Spong] The differencebetween them is that in the first case, the joints have only one degree of freedom,while in the latter case the joints have more degrees of freedom. Since each jointconnects two links, a robotic arm with m joints will contain m+1 links. In orderto get the the relationship between the joints and links for a specific robotic arm,the robot configuration model for the joints and links should be build to form thetransformation in between.

One common model for clarifying this relationship of joints and links is using theDenavit-Hartenberg convention. In this thesis, the Denavit-Hartenberg convention isperformed based on the physical configuration of the Denavit-Hartenberg parametersof the robotic arm. It provides forward (or configuration) kinematic equations forrigid robots. Usually it concerns about the joints of the robotic arm and the positionand orientation of the tool or end-effector. In this thesis, these joints are the anglesbetween the links, because these joints are revolute joints.

The purpose of using this convention is to perform the forward kinematics for therobotic arm, which will be compared with the result of the inverse kinematics using


factor graph. On the other hand, by confirming the physical configuration of therobotic arm, the robotic arm’s working space can also be confirmed, which formsthe moving limits for the robotic arm in the grasp-and-place scenario.

In this convention, a homogeneous transformation Ai is used to present a productof four basic transformations:

Ai = Rotz,θiTransx,diTransx,aiRotx,ai (2.1)

Ai =

cθi −sθi 0 0sθi cθi 0 00 0 1 00 0 0 1

1 0 0 00 1 0 00 0 1 di0 0 0 1

×

1 0 0 ai0 1 0 00 0 1 00 0 0 1

1 0 0 00 cαi

−sαi0

0 sαicαi

00 0 0 1

(2.2)

Ai =

cθi −sθicαi

sθicαiaicθi

sθi cθicαi−cθisαi

aisθi0 sαi

cαidi

0 0 0 1

(2.3)

Where the four quantities di, θi,ai, αi are parameters associated with link i and jointi.[M.W87].

Here the four parameters are named link length, link twist, link offset andjoint angle. In order to make the meaning of this formula more clear. The fourparameters are named and explained in the following way:

• d(di) : Offset from previous z to the common normal

• θ(θi) : Angle difference with previous coordinate, from old x to new x

• γ(ai) : Length of the common normal.

• α(αi): Angle difference with previous coordinate, from old z axis to new zaxis.

These variables are the essential four parameters for the Denavit-Hartenberg con-vention. Instead of using A, transformation T is used to represent the relationshipbetween the frames of the robotic arm.

The T-matrices are given by:


T 01 = A1 (2.4)

T 02 = A1A2 (2.5)

So the above transformation can also be described as the following formula[LCRZ96a][LCRZ96b]:

n−1Tn =

cos(θn) − sin(θn) cos(αn) sin(θn) sin(αn) rn cos(θn)sin(θn) cos(θn) cos(αn) − cos(θn) sin(αn) rn sin(θn)

0 sin(αn) cos(αn) dn0 0 0 1

(2.6)

n−1Tn =

R T

0 0 0 1

(2.7)

Here the upper left sub-matrix R represent the rotation and the upper right sub-matrix T represent the position values of the number nth frame.

In order to transform from the frame of the end-effector to the base frame of therobotic arm, the relationship between the position values from frame to frame shouldalso be studied.

By iterative using the T-matrices, the transformation from the end-effector to thebase frame can be described in the following formula:(here the example is of twoiteration)

T 02 = A1A2 =

c12 −s12 0 a1c1 + a2c12s12 c12 0 a1s1 + a2s120 0 1 00 0 0 1

(2.8)

Notice in this formula,

x = a1c1 + a2c12; (2.9)


y = a1s1 + a2s12; (2.10)

They are the coordinates of the end-effector in the base frame. Further more, therelationship for the transformation between each frame from end-effector to the baseframe can be calculated as following:

The position relationship of link n with respect to n-1 may be represented by aposition matrix indicated with the symbol T [LCRZ96a][LCRZ96b]:

Pn−1 = n−1TnPn =>

xn−1yn−1zn−1

1

= n−1Tn

xnynzn1

(2.11)

Here the xn,yn,zn variables and the xn−1,yn−1,zn−1 variables represent the relativeposition between two body link. With this transformation, given the coordinate ofthe end-effector, its position coordinate in the base frame can be confirmed.

In the thesis, the Robotic Arm is the WidowX Robot Arm Mark II provided bythe Trossen Robotics company. It is a 6 axis robotic arm and has a working spacebetween 10cm to maximal 41cm. It also has a lifting weight force up to 800 gram.The robotic arm used in this thesis has only revolute joints. The joint values arealso included in the four parameters to be used in the transformation. In this sense,given the joint values, using the transformation of P given above2.62.11, not onlythe position of the end-effector can be calculated, but also the position values ineach joint can be confirmed.

In this way, the Denavit-Hartenberg convention can be easily performed. Using thismodel, the robot working space can be build and confirmed, which proved to bevery useful in later experiment. By building the working space, if the results of theinverse kinematics using factor graph proves to exceed the working space limits ofthe robot, these results should be filtered and informed in advance to avoid damagesto the robotic arm when reaching to places they should not to.

The working space can be build in the following way:

Firstly, by manually measuring the joints’ limits of the robotic arm, the workinglimits for each joint can be confirmed. These can be done by using protractor andrecording the real range of individual joint.


Figure 2.1: Working limits of the robotic arm in plane in side view[Rob]

Secondly, using the Denavit-Hartenberg convention, the position values of the end-effector in the base frame can be confirmed, given the corresponding joint workingrange as the parameters for the Denavit-Hartenberg convention.The working limits of the robotic arm can be shown in the the figure2.1 and 2.2

Another thing to point out is that, in this thesis the robotic arm has no sensorattached to the end-effector to sample the position value, by undertaking the imi-tation learning and performing the inverse kinematics, the position value cannot bemeasured by the robotic arm itself. In this sense, the position values can either beobtained by using measurement from other system or using the forward kinematicsof Denavit-Hartenberg convention.

In the convention, the joint data can be measured by simultaneously reading theangle value from the motors located in each joint on the robotic arm. By recordingthe angle values of each joint from time to time , the angle datasets is build. Withthe help of the Denavit-Hartenberg forward kinematics, the robotic arm’s movingtrajectory can be generated to get the position values of the end-effector correspond-ing to each sampled angle value. In this way, the grasp-and-place task of the roboticarm can also be performed by executing the angle datasets, so the robot will alsomove along the trajectory to the target position. The target position values of theend-effector is then compared with the result using inverse kinematics using factorgraph in later experiment.

2.2.2 The imitation learning

After the physical configuration of the robotic arm is confirmed, it is possible to startwith the imitation learning part. In this grasp-and-place scenario, the imitationlearning plays a crucial part at the beginning. With the guidance of the human, therobotic arm can be easily guided from the initial position to the target position.


Figure 2.2: Working limits of the robotic arm in plane in overview[Rob]

Programming robots to perform complex tasks is difficult with classical methodsfor instructing robots, such as textual or graphical user interface (GUI)-driven pro-gramming techniques [GB03][EPD13]. In this sense, an intuitive way for teachingthe robot certain task is through human demonstration and make the robot imitatehuman movements.

In the thesis’s scenario, the grasp-and-place task can be performed by generating thetarget trajectory and pre-program the robotic arm for accomplish the task, whichproves time consuming for programming a single task and not easy to change whenthe task is modified. The Imitation learning is an approach to address such skillacquisition problem flexibly : The robotic arm records the demonstration of a taskfrom the teacher and learning the task using learning algorithm to perform the taskagain.

The basic problem in the grasp-and-place scenario in this thesis is that the humanteaching for the task forms slightly different trajectory in each demonstration. Inorder to perform the scenario more accurately and to move the robotic arm moreprecisely to the target position, the imitation learning algorithm should adapt tothe movement changes in each demonstration and form the optimized trajectory forthe robotic arm to perform with.

In contrast with the imitation learning, some researches have already developedsome other ways to teach the robotic arm to perform certain task, as in the same


scenario in the grasp-and-place task in this thesis.

The work of Peter Englert et al uses the simulations for the robot trajectory pre-diction instead of sampling real robot trajectories, it reduces the interaction timewith the robot when doing sampling. They developed the learning probabilisticforward models for probabilistic imitation learning via trajectory matching. Theirgoal is to imitate the expert’s behaviour by finding a policy π∗that minimizes theKL divergence between the distribution p(τπ)over demonstrated trajectories and thedistributionp(τπ) over predicted trajectories when executing a policy π.[EPD13].

Then they can do the trajectory representation for the robotic arm to execute. Buttheir approach is not robust to all kinds of correspondence problems, particularly ifthe required control commands for imitating the teacher exceed the torque limits ofthe robot. In this case, they cannot imitate the teacher.

Comparing to the forward kinematics prediction model by Peter Englert et al, thesampling system used in this thesis contains two parts. One part is the 3D cameratracking system for sampling the position of the end-effector; the other part is theservo motor in each joint on the robotic arm, through which the joint value for eachmotor at given time can be read out and stored. In this case, the interaction’s timeof the 3D camera tracking system with the robotic arm is zero; the interaction’stime with the servo motor on the robotic arm is also low.

Using this 3D camera tracking system, the position values of the end-effector on therobotic arm can be recorded. Then proper model should be developed for processingthese data and simulating the movement. Approach developed by Peter Englert et aluses the forward model of the robot’s dynamics to generate trajectory predictions fora given controller . But this approach has its limits and uncertainties in prediction.[EPD13] In this sense, in this thesis the Gaussian Mixture Model (GMM) and theExpectation Maximization algorithm are adopted for building model for the recordeddata. Then the Gaussian mixture regression(GMR) are used for optimizing theGMM model and generate an expected trajectory.

2.2.3 Optimization with Gaussian Mixture Regression model

A Gaussian Mixture Model (GMM) is a parametric probability density function rep-resented as a weighted sum of Gaussian component densities. GMMs are commonlyused as a parametric model of the probability distribution of continuous measure-ments or features in a biometric system, such as vocal-tract related spectral featuresin a speaker recognition system.GMM parameters are estimated from training datausing the iterative Expectation-Maximization (EM) algorithm or Maximum A Pos-terior (MAP) estimation from a well-trained prior model[Rey09]. The GMM often


use the Gaussian mixture regression(GMR) for optimization of the sampled datasets.

The clear advantage of Gaussian mixture regression (GMR) over a stochastic re-trieval process is that it provides a fast and analytic way in reconstructing the“best”sequence from a Gaussian model Regression using GMMs offers a way of ex-tracting, from a dataset, a single generalized signal made up from the set of signalsused to train the model. [CGB07]

Mathematical description for a GMM model is as following :

A common Gaussian Mixture Model is a weighted sum of M component Gaussiandensities, which can be described by the following formula:

p(x|λ) =M∑i=1

ωig(x|µi,∑

i) (2.12)

where x is a D-dimensional continuous-valued data vector (i.e. measurement orfeatures), ωi , i = 1, . . . , M , are the mixture weights, and g(x|µi,

∑i),i = 1, . . .

, M , are the component Gaussian densities.[Rey09]

More precisely, the above function can be described as follows:

The common Gaussian Mixture Model is a weighted sum of M component Gaussiandensities, where x is a D-dimensional continuous-valued data vector( i.e. measure-ment or features), ωi,i = 1, . . . , M , are the mixture weights.

Each component Gaussian density is a Gaussian function with D-variate, which hasthe following equations:

g(x|µi,∑

i) =

1

(2π)D2 |∑

i |12

exp{−1

2(x− µi)′

∑−1

i(x− µi)} (2.13)

Here the µi is a mean vector and the∑

i is the covariant matrix. The constrain

for this Gaussian function isM∑i=1

ωi = 1. To complete the Gaussian Mixture Model,

these parameters should be configured:

λ = {ωi, µi,∑

i} i = 1, 2...M (2.14)


The {ωi, µi,∑

i} here represent the mixture weight, the mean vector and the covari-ant matrix.

In order to have an estimation on these parameters, the Expectation Maximiza-tion (EM) algorithm is introduced. Maximum-likelihood estimation of the mixtureparameters is performed iteratively using the standard expectation maximization(EM) algorithm[AND77].

The EM algorithm use the following equations for estimating the parameters above:

Calculation of the mixture weights:

ωi =1

T

T∑i=1

Pr(i|xt, λ) (2.15)

Calculation of the mean vector:

µi =

T∑i=1

Pr(i|xt, λ)xt

T∑i=1

Pr(i|xt, λ)

(2.16)

Calculation of the variance matrix:

σi2 =

T∑i=1

Pr(i|xt, λ)x2t

T∑i=1

Pr(i|xt, λ)

− µi2 (2.17)

Here the xi is a individual variable and Pr is the a posterior probability for compo-nent i, which is given by paper: [Rey09]

Pr(i|xt, λ) =ωig(xt|µi,

∑i)

M∑k=1

ωkg(xt|µk,∑

k)

(2.18)

In this way, all the necessary parameters for GMM are configured. Afterwards theGMR are computed and the expected regression trajectory is generated.


Figure 2.3: The general process of Robot Learning from Demonstration frameworkusing GMM and GMR[MCM13]

The general processing chain of the imitation learning using GMM and GMR canbe shown in the figure2.3 [MCM13].

The purpose of using the Gaussian Mixture Model is to generate a smooth optimizedtrajectory of the robotic arm’s movement from the Gaussian Mixture Model. Whenthe above listed parameters are generated for the GMM, (using equation2.12), theresulting probability density function is formed.

One thing to mention about is that, by calculating the EM algorithm, in order toreduce the calculation time, the method of K-Mean clustering is introduced.

The K-means is based on the notion of the cluster center, which is a point in the dataset, but usually not the data themselves. In fact, the data which are centred formclusters. The k-means clustering algorithms usually tries to minimize the averagesquared distance between points in the same cluster, i.e. if d1, d2,. . . , dn are the ndocuments and c1, c2,. . . , ck are the k clusters centroids, k-means tries to minimizethe global criterion function:[BT10]

k∑i=1

n∑j=1

sim(dj, ci) (2.19)

With k-mean clustering algorithm the GMR process will be efficiently accelerated.

Through imitation learning, we guide the robot to reach a target place many times,during which the robot should record its position values and angel values. Theposition values consist of mainly three variables: x, y and z, with which the posi-tion of the end-effector can be located. Another variable is also to be taken intoaccount, which is the angle value. As a matter of fact, there is a certain angle setscorresponding to each position value. In this angle sets, for each position of theend-effector, there exists correspondingly a group of angles, called angle variables inthis thesis.

The angle data sets contains several such groups of angles along the task trajectory.In planar movement of the robotic arm , each angle group is a vector value consist


Figure 2.4: Robotic arm’s moving trajectory with different position values and theircorresponding angle group values

of 3 different angles, while in 3D movement, the angle group contains 4 differentangles of the robotic arm.

This concept can be shown in the following figure:

From the figure 2.4 it is clear to see that, along the robotic arm’s moving trajectory,for one position of the end-effector, there is correspondingly a set of robotic arm’sangle values. In fact, the convention from the position values to the angle valuesis called the inverse kinematics, while the convention from the angle values to theposition values is called the forward kinematics. In this thesis, these two notions areoften referred to. They both forms the fundamental principles for robotic controlarea.

As is introduced above, each position variable of the end-effector can be computedand optimized using the Gaussian Mixture Model and the Gaussian Mixture Re-gression model. After calculation, the updated optimized density function of themixture Gaussian model for each variable is then formed. Given different trajectoryposition value inputs and the result of GMR algorithm, the expected mean values


of the input trajectories can be calculated.

The use of GMM and GMR form one optimized trajectory as the result. Thenthis trajectory is called the task trajectory, which will represent the demonstrationprocess in the imitation learning. Then with this optimized trajectory data and theother GMR input trajectory data, man can calculate the joint parameters using theinverse kinematics. In this sense, the factor graph model should be developed, inorder to get the joint parameters(angle values) at each position on the task trajec-tory.

2.2.4 Factor Graph model overview

Before we begin with the design of Factor Graph, the concept of the probabilisticgraphical models should be introduced here. Usually there exist two main classes ofthe probabilistic graphical model:

The first are so-called directed graphical models, which are also popular with thename Bayesian Network; and the second are called undirected graphical models,which are also commonly referred to as Markov Random Fields. [SMJ13]

The difference of the two models is that in Bayesian model, the graphical modelhas directed link between each variables, which represents the conditional relationbetween the variables; while in Markov Random Fields, the graphical model hasnon-direction link between the variables.

The combination of both graphical model results in the concept of the factor graph.The purpose of this combination is that , it might be interested for further analysiswhen the model can preserve more information about the form of the distributionthan either Bayesian Network or Markov Network can do alone. [Fre03][SMJ13].Toconstruct a factor graph, the Bayesian Network and the Markov Random Fieldsoffers the bases for the network structure.

For example, given a model with five variables (A,B,C,D,E) and the following rela-tion:

p(A,B,C,D,E) = p(E|C,D)p(C|A,B)p(A)p(B)p(C) (2.20)

For this relation, the expression can be factorized as:

p(A,B,C,D) = f1(A)f2(B)f3(D)f4(ABC)f5(CDE) (2.21)


Figure 2.5: Construction of a factor graph from a structured factorization of condi-tional probability[SMJ13]

In this way, the resulting factor graph is shown in the figure:2.5

Figure 2.5 shows that the factor graph preserves the structural information from itsunderlying Bayesian network.[SMJ13]. This is a factor graph without loop. For afactor graph with loop, it might form a cyclic structure and thus exist oscillationdue to a loop in the graph. In this sense, by constructing the network of factorgraph, the loop should be avoided.

In this thesis the Factor Graph introduced is based on the Bayesian Network(BN).It uses the belief propagation method to pass messages along its edges and usesum-product algorithm to calculate the target node’s value. By introducing thefactor graph, the inverse kinematics is performed to infer the position values of theend-effector of the robotic arm to the joint values of the angles on the robotic arm.

2.2.5 General design with the factor graph model for inversekinematics

The factor graph model developed in this thesis consists mainly of the two parts:the position variables of the end-effector (x,y,z) and the joint variables (mainly theangle values).

By building the probabilistic graphical model using a factor graph, the belief propa-gation is introduced. It is one of the exact inference mechanism. Using this method,


it ensures that a variable node will update its belief after receiving messages from allits neighbouring factor nodes.[SMJ13]One of the popular algorithms for belief prop-agation is the sum-product-algorithm. It relies on the message passing among nodesin the graph. By performing the sum-product-algorithm, following parts should betaken into consideration:

Nodes variable state discretion

In this part, the input nodes variables with certain value range will be divided intodiscrete states, each states represent one value of one node variable. For example,if variable A represents the position x of the robotic arm and has its value rangefrom 0cm to 5cm. By defining A’s value into 6 discrete states, each state has thevalue of 0cm, 1cm, 2cm, 3cm, 4cm and 5cm, then A can be represented with these6 discrete states.

Generating Gaussian distributed inputs

In this part, a normal Gaussian distribution will be built as input for the nodevariables. The Gaussian distribution will then be applied to the node variable.Since each node has many states, this operation assign each node state with itsown probability. Then the whole set of probability for the node variable will benormalized to make sure that the probabilities of the states adds up to one in theend.

Probability calculation of factor nodes

The probability calculation of factor nodes takes the probability distribution of eachnode variable and output the factor probability. This probability can either be jointprobability or the conditioning probability. For calculating joint probability, forexample, if there exist two variables A and B, then the calculation is:

p(A,B) = p(A)× p(B) (2.22)

for the conditioning probability the calculation is

p(A,B) = p(A|B)× p(B) = p(B|A)× p(A) (2.23)

In this thesis, the factor probability takes the joint probability.


Marginal calculation of incoming message

The marginal calculation calculates the marginal probability distribution for eachstate in the target node. By adding up the sum of the marginal distribution from themessage passing from other parts of the graphic network, the marginal calculationfor each states in the target node variable is then computed.

For example, let x1, x2, x3 be a collection of states for one variable,let h1, h2, h3 bethe 3-valued function of these states, the marginal summary for x2 is denoted by

∑x2

h(x1, x2, x3) :=∑x1∈A1

∑x3∈A3

h(x1, x2, x3) (2.24)

So generally we have:

gi(xi) :=∑xi

g(x1, ...xn) (2.25)

This formula shows the general form of summary for xi of g.[KFL01]

The messages passing along the network can be divided into two types: one sentfrom variable node x to factor node f, and the other sent from factor node f tovariable node x, which can be shown in the following equations:

µx→f ∝∏

h∈Nx\f

µh→x(x) (2.26)

µf→x(x) ∝∏Nf\x

f(Xf

∏y∈Nf\x

µy→f(y)

(2.27)

Where x and y are variables; f and h are factors; Nf and Nx are the sets of neighboursof f and x respectively;

∑denotes marginalization over a potential table;

∏denotes

products of the potential tables.[NYP11]

Expectation calculation of the target node variable

The expectation calculation transforms the probability distribution of the targetnode variable into the real value of this variable. Simply multiplying the probabilitydistribution of the target node variable with its own states values and adding thesemultiply result up, the output real state value can be computed.


2.2.6 Design of factor graph model in this thesis

In this thesis, the variables taken into consideration are mainly four variables: theposition variables x, y, z of the end-effector of the robotic arm and the angle variables(θ1, θ2, θ3, θ4).

Here is to mention the concept of conditional independence:

In the probability theory, conditional independence is denoted with the followingstatements:

Given variable X,Y and Z , if

p(x, y|z) = p(x|z)p(y|z) (2.28)

Then the X and Y are conditional independent given variable Z.[Daw79].

In this sense, given the following example: given variable X, Y, Z and D. SupposeX and Y are conditional independent,then if these four variables form the followingstructure, their relationship can be confirmed.

Figure 2.6: Conditional independence with X,Y,Z and D

Since X and Y are conditional independent , then given a new variable D, X and Yare still conditional independent, if Z is also given.

By transferring the example 2.6 into the the more complexer graphical structure,we can extend the idea of conditional independence into wider range of applications. So here in the following example, we have position variable x,y,z and two anglevariables θandβ. The angle variable θandβ are given variables. So we have thefollowing structure:

Suppose the variable θ, β are angle values of the robotic arm, the X, Y, Z are theposition values of the end effector. In normal case, the position of the end-effector are


Figure 2.7: Conditional independence with X,Y,Z and θ, β

determined by the angle values of the robotic arm. And X, Y and Z are independentvariables in the graph model 2.7,given the variable θ, β.

According to the above notions, the X , Y and Z are conditional independent. Thenthe probability formula for conditional independence in this case can be extendedinto the following form:

p(x, y, z|θ, β) = p(x|θ, β)p(y|θ, β)p(z|θ, β) (2.29)

It can also be inferred:

p(x, y, z|θ) = p(x|θ)p(y|θ)p(z|θ) (2.30)

p(x, y, z|β) = p(x|β)p(y|β)p(z|β) (2.31)

In this sense, by performing inverse kinematics using factor graph, the graph 2.7can be transformed into separate graphs, each graph contains one angle variablein the joint space and X,Y,Z variable in the task space. Then it is possible to usethe conditional independence theory to get the probability value of θandβ given theprobability of X , Y and Z by performing the inverse kinematics using factor graph.

The resulted factor graph model can be simplified into the following form:


Figure 2.8: Generic design model using factor graph used in this thesis

In the figure 2.8, θ is the angle variable of the robotic arm. The robotic arm inthis thesis has 4 degree of freedom in the of grasping and placing task. So the θ isa general symbol, it represent each angle variable corresponding to each degree offreedom. So θ can be four variables: θ1 ,θ2 , θ3 ,θ4

In this graphical model, the factor node use the joint probability of the variables x,y,zand θ. Then sum-product-algorithm is performed and the result will be explainedin detail in chapter four.

By performing the imitation learning process introduced in figure 2.3, the optimizedmoving trajectory of robotic arm’s is confirmed. According to figure 2.4, along themoving trajectory of the robotic arm, the data of the position values and the anglevalues are recorded. So in the datasets there are variable x,y,z and angle variable,each has its own values’ range.

Each variable is discretized into different states representing different position pointson the task trajectory. The inverse kinematics using Factor Graph is performed toget the real value of the angle from the input variables x,y and z. We can then usethese angle values to control the robot finishing the task movement to reach thetarget.

However, the factor graph running on the PC don’t have the characteristic of dataparallelism. The reason for performing the data parallelism is that it allows thedata processing parallel simultaneously happens. In this case , it will reduced thetime needed for performing the sum-product-algorithm. Especially when the statesof each node and node’s number in the factor graph grows, then the calculation ofthe joint probability in the factor node will grow significantly. This increases thecalculation time of performing the sum-product-algorithm.


With data parallelism, the calculation can be divided into smaller parts and beprocessed simultaneously. Using FPGA, such data parallel processing can be userdefined and realized. Thus it reduces the time for data processing. At the sametime, it shifted the load from the processing micro controller into the FPGA, thusmake a balance for the load.

2.2.7 Parallel design model for the inverse kinematics usingFactor Graph

Some earlier works have looked into the data parallel processing mechanism. Usingthe method introduced in the paper by Prasanna et al, it is possible to implementthe parallel processing algorithm on multi-core/many-core processors.[NYP11]Theyexplore data parallelism for two primitives for belief propagation in the factor graph:table multiplication and table marginalization.

Their parallel design model for the inverse kinematics using Factor Graph can bedivided into mainly four steps:

1. Instead of storing the states of variable X along with their potential values, itonly store the potential values in a one-dimensional table F. Using the followingtwo equations, an index mapping between the index of the table F( table forstore the potential values) and the index of the states of variable X is build.

i =d∑

k=1

xk

k−1∏j=1

rj (2.32)

i = b i

oskc mod rk (2.33)

Where rk,rjrepresent the number of states in each variable X. And i is theindex in the potential table F and i is the index of the states of the variableX. Also

osk =k−1∏j=1

rj (os1 = 1) (2.34)


oskis precomputed for each variable xk of f.[NYP11]

2. On a shared memory parallel system with P threads, the potential table F isdivided into P chunks. In table multiplication, each thread updates its ownchunk.

3. In table marginalization, the output message needs to be updated using thepotential factors from all chunks.Thus, when computing marginal, it let eachthread keep a local resulting message computed from its own chunk.

4. Then, these local messages are aggregated to generate the final message.

Generally, their algorithm can be shown in the following table [NYP11]:

Algorithm 1 Message computation kernel

Input: factor f, variable xk of f, potential tableF of f, number of threads POutput: message Xk sent from f to xk

1. F ∗=F

2. for p=0 to P pardo

{local computation for the product of F with Xj}

3. for j=1 to d, j 6= k do

4. Compute Xj

5. fori = |F |Pp to |F |

P(p+ 1)− 1 do

6. i = indexmapping(i, f, xj) using Eq.(2.33)

7. F ∗(i) = F ∗(i)×Xj(i)

8. end for

9. end for

{local computation for the marginal Xk fromF }


10. X(p)k =

−→0

11. for i = |F |Pp to |F |

P(p+ 1)− 1 do

12. i = indexmapping(i, f, xj) using Eq.(2.33)

13. X(p)k (i) = X

(p)k (i) + F ∗(i)

14. end for

15. end for

{global update for Xk fromthe local results}

16. Xk =∑

pX(p)k

In this way, they gained more than 30x speed-up compared with a serial implemen-tation on a platform with 32 cores.

2.2.8 Limitation of the design from Viktor et al

The design of Viktor et al runs well on a multi-core/many-core system. On the PCthe design is tested with a 4 core Intel AMD environment and performs good result.However when implementing it with the FPGA, it confronts some problem. In thework of Viktor et al, they implemented their design on a 32-core Intel Nehalem-EXbased system.[NYP11]

In this thesis, the design tool is Vivado HLS tool from Xilinx and it is used totransform the C code into the VHDL code and generating the FPGA block. Whenusing this design, each FPGA block simulates one core to do the above algorithm.

But the algorithm above generates a message containing a set of the potential values.These potential values belongs to different states of the target node in the factorgraph. In this case, it is the target node variable Xk. This message form cannotbe directly synthesized in the FPGA. Thus their design of parallelism cannot beimplemented into the FPGA board.

One possible solution may be using the FIFO structure to transmit these data, butit increases the complexity of the design job. So in order to simplify the FPGAdesign flow, in this thesis the design concept from Viktor et all will not be used forimplementing parallelism in the FPGA.


2.2.9 My Design using Factor Graph model

In order to make the design capable for synthesize, some changes should be made tothe factor graph calculation. In this sense, I developed my own design for paralleldata processing.

Based on the belief propagation and sum-product-algorithm, my factor graph followsthe general procedure of the common factor graph calculation. The changes to makethe design capable for parallelism is as following:

Instead of randomly using the joint probability to build the potential values of thefactor node variable, in my design, I place the the potential values of the factornode variable in special orders, so that each potential values that will be used formarginal calculation of a given output state will be put into one category. So thatwhen calculating these marginal probabilities of each state, it can be easily done byadding all the potential values in the same category together. The design flow canbe shown in the following tables:

Algorithm 2: Factor Graph training

Input:number of training variables for X, Y, Z, Angle :“N”, number of states foreach variable :“nX, nY, nZ, nangle”, state values of X,Y,Z, Angle :“X,Y,Z ,Angle”.

Output:trained factor :“factor”

1. for i=0 to i=N do

2. Judge the inputs of X,Y,Z and Angle belongs to which state.

3. DistributionX=Gaussian(X,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of X input

4. DistributionY=Gaussian(Y,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of Y input

5. DistributionZ=Gaussian(Z,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of Z input

6. DistributionAngle=Gaussian(angle,µ,σ) //Using Gaussian distributionto simulate potential distributions of Angle input

7. end do

8. for i=0 to i=N do // Normalize the potential distribution


9. Px[i] = {distributionX[i]} /sumX

10. Py[i] = {distributionY [i]} /sumY

11. Pz[i] = {distributionZ[i]} /sumZ

12. PAngle[i] = {distributionangle[i]} /sumAngle

13. end do

14. for i=0 to i=N do //calculate factor table using the joint distribution ofvariables

15. for j=0 to j=N do

16. for k=0 to k=N do

17. for l=0 to l=N do

18. Factor[n] = Px[l]× Py[k]× Pz[j]× PAngle[i]

19. end do //the order is to multiply potential value of X,Y,Z first,then multiplypotential value of angle, so each angle potential value responds to N×N×Ngroup of X,Y,Z values,this is to put the X,Y,Z potential values into samecategory.

20. factor = (factortable1+factortable2+· · ·+factortablen)/sum(factortables)// add all the trained factor table up and normalize the factors.

Algorithm 3: Factor Graph Validation

Input:number of validation variables for X, Y, Z : “N”, number of states for eachvariable: “nX, nY, nZ, nangle”, state values of X,Y,Z, Angle: “X,Y,Z ,Angle”,Trained factor table: “factor”.Output:active state value of angle variable “expectation“


2. Judge the inputs of X,Y,Z belongs to which state.

3. DistributionX=Gaussian(X,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of X input

4. DistributionY=Gaussian(Y,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of Y input


5. DistributionZ=Gaussian(Z,µ,σ) //Using Gaussian distribution to sim-ulate potential distributions of Z input

6. end do

7. for i=0 to i=N do // Normalize the potential distribution

8. Px[i] = {distributionX[i]} /sumX

9. Py[i] = {distributionY [i]} /sumY

10. Pz[i] = {distributionZ[i]} /sumZ

11. PAngle[i] = {distributionangle[i]} /sumAngle

12. end do

13. for i=0 to i=N do //calculate messages

14. for j=0 to j=N do

15. for k=0 to k=N do

16. Message[n] = Px[i]× Py[j]× Pz[k]

17. end do

18. { Define number of blocks P}

19. for p=0 to p=N do

20. Pangleupdate = marginalization(message, factor,N, p) //using marginal-ization on each block to update Potential values of angles

21. end do


23. Pangleupdate[i] = Pangleupdate[i]/sum(Pangleupdate[i]) //normalize the result of po-tential values of angle

24. end do


Expectationangle = sum(Pangleupdate[i] × angle[i]) //performing expectationcalculation

26. end do

27. {return the result}


28. Expectationangle

Algorithm 4:Table marginalization

Input:message passed from the network “message”, joint distribution of the vari-ables“factor”, number of states of X,Y,Z “N”, block number “p”Output:marginalized potential values for each state of angle, “marginal”

1. Marginal=0 //initialize marginal variable

2. for i=0 to i=N×N×N do //update the angle potential values and marginalizeit

3. Pangletemp[i] = message[i]× factor[i+ p×N ×N ×N ]

4. Marginal = Marginal + Pangletemp[i]

5. end do

6. {return the result}

7. marginal

Algorithm 2 and 3 shows the general process of performing inverse kinematics usingfactor graph, while algorithm 4 shows marginalization for each state in the targetnode variable Xk.

Notice in the algorithm 4, the return value of the table marginalization is a floatingnumber. In this sense, by creating a module in Vivado-HLS, it can be directlytransformed from C algorithm into VHDL module, which can be synthesized andimplemented into the FPGA. So based on the algorithm 4, several FPGA IP blockscan be built to perform the marginal computation.

In this sense, we can separate the marginalization part from the algorithm 3. Thenwe can create n number of IP blocks in the FPGA and perform the data parallelprocessing. The number of blocks used in the FPGA should be however equal tothe number of states in the target node variable, which means that given n statesfor the target node variable, there will be correspondingly n blocks in the FPGA formarginalization calculation.


From the above given algorithms, we can tell that both algorithms from Viktor et aland me are using the principle of sum-product algorithm to calculate the messagepropagation in the factor graph. But my algorithm is different from the algorithmof Viktor’s.

The main difference is that, in the design of Viktor et al, they use an index mappingmechanism to transform the index of the factor table to the index of different stateof the target variable’s table. Then in the target node variable its potential valuesis calculated using sum-product algorithm. While in my design for the factor graph,I list the factor table and the message table in an order, with which the tables formtheir inside ordering. Using this ordering method, I can directly use the index ofthe factor table and the message table for joint computation, message computationand marginal computation. So I don’t have to do additional steps to match eachstate of an variable with its potential value.

In this sense, I excluded the index mapping mechanism from Viktor et al. Andthen algorithm 2 and 3 will be running on the on board Linux system on the ZynqSoC ARM cores. It calls blocks using algorithm 4, where different blocks for doingthe multiplication and the marginalization can be created in the FPGA and finallydoing parallel algorithm for the Factor Graph.

2.2.10 Limitation of my design

The limitation of this design is that the on-board FPGA has its own intrinsic resourcelimits. So when the number of states for each position variable’s values grows,the resource within the FPGA will also have the chance to be run out. So byimplementing the data parallelism mechanism on the FPGA, the resource on theFPGA should also be taken into consideration.

On the other hand, using double floating point design in the FPGA can consumetoo much resources. The usage of floating point design on the FPGA may alsointroduce some systematic error into the calculation result. The FPGA has its owncalculation bias when performing sum or multiply calculation. So when performingthe sum-product algorithm for the belief propagation of the factor graph, it may alsoleads to slight systematic error. The result may have some bias from the simulationresult represented by the MATLAB.

Another limitation of this design is that the robotic arm has no sensors on theend-effector,so that it cannot automatically record the position values on the end-effector. On the contrary, it can only record the joint angel values each time. Inorder to test the functionality of the model proposed, the forward kinematics modelhas to be used to predict the end-effector position. A commonly used method is


the denavit-hartenberg algorithm. Another proposed solution is by using a globalcamera tracking system placed in a room to locate the exact position of the end-effector in a space limited room, where every position in this room can be trackedand labelled. This will be the continuation of this project.

41

Chapter 3

Implementation

Generally speaking , my FPGA design flow can be implemented on for normal FPGAboard. The technical implementation can also be divided into several parts: datacollection, hardware design and software design.

3.1 General introduction for the platform

The hardware platform of my design is based on the Xilinx Zynq ZC702 SoC (systemon chip) and a six axis robot-arm. The Zynq ZC702 SoC platform containes twoARM core and the on board FPGA, which can then be used for implementing withFactor Graph design. The implemented factor graph in the FPGA then can becalled and processed from the ARM processors. The robot-arm is connected to theZynq SoC platform and the Zynq SoC is connected to the PC. The Zynq SoC runsan on-board Linux system and can receive commands send by the PC as well assends commands to the FPGA, which begin the calculation of the factor graph onthe FPGA.The robotic arm used in this thesis is provided by the Trossen Robotics, the ”Wid-owX”.A glance of the structure of the robotic arm can be shown in figure:3.1.

3.2 Data collection

Before starting with other implementation, sufficient data should be collected forthe implementation. The main data to be collected are the position values of theend-effector and the joint parameters of the robotic arm.

3.2.1 Collection of position values

The initial plan was to create the dataset of the robotic arm moving along a tra-jectory. By using a ‘Motive’ 3D camera tracking system to capture the position

42 CHAPTER 3. IMPLEMENTATION

Figure 3.1: Structure of the robotic arm[Rob]

values and the joint values during the movement of the robotic arm, the dataset ofthe trajectory can be created.

The ‘Motive’ 3D camera tracking system uses 8 cameras located in each corner ofthe room. Three markers that are orthogonal with each other are attached to theend-effector of the robotic arm, which represent the X,Y and Z position values ofthe end-effector. While the robotic arm is moving by human guidance, the cameratracking system automatically records the markers position values and broadcastthe recording through UDP package in the local Ethernet network. Then by usinga third party tool, it is possible to capture these UDP package from the LAN anddownloaded to the PC. Then by processing the UDP package, the position valuesof the end-effector can be obtained.

3.2.2 Collection of angle values

The collection of angle group values of the robotic arm can be done by readingthe angle values from time to time. The robotic arm’s motor server driver offersfunctions to read out the current angle values of the servo. Combined with someshell scripts, through serial communication, the angle values can be recorded.

3.3. HARDWARE IMPLEMENTATION 43

Combining the angle values with the corresponding position values, the datasets forthe robotic arm’s moving trajectories can be formed.With this dataset, it is possibleto use the Gaussian Mixture Model and run data processing using MATLAB ona PC. Afterwords these optimized data are sent to the Zynq SoC to be processedon the FPGA. The returned results shows the joint parameters of the robotic arm,which can be send to the robotic arm motor server to perform certain action. Inthis sense, the grasp-and-place action can be reproduced.

3.3 Hardware Implementation

The hardware platform consists of mainly three parts: the SoC(system on chip)embedded system , the hardware interface between the SoC and the robotic arm ,and the robotic arm itself.

Some other parts necessary for the platform to function properly should also betaken into considerations: such as the Trenz TE0720 Carrier Board for the ZynqZC702 SoC, which provides the Zynq board its programming chain ; and a PC,which prepares the SD card with a pre-compiled Linux system and downloads theFPGA design into the Zynq SoC board through JTAG chain. The SD card is thenused to boot the Linux .

3.3.1 Trenz Carrier Board

In this thesis the Zynq ZC702 SoC is embedded on the Trenz TE0720 carrier board.The structure of the Zynq ZC702 SoC consists of two on chip ARM dual-core(Cortex-A9 MPCore @ up to 866 MHz) and the on board FPGA as the periph-eral devices controlled by the ARM cores.

The Structure can be shown in the figure:3.2.In this structure, it is clear to see, twoarm cores are located on the processing system side and the FPGA is located onthe programmable logic side. The Zynq processing system communicates with theFPGA as well as outside devices through multiple peripheral units.

The on board FPGA can be programmed through the jtag programming chain onthe Trenz board. The jtag programming chain can be shown in the figure:3.3.


Figure 3.2: Trenz0720 SoC structure[Tea]

3.3.2 FPGA Design

General Concept

The Vivado design tool provided by the Xilinx company is used to create FPGAproject for hardware implementation. By creating individual IP(intellectual prop-erty) in Vivado for the FPGA, we are able to define specific function on each IPblock, which will later be used in the Factor Graph calculation.

After connecting the individual IPs with the processing system,the connection re-lationship between the processing system and the program logic blocks can be con-firmed.Then the software design should be implemented based on the hardware blockthat are used, which can be achieved by using the Vivado SDK( Software Devel-opment Kit) provided by the Xilinx Company. Afterwards the hardware synthesisshould be run to check the errors of the FPGA design. If everything is fine, theFPGA design can finally be implemented into the FPGA in the Zynq ZC702 SoC.


Figure 3.3: Trenz jtag programming chain[Teb]

IP design

In order to construct a parallel design for the inverse kinematics using factor graph onthe FPGA, the IP blocks for each parallel core should be created. Using Vivado HLSdesign flow, it is possible to transform the C code to the VHDL code and outputsthe IP block. There are two parts in the design flow : the parallelism functionfile and the test-bench file. The parallelism function file is implemented with theparallel calculation function of the factor graph, while the test-bench file calls theparallelism function and runs the result generated by the parallelism function andperforming the sum-product-algorithm to get the result of the Factor Graph.

The test-bench file is used to provide necessary testing data for testing the func-tionality of the IP block. It is usually used in combination with the C-code designof the IP blocks. Once this testing succeeds, then the functionality of the IP blocksare proved to be logically correct.

The IP design using Vivado-HLS flow can be show in the figure:3.4. Using Vivado-HLS design flow, it simplifies the FPGA design. The design work mainly focuses onthe C design. The rest design flow will be automatically finished using Vivado-Hlsdesign flow. After processing with the Vivado-HLS flow, the customized IP blockcan be generated from C code. In this thesis, the generated IP block is called npcore.


Figure 3.4: Vivado-hls design flow for IP design[df]

It is used for marginal computation in the factor graph sum-product-algorithm.

Figure3.5 shows the IP block generated by using Vivado HLS. Through this IP corediagram it is clear to see that it has three data transfer ports:

• ”s axi AXILiteS” port for receiving the index number i to indicate the cur-rent state to be marginalized. It is also responding for sending back the resultof the marginal computation, once the calculation is done.

• ”message” port for receiving the message matrix of factor graph.

• ”factor” port for receiving the factor matrix of factor graph

After this IP design is finished , it is ready to be imported into the Vivado DesignSuit Environment for system model construction on the FPGA.


Figure 3.5: Marginal computation block ”npcore” generated by Vivado HLS

Model design

The parallelism for the factor graph implemented on the FPGA can be structuredin the following way:

The whole design consists of three parts: FPGA IP blocks for processing FactorGraph data (called npcore block), connections between the FPGA IP block and theprocessing system and the processing system itself.

An overview of the relationship of these blocks can be shown in the figure:3.6. Fromthe overview of the hardware map it can be seen: The left part of the design isthe Zynq processing system block; In the middle are the interconnecting blocks, onthe right part are individual ”npcore” IP blocks which will be parallel performingmarginal computation.

In order to make the relationship of the block connections more clear to the reader,here I zoom different part of the design into more clear layout and explain thefunction of each part in below.

On the left part in the design overview, the FPGA marginal IP blocks are connectedthrough DMA(Direct Memory Access) and AXI(Advanced eXtensible Interface) in-terconnection bus to the processing system. The DMA is in charge of transferringdata stream from the processing system to the ”npcore” IP blocks developed byVivado-HLS.

Between DMA and the AXI stream interconnection units, there is one interface blockcalled ”AXI-MM2S-Mapper”(AXI memory mapped to stream mapper, it helps theDMA to transform the data from the memory mapped data form into the stream


Figure 3.6: FPGA modules map overview

data form. In this way, the data will be received by the parallel IP blocks. Theconnection of this part can be shown in the figure bellow.

In the middle is the AXI stream interconnection, these interconnections are in chargeof sending the data stream of the message and factors to the individual IP blocksfor marginal computation. They can be shown in the figure below.

On the right part in the design overview are parallel listed IP blocks, namely the”npcore” IP blocks. These IP blocks act as the marginal computation units forprocessing data for Factor Graph. It has three inputs and one output. The inputsare the joint distribution data , the index indicating the current state of angle valuesfor marginal computation , and the message passing to the IP block. These IP blocksare shown below.


Figure 3.7: Connection from Zynq processing system to the DMA and AXI-MM2S-Mapper

Figure 3.8: Connection from AXI-MM2S-Mapper to AXI-Stream-interconnect

The processing system firstly initialize the logic IP blocks on the FPGA. Then itoutputs data stream to the FPGA marginal IP blocks through AXI stream intercon-nections. These data stream are outputted into the AXI stream ports of the npcoreblock, together with the current state index of angle values. The FPGA marginalIP block then performs the marginal computation for the Factor Graph. Then itgenerate an interrupt to the processing system, indicating the IP block has finishedthe calculation. Finally it sends the result back to the processing system using AXILite communication.

When the processing system receives such interrupt , it stores the return valuefrom the marginal IP block and prepare for the next computation result from othermarginal IP blocks, when all the necessary results for all different states of anglevariable are collected. The processing system computes the expectation value for


Figure 3.9: AXI interconnections

Figure 3.10: Parallel marginal computation IP blocks generated by Vivado-HLS

the angle variable. Then it updates the current real active index value of the anglevariable and finishes with the Factor Graph calculation. The relationship betweeninterrupt sources with the Zynq processing system can be shown below.

In this sense, each ”npcore” blocks are implemented in parallel form in the FPGAdesign and can perform the marginal calculation tasks simultaneously. In this way ,the FPGA hardware design realized the parallelism for processing data using factorgraph.

Here is also important to notice the control line on the FPGA, without which theblocks on the FPGA can never run. As is shown on the figure, there is an AXILite Interface for the DMA block and each npcore block on the FPGA board. Forthe DMA block, the AXI Lite interface are used for initialization of the block beforetransferring any data through DMA, while the initialization of the marginal IP blockis also through AXI Lite Interface. The control through AXI interconnect can beshown in the following figure.


Figure 3.11: Connection for the interrupt system of this design

Figure 3.12: AXI Control Line


Synthesize and generate bit file

After the model design is finished, the Synthesize for the FPGA design should beperformed to check whether it is suitable for hardware implementation. Then weshould run the FPGA implementation to transform the FPGA design into real hard-ware structure. If all goes well, the bit file for this FPGA design can be generated,which is ready for downloading to the FPGA board in the Zynq SoC.

Running FPGA design on the Zynq SoC

The FPGA design bit stream can be downloaded into the FPGA board using jtagprogramming chain. But in order to test its functionality, in this thesis, an on-boardLinux system is built to provide an environment for running the FPGA design onthe Zynq SoC.

There are already several existing version of Linux system available, which can run onthe Zynq SoC. But since the FPGA design in this thesis targets at the functionalityof data parallel processing. Some of the Linux systems are not suitable for the FPGAdesign in this thesis. After comparing several Linux systems and running them onthe Zynq SoC, the petalinux system is chosen for the FPGA implementation.

The reason for choosing this Linux system is that, it is compatible with the XilinxVivado Software Development Kit. Many feature of this Linux system can be definedby the user, according to the FPGA design. This makes the FPGA design moreflexible.

In this sense, when the FPGA design is finished. In order to boot up petalinux onthe Zynq SoC, the following components should be prepared:

• BOOT.bin file: this file contains mainly three files: the bit file of FPGAdesign; the FSBL(First Stage Boot Loader) image; and the u-boot image.The BOOT.bin file is the essential image file for booting up Linux system.

• Image.ub file: this file contains two important parts for the Linux system: thedevice tree and the kernel of the Linux system.

Another task needs also to be configured before booting up the petalinux usingthe above files. Since the FPGA design is using the virtual address to allocate theIP blocks on the FPGA. In the Zynq SoC environment, when the MMU(MemoryManagement Unit) on the Zynq SoC is activated, the virtual address for the IPdevice should be transformed into physical address, so the FPGA IP blocks can berecognized by the processing system. In order simplify the task and to access the IPblocks, the Linux UIO(Userspace I/O) mechanism is adopted. To enable the FPGAdevice for this mechanism, the device tree should be modified before compiling the

3.4. SOFTWARE IMPLEMENTATION 53

Linux booting image. In the device tree, the IP blocks should be announced to becompatible with the ”generic-uio” interface. Then in the Linux kernel configuration,the kernel should be configured to support the UIO mechanism.

With the above preparation, the petalinux can be booted up in the Zynq SoC. Thiscan be done by storing all the necessary files into the SD card, then using the u-bootmode in the Zynq SoC, the petalinux can be chosen to boot from the SD card. Aftersuccessfully booting up the petalinux, the IP blocks can be seen in the device listin the petalinux system. With this, the IP blocks are confirmed to be successfullyloaded in the SoC environment.

3.3.3 Hardware Interface

The Trenz carrier board is working with 12 volt Power supply and outputs the signalusing Pmod connector. These Pmod pins works with the voltage range not biggerthan 3.3 volt. However, the outputs signals from the robotic arm using UART is5 volt. In this case, there exists possibility that the Trenz carrier board will beburned by receiving signals which have bigger voltage than 3.3 volt, that is , whenthe output from the robotic arm provides a 5 voltage. So a hardware interfaceshould be created for transforming the voltage between the Trenz carrier board andthe robotic arm.

The main output pins on the Pmods on the Trenz Carrier Board are two signals:the Tx and the Rx. The Tx signal sends output signal from the Trenz board to therobotic arm, while the Rx signal receives the signal from the robotic arm to the Trenzboard. On the other side, the robotic arm is using a three wires UART to process thesignals. These three wires are Vdd, Data and GND. The Vdd is the power supplyfor the robotic arm and the GND is the ground connection. The robotic arm sendsand receive signal uses the same wire called Data wire.After achieving the hardwareinterface, we are ready to connect the robotic arm with the Trenz board and controlthe robotic arm using the Trenz board.

3.4 Software Implementation

The software Implementation is based on the hardware implementation, which con-sist of mainly two parts: implementation on the Zynq SoC and implementation forthe robot.


3.4.1 Implementation on the Zynq SoC

Using the Vivado SDK the software developed for the FPGA IP blocks can be de-signed and implemented. Using cross-compile mechanism, the c program developedon the PC can be transformed into the SoC environment.

Marginal parallel computation model

The marginal parallel computation model is the core of performing the parallelcomputation for the inverse kinematics using factor graph.The algorithm design flow is described in the table of ”algorithm 4” in section 2.2.9.

Test-bench model

The test-bench model is used mainly for prepare the necessary data for testing theFPGA IP blocks. When succeeded, it proves the the FPGA IP blocks to be logicallycorrect.

The algorithm design flow is described in the table ”algorithm 2” in section 2.2.9.

On chip application development

On the other side, in order to control the robot from the SoC, a cross compileenvironment for the ARM processor has to be established so that the program script,which is written on the PC can be transformed into the proper format for ARM. Inthe thesis, the running system on the PC is Ubuntu 12.04, while the running systemon the ARM processor is another version of Linux system.

The on board Linux system is in charge of controlling the FPGA for parallel calcu-lation as well as for controlling the robot. The Linux system should be configuredand loaded onto a SD card first, before running on the Zynq ARM processors. Inthis thesis, the on board Linux system is petalinux.

Then proper on chip program should be written in order to control the FPGAIP blocks and test its functionality. By developing the software application, theXilinx SDK(Software Development Kit) is used in this thesis. Another softwareapplication development platform should also be established, it is the petalinuxsoftware development environment.

The development flow is described so: Firstly the a user application is createdin the petalinux project. The next step is to compile and build the application

3.4. SOFTWARE IMPLEMENTATION 55

using petalinux system build tool. This compiled application will be built into thepetalinux system image. Then using the SD card boot mode, this application willbe installed into the system. Finally by booting the petalinux system from the SDcard, we can execute the application inside the petalinux system for testing anddebugging.

3.4.2 Implementation for the robot control

For testing the robot function, the Ubuntu 12.04 environment is used for sendingcontrolling command. Using Dynamixel MX servos driver program and the hardwaresupport library developed for controlling from the official website, it is possible tocall the control function through linux shell command sets.

By writing a shell script, each movement of the robot can be automatically recorded,which proves to be very important for later experiment. The angle values by themovement can then be sampled in time interval for every 0.1 second and output intoa text file for later use. The starting point of the recording and the stopping pointcan be manually controlled. In this sense, the data will be recorded, only whenhuman guide the robotic arm to move along a trajectory during the demonstration.

Before connecting the robotic arm to the Zynq processing system, the serial com-munication in between should firstly be established, through the following settingsin the Linux shell:

• stty -F /dev/ttyUSB0 raw speed 4000000 //setting the speed to 4M

• stty -F /dev/ttyUSB0 -parity //setting parity as none

• stty -F /dev/ttyUSB0 -crtscts //disable RTS/CTS

• stty -F /dev/ttyUSB0 -clocal //setting carrier watch off

In this thesis, the robotic arm should be guided manually from the starting pointto the target point, during which ,the angle values of the robotic arm’s joints willbe sampled and recorded. The reason to use the Linux shell commands is that it issuitable for all Linux system and doesn’t need cross-compile environment. In this


sense, the shell commands developed on the Ubuntu system on the PC can alsodirectly used on the Zynq Linux system.

The structure of the command to read the robotic arms angle values is a commonLinux command:

• echo -en ”?PR” >/dev/ttyUSB0 //send ”?PR” command to the robotic arm

• cat /dev/ttyUSB0 //open a terminal to receive the output of the robotic arm

In this way, the robotic arm can be controlled by the Linux system. So by creating ashell script, the Linux program can repeatedly send commands and receive outputsfrom the robotic arm.

3.5 Design verification

3.5.1 Denavit-Hartenberg convention verification

To verify the design of the imitation learning and the inverse kinematics using factorgraph, necessary data should be prepared. In order to do this, we can generate therobotic arm’s moving trajectory using forward kinematics of Denavit-Hartenbergconvention. Then different moving trajectories are generated, which will be consid-ered as the input for the GMR model and the training data for the inverse kinematicsusing factor graph.

In order to generate the moving trajectory of the robotic arm, the Denavit-Hartenbergsparameters should be confirmed. It is also needed to get the moving limitation ofeach joint of the robotic arm. This is because the robotic arm has physical con-strains, so each joint can not move in 360 degrees, but they are moving with limits.So by taking 100 possible joint value combinations with each joint, it is possibleto use the Denavit-Hartenberg convention to perform the forward kinematics togenerate the moving trajectory of the robotic arm.

By adding some noise to the joint values along the moving trajectory, it is possibleto generate three slightly different moving trajectory. This different trajectories cansimulate the 3 demonstrations by the human guidance with the robotic arm, becausethe human demonstration may have differences in each demonstration, even for thesame task.

3.5. DESIGN VERIFICATION 57

3.5.2 GMR verification

After getting the different trajectory data, it is possible to put these data to theGaussian mixture model and use the Gaussian mixture regression to generate theoptimized trajectory. The corresponding detail can be found in chapter 2.2.3. Whenfinished, the optimized trajectory, namely the optimized demonstration, can beproduced.

3.5.3 Inverse kinematics model using Factor Graph verifi-cation

In this step, by giving the trajectory position values such as X, Y and Z, and bygiving the corresponding joint values, it is possible to train the factor graph to geta fixed table of factor, which describe the relationship between the input and theoutput variables. The corresponding design model can referred to chapter 2.2.6.

After training the factor graph with three demonstration data, the factor in this de-sign is fixed and trained. Then to the inverse kinematics can be performed by givingthe optimized trajectory’s position values as input and getting the correspondingjoint values as output. In this way, the inverse kinematics is performed with factorgraph.

3.5.4 Compare the result of inverse kinematics and forwardkinematics

Using the joint data generated from the factor graph, we perform the forward kine-matics using Denavit-Hartenberg algorithm to regenerate the robotic arm’s movingtrajectory. Then the result is compared with the original result of the Denavit-Hartenberg convention in the first case, to see the difference between the perfor-mance of the inverse kinematics using factor graph and the performance of theforward kinematics using Denavit-Hartenberg algorithm.

Using the inverse kinematics with factor graph, it is possible to get the joint valuesas result and to verify the accuracy of this inverse kinematics. Then by using theseresult it is possible to regenerate the trajectory using Denavit-Hartenberg conversionto see the difference between the result of inverse kinematics and forward kinematics.

59

Chapter 4

Experimental Results

Experimental results shown below are the individual test result for each model. Thecombination test of all the models have not been done yet, due to the robot measureproblem, which will be explained later. The results show good performance for theforward kinematics using Denavit-Hartenberg algorithm. The other performance ofthe GMM/EM and the inverse kinematics using Factor Graph is also given below.

4.1 Result of the Denavit-Hartenberg algorithm

The experimental result for the forward kinematics using the Denavit-HartenbergAlgorithm is given in the flowing graph:

θ1 θ2 θ3 X Y-1.74532952 -1.047197551 -2.059488518 -140.3752738 -40.49772849-1.74532952 -1.047197551 -2.042035225 -140.1700037 -40.12979532-1.74532952 -1.047197551 -2.024581933 -140.9707292 -39.81069295-1.74532952 -1.047197551 -2.00712864 -140.7765973 -39.54051858-1.74532952 -1.047197551 -1.989675348 -140.5867533 -39.3193545

Table 4.1: Partial result demo of the DH algorithm

The result in table 4.1 shows the relationship between the angles of robots and theirposition values of x and y in a planar. This DH algorithm can actually generate therelationships between all the possible positions of the robotic arm with their controlangles. By taking several samples from the generated table and measure them withthe real robot using rule and protractor manually one by one, it can be proved thatthese results are correct.

60 CHAPTER 4. EXPERIMENTAL RESULTS

These angles and their position values can be visually displayed through a figure ofthe robot moving trajectory given the table above. With these data, the roboticarm’s moving trajectory can be generalized, in the figure below, the trajectory isdrawn in 2D planar.

Figure 4.1: Result of forward kinematics using DH parameter shows moving trajec-tory of the robot

Th figure 4.1 shows the prediction of the robot’s moving trajectory. In this sense,when the angles of robotic arm are acquired, through performing forward kinematics,the position of the end-effector is known.

The above model for performing the Denavit-Hartenberg algorithm can also beextended into 3D space. Since the grasp-and-place scenario in this thesis happensin 3D space, the robotic arm’s movement should also include the position value ofZ. Then the result in figure 4.1 can be transformed into the flowing figure:

4.1. RESULT OF THE DENAVIT-HARTENBERG ALGORITHM 61

Figure 4.2: Robotic arm’s movement in 3D space

Figure4.2 shows that the forward kinematics using Denavit-Hartenberg algorithmalso works well in 3D space scenario.

In order to verify the design , here in this theses three slightly different trajectoryare generated as three different demonstrations, the result can be seen in below.

Figure 4.3 shows three slightly different trajectories. These trajectory data will beused later in the Gaussian Mixture Regression Model and the inverse kinematicsmodel using factor graph.


Figure 4.3: Simulation of three demonstration trajectory by forward kinematics

4.2 Result of the Gaussian Mixture Model

The experimental result from the Gaussian Mixture Model here uses the prototypemodel developed by the Sylvain Calinon, which is described in the book ”RobotProgramming by Demonstration: A Probabilistic Approach”.[Cal09][CGB07]

In this method, the EM algorithm is introduced to optimize the parameters for theGaussian Mixture Model:

λ =

{ωi, µi,

∑i

}. i = 1, . . . ,M. (4.1)

These parameters are mainly the mixture weight, the mean vector and the covariantmatrix.

Then the Gaussian density function can be formed using equation :

p(x|λ) =M∑i=1

ωig(x|µi,∑i

). (4.2)

4.3. RESULT OF THE INVERSE KINEMATICS USING FACTOR GRAPH 63

The expectation of the input data is calculated to form the optimized expectedvalue. The result can be seen in the following figure.

Figure 4.4: Result of the Gaussian Mixture Model using simulation data

Figure 4.4 shows the trajectories before applying Gaussian mixture regression modeland the optimized trajectory after applying Gaussian mixture regression model.From the result it is clear to see that the Gaussian mixture model smooths the mov-ing trajectory into one expected moving trajectory. In this sense, the 3D Gaussianmodel can be applied to the robotic arm’s scenario in this thesis. The result of theGaussian Mixture Regression will be used for training the factor graph and later forperforming comparison when doing the inverse kinematics.

4.3 Result of the Inverse Kinematics using Factor

Graph

Given the result of the Gaussian Mixture Regression and the result of the threedemonstration data provided by the Denavit-Hartenberg convention, the process ofperforming inverse kinematics can be divided into two parts:

• Training the Factor Graph

• Getting the joint data from the position data

4.3.1 Training Factor Graph

The training datasets are provided by the demonstration data from the forwardkinematics using Denavit-Hartenberg conversion. Each dataset, there contains the


position variables in the task space and the joint variables in the joint space. Theinputs for each factor are the combination of position variables in the task spaceand the corresponding angle variable from the joint space.

By giving each variable Gaussian input, the factor is build by calculating the jointdistribution through taking each possibility from each variable connected to thefactor node.

Three datasets are given during the training of the factor. As a result of the training,three factors are generated from each data sheet. According to the MLE(MaximumLikelihood Estimation), these factors from the datasets are summarized. Then theestimation for the factor parameter can be calculated by normalize each factor withthe sum of these factors.

4.3.2 Getting the joint data from the position data

After training the factor node in the factor graph model, the validation process canbegin to infer the joint space from the task space.

By using the validation data and the same factor graph model, the expectationvalue of the joint variable can be calculated. The corresponding joint value can becomputed using inference by giving the position values. The joint value in this thesisis the angle value of the robotic arm.

The result of the validation can be seen in the following figure:

The result shown in figure4.5 is calculated for each variable with 15 states . Thered dots are the training data, while the blue dots are the resulted angles of therobotic arm. The figure shows some edges of the result in contrast to the smoothinput training data. This is because the number of states used in the factor graphto perform the inverse kinematics are not enough to do precise inverse kinematics,so it can only give a roughly estimation of the input training data.

By increasing the number of states used in the factor graph, this result can be muchmore smooth, and present a better performance, which can be seen in the followingfigure:

In figure 4.6 the factor graph is calculated with 50 states. From this figure it is clearto see the edges are dramatically reduced due to the increase of number of statesfor each variable in the factor graph.

4.3. RESULT OF THE INVERSE KINEMATICS USING FACTOR GRAPH 65

Figure 4.5: Angle value of robotic arm by performing inverse kinematics using factorgraph.

4.3.3 Regeneration of the trajectory

After getting the joint results of angel1, angle2, angle3 and angle4 one by one fromthe inverse kinematics using factor graph, we can combine them together into agroup. Then the joint values can be formed into an angle set. To verify the cor-rectness of the resulted angle sets, the forward kinematics of Denavit-Hartenbergconvention is again introduced to form a trajectory using the angle sets.

The result is shown in figure 4.7. The result shows good match between the opti-mized result using Gaussian Mixture Regression and the result of inverse kinematicsusing factor graph. Generally speaking, the result by using the factor graph modelin this thesis proved good match between most of the calculated values and theexpected values.


Figure 4.6: Angle value of robotic arm joints resulted from performing inverse kine-matics using factor graph with 15 states.

4.4 Result comparison between my design and

Viktor’s Design

In order to compare the performance of the design for performing sum-productalgorithm using Factor Graph between my design and the design from Viktor et al,both algorithms are running on the same platform ( on the same PC platform: 4XAMD Phenom(tm) II X4 945 Processor).

During the testing, the original training data which contains two variables X andY is fed into the factor graph. Each variable has 1000 values for training the factorgraph. Then by providing the same validation data, the result of my design iscompared with the design developed by Viktor et al.

The graphical model for this testing is using a single input variable X and resultsto a single output variable Y.

The reason to use a noisy training data is that, through performing inverse kine-matics using factor graph, the noise in the training data will be filtered, in the end,the factor graph can also form a smooth curve, given enough number of states forthe variables. By increasing the number of states to 100, the following curve can beformed.

4.4. RESULT COMPARISON BETWEEN MY DESIGN AND VIKTOR’S DESIGN 67

Figure 4.7: Regeneration of the trajectory given result of the inverse kinematics

Figure 4.8: Factor Graph Model for testing

Figure 4.10 shows that with more number of states in each variables, the result willbe better. In this sense, the factor graph can also be used as a regression model,which shows interesting contrast to the regression model using Gaussian MixtureModel.

Using the factor graph graphic model from the figure 4.8, the inverse kinematicstesting model is confirmed. The original training data is shown in the figure:4.9.Through this figure it is clear to see that the training data contains some noise alongits main curve. By giving this training data to the factor graph model above andby using 15 states for variables X and Y, the following results are achieved.


Figure 4.9: Original training data

For comparison, the result of performing inference using the model from Viktor etal shows the following result:From the figure4.11 and 4.12 it is clear to see, that by performing the sum-productalgorithm, the result of my design is closer to the real training data compared to theresult from the parallel algorithm developed by Viktor et al. Furthermore for Victor’salgorithm, the time cost by performing the calculation increases significantly withthe increase of the number of states, while the time cost of my design by performingthe sum-product algorithm grows not significantly.

4.4. RESULT COMPARISON BETWEEN MY DESIGN AND VIKTOR’S DESIGN 69

Figure 4.10: Result of my design with 100 states

Figure 4.11: Result of my design with 15 states


Figure 4.12: Result of Victor’s design with 15 states

Figure 4.13: Comparison of time cost between my design and Viktor’s design byperforming inverse kinematics using factor graph

71

Chapter 5

Discussion

5.1 Bias of the result for inverse kinematics cal-

culation using Factor Graph

Figure4.11 shows that the result for the inverse kinematics calculation using FactorGraph is not always perfect. There are some bias existing between the inferenceresult and the desired result(from dataset).

The bias in the result are caused by using different way of setting the factor param-eter.

For example, given a model of two variables A and B. When using the joint proba-bility for calculating the potential value of the factor, the calculation is :

p(A,B) = P (A)× p(B) (5.1)

In fact, in order to have a more accurate result, it’s better to use:

p(A,B) = p(A|B)× p(B) = p(B|A)× p(A) (5.2)

The equation5.2 has better performance than using equation5.1.The comparisonresult can be shown in the work of Indar et al.[SMJ13]

It is also noticeable that by using 15 states for each variable, the result of performinginverse kinematics using factor graphs does not show good match with the curveof the training data. This is because using smaller number of states, the inversekinematics using factor graph can only have limited information about the state ofeach variable, so it can only do rough estimation. So the result exits many apparent

72 CHAPTER 5. DISCUSSION

edges. This can be solved by increasing the state’s number, then the result curvewill be much smoother.

5.2 Uncertainty in the measuring system

When tracking the robotic arm’s movement during the tasks demonstration, the3D ”Motiv” camera tracking system is used. At the same time, using the samplingprogram on Linux Ubuntu system, the angle values are also recorded.

However, there exist still uncertainty of the measuring system. This is becausewhen recording the position values of the robotic arm’s end-effector, the sample ofthe robotic arm’s angle should be simultaneously parallel processed.

But since the 3D camera tracking system and the Linux Ubuntu system on thelaptop are two different system, the result may have some uncertainty due to thedifferent sampling rate. In this sense, the measuring systems may have mismatchingbetween the position values and the angle values.

One possible solution to this problem is by reconfigure the sampling rate of the ”3DMotive” tracking system, then set proper time interval in the Ubuntu system on thelaptop, so that it uses the same sampling rate.

Another possible solution maybe using the ”3D Motive” itself to tracking the jointangles values during robot movement. In order to achieve this, additional markersshould be attached to the joints of the robotic arm. Then additional tests shouldbe run to get the values of the joint parameters.

5.3 Benefits of my design

Combined with imitation learning, my design of the factor graph model performsthe inverse kinematics for the robotic arm from the task space to the joint space.With this combination,the robotic arm’s control can be applied to wider range ofapplications.

For example, by demonstration, the robotic arm learns how to reach to the targetposition through imitation learning. During demonstration process, the imitationlearning provides the training process for the robotic arm. So given a new task,even if the task’s target is not on the demonstration trajectory, as long as the task’starget is inside the range of the position variables in the task space, the roboticarm can still reach the target through performing inverse kinematics using factor

5.4. COMPARISON BETWEEN GAUSSIAN MIXTURE REGRESSION AND USING FACTOR GRAPH ASREGRESSION MODEL 73

graph. In this sense, the robotic arm can reach all the points in the task space. Thisgreatly improved the performance of the robotic arm and extend the benefits of theimitation learning.

5.4 Comparison between Gaussian mixture regres-

sion and using factor graph as regression model

In this thesis, the Gaussian mixture regression model is used for optimizing theinput trajectories. After the regression computation, an optimized trajectory isformed. Meanwhile, by performing the inverse kinematics using factor graph firstly,and then regenerating the robotic arm’s moving trajectory using forward kinematicswith Denavit-Hartenberg convention, an optimized trajectory can also be generated.

This leaves interesting point of comparing these two models for regression com-putation. In fact, the factor graph can also perform regression computation. Forexample, in figure 4.10 we can notice that the resulting curve is much smootherand has less noise compared to the curve of the training data. From this we caninfer that given noisy data, through training the factor graph with different datasets, the factor nodes can be estimated by using MLE(Maximum Likelihood Estima-tion) theory. Then given enough validation data, the factor graph can also performregression calculation.

The reason behind is that in factor graph, each variable has different states. Soby given sufficient states for each variable, the factor graph gets more informationabout the task space. Then it can offer a better estimating for the factor nodeand generates better result. In fact, it performs the mapping mechanism betweendifferent variables . So if some noise exist in the data sets, these noise can be filteredby this mapping mechanism. In this sense, the factor graph model in this thesis alsooffers another approach for regression computation.

5.5 Future Development

5.5.1 FPGA Development

Hierarchy structure

The FPGA design for implementing inverse kinematics using Factor Graph in thisthesis uses simply one hierarchy structure between the marginal IP blocks and theZynq Processing system. It only have 15 marginal IP blocks, due to the AXI streaminterconnection block limitation, which can support maximum 16 connections on it.

74 CHAPTER 5. DISCUSSION

In the future the FPGA design can be extended into more IP blocks parallelism byusing more levels hierarchy structure in the FPGA design, which will offer betterperformance when the states number of each variables in the factor graph increasesignificantly.

Run time optimization

Another problem for further development is the latency of running the FPGA design.This issue is not studied in this thesis, which shows an interesting point for furtherresearch, when performing the inverse kinematics using factor graph on the FPGA.

5.5.2 Improving my design of inverse kinematics using fac-tor graph

As is discussed in section 2.2.10, the number of parallel calculation units on theFPGA strictly limits the number of states of the target variable in the factor graphmodel.

In this sense, the future work can be an extension of my design model to moreflexible parallelism model. So the number of parallel calculation units can be definedaccording to the demands of the resources on the FPGA board or the demands ofthe user.

In contrast with this situation, the parallelism algorithm developed by Viktor et alshows better treatment when dealing with the number of parallel units as demanded.

The design by Viktor et al returns as result each time an data stream in arrayform.In order to deal with this data form, we may use the AXI FIFO interface forcommunicating with the Zynq processing system, which shows also an interestingpoint for further research.

75

Chapter 6

Conclusion

This thesis proposes a possible way to combine the imitation learning with the factorgraph.

By using the imitation learning, the robotic arm can be trained through demon-stration of the movement of the human hand. Using Gaussian mixture model formodelling the different movements and using Expectation Maximization algorithm,the essential parameters of the Gaussian mixture model can be identified. Af-terwards the Gaussian mixture regression algorithm can be performed to form anoptimized moving trajectory of the robotic arm.

Furthermore through the design of inverse kinematics using factor graph in thisthesis, the desired joint parameters of the robotic arm can be confirmed. Thiscalculation is done by implementing a parallelism data processing mechanism in theFPGA on the Zynq SoC.

Through the Linux system running on the Zynq ARM processor, the result of per-forming the inverse kinematics can be confirmed and later will be used for inferringthe joint parameters to control the robotic arm and finish the grasp-and-place sce-nario.

Meanwhile, the predicted robotic arm’s moving trajectory can also be formed by us-ing forward kinematics algorithm using Denavit-Hartenbergs convention algorithm,which gives interesting comparison to the result of performing the inverse kinematicsmodel using factor graph in this thesis.

76 CHAPTER 6. CONCLUSION

LIST OF FIGURES 77

List of Figures

2.1 Working limits of the robotic arm in plane in side view[Rob] . . . . . 17

2.2 Working limits of the robotic arm in plane in overview[Rob] . . . . . 18

2.3 The general process of Robot Learning from Demonstration frame-work using GMM and GMR[MCM13] . . . . . . . . . . . . . . . . . 22

2.4 Robotic arm’s moving trajectory with different position values andtheir corresponding angle group values . . . . . . . . . . . . . . . . . 23

2.5 Construction of a factor graph from a structured factorization of con-ditional probability[SMJ13] . . . . . . . . . . . . . . . . . . . . . . . 25

2.6 Conditional independence with X,Y,Z and D . . . . . . . . . . . . . . 28

2.7 Conditional independence with X,Y,Z and θ, β . . . . . . . . . . . . . 29

2.8 Generic design model using factor graph used in this thesis . . . . . . 30

3.1 Structure of the robotic arm[Rob] . . . . . . . . . . . . . . . . . . . . 42

3.2 Trenz0720 SoC structure[Tea] . . . . . . . . . . . . . . . . . . . . . . 44

3.3 Trenz jtag programming chain[Teb] . . . . . . . . . . . . . . . . . . . 45

3.4 Vivado-hls design flow for IP design[df] . . . . . . . . . . . . . . . . . 46

3.5 Marginal computation block ”npcore” generated by Vivado HLS . . . 47

3.6 FPGA modules map overview . . . . . . . . . . . . . . . . . . . . . . 48

3.7 Connection from Zynq processing system to the DMA and AXI-MM2S-Mapper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

3.8 Connection from AXI-MM2S-Mapper to AXI-Stream-interconnect . . 49

3.9 AXI interconnections . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

3.10 Parallel marginal computation IP blocks generated by Vivado-HLS . 50

3.11 Connection for the interrupt system of this design . . . . . . . . . . 51

3.12 AXI Control Line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

4.1 Result of forward kinematics using DH parameter shows moving tra-jectory of the robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

4.2 Robotic arm’s movement in 3D space . . . . . . . . . . . . . . . . . . 61

4.3 Simulation of three demonstration trajectory by forward kinematics . 62

4.4 Result of the Gaussian Mixture Model using simulation data . . . . . 63

4.5 Angle value of robotic arm by performing inverse kinematics usingfactor graph. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

78 LIST OF FIGURES

4.6 Angle value of robotic arm joints resulted from performing inversekinematics using factor graph with 15 states. . . . . . . . . . . . . . . 66

4.7 Regeneration of the trajectory given result of the inverse kinematics . 674.8 Factor Graph Model for testing . . . . . . . . . . . . . . . . . . . . . 674.9 Original training data . . . . . . . . . . . . . . . . . . . . . . . . . . 684.10 Result of my design with 100 states . . . . . . . . . . . . . . . . . . . 694.11 Result of my design with 15 states . . . . . . . . . . . . . . . . . . . . 694.12 Result of Victor’s design with 15 states . . . . . . . . . . . . . . . . . 704.13 Comparison of time cost between my design and Viktor’s design by

performing inverse kinematics using factor graph . . . . . . . . . . . . 70

BIBLIOGRAPHY 79

Bibliography

[AND77] Dempster A.P., Laird N.M., and Rubin D.B. On learning, representing,and generalizing a task in a humanoid robot . Journal of the RoyalStatistical Society. Series B (Methodological), 39(1):1–38, 1977.

[ARAJ10] Jain Ashish, Martinez-Cantin Ruben, Bernardino Alexandre, andSantos-Victor Jose. Inference based Trajectory Optimization for Im-itation based Grasping . IEEE/RSJ International Conference on Intel-ligent Robots and Systems, Taipei, Taiwan, pages 18–22, 2010.

[BT10] C. Bouras and V. Tsogkas. W-kmeans: Clustering News Articles Us-ing WordNet. Knowledge-Based and Intelligent Information and Engi-neering Systems. Proceedings 14th International Conference, KES 2010,3:379–388, 2010.

[Cal09] S. Calinon. Robot Programming by Demonstration: A Probabilistic Ap-proach. EPFL/CRC Press, 2009. EPFL Press ISBN 978-2-940222-31-5,CRC Press ISBN 978-1-4398-0867-2.

[CGB07] S. Calinon, F. Guenter, and A. Billard. On learning, representing andgeneralizing a task in a humanoid robot. IEEE Transactions on Systems,Man and Cybernetics, Part B, 37(2):286–298, 2007.

[Daw79] A.P. Dawid. Conditional Independence in Statistical Theory. Journalof the Royal Statistical Society, Series B, 1(41):1–31, 1979.

[df] Tech design forum. . URL: http://www.techdesignforums.com/

practice/technique/vivado-xilinx-overview/.

[EPD13] P. Englert, A. Paraschos, and M.P. Deisenroth. Probabilistic model-based imitation learning . Adaptive Behavior, 21(5):388–403, 2013.

[Fre03] Brendan J. Frey. Extending Factor Graphs so as to Unify Directedand Undirected Graphical Models. In Proceedings of the NineteenthConference on Uncertainty in Artificial Intelligence, 2003.

[GB03] Biggs Geoffrey and MacDonald Bruce. A survey of robot pro-gramming systems, 2003. URL: http://pdf.aminer.org/000/355/

http://www.techdesignforums.com/practice/technique/vivado-xilinx-overview/

http://www.techdesignforums.com/practice/technique/vivado-xilinx-overview/

http://pdf.aminer.org/000/355/292/towards_programming_tools_for_robots_that_integrate_probabilistic_computation_and.pdf


80 BIBLIOGRAPHY

292/towards_programming_tools_for_robots_that_integrate_

probabilistic_computation_and.pdf.

[KFL01] F.R Kschischang, B.J. Frey, and H.-A. Loeliger. Factor Graphs andthe Sum-Product Algorithm. Information Theory, IEEE Transactions,47(2):498–519, 2001.

[LCRZ96a] G. Legnani, F. Casolo, P. Righettini, and B. Zappa. A homogeneousmatrix approach to 3D kinematics and dynamics I Theory. Mechanismand Machine Theory, 5(31):573, 1996.

[LCRZ96b] G. Legnani, F. Casolo, P. Righettini, and B. Zappa. A homogeneousmatrix approach to 3D kinematics and dynamics II. Applications tochains of rigid bodies and serial manipulators. Mechanism and MachineTheory, 5(31):589, 1996.

[MCM13] S. Michieletto, N. Chessa, and E. Menegatti. Learning how to approachindustrial robot tasks from natural demonstrations. 2013 IEEE Work-shop on Advanced Robotics and its Social Impacts (ARSO), pages 255–260, 2013.

[M.W87] Spong M.W. Modeling and control of elastic joint robots . Transactionsof the ASME. Journal of Dynamic Systems, Measurement and Control,109(4):310–319, 1987.

[NYP11] Ma Nam, Xia Yinglong, and V.K. Prasanna. Data Parallelism for Be-lief Propagation in Factor Graphs . Conference:2011 23rd InternationalSymposium on Computer Architecture and High Performance Comput-ing (SBAC-PAD 2011), pages 56–63, 2011.

[Rey09] Douglas Reynolds. Gaussian Mixture Models, 2009. URL:http://link.springer.com/referenceworkentry/10.1007/

978-0-387-73003-5_196?no-access=true.

[RNS95] K. Roy-Neogi and C. Sechen. Factor Graphs and the Sum-Product Algo-rithm. Field-Programmable Gate Arrays, 1995. FPGA ’95. Proceedingsof the Third International ACM Symposium, pages 146–152, 1995.

[Rob] Trosson Robotics. . URL: http://www.trossenrobotics.com/

widowxrobotarm.

[SMJ13] I. Sugiarto, P. Maier, and Conradt J. Reasoning with discrete factorgraph. Conference on Robotics, Biomimetics,Intelligent ComputationalSystems, pages 170–175, 2013.




http://link.springer.com/referenceworkentry/10.1007/978-0-387-73003-5_196?no-access=true

http://link.springer.com/referenceworkentry/10.1007/978-0-387-73003-5_196?no-access=true

http://www.trossenrobotics.com/widowxrobotarm

http://www.trossenrobotics.com/widowxrobotarm

BIBLIOGRAPHY 81

[Tea] Trenz-electronic. . URL: http://www.trenz-electronic.

de/de/produkte/fpga-boards/trenz-electronic/

trenz-electronic-te0720-zynq.html.

[Teb] Trenz-electronic. . URL: https://wiki.trenz-electronic.de/

display/4X5B/TE0703+JTAG+Programming+Guide.

http://www.trenz-electronic.de/de/produkte/fpga-boards/trenz-electronic/trenz-electronic-te0720-zynq.html



https://wiki.trenz-electronic.de/display/4X5B/TE0703+JTAG+Programming+Guide

https://wiki.trenz-electronic.de/display/4X5B/TE0703+JTAG+Programming+Guide

Embedded Factor Graph for Controlling a Low-Cost Robotics Arm … · 2015-07-30 · Embedded Factor...

Documents

Transcript of Embedded Factor Graph for Controlling a Low-Cost Robotics Arm … · 2015-07-30 · Embedded Factor...