Design and Implementation of IndustrialRobot Controllers

February 4, 2019

Gyoung H. Kim, Ph.D.gyounghkim@open-robotics.com

산업용로봇제어기의설계및구현

February 4, 2019

공학박사 김경환

gyounghkim@open-robotics.com

Disclaimer

The information contained in this report is for general information purposes only.

The information is provided by the author and open-robotics.com. While we endeav-

our to keep the information up to date and correct, we make no representations or

warranties of any kind, express or implied, about the completeness, accuracy, relia-

bility, suitability or availability with respect to the report or the information, prod-

ucts, services, or related graphics contained in this report for any purpose. Any re-

liance you place on such information is therefore strictly at your own risk.

In no event will we be liable for any loss or damage including without limitation,

indirect or consequential loss or damage, or any loss or damage whatsoever arising

from loss of data or profits arising out of, or in connection with, the use of this

report.

5

Preface

저자는 국내외의 산업용 로봇, 수중 로봇, 의료 로봇, 반도체 로봇의 controller 및 soft-

ware 개발에 참여한 바 있으며, 국내 산업용 로봇 개발의 초창기부터 현재에 이르기까

지 다소 긴 기간(30년)에 걸쳐 산업용 로봇에 대해 생각해 볼 기회가 많았던 engineer

라고 할 수 있다. 저자가 산업용 로봇 분야에 참여하면서 가졌던 기술적 의문점에 대한

해답이나 생각해 온 개발 방향에 대해, 시간이 날 때마다 memo 해 본 것들을 보고서의

형식으로 편집하여 보았다. 본 보고서를 통해 기존의 주요 연구 및 개발 결과와 저자의

관점에서 본 제어기의 개발 방향 및 방법을 소개함으로써, 실제 산업용 로봇 제어기의

개발 시 당면하는 많은 기술적 문제에 그 해답을 제시하고자 한다.

일반적으로 공학 분야의 연구 실적은 유명 학술지에 게재되는 논문 수에 의해 평

가되는 경우가 많다. 대부분의 학술지는, 기존의 연구를 개선하거나 실제 적용하여 그

실험 결과를 보이는 논문 보다 새로운 방향의 연구를 제안하는 논문을 선호하는 경향이

있다. 산업용 로봇에 관련한 연구가 초기 연구 과정을 거쳐 성숙 단계를 지났기 때문

에 새로운 연구 결과를 이끌어 내기가 어려울 수 있으며, 산업용 로봇을 이용한 실험을

실제 수행하기 위해서는 그 실험 환경 구축에 많은 노력, 비용, 시간이 필요하게 된다.

연구를 위해 산업용 로봇을 구입한다 하더라도, 그 로봇의 제작 회사가 연구에 필요한

software source code나 controller의 회로도, robot arm의 drawing 등과 같은 상세한

정보를 연구 개발자에게 제공하지 않는 경우가 대부분이다. 만일 특정 연구를 위해 자

신의 robot arm과 robot controller를 설계 하여 제작하고자 한다면, 그러한 작업 역시

상당히 큰 규모의 연구 및 개발 사업이 될 수 있다. 또한 새로운 분야에 각종 연구비가

집중 지원되는 경향이 있기 때문에, robotics 분야의 많은 연구자들이 산업용 로봇 분야

보다는 mobile robots, humanoids와 같은“intelligent robots” 분야에 연구 노력을 집중

하고 있는 현실이다.

산업용 로봇 개발 시 필요한 대부분의 기술은 robotics의 초기 연구 단계에서 이미

시도 되었기 때문에, 산업용 로봇 개발에 직접 적용할 수 있는 연구 결과는 최소 15

년 전으로 거슬러 올라간다. 산업용 로봇 개발자가 최근 몇 년 동안에 발표된 학술지나

발간된 서적 database에서 자신이 원하는 solution을 찾을 수 없는 경우, 자신이 당면한

기술적 문제에 대한 해결 방안을 독자 적으로 구하여 자신의 로봇 제어기에 구현하는

경우가 많다. 많은 연구를 통해 이미 solution이 존재하는 문제에 대한 해답을 구하기

위해 불필요한 노력과 시간을 소모 하거나, 독자적으로 구현한 결과에 오류가 존재할

7

PREFACE

수도 있다. robotics의 초기 연구 분야는 산업용 로봇이 대부분 이였기에, 학술 자료의

검색을 1980년대 초기까지 확대하면 수 많은 학술 논문, 연구 기관의 내부 기술 보고서,

절판 되어 더 이상 구할 수 없는 기술 서적 목록을 접하게 된다. 이와 같이 30년 동안

축적된 막대한 기술 자료에서 개발자가 실제 개발에 참고해야 할 기술 자료를 찾아 내

고 입수하는 것은 결코 용이하지 않다.

산업용 로봇에 대한 대부분의 기술 자료는 문서 형태로서 존재하기 때문에, 실제

제어기를 설계 하기 위해서는 관련 기술 자료를 확보한 후 이를 참조하여 hardware와

software를 bottom-to-top 방식으로 설계, 구현하는 경우가 많다. 새로운 algorithm을

robot controller의 software에 적용하기 위해서는 먼저 그 algorithm을 coding하고, 기

존의 software를 수정한 후, 작성된 code를 추가하여 검증 과정을 수행하게 된다. 경우

에 따라서는 controller hardware의 수정이나 upgrade가 필요할 수도 있다. software

의 기능 추가 역시 유사한 작업 과정을 거치게 된다. 이러한 일련의 작업에 소요되는

노력과 시간이 상당하기 때문에, 앞 서 설명한 bottom-to-top 방식의 설계 방법은 바

람직하지 않는다고 할 수 있다. 현재까지 산업용 로봇 또는 robot arm을 위해 다수

의 robot controller가 설계, 개발되었으며, 각각의 robot controller에는 개발 당시의 최

신 연구 결과(state-of-the-art)가 충분히 반영되어 있으리라 생각할 수 있다. 만일 학

계나 산업체에서 개발된 여러 robot controller에 대한 완벽한 기술 자료(source code,

schematic diagrams, manuals)를 입수하여 이를 충분히 비교 분석한다면, industrial

robot controller의 설계 및 개발에 있어 좋은 guide가 될 수 있으며, 이를 토대로 robot

controller의 hardware 및 software platform을 설계 한 후 robot controller의 각 부분

을 설계, 구현하는 개발 방법을 시도해 볼 수 있다.

저자는 1986년 산업용 로봇 programming system에 대한 석사 학위 논문을 준비

하는 과정에서, 현재 산업용 로봇 개발자가 부딪치는 것과 유사한 문제에 당면하였다.

robot control, trajectory generation, kinematics, dynamics, programming 중 특정 분

야에 제한된 이론이나 실험 결과를 기술한 학술 논문과 Richard Paul이 1981년에 발간

한 robotics 분야 최초의 textbook이, 당시 저자가 입수할 수 있는 모든 기술 자료였다.

저자의 “지적 호기심” 때문에, 수 개월에 걸쳐 Unimation의 PUMA 560 robot controller

에서 ROM 내용을 추출한 후 이를 disassembling 하여 robot controller의 software 구

성과 그 동작을 trace 해 본 적이 있다. 이러한 분석 결과가 후에 국내 최초의 SCARA-

type robot 개발에 활용되었다. 저자가 해외 대학교에서 박사 과정 학생으로 있는 동안,

RCCL, Kali, Multi-RCCL 등과 같은 robot programming system과 여러 robot controller

를 접할 수 있는 기회가 있었으며, 이러한 system을 분석해 봄에 따라 로봇 제어기에

대한 전반적인 이해가 가능하였고 implementation 시 발생하는 여러 문제와 그 해결

방법을 직접 시험해 볼 수 있었다. 최근 몇 년 동안 저자는, 저자가 제안한 개발 방법을

8

PREFACE

실제 산업용 로봇 제어기 개발에 일부 적용하여, 그 effectiveness를 검증해 본 바 있다.

robotics는, computer science, electrical engineering, mechanical engineering을

포함한 여러 분야가 연계된 interdisciplinary 학문 분야이다. industrial robot contrllers

을 개발하려면 기본적으로 관련 분야에 대한 충분한 지식이 요구된다. 뿐만 아니라 실

제로 robot controller의 설계, 구현을 하려면, robot controller 자체에 대한 많은 경험

과 지식이 요구된다. 일반적으로 robotics 분야의 research papers는 특정 문제에 대한

새로운 연구 결과를 발표하기 위해 작성된다. 이에 반해 대부분의 robotics textbooks

은, 여러 분야의 연구 결과를 teaching에 적합하도록 편집하여 개념 중심으로 설명하고

있다. research papers와 textbooks의 작성 목적을 고려해 보면, 둘 다 industrial robot

controllers의 설계 및 구현 방법을 상세하게 다룰 수 없다는 태생적 제약을 가지고 있

음을 알 수 있다. 저자는, 저자의 경험과 지식을 바탕으로 기존의 industrial robot con-

trollers의 설계와 구현에 사용된 모든 분야의 기술적 자료를 정리하고 분석하여 하나의

guidebook 또는 report를 작성하고자 하였다. 이 보고서는 industrial robot controllers

의 개발자를 대상으로 작성되었기 때문에, robotics textbooks에서 널리 다루는 내용은

가능한 한 생략하였다. 다만 robot controller의 개발을 위해 보다 상세한 설명이 필요

한 topic이나 textbooks 또는 research papers에 따라 서로 다른 algorithms을 사용하여

혼동을 일으키는 topic의 경우 이에 대한 설명을 포함하였다.

이 보고서는 다음과 같이 3 개의 parts와 20 개의 chapters로 구성되어 있다.

• Part I. Design of industrial robot controllers.

Part I에서, 저자는 industrial robot controller의 설계 방법을 제안하며, industrial

robot controllers를 이해하고 설계하기 위해 필요한 이론, 연구 결과, 기술 자료,

서적, software packages 등을 모두 수집하여 그 내용을 정리하고 이를 관련 분

야에 따라 14 개의 chapters로 분류한다.

– Chapter 1. Hardware and software platforms of industrial robot controllers.

Chapter 1에서 robot programming과 control 분야의 발전 과정과 robot con-

troller의 hardware와 software 구조를 간단히 설명한 후, off-the-shelf hard-

ware와 software를 활용한 robot controller의 개발 방법을 제안한다.

– Chapter 2. Programming and control of the PUMA robots.

Unimationt의 PUMA robots은 제작 회사에서 controller의 회로도와 robot

arm의 기계 도면을 제공하는 유일한 industrial robots이었으며, industrial

robots에 대한 많은 연구 결과가 PUMA robots을 이용하여 이루어졌다.

Chapter 2에서 PUMA robots의 controller 및 programming system에 대한

연구 및 개발 결과를 보이고 이를 통해 industrial robot controllers의 발전

9

PREFACE

과정을 살펴 보기로 한다.

– Chapter 3. Kinematics and differential kinematics.

forward kinematics (direct kinematics라고도 부른다)는 주어진 joint vari-

ables로부터 end-effector의 position과 orientation을 계산하는 과정이며, in-

verse kinematics 는 주어진 end-effector의 position과 orientation으로부터

joint variables을 구하는 과정이다.

Chapter 3에서 PUMA 560 robot을 대상으로 forword 및 inverse kinematic

equations을 solve 하는 과정을 상세하게 설명한다. Cartesian space에서 주

어진 velocity를 만족하면서 end-effector를 destination location으로 이동 시

키기 위해서는, destination location에 대응 되는 joint angles 을 구할 뿐만

아니라 end-effector의 velocity에 대응하는 joint velocity를 고려할 필요가

있다. differential kinematics는 jacobian matrix를 이용하여 표현할 수 있으

므로, jacobian의 정의, 계산 방법, Degree-of-Freedom과의 관계에 대해서도

설명한다.

– Chapter 4. RTOS for robot programming.

robot controller를 위해 전용의 RTOS (real-time operating system)을 개발

할 수 있으나, 기존의 vxworks, QNX, RTAI/Linux, Xenomai/Linux 등과 같

은 RTOS의 활용도 가능하다.

Chapter 4에서 soft real-time systems 또는 hard real-time system에서 real-

time applications의 개발을 위한 사용되는 POSIX.1b API에 대해 설명하고,

Linux kernel을 확장 또는 수정하여 RTOS를 구현하는 방법인 RTAI, xeno-

mai, RT-Preempt patch에 대해 각각의 동작 방식과 설치 방법을 차례로 설

명한다.

– Chapter 5. Design of a BASIC interpreter for embedded systems.

BASIC language는 langauage의 구조가 간단하며, 동작 시 많은 양의 mem-

ory를 필요로 하지 않고, interpreter 방식으로 program을 실행시킬 경우 de-

bugging이 편리하다는 장점 때문에 embedded systems의 programming 또

는 control에 널리 활용된다.

Chapter 5에서 compiler construction tool인 Flex/Bison와 이용하여 BASIC

interpreter를 설계 하는 방법과 C-language를 이용하는 BASIC interpreter

를 설계 하는 방법을 설명하고, embedded system을 위해 BASIC interpreter

를 설계 할 때 고려해야 할 사항에 대해서도 설명한다.

– Chapter 6. Design of a robot programming system.

10

PREFACE

robotics에 대한 여러 textbooks과 많은 연구 논문이 존재한다. Kinematics,

dynamics, control 분야에 대해서는 상세한 연구 결과를 접해 볼 수 있으나,

실제 robot manipulators을 산업 현장에서 사용하기 위해서는 teaching이나

programming 과정이 필요하다.

Chapter 6에서 저자의 개인적 경험을 서술하여 robot programming에 대한

기술 자료 확보나 지식 축적이 어려운 이유를 보이고, robot language의 발

전 과정과 개발 방향에 대해 설명한다. C library와 BASIC interpreter를 결

합하여 interpreter 방식의 robot langauge를 설계, 구현할 수 있음을 보이고,

robot language 개발의 단계 별 구현 방법과 주의할 점에 대해 설명한다.

– Chapter 7. Design of a trajectory generator.

trajectory generation은, robot이 desired path를 따라 움직이도록 robot con-

troller의 servo system을 위한 reference joint angles 또는 end-effectors의

positions을 time의 function으로 계산하는 task이다. robot controller에서

trajectory generation task를 실행하는 hardware 또는 software module를

trajectory generator (TG)라 부른다.

Chapter 7에서 robot programming system에서 사용되는 orientation의 rep-

resentation과 interpolation 방법에 대해 비교하고, Paul의 textbook, RCCL

v1.0, RFMS, Multi-RCCL의 trajectory generation methods과 그 implemen-

tation에 대해 구체적으로 설명한다. TG의 kinematic singularity 대응 방법

과 TG와 servo controller의 interface에서 고려해야 할 사항에 대해 설명하

고, TG task와 servo control task를 각각 TG processor와 servo processor

에 assign 할 때 발생할 수 있는 clock synchronization problem의 solution

을 제시한다. 마지막으로, 실제 구현된 trajectory generators의 source code

를 보인다.

– Chapter 8. Design of a graphic robot simulator.

graphic robot simulator는 robot program의 실행에 따른 robot의 motion

을 preview 하기 위해 사용된다. 또한 teach pendant simulator는 teaching

및 robot status의 monitoring에 사용되며, graphic robot simulator, teach

pendant simulator을 robot programming system과 결합하면 off-line robot

programming system을 구축할 수 있게 된다.

Chapter 8에서 기존의 graphic simulator와 robot controllers의 teach pen-

dants를 survey 하여 그 설계 방법과 기능을 비교하고, graphic robot sim-

ulator와 teach pendant simulator의 설계 방법과 robot programming sys-

tem과의 interface 방법에 대해 설명한다 또한 RCCL v1.0 robot program-

11

PREFACE

ming system에 graphic simulator와 teach pendant simulators를 추가하여

off-line programming system을 build 하는 과정을 보인다.

– Chapter 9. Python and Lua for industrial robot programming.

대부분의 industrial robots의 programming systems은 user programs을 in-

terpreter 방식으로 실행한다. 이러한 interprter는 ANSI BASIC language를

부분적으로 구현한 language에 robot motion 관련한 명령이 추가된 형태를

가지고 있다. 그러나 이러한 방식으로 설계된 robot programming systems

은, 새로운 명령이나 기능의 추가가 필요한 경우 interpreter의 수정과 de-

bugging에 많은 시간과 노력이 필요하다는 문제점을 가지고 있다. C 또는

C++ library를 이용하여 modular robot programming system을 설계하고

Python 또는 Lua와 같은 interpreter 방식의 general-purpose programming

lanaguge를 robot programming system의 interpreter로 사용하면 custom

interpreter를 설계가 필요하지 않으며, robot programmingsystem의 design

과 maintenance가 간단해진다.

Chapter 9에서 Python languge를 이용하여 robot programming system의

interpreter을 설계 하는 방법을 각각 설명한다. 또한 robot programming

system 전체를 python langugage로 개발하는 방법을 설명하며 그 구현 예

로 pyRCCL system을 보인다. 마지막으로, Lua language와 C library를 이

용하여 robot programming system을 설계하는 방법을 설명한다.

– Chapter 10. Dynamics of serial robots.

robot dynamics는 sources of motion (forces, moments 등)와 resulting mo-

tion (position, velocity 등) 사이의 관계에 대한 연구 분야이다. direct (또

는 forward) dynamics는 인가 된 joint torques에 따른 joint motion을 계

산하는 과정이며, inverse dynamics는 desired joint motion을 위해 요구되

는 joint torque를 계산하는 과정이라 할 수 있다. robot manipulator의 dy-

namic model은 manipulator의 mechanical design 뿐만 아니라 control al-

gorithm의 design 및 검증에도 사용된다.

Chapter 10에서 Lagrangian fomulation 및 Newton-Euler formulation을 사

용하여 manipulator의 dynamic model을 derive 하는 과정에 대해 상세하게

설명한다. 또한 dynamics-based control에 널리 사용되는 recursive Newton-

Euler algorithm의 forward 및 backward computation에 대해 그 유도 과정

및 계산 방법을 각 step 별로 자세히 설명한다. 마지막으로, ROBOOP를 사

용하여 PUMA 560 robots의 dynamics 계산 및 simulation의 예를 보였으

며, symbolic computation package와 recursive Newton-Euler equations을

12

PREFACE

이용하여 manipulator의 closed-form dynamic model을 구하는 방법을 설명

한다.

– Chapter 11. Robot motion control.

넓은 의미의 “robot control”은 robot의 “motion and force control”을 의미

하며, 좁은 의미의 robot control은 robot의 motion control 또는 position

control을 의미한다. 이 chapter에서 robot control은 후자의 의미로 사용

된다.

Robot controller를 robot programming을 담당하는 main processor 부분과

robot의 servo control을 담당하는 servo controller의 두 부분으로 구성할 수

있다. 참고로, 산업체에서 “servo control” 또는 “servo”란 용어는 servomotor

의 position control의 의미로 사용된다. servo controller는, main processor

의 TG로부터 set points 값을 받아 이를 reference position으로 설정하여

joint servomotors의 position control을 수행하게 된다.

Chapter 11에서, digital servo controller의 design을 위해, 주어진 continuous-

time domain의 plant model에 대해 digital controller를 design 하는 방법

과 plant model의 parameters를 experiments를 통해 identification 하는 방

법을 설명하였다. AC servomotors를 사용하는 servo system의 설계 방법을

설명하였으며, Unimation MK I과 UNIVAL controller의 servo system 분석

결과를 보였다 feedforward control을 이용한 dynamics-based controller의

설계 방법과 gravity compensation schmes에 대해 설명하였다. 마지막으로,

input shaping에 대해 설명하고 wafer transfer robots에 input shaping의 적

용 예를 보였다.

– Chapter 12. Force control.

force control는, robot과 environment 또는 robot이 grasping 하고 있는 ob-

ject와 environment 사이의 interaction forces의 control을 의미한다. force

control은 industrial robot control의 중요한 research topic이며 다양한 force

control methods가 연구, 개발되었다. force control을 industrial applica-

tions에 적용하려면 force control을 지원하는 robot programming system의

개발이 필수적이다. force control에 대한 많은 논문이 발표되었음에도 불구

하고, force control을 지원하는 robot programming system의 구현 방법을

설명한 기술 자료 및 source code는 공개되어 있지 않다.

Chapter 12에서, RCCL v1.0, Multi-RCCL, Adept V+ lanagauge을 대상으로

각 robot system에서 사용된 force control methods과 그 구현 방법을 설명

하며 programming examples을 보인다. 특히 RCCL v1.0의 trajectory gen-

13

PREFACE

erator를 case study의 대상으로 하여 trajectory generator에 force control

을 incorporate 하는 방법을 상세하게 설명한다.

– Chapter 13. Program execution in a robot controller.

industrial robots과 그 controllers에 대한 많은 textbooks과 연구 논문이 발

표되었으나, robot program이 실행될 때 robot controller의 hardware와 soft-

ware가 어떻게 서로 연결되어 동작하는 지를 종합적으로 설명한 기술 자료

를 찾아 볼 수 없었다.

robot system은 robot language, trajectory generator, servo controller, robot

manipulator를 포함한 여러 function blocks를 가진다. robot program의 in-

struction 중 robot motion의 가장 기본이 되는 instuction은 “move”라 할

수 있다. Chapter 13은, move instruction을 대상으로 program instruction

의 execution에 따른 function block의 execution, function blocks 사이의

information flow 및 communication, function block execution의 synchro-

nization에 대해 설명한다.

– Chapter 14. Programming and control of multiple industrial robots.

Chapter 14는 industrial multi-robot systems의 programming과 control 방

법을 설명한다. multi-robot system을 구축하려면 multi-robot controller와

multi-robot programming을 지원하는 robot langauge의 설계 및 구현이 필

요하다. 기존의 multi-robot controllers와 robot languages를 비교, 설명함

으로써 multi-robot systems의 구축 방법을 제시한다.

multi-robot programming의 basic concepts인 synchronization, coordina-

tion, cooperation을 정의하고 이에 대해 설명한다. 다양한 방식으로 구현

된 multi-robot controllers의 hardware structures를 비교하고, multi-arm

robots의 controllers는 multi-robot controllers와 동일한 방법으로 설계할 수

있음을 보인다. ABB, KUKA, Yaskawa가 개발하여 판매하고 있는 multi-robot

systems의 design concepts, hardware structures, program instructions을

비교하고 programming examples을 보인다. 마지막으로 Multi-RCCL sys-

tems의 hardware 및 software structures를 설명하고, cooperative tasks의

programming 방법을 보인다.

• Part II. Implementation of industrial robot controllers.

commercial industrial robot controller를 비교해 보면, 그 I/O 및 functions이

서로 유사함을 발견할 수 있다. 또한 robot controller의 hardware 및 software

structures를 살펴 보면 그 역시 어느 정도 보편화 되어 있다고 할 수 있다. robot

14

PREFACE

controller의 설계 과정을 통해 robot controller의 functional block diagram이

구해지고 robot controller의 functional block diagram이 완성되면, 각 function

block을 보다 구체적으로 설계하고 이를 hardware 또는 software로서 구현할 지

를 결정한다. 실제로 robot controllers를 제작하려면 schematic diagrams의 작

성, software coding 등과 같은 작업이 필요하며, 이러한 작업은 robot controller

의 구현 단계에서 수행된다.

Part II는, robot controller의 reference design이 주어졌다고 가정하고, 이를 구현

하는 방법에 대해 설명한다.

– Chapter 15. Overview of robot controller implementation.

robot controller가 main controller와 servo controller의 두 부분으로 구성되

어 있다고 가정하고, main controller와 servo controller의 hardware 및 soft-

ware 구현 방법에 대해 차례로 설명한다. off-the-shelves hardware와 soft-

ware를 이용한 구현 방법을 제시하였기 때문에, 이러한 접근 방법을 따르면

개발에 소요되는 시간과 노력을 줄일 수 있을 뿐만 아니라, robot controller

개발 완료 후에 새로운 robots이나 응용에 따라 요구되는 robot controller의

사양 변경이나 기능 추가에 따른 robot controller의 hardware 및 software

의 modification이 용이하다는 장점이 있다.

– Chapter 16. Implementation of a main controller.

Linux/PC를 embedded systems의 controller로서 사용하는 경우, Linux의 설

치 및 설정 방법과 hard disk 대신 CompactFlash card나 SSD를 사용하는

방법에 대해 설명한다. 특히 Linux/PC를 robot controller의 main processor

로 사용한다고 가정하고, 사용 가능한 각종 hardware interface와 그 device

driver의 개발 및 구현 방법에 대해 설명한다. PCI bus는 PC의 hardware

interface 중 industrial 또는 real-time applications에 가장 보편적으로 사

용된다. 따라서 PCI serial cards, fieldbus cards, parallel cards의 Linux 및

RTDM device drivers의 개발 과정을 상세히 설명한다.

– Chapter 17. Implementation of a servo controller.

robot controller를 위한 servo controller를 multi-axis servo controller로 간

주하고, robot controller에 적합한 multi-axis servo controller의 구조와 그

구현 방법에 대해 설명한다. 먼저 DSP-based reference design을 이용한 single-

axis servo controllers의 구현 방법을 소개하며, commercial single-axis servo

drive의 interface 기능과 구조에 대해 설명한다. hardware structure에 따라

multi-axis servo controller의 types을 정의하며, 각 type의 장, 단점을 비교

15

PREFACE

한다. 또한 robot servo controller를 위한 servo processor board와 power

stage의 구현 방법을 각각 설명한다. 마지막으로 servo controller 구현 시

흔히 발생하는 EMI 문제와 그 해결 방법을 제시한다.

– Chapter 18. Implementation of EtherCAT-based robot controllers ethercat-

based motion controller는 기본적으로 ethercat bus의 master로 동작하는

main controller와 여러 개의 ethercat slave servo drives로 구성되며, 필요

에 따라 ethercat IO terminals이 ethercat slave devices로 추가된다. robot

arm을 multi-axis mechanical system으로 간주하면, ethercat-based motion

controller를 robot controller로 사용할 수 있다.

Chapter 18은 ethercat-based robot controller에 RCCL을 결합하여 indus-

trial robot controller를 구현하는 방법을 설명한다. 먼저 ethercat-based robot

controller의 hardware 및 software structures에 대해 설명한다. ethercat

servo drives의 CiA 402 device profile에 대해 설명하고, ethercat digital IO

terminals과 ethercat servo drives의 programming 방법을 보인다. 마지막

으로, RCCL을 ethercat-based robot controller에 porting 하는 방법을 단계

별로 상세히 설명한다.

• Part III. CNC and motion control.

– Chapter 19. Computer Numerical Control (CNC).

CNC systems은 많은 면에서 robot systems과 유사하나, 그 차이점 역시 존

재한다. motion control 분야의 controller는 CNC controllers의 기술을 활용

하여 개발된 경우가 많으며, 실제로 motion controller의 용어 중 상당 부분

은 CNC systems의 용어를 그대로 사용하고 있다. 또한 motion controllers

를 이용하여 robot controllers를 구현할 수 있기 때문에, motion controllers

의 기반인 CNC controllers에 대한 정확한 이해가 필요하다.

Chapter 19에서, CNC controller를 간단히 소개하고 그 기능과 구현 방법을

robot controller와 비교함으로써, motion control 관련 제품을 사용하여 구

현된 robot controller의 장점과 단점을 설명하기로 한다.

– Chapter 20. Motion control.

motion control은 automation의 한 분야로서 hydraulic pump, linear actua-

tor, 또는 electric motor 등을 사용하여 machines의 position 또는 velocity를

control 한다. motion control은 robots와 CNC machine tools의 control의

중요 부분이며, packing, printing, semiconductor production, assembly in-

dustries와 같이 여러 산업 전반에 걸쳐 자동화 설비에 널리 사용되고 있다.

16

PREFACE

PC-based motion control systems은 CNC systems의 개발 결과를 바탕으로

하고 있으며 다양한 분야에서 활용되고 있다. Chapter 20에서, robot con-

trollers의 구현에 참조하기 위해 PC-based motion control systems의 구조와

주요 기능 modules에 대해 소개한다. PC-based motion control systems의

핵심 module인 PCI motion control board의 device driver, library, applica-

tion, GUI 개발 과정을 실제 예를 들어 상세하게 설명하며, PC-based mo-

tion control systems을 어떠한 mechanical systems에 적용할 수 있는 지에

대해서도 설명한다.

정리하면, 이 report는 industrial robot controllers의 설계 및 구현에 필요한 기

존의 이론과 실제를 자세히 설명하고 industrial robot controller의 reference design과

그 implementation을 제시한다. industrial robot controller 개발자가 reference robot

controller의 minor customization을 통해 자신의 robot controller를 용이하게 설계, 구

현할 수 있도록, reference robot controller의 hardware schematics, software source

code를 모두 제공하며 사용된 각종 algorithms 역시 자세히 설명한다.

17

Table of Contents

Page

Disclaimer 5

Preface 7

Table of Contents 19

List of Figures 20

List of Tables 21

List of Programs 22

Part I. Design of Industrial Robot Controllers 23

1 Hardware and software platforms of industrial robot controllers 25

1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

1.2 Robot Programming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

1.3 Robot Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

1.4 Hardware platforms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

1.5 Software platforms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

1.6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

1.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

2 Programming and control of the PUMA robots 43

2.1 PUMA robot systems, VAL, and RCCL . . . . . . . . . . . . . . . . . . . . 43

2.1.1 PUMA robot system . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

2.1.1.1 MK I PUMA system . . . . . . . . . . . . . . . . . . . . . 44

19

TABLE OF CONTENTS

2.1.1.2 Mark III PUMA system . . . . . . . . . . . . . . . . . . . 52

2.1.1.3 Unival PUMA system . . . . . . . . . . . . . . . . . . . . . 57

2.1.2 VAL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

2.1.2.1 Coordinate systems in VAL . . . . . . . . . . . . . . . . . 64

2.1.2.2 Summary of the VAL language . . . . . . . . . . . . . . 77

2.1.2.3 Internals of the VAL language . . . . . . . . . . . . . . . 85

2.1.3 PUMA control without VAL . . . . . . . . . . . . . . . . . . . . . . 90

2.1.4 RCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93

2.1.4.1 Hardware and software structures . . . . . . . . . . . . 94

2.1.4.2 Execution of real-time tasks . . . . . . . . . . . . . . . . 97

2.1.4.3 Description of robot tasks . . . . . . . . . . . . . . . . . 100

2.1.5 Multi-RCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

2.1.5.1 Hardware structure of Multi-RCCL . . . . . . . . . . . . 105

2.1.5.2 Software structure of Multi-RCCL . . . . . . . . . . . . 106

2.1.5.3 RCI (Real-time Control Interface) . . . . . . . . . . . . 108

2.1.5.4 Description of multi-robot tasks . . . . . . . . . . . . . . 117

2.1.5.5 Graphic simulator . . . . . . . . . . . . . . . . . . . . . . . 119

2.1.6 RCCL as a generic software architecture of robot controllers . 119

2.2 RCCL variants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122

2.2.1 Level II . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122

2.2.2 Kali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124

2.2.3 ALVIN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129

2.2.4 RWRCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

2.2.4.1 Hardware structure . . . . . . . . . . . . . . . . . . . . . . 132

2.2.4.2 Software structure . . . . . . . . . . . . . . . . . . . . . . 133

2.2.4.3 Implementation of moper and joint servo

as Linux kernel modules . . . . . . . . . . . . . . . . . . 136

2.2.5 QRobot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137

2.2.6 ARCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

2.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140

3 Kinematics and differential kinematics 145

3.1 Rigid motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

20

TABLE OF CONTENTS

3.1.1 Homogeneous representation . . . . . . . . . . . . . . . . . . . . . 147

3.1.1.1 Rigid body position . . . . . . . . . . . . . . . . . . . . . 148

3.1.1.2 Rigid body velocity . . . . . . . . . . . . . . . . . . . . . . 151

3.1.2 Exponential coordinates representation . . . . . . . . . . . . . . . 157

3.1.2.1 Rigid body position . . . . . . . . . . . . . . . . . . . . . 157

3.1.2.2 Rigid body velocity . . . . . . . . . . . . . . . . . . . . . . 163

3.1.3 Representation of screw motions with exponentials

of twists . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168

3.2 Kinematics of robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173

3.2.1 Denavit-Hartenberg parameters and link coordinate

frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174

3.2.1.1 Standard DH convention . . . . . . . . . . . . . . . . . . 174

3.2.1.2 Modified DH convention . . . . . . . . . . . . . . . . . . 182

3.2.2 Forward Kinematics of the PUMA 560 robots . . . . . . . . . . . 184

3.2.3 Inverse kinematics of the PUMA 560 robots . . . . . . . . . . . 191

3.2.3.1 Inverse kinematics solutions published in

journal papers . . . . . . . . . . . . . . . . . . . . . . . . . 191

3.2.3.2 Implementation of the inverse kinematics

in C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 205

3.3 Differential kinematics of robots . . . . . . . . . . . . . . . . . . . . . . . 211

3.3.1 Differential translation and rotation of frames . . . . . . . . . . 212

3.3.2 Definition and computation of the manipulator jacobian . . . . 215

3.3.2.1 Vector cross method . . . . . . . . . . . . . . . . . . . . . 215

3.3.2.2 Differential translation and rotation method . . . . . . 216

3.3.3 Jacobian of a 2-DOF planar robot . . . . . . . . . . . . . . . . . . 221

3.3.3.1 Computation of the Jacobian and its inverse . . . . . 221

3.3.3.2 Geometric interpretation of the Jacobian . . . . . . . . 229

3.3.3.3 Kinematic singularity . . . . . . . . . . . . . . . . . . . . 232

3.3.3.4 Cartesian motion passing through singu-

larity regions . . . . . . . . . . . . . . . . . . . . . . . . . . 236

3.3.3.4.1 Calculation of joint angles by solv-

ing inverse kinematics . . . . . . . . . . . . . . . 236

3.3.3.4.2 Inverse kinematics solver with a

rate limiter . . . . . . . . . . . . . . . . . . . . . . 246

21

TABLE OF CONTENTS

3.3.3.4.3 On-line trajectory time-scaling . . . . . . . . . . 253

3.3.3.4.4 Resolved motion position control . . . . . . . . 263

3.3.4 Jacobian of the PUMA 560 robot . . . . . . . . . . . . . . . . . . 299

3.3.4.1 Computation of the jacobian . . . . . . . . . . . . . . . 299

3.3.4.2 Singular configurations of the PUMA 560 robot . . . 301

3.3.4.3 Jacobians of the PUMA 560 with respect

to different reference frames . . . . . . . . . . . . . . . 307

3.3.4.4 Singular directions of the PUMA 560 robot . . . . . . 311

3.3.5 Jacobian-based methods for inverse kinematics . . . . . . . . . . 313

3.3.5.1 Jacobian inverse method . . . . . . . . . . . . . . . . . . 313

3.3.5.2 Jacobian pseudo-inverse method . . . . . . . . . . . . . 330

3.3.5.3 Damped least-squares method . . . . . . . . . . . . . . . 339

3.4 Robot kinematics using exponential coordinates . . . . . . . . . . . . . 352

3.4.1 End-effector position . . . . . . . . . . . . . . . . . . . . . . . . . . . 352

3.4.2 End-effector velocity and manipulator Jacobian . . . . . . . . . 363

3.4.3 Conversion between D-H parameters and POE parameters . . 377

3.4.3.1 Conversion from D-H parameters to POE parameters 378

3.4.3.2 Conversion from POE parameters to D-H parameters 392

3.5 Kinematics using quaternions and dual quaternions . . . . . . . . . . . 394

3.5.1 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394

3.5.1.1 Operations and properties of quaternions . . . . . . . 395

3.5.1.2 Representation of a rotation . . . . . . . . . . . . . . . . 403

3.5.1.3 Representation of angular velocity and an-

gular acceleration . . . . . . . . . . . . . . . . . . . . . . . 406

3.5.1.4 Quaternion-vector pairs . . . . . . . . . . . . . . . . . . . 408

3.5.2 Dual quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 410

3.5.2.1 Operations and properties of dual quaternions . . . . 411

3.5.2.2 Representation of a rigid body’s position . . . . . . . . 414

3.5.2.3 Representation of a rigid body’s velocity . . . . . . . . 420

3.5.3 Screw Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 428

3.5.3.1 Line Transformation . . . . . . . . . . . . . . . . . . . . . 428

3.5.3.2 Screw motion and dual quaternions . . . . . . . . . . . 433

3.5.3.3 Robot kinematics based on screw theory . . . . . . . . 437

3.5.3.3.1 Screw motion of robot joints . . . . . . . . . . 437

22

TABLE OF CONTENTS

3.5.3.3.2 Quaternion-vector pair representation . . . . . 438

3.5.3.3.3 Dual-quaternion representation . . . . . . . . . 451

3.5.4 Implementation and application examples . . . . . . . . . . . . . 455

3.5.4.1 Kinematics of PUMA 560 robots using quaternion-

vector pairs . . . . . . . . . . . . . . . . . . . . . . . . . . 455

3.5.4.1.1 Forward kinematics . . . . . . . . . . . . . . . . 455

3.5.4.1.2 Inverse kinematics . . . . . . . . . . . . . . . . . 458

3.5.4.2 Forward kinematics of PUMA 560 robot

using screw parameters and dual quaternions . . . . 465

3.5.4.3 Dual quaternions in python . . . . . . . . . . . . . . . . 472

3.5.4.4 Coordinate transformation . . . . . . . . . . . . . . . . . 481

3.5.4.5 Rigid motions and robotics toolbox . . . . . . . . . . . 485

3.5.4.6 DQ Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . 494

3.5.4.7 Luadq . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 511

3.5.4.8 Screws.m: Robot kinematics package for Mathematica 564

3.6 Case study: SCARA-type wafer transfer robots . . . . . . . . . . . . . . 589

3.7 Case study: forward and inverse kinematics of the Adept

s650 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 602

3.7.1 Forward kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 602

3.7.2 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 609

3.7.2.1 Algebraic method . . . . . . . . . . . . . . . . . . . . . . . 609

3.7.2.2 Geometrical method . . . . . . . . . . . . . . . . . . . . . 618

3.7.2.3 Robot configurations . . . . . . . . . . . . . . . . . . . . . 629

3.7.2.4 Implementation of the inverse kinematics

in C/C++ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 634

3.7.2.4.1 Geometrical method . . . . . . . . . . . . . . . . 634

3.7.2.4.2 Algebraic method . . . . . . . . . . . . . . . . . . 656

3.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 666

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 667

4 RTOS for Robot Programming 671

4.1 POSIX.1b real-time extensions . . . . . . . . . . . . . . . . . . . . . . . . . 671

4.1.1 POSIX versions and their description . . . . . . . . . . . . . . . . 671

4.1.2 Programming examples . . . . . . . . . . . . . . . . . . . . . . . . . 674

23

TABLE OF CONTENTS

4.1.2.1 Process scheduling . . . . . . . . . . . . . . . . . . . . . . 674

4.1.2.2 Thread scheduling . . . . . . . . . . . . . . . . . . . . . . 675

4.1.3 Low-latency Linux kernels . . . . . . . . . . . . . . . . . . . . . . . 681

4.2 RTAI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 685

4.2.1 Introduction to RTAI . . . . . . . . . . . . . . . . . . . . . . . . . . . 685

4.2.2 Installation of RTAI . . . . . . . . . . . . . . . . . . . . . . . . . . . 686

4.2.2.1 Setting up BIOS . . . . . . . . . . . . . . . . . . . . . . . . 687

4.2.2.2 RTAI kernel patching and kernel compiling . . . . . . 689

4.2.2.3 Compiling and installing RTAI . . . . . . . . . . . . . . . 690

4.2.2.4 Running the testsuite of RTAI . . . . . . . . . . . . . . . 691

4.2.2.5 Installation of Comedi package for paral-

lel I/O boards . . . . . . . . . . . . . . . . . . . . . . . . . 691

4.2.2.6 Setting up RTAI for supporting the Comedi

package . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 692

4.2.2.7 Testing the comedi driver of the parallel

I/O board . . . . . . . . . . . . . . . . . . . . . . . . . . . 693

4.2.2.8 Development and installation of a RTDM

serial-port device driver . . . . . . . . . . . . . . . . . . . 693

4.2.3 Instllation of RTAI on the PCs with multi-core CPUs. . . . . . . 695

4.2.3.1 Hardware configuration of a multicore CPU

PC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 695

4.2.3.2 Installation of RTAI on a multicore CPU PC . . . . . . 697

4.2.3.3 Core allocation in RTAI/PC systems . . . . . . . . . . . 700

4.2.4 Examples of RTAI instllation . . . . . . . . . . . . . . . . . . . . . . 702

4.2.5 Installation and use of rtnet . . . . . . . . . . . . . . . . . . . . . . 705

4.2.5.1 Instllation of rtnet . . . . . . . . . . . . . . . . . . . . . . 705

4.2.5.2 Testing the rtnet . . . . . . . . . . . . . . . . . . . . . . . 707

4.2.5.2.1 local loopback test . . . . . . . . . . . . . . . . . 707

4.2.5.2.2 ping test with two PCs . . . . . . . . . . . . . . 708

4.2.5.2.3 Ping test without tmda module . . . . . . . . . 709

4.2.5.3 Industrial realtime networking using rtnet . . . . . . . 711

4.2.6 RTDM serial device drivers . . . . . . . . . . . . . . . . . . . . . . 712

4.2.6.1 Structure of serial device drivers . . . . . . . . . . . . . 712

4.2.6.2 Problems of the serial device drivers . . . . . . . . . . 714

24

TABLE OF CONTENTS

4.2.6.3 Modification of the serial device driver . . . . . . . . . 715

4.2.6.4 Callback function of the serial device drivers . . . . . 716

4.2.7 Programming examples . . . . . . . . . . . . . . . . . . . . . . . . . 718

4.2.7.1 FIFO communication between kernel-space

RT and user-space linux tasks . . . . . . . . . . . . . . . 718

4.2.7.2 Share-memory communication between kernel-

space RT and user-space linux tasks . . . . . . . . . . . 723

4.3 Xenomai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 728

4.3.1 Xenomai for x86 processors . . . . . . . . . . . . . . . . . . . . . . 730

4.3.1.1 Installation of kernel and xenomai from tarballs . . . 730

4.3.1.2 Buidling kernel and xenomai packages for

ubuntu/debian systems . . . . . . . . . . . . . . . . . . . 733

4.3.2 Kernel and BIOS configuration for a Xenomai/PC . . . . . . . . 737

4.3.3 Common mistakes in configurating Linux/Xenomai kernel. . . 738

4.3.4 Xenomai for ARM processors . . . . . . . . . . . . . . . . . . . . . 742

4.3.4.1 Beagleboard-XM . . . . . . . . . . . . . . . . . . . . . . . . 743

4.3.4.2 Beaglebone Black . . . . . . . . . . . . . . . . . . . . . . . 752

4.3.5 GPL and non-GPL modules . . . . . . . . . . . . . . . . . . . . . . 755

4.3.6 Programming examples . . . . . . . . . . . . . . . . . . . . . . . . . 756

4.3.6.1 Native skin . . . . . . . . . . . . . . . . . . . . . . . . . . . 756

4.3.6.2 POSIX skin . . . . . . . . . . . . . . . . . . . . . . . . . . . 761

4.3.6.3 Native+POSIX skins . . . . . . . . . . . . . . . . . . . . . 769

4.3.6.4 RTDM skin . . . . . . . . . . . . . . . . . . . . . . . . . . . 772

4.3.6.5 Shared-memory services of the POSIX skin . . . . . . 780

4.3.6.6 Memory heap services of the native skin . . . . . . . . 791

4.3.6.7 Message pipe services of the native skin . . . . . . . . 794

4.3.6.7.1 Comminication between a kernel-

space Xenomai task and a user-

space linux task . . . . . . . . . . . . . . . . . . . 795

4.3.6.7.2 Comminication between a user-space

xenomai task and a user-space linux

task . . . . . . . . . . . . . . . . . . . . . . . . . . 804

4.4 RT-preempt kernel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 808

4.4.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 809

25

TABLE OF CONTENTS

4.4.2 Installation of RT-preempt kernels . . . . . . . . . . . . . . . . . . 811

4.4.2.1 Installation of a RT-preempt kernel on De-

bian Wheezy . . . . . . . . . . . . . . . . . . . . . . . . . . 812

4.4.2.2 Installation of the RT-preempt kernel 3.14.25-

rt22 on Ubuntu 14.04 . . . . . . . . . . . . . . . . . . . . 813

4.4.2.3 Installation of the RT-preempt kernel 3.18.7-

rt1 on the Ubuntu 14.04 . . . . . . . . . . . . . . . . . . 834

4.4.2.4 Installation of the RT-preempt kernel 4.8.11-

rt7 on Ubuntu 16.04 . . . . . . . . . . . . . . . . . . . . 835

4.4.3 Programming periodic threads . . . . . . . . . . . . . . . . . . . . 845

4.4.4 Howto, tips, and techniques for RT-preempt ker-

nels and RT applications . . . . . . . . . . . . . . . . . . . . . . . . 851

4.4.4.1 Howto: Build an RT-application . . . . . . . . . . . . . 851

4.4.4.2 Tips and techniques for RT-preempt kernels . . . . . . 865

4.5 Realtime USB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 872

4.5.1 Linux device drivers . . . . . . . . . . . . . . . . . . . . . . . . . . . 872

4.5.1.1 Linux USB subsystem . . . . . . . . . . . . . . . . . . . . 872

4.5.1.2 Existing device drivers of FTDI usb-to-serial

converters . . . . . . . . . . . . . . . . . . . . . . . . . . . 889

4.5.1.2.1 Kernal-space device driver . . . . . . . . . . . . 890

4.5.1.2.2 User-space driver, libftdi . . . . . . . . . . . . . 914

4.5.1.2.3 Commands of FTDI USB slave converters . . 933

4.5.1.3 Loopback test programs for FTDI usb-to-

serial converters . . . . . . . . . . . . . . . . . . . . . . . . 938

4.5.1.3.1 Loopback test program for ftdi_sio.ko . . . . . 938

4.5.1.3.2 Loopback test program for libftdi . . . . . . . 941

4.5.1.4 Writing a “simple” kernel-space device driver

for a FTDI usb-to-serial converter . . . . . . . . . . . . 946

4.5.1.4.1 Device driver with non-blocking IO . . . . . . 946

4.5.1.4.2 Device driver with blocking IO . . . . . . . . . 968

4.5.2 RTDM USB device drivers . . . . . . . . . . . . . . . . . . . . . . . 990

4.5.2.1 Existing RTDM device drivers for USB devices . . . . 990

4.5.2.1.1 usbRTdmSkeleton driver of the usb20rt . . . 990

4.5.2.1.2 FTDI device driver of the usb4rt . . . . . . . . 1026

26

TABLE OF CONTENTS

4.5.2.1.3 Porting the Linux USB stack to a

RTDM USB stack . . . . . . . . . . . . . . . . . . 1032

4.5.2.2 Writing a RTDM device driver for a FTDI

usb-to-serial converter . . . . . . . . . . . . . . . . . . . . 1081

4.5.3 Case study: Writing Linux and RTDM device drivers

for a USB-to-FIFO bridge . . . . . . . . . . . . . . . . . . . . . . . . 1098

4.5.3.1 Linux user-space device driver: libftdi . . . . . . . . . 1100

4.5.3.1.1 Synchronous API . . . . . . . . . . . . . . . . . . 1100

4.5.3.1.2 Asynchronous API . . . . . . . . . . . . . . . . . 1116

4.5.3.2 Linux kernel-space device driver . . . . . . . . . . . . . 1120

4.5.3.2.1 Synchronous API . . . . . . . . . . . . . . . . . . 1120

4.5.3.2.2 Asynchronous API . . . . . . . . . . . . . . . . . 1142

4.5.3.3 RTDM device driver based on usb4rt . . . . . . . . . . 1155

4.6 Realtime networking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1179

4.6.1 IEEE 1588 for clock sychronization among processors . . . . . 1179

4.6.1.1 STM3210C-EVAL board as a PTPd master . . . . . . . 1181

4.6.1.1.1 flash-burning the IEEE 1588 demo

firmware . . . . . . . . . . . . . . . . . . . . . . . 1181

4.6.1.1.2 Rebuilding the IEEE-1588 demo firmware . . 1183

4.6.1.1.3 PTPd demo . . . . . . . . . . . . . . . . . . . . . . 1185

4.6.1.2 STM32F4 Discovery board as a PTPd slave . . . . . . 1188

4.6.1.2.1 Hardware and software setup . . . . . . . . . . 1188

4.6.1.2.2 PTPd test . . . . . . . . . . . . . . . . . . . . . . . 1190

4.6.2 Ethernet Powerlink: openPOWERLINK demos . . . . . . . . . . . 1193

4.6.2.1 Windows PCs . . . . . . . . . . . . . . . . . . . . . . . . . 1194

4.6.2.2 Linux PCs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1197

4.6.2.3 Real-time Linux PCs . . . . . . . . . . . . . . . . . . . . . 1200

4.6.2.3.1 Running the openpowerlink demo

using PCAP user-space daemon . . . . . . . . . 1203

4.6.2.3.2 Running the openpowerlink demo

using a kernel-space driver . . . . . . . . . . . 1207

4.6.3 EtherCAT: digital IO demos (I) . . . . . . . . . . . . . . . . . . . . 1211

4.6.3.1 SYS/BIOS SDK and AM3359 ICE . . . . . . . . . . . . . 1211

4.6.3.2 EtherCAT master/slave setup . . . . . . . . . . . . . . . 1212

27

TABLE OF CONTENTS

4.6.3.3 EtherCAT master programming examples . . . . . . . . 1218

4.6.3.4 Distributed Clocks (DC) . . . . . . . . . . . . . . . . . . . 1235

4.6.3.5 Digital IO demos with AM3359 ICEs . . . . . . . . . . 1244

4.6.3.5.1 Free-run mode of slaves . . . . . . . . . . . . . 1244

4.6.3.5.2 DC Time controlled by master (Bus-

shift mode) . . . . . . . . . . . . . . . . . . . . . . 1261

4.6.4 EtherCAT: digital IO demos (II) . . . . . . . . . . . . . . . . . . . 1301

4.6.4.1 Raspberry Pi boards . . . . . . . . . . . . . . . . . . . . . 1301

4.6.4.2 SPI-based ethernet interface . . . . . . . . . . . . . . . . 1304

4.6.4.3 Xenomai and SOEM . . . . . . . . . . . . . . . . . . . . . 1307

4.6.4.4 Digital IO demos with XMC4800 kits . . . . . . . . . . 1310

4.6.4.4.1 Free-run mode . . . . . . . . . . . . . . . . . . . 1311

4.6.4.4.2 Master-shift mode . . . . . . . . . . . . . . . . . 1320

4.7 Design of robot programming systems using RCCL, real-

time Linux, and PCs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1334

4.7.1 Hardware and software architectures of RCCL, Mul-

tiRCCL, and RWRCCL . . . . . . . . . . . . . . . . . . . . . . . . . . 1334

4.7.1.0.1 RCCL . . . . . . . . . . . . . . . . . . . . . . . . . 1334

4.7.1.0.2 MultiRCCL . . . . . . . . . . . . . . . . . . . . . . 1336

4.7.1.0.3 RWRCCL . . . . . . . . . . . . . . . . . . . . . . . 1338

4.7.1.0.4 RCCL 5.1.4 . . . . . . . . . . . . . . . . . . . . . . 1341

4.7.2 Porting Multi-RCCL to 64-bit Linux/PC systems . . . . . . . . . 1343

4.7.3 Porting RCCL v1.0 to a realtime Linux/PC . . . . . . . . . . . . 1357

4.7.3.1 Porting RCCL v1.0 to Linux . . . . . . . . . . . . . . . . 1358

4.7.3.1.1 Porting RCCL v1.0 to BSD Linux . . . . . . . . 1359

4.7.3.1.2 Porting RCCL v1.0 to SYS V Linux . . . . . . 1360

4.7.3.2 Developing a graphic simulator for RCCL v1.0 . . . . 1360

4.7.3.3 Porting the RCCL v1.0 to RTAI . . . . . . . . . . . . . . 1363

4.7.3.4 Future projects for the RCCL v1.0 . . . . . . . . . . . . 1383

4.7.3.5 Porting RCCL v1.0 to RT-preempt Linux . . . . . . . . 1384

4.7.4 Porting RWRCCL to real-time Linux/PC . . . . . . . . . . . . . . 1387

4.7.4.1 Porting RWRCCL to RTAI . . . . . . . . . . . . . . . . . . 1387

4.7.4.1.1 Installation of the RWRCCL . . . . . . . . . . . 1388

4.7.4.1.2 Implementation of a dry-run mode . . . . . . 1388

28

TABLE OF CONTENTS

4.7.4.1.3 Porting RWRCCL from rtlinux to RTAI . . . . 1390

4.7.4.2 Porting RWRCCL to Xenomai . . . . . . . . . . . . . . . 1392

4.7.4.2.1 Porting RWRCCL from RTAI to Xenomai . . . 1392

4.7.4.2.2 Running RCCL programs in FAKE mode . . . 1419

4.7.4.2.3 Running RCCL programs in DRY-

RUN mode . . . . . . . . . . . . . . . . . . . . . . 1425

4.7.4.3 Porting RWRCCL to Preempt-rt Linux . . . . . . . . . . 1427

4.7.4.3.1 Inter-process communication with

fifo and shared-memory . . . . . . . . . . . . . . 1428

4.7.4.3.2 Fake mode . . . . . . . . . . . . . . . . . . . . . . 1435

4.7.4.3.3 Dry-run mode . . . . . . . . . . . . . . . . . . . . 1451

4.7.5 Design examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1469

4.7.5.1 A generic robot controller with a Linux

PC and commecial servo drives . . . . . . . . . . . . . . 1469

4.7.5.2 ALVIN robot system . . . . . . . . . . . . . . . . . . . . . 1517

4.7.5.3 TASER robot with two PA10-6C arms . . . . . . . . . . 1536

4.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1542

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1543

5 Design of a BASIC interpreter for embedded systems 1545

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1545

5.2 BASIC interpreter construction using Flex and Bison . . . . . . . . . . 1546

5.2.1 Flex/Bison as a compiler construction tool . . . . . . . . . . . . 1546

5.2.2 Generation of a reentrant lexer/parser. . . . . . . . . . . . . . . . 1553

5.2.2.1 Advanced calculator example . . . . . . . . . . . . . . . 1554

5.2.2.2 Reentrant version of the Advanced calcu-

lator example . . . . . . . . . . . . . . . . . . . . . . . . . 1573

5.2.3 Yacc/Bison Grammar files for Basic programming language . . 1600

5.2.3.1 PLY-BASIC . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1600

5.2.3.2 YABSIC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1609

5.3 BASIC to C tranlators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1633

5.3.1 GLBCC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1634

5.3.2 BaCon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1637

5.4 Basic interpreters and compilers written in C . . . . . . . . . . . . . . . 1640

29

TABLE OF CONTENTS

5.4.1 tinybc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1641

5.4.1.1 Program statements of the tinybc BASIC interpreter 1641

5.4.1.2 Internals of the tinybc BASIC interpreter . . . . . . . . 1642

5.4.1.2.1 Program execution flow . . . . . . . . . . . . . . 1642

5.4.1.2.2 Thread-safe execution . . . . . . . . . . . . . . . 1646

5.4.1.2.3 Data structures . . . . . . . . . . . . . . . . . . . 1647

5.4.1.2.4 Evaluation of arithematic expressions . . . . . 1649

5.4.2 Gambas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1651

5.4.2.1 Architecture of the Gambas . . . . . . . . . . . . . . . . 1651

5.4.2.2 Example code . . . . . . . . . . . . . . . . . . . . . . . . . 1652

5.4.2.2.1 A simple application: Hello . . . . . . . . . . . 1652

5.4.2.2.2 Calling C functions from Gambas programs . 1659

5.4.2.2.3 Calling Gambas functions from C code . . . . 1660

5.4.2.2.4 Libffi-related code of Gambas . . . . . . . . . . 1662

5.4.3 FreeBasic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1676

5.4.4 MY-BASIC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1678

5.5 Design of the BIFES (Basic interpreter for embedded systems) . . . . 1684

5.5.1 Curses library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1684

5.5.2 Variable names and types . . . . . . . . . . . . . . . . . . . . . . . 1685

5.5.3 Wrapper for embedding . . . . . . . . . . . . . . . . . . . . . . . . 1687

5.5.4 Global and local variables . . . . . . . . . . . . . . . . . . . . . . . 1689

5.5.5 Call by value (passing values to other programs) . . . . . . . . 1692

5.5.6 Multi-threading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1698

5.5.7 Built-in functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1703

5.5.8 Tracing and single-step execution . . . . . . . . . . . . . . . . . . 1705

5.5.9 C-language Functions . . . . . . . . . . . . . . . . . . . . . . . . . . 1711

5.5.10 User-defined functions . . . . . . . . . . . . . . . . . . . . . . . . . . 1714

5.5.11 Control flow statements . . . . . . . . . . . . . . . . . . . . . . . . 1718

5.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1728

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1730

6 Design of a robot programming system 1731

6.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1731

6.2 VAL robot programming system . . . . . . . . . . . . . . . . . . . . . . . . 1734

30

TABLE OF CONTENTS

6.3 Design of a robot language . . . . . . . . . . . . . . . . . . . . . . . . . . 1737

6.3.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1737

6.3.2 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1739

6.3.3 Multi-tasking in robot languages . . . . . . . . . . . . . . . . . . . 1742

6.3.3.1 Multi-tasking control in VAL and PacScript

languages . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1742

6.3.3.2 Thread programming in Unix/Linux . . . . . . . . . . . 1745

6.3.3.3 Multi-threading in robot languages . . . . . . . . . . . 1750

6.3.4 Design of a robot langauge/interpreter – VAL . . . . . . . . . . 1752

6.3.4.1 RCCL porting . . . . . . . . . . . . . . . . . . . . . . . . . 1753

6.3.4.2 Design of lexer and parser using Lex/Yacc . . . . . . 1756

6.3.4.3 Program-instruction handling routines . . . . . . . . . . 1770

6.3.4.4 Parse-tree generation for detecting syntax errors . . . 1774

6.3.4.4.1 Parse-tree representation . . . . . . . . . . . . . 1774

6.3.4.4.2 Parse-tree generation . . . . . . . . . . . . . . . 1777

6.3.4.5 Source files of the VAL interpreter . . . . . . . . . . . . 1781

6.3.5 Design of a robot language/interpreter – BIFES . . . . . . . . . 1783

6.3.5.1 Program statements for robot motion . . . . . . . . . . 1784

6.3.5.2 Extension and expansion of a BASIC interpreter . . . 1786

6.3.5.3 Dynamic loading of C libraries . . . . . . . . . . . . . . 1790

6.3.5.4 Custom dynamic memory allocation . . . . . . . . . . . 1800

6.3.5.5 BASIC-language statements to call RCCL functions . 1811

6.3.5.6 Source files of the BIFES interpreter . . . . . . . . . . 1813

6.3.6 Design of a robot language/interpreter – BIFERS . . . . . . . . 1816

6.3.6.1 Design of the BIFERS interpreter . . . . . . . . . . . . . 1816

6.3.6.2 Lexical analysis of program statements . . . . . . . . . 1819

6.3.6.3 Scanning, Pasing and linked-list construction . . . . . 1822

6.3.6.4 Execution of program statements . . . . . . . . . . . . . 1825

6.3.6.5 BASIC-langage statements corresponding

to RCCL functions . . . . . . . . . . . . . . . . . . . . . . 1826

6.3.6.6 Source files of the BIFERS interpreter . . . . . . . . . 1828

6.4 Teaching in robot programming . . . . . . . . . . . . . . . . . . . . . . . . 1829

6.4.1 Teaching in world and tool coordinate systems . . . . . . . . . 1829

6.4.1.1 Homogeneous transformation matrices . . . . . . . . . 1829

31

TABLE OF CONTENTS

6.4.1.2 Specification of target transforms by teaching . . . . 1832

6.4.1.3 Computation of target transforms in world

and tool coordinate systems . . . . . . . . . . . . . . . . 1834

6.4.1.4 Specification of the orientation using Eu-

ler and roll-pitch-yaw angles. . . . . . . . . . . . . . . . 1837

6.4.1.4.1 Specification of the target trans-

form (W POIN T) . . . . . . . . . . . . . . . . . . 1839

6.4.1.4.2 Specification of the base transform

(W BASE) . . . . . . . . . . . . . . . . . . . . . . . 1842

6.4.1.4.3 Specification of the tool transform

(6TOOL) . . . . . . . . . . . . . . . . . . . . . . . 1843

6.4.2 VAL programming system . . . . . . . . . . . . . . . . . . . . . . . 1845

6.4.3 RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1848

6.4.3.1 Teaching example . . . . . . . . . . . . . . . . . . . . . . . 1849

6.4.3.2 Intenals of the teach() function . . . . . . . . . . . . . . 1851

6.4.4 Multi-RCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1855

6.4.4.1 Receiving teach commands from keyboard . . . . . . . 1858

6.4.4.2 Teaching with a teach pendant . . . . . . . . . . . . . . 1863

6.4.4.3 Teaching with the Robotsim simulator . . . . . . . . . 1864

6.5 Design of a robot operating system . . . . . . . . . . . . . . . . . . . . . 1864

6.5.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1864

6.5.2 Definition and implmentation of robot operating systems . . . 1866

6.5.3 RTOS for Robot operating systems . . . . . . . . . . . . . . . . . . 1867

6.5.4 Task scheduling in robot operating systems . . . . . . . . . . . . 1869

6.5.4.1 RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . 1869

6.5.4.2 Adept V+ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1870

6.5.4.3 RCCL v1.0 + interpreter . . . . . . . . . . . . . . . . . . 1875

6.5.4.4 Kali, Multi-RCCL, and RWRCCL . . . . . . . . . . . . . . 1876

6.6 ROS as a meta-operating system . . . . . . . . . . . . . . . . . . . . . . . 1880

6.6.1 Overview of ROS and ROS-Industrial . . . . . . . . . . . . . . . . 1880

6.6.2 Installation of ROS-Industrial Hydro . . . . . . . . . . . . . . . . . 1882

6.6.3 Path-planner demo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1883

6.6.4 Jog control demo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1886

6.6.4.1 minas package . . . . . . . . . . . . . . . . . . . . . . . . . 1886

32

TABLE OF CONTENTS

6.6.4.2 fake_joint package . . . . . . . . . . . . . . . . . . . . . . 1902

6.6.4.3 jog_control package . . . . . . . . . . . . . . . . . . . . . 1904

6.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1908

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1909

7 Design of a Trajectory Generator 1913

7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1913

7.2 Interpolation and blending in trajectory generation . . . . . . . . . . . 1915

7.2.1 Position interpolation and path blending . . . . . . . . . . . . . . 1915

7.2.1.1 Path blending using a quadratic Bézier curve . . . . . 1922

7.2.1.2 Interpolation using a fifth-order polyno-

mial function . . . . . . . . . . . . . . . . . . . . . . . . . 1926

7.2.1.3 Interpolation using a seventh-order poly-

nomial function . . . . . . . . . . . . . . . . . . . . . . . . 1932

7.2.1.4 Path blending using a convex average for

continuous acceleration . . . . . . . . . . . . . . . . . . . 1938

7.2.1.5 Path blending using a convex average for

continuous jerk . . . . . . . . . . . . . . . . . . . . . . . . 1952

7.2.1.6 Blending extrapolated paths . . . . . . . . . . . . . . . . 1965

7.2.2 Rotation representation . . . . . . . . . . . . . . . . . . . . . . . . . 1975

7.2.2.1 Left-handed and right-handed coordinate systems . . 1976

7.2.2.2 Conversion between axis-angle and quater-

nion representations . . . . . . . . . . . . . . . . . . . . . 1977

7.2.2.3 Conversion between rotation matrix and quaternion 1984

7.2.2.4 Conversion between rotation matrix and

axis-angle representation . . . . . . . . . . . . . . . . . . 1989

7.2.2.5 Conversion between rotation matrix and

roll-pitch-yaw angles . . . . . . . . . . . . . . . . . . . . . 1992

7.2.3 Rotation interpolation and blending . . . . . . . . . . . . . . . . . 2001

7.2.3.1 Interpolation of rotation matrices . . . . . . . . . . . . 2003

7.2.3.2 Interpolation of quaternions . . . . . . . . . . . . . . . . 2007

7.2.3.3 Rotation blending . . . . . . . . . . . . . . . . . . . . . . . 2017

7.2.3.3.1 Rotation-vector interpolations . . . . . . . . . . 2020

33

TABLE OF CONTENTS

7.2.3.3.2 Quaternion interpolations using poly-

nomial functions . . . . . . . . . . . . . . . . . . 2026

7.2.3.3.3 Spherical quadratic interpolation . . . . . . . . 2029

7.2.3.3.4 Rotation blending with a convex

average for continuous acceleration . . . . . . 2052

7.2.3.3.5 Rotation blending with a convex

average for continuous jerk . . . . . . . . . . . 2079

7.2.3.3.6 Blending two extrapolated rotations . . . . . . 2087

7.2.4 Pose interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2106

7.2.4.1 Pose interpolation using matrices . . . . . . . . . . . . . 2106

7.2.4.2 Pose interpolation using quaternion-vector

pairs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2112

7.2.4.3 Pose interpolation using dual quaternions . . . . . . . 2118

7.3 Trajectory generation methods . . . . . . . . . . . . . . . . . . . . . . . . 2145

7.3.1 Operation modes of trajectory generators . . . . . . . . . . . . . 2145

7.3.2 Paul’s trajectory generation method . . . . . . . . . . . . . . . . . 2150

7.3.2.1 Task description . . . . . . . . . . . . . . . . . . . . . . . . 2150

7.3.2.2 Program . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2162

7.3.2.3 Conveyor tracking . . . . . . . . . . . . . . . . . . . . . . 2164

7.3.2.4 Trajectory generation from position equations . . . . 2166

7.3.2.5 Joint motion . . . . . . . . . . . . . . . . . . . . . . . . . . 2172

7.3.2.5.1 Joint motion with non-moving targets . . . . 2172

7.3.2.5.2 Joint motion with moving targets . . . . . . . 2174

7.3.2.6 Cartesian motion . . . . . . . . . . . . . . . . . . . . . . . 2181

7.3.2.6.1 Motion between positions . . . . . . . . . . . . 2181

7.3.2.6.2 Transitions between path segments . . . . . . 2187

7.3.2.6.3 Trajectory generation example . . . . . . . . . 2191

7.3.3 Trajectory generator of the RCCL v1.0 . . . . . . . . . . . . . . . 2206

7.3.3.1 State definition . . . . . . . . . . . . . . . . . . . . . . . . 2207

7.3.3.2 Trajectory generation from position equations . . . . 2208

7.3.3.2.1 linear function with polynomial blends . . . . 2208

7.3.3.2.2 Determination of the segment and

acceleration times . . . . . . . . . . . . . . . . . 2214

7.3.3.2.3 Acceleration and deceleration times . . . . . . 2218

34

TABLE OF CONTENTS

7.3.3.3 Motion requests . . . . . . . . . . . . . . . . . . . . . . . . 2222

7.3.3.4 Path segment blending for moving targets . . . . . . . 2226

7.3.3.4.1 Position equations for path segments . . . . . 2229

7.3.3.4.2 Trajectory generation for moving targets . . . 2233

7.3.3.4.3 Velocity compensation scheme . . . . . . . . . . 2237

7.3.3.5 Source code of the RCCL v1.0’s trajectory generator 2243

7.3.3.6 Trajectory generation examples . . . . . . . . . . . . . . 2261

7.3.3.7 Bug correction . . . . . . . . . . . . . . . . . . . . . . . . . 2268

7.3.4 Trajectory generator of the RFMS . . . . . . . . . . . . . . . . . . 2277

7.3.4.1 TG state definition and trajectory gener-

ation at each TG state . . . . . . . . . . . . . . . . . . . 2278

7.3.4.2 Trajectory generation from position equations . . . . 2287

7.3.4.2.1 Joint motion . . . . . . . . . . . . . . . . . . . . . 2288

7.3.4.2.2 Cartesian motion . . . . . . . . . . . . . . . . . . 2296

7.3.4.3 Motion-stopping state . . . . . . . . . . . . . . . . . . . . 2308

7.3.4.4 Source code of the RFMS’ trajectory generator . . . . 2316

7.3.5 Trajectory generator of the Multi-RCCL . . . . . . . . . . . . . . . 2324

7.3.5.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2324

7.3.5.1.1 Cartesian motion . . . . . . . . . . . . . . . . . . 2327

7.3.5.1.2 Joint motion . . . . . . . . . . . . . . . . . . . . . 2347

7.3.5.1.3 Summary of the trajectory gener-

ation algorithm . . . . . . . . . . . . . . . . . . . 2353

7.3.5.2 Transitions between path segments . . . . . . . . . . . 2358

7.3.5.2.1 Initializing the blend parameters . . . . . . . . 2359

7.3.5.2.2 Blending rotations . . . . . . . . . . . . . . . . . 2360

7.3.5.3 Drive parameters . . . . . . . . . . . . . . . . . . . . . . . 2363

7.3.5.3.1 Fixed target of the next path segment . . . . 2363

7.3.5.3.2 Moving target of the next path segment . . . 2365

7.3.5.4 Segment and transition times . . . . . . . . . . . . . . . 2368

7.3.5.4.1 Transition times . . . . . . . . . . . . . . . . . . . 2368

7.3.5.4.2 Motion segment time . . . . . . . . . . . . . . . 2371

7.3.5.5 Implementation of the TG . . . . . . . . . . . . . . . . . 2372

7.3.5.5.1 Cartesian motions . . . . . . . . . . . . . . . . . 2375

7.3.5.5.2 Joint motions . . . . . . . . . . . . . . . . . . . . 2380

35

TABLE OF CONTENTS

7.3.5.6 Source code of the Multi-RCCL’s trajec-

tory generator . . . . . . . . . . . . . . . . . . . . . . . . . 2383

7.3.6 Trajectory generation using trapezoidal velocity profiles . . . . 2406

7.3.6.1 Trapezoidal velocity profile . . . . . . . . . . . . . . . . . 2406

7.3.6.2 Smooth transitions between two trapezoidal

velocity profiles . . . . . . . . . . . . . . . . . . . . . . . . 2410

7.3.6.3 Trapezoidal velocity profile (TVP) gener-

ation for multiple joints . . . . . . . . . . . . . . . . . . . 2414

7.3.6.4 Trajectory generators embedded in CANopen

servo drives . . . . . . . . . . . . . . . . . . . . . . . . . . 2416

7.3.7 Case study: trajectory generator design using RFMS

and SLERP algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . 2422

7.4 Trajectory generation in singulary regions . . . . . . . . . . . . . . . . . 2471

7.4.1 Position-based and velocity-based trajectory generation . . . . 2471

7.4.2 Singularity-robust trajectory generation methods . . . . . . . . . 2475

7.4.3 Singularity handling in RCCL v1.0 . . . . . . . . . . . . . . . . . . 2481

7.5 Clock sychronization, interpolation, and extrapolation . . . . . . . . . 2484

7.5.1 Clock synchronization between a TG processor and

a servo processor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2487

7.5.2 Linear interpolation by the servo processor . . . . . . . . . . . . 2489

7.5.3 Quadratic interpolation by the servo processor . . . . . . . . . . 2494

7.5.4 Linear and quadratic extrapolation . . . . . . . . . . . . . . . . . 2497

7.6 Interface between a trajectory generator and a servo controller . . . 2503

7.6.1 VME bus with a shared-memory board and mail-

box interrupts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2504

7.6.2 PCI bus with a DPRAM (dual-port RAM) . . . . . . . . . . . . . 2505

7.6.3 Custom parallel interface . . . . . . . . . . . . . . . . . . . . . . . . 2505

7.6.4 Serial interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2506

7.6.5 Pulse-type interface . . . . . . . . . . . . . . . . . . . . . . . . . . . 2507

7.6.6 Network interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2508

7.7 Implementation examples of clock synchronization and

TG-to-servo interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2510

7.7.1 Unimation MK I system . . . . . . . . . . . . . . . . . . . . . . . . . 2510

7.7.2 RCCL v1.0 system . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2512

36

TABLE OF CONTENTS

7.7.3 Multi-RCCL system . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2526

7.7.4 RWRCCL system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2533

7.7.5 PC-based motion control system with an interpo-

lation board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2544

7.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2546

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2547

8 Design of a graphic robot simulator 2551

8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2551

8.2 Coin3D, Qt, SoQt for 3D graphics . . . . . . . . . . . . . . . . . . . . . . 2554

8.2.1 Installation of Qt, Coin3D, and SoQt on Linux and Windows 2554

8.2.2 Coin3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2560

8.2.3 Open Inventor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2561

8.2.3.1 Introduction to the Open Inventor . . . . . . . . . . . . 2561

8.2.3.2 Architecture of the Open Inventor . . . . . . . . . . . . 2565

8.2.4 Qt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2573

8.2.4.1 Modules and development tools of Qt . . . . . . . . . 2573

8.2.4.2 Widgets Example: wiggly . . . . . . . . . . . . . . . . . . 2575

8.2.4.3 Writing Qt applications with Qt Creator . . . . . . . . 2578

8.2.5 SoQt and Pivy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2590

8.2.5.1 An Inventor example: 02.4.Examiner . . . . . . . . . . 2591

8.2.5.2 A SoQt example: 02.4.Examiner . . . . . . . . . . . . . 2594

8.2.5.3 Inventor/VRML viewer . . . . . . . . . . . . . . . . . . . 2596

8.2.5.4 Pivy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2598

8.3 Graphic simulators based on Qt and Coin3D . . . . . . . . . . . . . . . 2600

8.3.1 Qilex . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2600

8.3.1.1 Intoroduction to the Qilex robot simulator . . . . . . 2600

8.3.1.2 Porting the Qilex package to Qt4 . . . . . . . . . . . . 2601

8.3.1.3 Using the Qilex for other industrial robots . . . . . . 2602

8.3.1.4 Receiving joint angles from an external program . . 2603

8.3.1.5 RCCL interface using a shared memory . . . . . . . . . 2604

8.3.1.6 Backporting the Qilex to Ubuntu 8.04 . . . . . . . . . 2609

8.3.2 FreeCAD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2611

8.3.2.1 Generation of the VRML models of robot arms . . . . 2611

37

TABLE OF CONTENTS

8.3.2.1.1 ABB IRB 2600 robot . . . . . . . . . . . . . . . . 2611

8.3.2.1.2 Structures of VRML files for robot

modeling . . . . . . . . . . . . . . . . . . . . . . . 2611

8.3.2.1.3 VRML model of the ABB IRB 2600

robot . . . . . . . . . . . . . . . . . . . . . . . . . . 2616

8.3.2.1.4 Coordinate calculation for VRML models . . . 2627

8.3.2.2 Robot simulation example . . . . . . . . . . . . . . . . . 2631

8.3.2.3 Receiving joint angles from an external program . . 2636

8.3.3 ensiie-irb4440 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2646

8.3.3.1 3D CAD model . . . . . . . . . . . . . . . . . . . . . . . . 2646

8.3.3.2 Minor modifications . . . . . . . . . . . . . . . . . . . . . 2647

8.3.3.2.1 menus . . . . . . . . . . . . . . . . . . . . . . . . . 2647

8.3.3.2.2 Termination of main() . . . . . . . . . . . . . . . 2649

8.3.3.3 Building and running the simulator . . . . . . . . . . . 2651

8.3.3.4 Simulator structure . . . . . . . . . . . . . . . . . . . . . . 2652

8.3.3.5 Drawing the robot accroding to joing angles . . . . . 2654

8.4 Design of graphic simulators for SCARA and PUMA 560 robots . . . 2662

8.4.1 SCARA robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2662

8.4.1.1 VRML models with local coordinate sys-

tems on links . . . . . . . . . . . . . . . . . . . . . . . . . 2662

8.4.1.2 Modification of the ensiie-irb4440 simu-

lator for SCARA robots . . . . . . . . . . . . . . . . . . . 2667

8.4.2 PUMA 560 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2670

8.4.2.1 3D CAD model in VRML format . . . . . . . . . . . . . 2670

8.4.2.2 Modification of the ensii-irb4440 simula-

tor for the PUMA 560 robot . . . . . . . . . . . . . . . . 2683

8.4.2.3 Displaying coordinate axes . . . . . . . . . . . . . . . . . 2691

8.4.3 Building graphic simulators for Windows OS . . . . . . . . . . . 2695

8.5 Design of a teach-pendant simulator . . . . . . . . . . . . . . . . . . . . 2698

8.5.1 Teach pendants developed by Unimation, Staubli,

and Adept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2698

8.5.2 GUI design for 6-axis robots . . . . . . . . . . . . . . . . . . . . . 2703

8.6 Interfaces between simulators and robot programming systems . . . 2708

8.6.1 Shared-memory interface . . . . . . . . . . . . . . . . . . . . . . . . 2708

38

TABLE OF CONTENTS

8.6.2 FIFO interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2717

8.6.3 TCP socket interface . . . . . . . . . . . . . . . . . . . . . . . . . . . 2722

8.6.3.1 Introduction to sockets . . . . . . . . . . . . . . . . . . . 2722

8.6.3.2 TCP echo server/client in blocking mode . . . . . . . 2725

8.6.3.3 TCP echo server/client in non-blocking mode . . . . . 2732

8.6.3.4 TCP socket interface for robot and TP simulators . . 2739

8.7 Design of robot and TP simulators for RCCL v1.0 . . . . . . . . . . . . 2740

8.7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2740

8.7.2 FIFO interface to robot and TP simulators . . . . . . . . . . . . 2741

8.7.3 Robot arm simulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 2743

8.7.4 Teach pendant simulator . . . . . . . . . . . . . . . . . . . . . . . . 2747

8.7.4.1 CRT terminal emulator . . . . . . . . . . . . . . . . . . . 2748

8.7.4.2 MK I teach pendant simulator . . . . . . . . . . . . . . 2750

8.8 Robot simulation software . . . . . . . . . . . . . . . . . . . . . . . . . . . 2752

8.8.1 Robotics Library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2753

8.8.1.1 Robot simulation with the rlCoachKin . . . . . . . . . . 2755

8.8.1.2 Robot simulation with the rlCoachMdl . . . . . . . . . 2760

8.8.2 Rviz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2773

8.8.2.1 Building robot models with URDF . . . . . . . . . . . . 2773

8.8.2.2 Using urdf with robot_state_publisher . . . . . . . . . . 2788

8.8.3 Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2797

8.8.4 OpenRave . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2802

8.8.5 robot-sandbox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2810

8.8.5.1 Open Dynamics Engine (ODE) . . . . . . . . . . . . . . 2810

8.8.5.1.1 Building and Installation . . . . . . . . . . . . . 2810

8.8.5.1.2 Pyhon bindings . . . . . . . . . . . . . . . . . . . 2813

8.8.5.2 Collision detection and collision response . . . . . . . 2816

8.8.5.2.1 PyODE example . . . . . . . . . . . . . . . . . . . 2816

8.8.5.2.2 Python Computer Graphics Kit example . . . 2825

8.8.5.3 Installation of the robot-sandbox . . . . . . . . . . . . . 2836

8.8.5.4 Internals of the robot-sandbox . . . . . . . . . . . . . . 2852

8.8.5.5 Embedding IPython . . . . . . . . . . . . . . . . . . . . . 2871

8.8.6 V-REP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2873

8.8.6.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2873

39

TABLE OF CONTENTS

8.8.6.2 Remote API clients . . . . . . . . . . . . . . . . . . . . . . 2879

8.8.6.2.1 Matlab clients . . . . . . . . . . . . . . . . . . . . 2879

8.8.6.2.2 Python clients . . . . . . . . . . . . . . . . . . . . 2890

8.8.6.2.3 C/C++ clients . . . . . . . . . . . . . . . . . . . . 2893

8.8.6.2.4 Lua clients . . . . . . . . . . . . . . . . . . . . . . 2903

8.8.6.3 ROS nodes . . . . . . . . . . . . . . . . . . . . . . . . . . . 2908

8.8.6.4 Simulation of humanoid robots and in-

dustrial robot arms . . . . . . . . . . . . . . . . . . . . . . 2918

8.8.6.4.1 Humanoid robots . . . . . . . . . . . . . . . . . . 2918

8.8.6.4.2 Industrial robot arms . . . . . . . . . . . . . . . 2920

8.8.6.4.3 Robot language interpreter inte-

gration tutorial . . . . . . . . . . . . . . . . . . . 2925

8.8.7 Case study: PUMA 560 robot simulation using a

URDF model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2955

8.8.7.1 Graphic simulation with the V-REP . . . . . . . . . . . 2955

8.8.7.1.1 Building a URDF model of PUMA

560 robots . . . . . . . . . . . . . . . . . . . . . . 2955

8.8.7.1.2 Writing a python client program . . . . . . . . 2962

8.8.7.1.3 Building a robot model . . . . . . . . . . . . . . 2966

8.8.7.1.4 Creating a custom user interface . . . . . . . . 2970

8.8.7.1.5 Collision detection . . . . . . . . . . . . . . . . . 2974

8.8.7.2 Graphic simulation with the rviz . . . . . . . . . . . . . 2981

8.8.7.2.1 Building a URDF model of PUMA

560 robots for the rviz . . . . . . . . . . . . . . 2981

8.8.7.2.2 Building a ROS package for PUMA

560 robots . . . . . . . . . . . . . . . . . . . . . . 2984

8.8.7.3 Building a URDF model from DH parameters . . . . . 2990

8.8.7.4 Adding physical and collision properties

to a URDF model . . . . . . . . . . . . . . . . . . . . . . . 3013

8.9 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3021

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3022

9 Python and Lua for Industrial Robot Programming 3025

9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3025

40

TABLE OF CONTENTS

9.2 Introduction to Pyhon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3027

9.2.1 Python Tutorial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3027

9.2.2 Extending and Embedding Python . . . . . . . . . . . . . . . . . . 3158

9.2.3 CFFI for Python . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3259

9.2.4 Cython for creating python bindings to C/C++ codes . . . . . 3271

9.2.5 Tracing/debugging Python code . . . . . . . . . . . . . . . . . . . 3274

9.2.5.1 sys.settrace() function . . . . . . . . . . . . . . . . . . . . 3274

9.2.5.2 pdb debugger . . . . . . . . . . . . . . . . . . . . . . . . . 3277

9.2.5.3 ipdb and IPython.debugger modules . . . . . . . . . . . 3278

9.2.5.4 pdb-clone debugger and Pyclewn . . . . . . . . . . . . . 3294

9.3 Design of a robot programming system using Python and RCCL . . . 3331

9.3.1 Compiling RCCL programs . . . . . . . . . . . . . . . . . . . . . . . 3331

9.3.2 Interfacing C libraries with Python . . . . . . . . . . . . . . . . . 3331

9.3.3 Design of the python binding of RCCL using ctypes

module . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3333

9.3.4 Executing periodic functions in python . . . . . . . . . . . . . . . 3339

9.3.5 Python bindings of RCCL and RTAI . . . . . . . . . . . . . . . . . 3340

9.4 Writing a robot programming system in Python . . . . . . . . . . . . . 3344

9.4.1 Porting the RCCL to Python . . . . . . . . . . . . . . . . . . . . . . 3346

9.4.2 C-to-Python porting . . . . . . . . . . . . . . . . . . . . . . . . . . . 3348

9.4.3 Graphic simulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3357

9.4.3.1 Shared-memory interface . . . . . . . . . . . . . . . . . . 3357

9.4.3.1.1 Shared-memory interface using the

python’s ctypes module . . . . . . . . . . . . . . 3357

9.4.3.1.2 Shared-memory interface using the

Qt’s qsharedmemory class . . . . . . . . . . . . 3360

9.4.3.2 Modification of an existing graphic simulator . . . . . 3372

9.4.3.3 Offsets between the trajectory generator

and the graphic simulator . . . . . . . . . . . . . . . . . 3374

9.4.4 pyRCCL demo package . . . . . . . . . . . . . . . . . . . . . . . . . 3374

9.4.4.1 Linux version . . . . . . . . . . . . . . . . . . . . . . . . . 3374

9.4.4.1.1 Installation . . . . . . . . . . . . . . . . . . . . . . 3374

9.4.4.1.2 Usage . . . . . . . . . . . . . . . . . . . . . . . . . 3376

9.4.4.2 Windows version . . . . . . . . . . . . . . . . . . . . . . . 3391

41

TABLE OF CONTENTS

9.4.4.2.1 Porting the Linux version of the

pyRCCL demo package to Windows . . . . . . 3391

9.4.4.2.2 Installation . . . . . . . . . . . . . . . . . . . . . . 3396

9.4.4.2.3 Usage . . . . . . . . . . . . . . . . . . . . . . . . . 3397

9.4.4.2.4 pyRCCL demos: play.py and manual.py . . . 3399

9.4.4.2.5 Creation of standalone executables

from the pyRCCL python scripts . . . . . . . . 3406

9.4.5 Implementation of the V+ programming system . . . . . . . . . 3412

9.4.5.1 V+ operating system . . . . . . . . . . . . . . . . . . . . . 3413

9.4.5.2 V+-to-python translator . . . . . . . . . . . . . . . . . . . 3416

9.4.5.3 V+ interpreter . . . . . . . . . . . . . . . . . . . . . . . . . 3434

9.4.5.4 V+ simulator . . . . . . . . . . . . . . . . . . . . . . . . . 3436

9.5 Introduction to Lua . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3447

9.5.1 Lua Tutorial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3447

9.5.2 Lua Installation on Ubuntu 14.04 . . . . . . . . . . . . . . . . . . 3598

9.5.3 Debugging Lua Code . . . . . . . . . . . . . . . . . . . . . . . . . . 3602

9.5.3.1 RemDebug . . . . . . . . . . . . . . . . . . . . . . . . . . . 3602

9.5.3.1.1 Debugging commands . . . . . . . . . . . . . . . 3602

9.5.3.1.2 Debugging example . . . . . . . . . . . . . . . . 3604

9.5.3.1.3 Internals of RemDebug . . . . . . . . . . . . . . 3606

9.5.3.2 ModDebug . . . . . . . . . . . . . . . . . . . . . . . . . . . 3616

9.5.3.3 debug.lua . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3648

9.5.3.4 ZeroBrane Studio . . . . . . . . . . . . . . . . . . . . . . . 3655

9.5.3.5 clidebugger . . . . . . . . . . . . . . . . . . . . . . . . . . . 3660

9.5.3.6 debugger.lua . . . . . . . . . . . . . . . . . . . . . . . . . . 3685

9.5.4 Calling C functions from Lua . . . . . . . . . . . . . . . . . . . . . 3695

9.5.4.1 C API . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3695

9.5.4.2 Luajit FFI library . . . . . . . . . . . . . . . . . . . . . . . 3706

9.5.4.3 Callback functions in Lua . . . . . . . . . . . . . . . . . . 3708

9.5.5 REPL (Read-Eval-Print Loop) . . . . . . . . . . . . . . . . . . . . . 3710

9.5.6 Lua programming tips . . . . . . . . . . . . . . . . . . . . . . . . . . 3722

9.6 Design of a robot programming system using Lua and RCCL . . . . . 3727

9.6.1 Compiling RCCL programs . . . . . . . . . . . . . . . . . . . . . . . 3727

9.6.2 Robot programming in Lua . . . . . . . . . . . . . . . . . . . . . . 3740

42

TABLE OF CONTENTS

9.6.2.1 Building a C library for robot programs . . . . . . . . 3740

9.6.2.2 Writing robot programs in Lua . . . . . . . . . . . . . . 3741

9.6.2.3 Writing callback functions in Lua . . . . . . . . . . . . . 3750

9.6.2.4 LuaRCCL robot programming system . . . . . . . . . . 3768

9.6.2.5 Debugging robot programs . . . . . . . . . . . . . . . . . 3771

9.6.3 Implementation of the V+ robot programming system . . . . . 3781

9.6.3.1 V+ programming examples . . . . . . . . . . . . . . . . 3781

9.6.3.1.1 Pick and place . . . . . . . . . . . . . . . . . . . . 3781

9.6.3.1.2 Defining a tool transformation . . . . . . . . . 3786

9.6.3.2 V+ language and V+ operating system . . . . . . . . . 3788

9.6.3.2.1 V+ language . . . . . . . . . . . . . . . . . . . . . 3788

9.6.3.2.2 V+ operating system . . . . . . . . . . . . . . . . 3790

9.6.3.3 Program control instructions . . . . . . . . . . . . . . . . 3792

9.6.3.4 Representation of position and orientation . . . . . . . 3794

9.6.3.5 Forward and inverse kinematics . . . . . . . . . . . . . 3803

9.6.3.5.1 Viper s650 robot . . . . . . . . . . . . . . . . . . 3803

9.6.3.5.2 Forward kinematics . . . . . . . . . . . . . . . . 3807

9.6.3.5.3 Inverse kinematics . . . . . . . . . . . . . . . . . 3826

9.6.3.6 Motion control instructions . . . . . . . . . . . . . . . . . 3851

9.6.3.6.1 Coordinate systems for motion control . . . . 3851

9.6.3.6.2 Motion control parameters . . . . . . . . . . . . 3853

9.6.3.7 Trajectory generator . . . . . . . . . . . . . . . . . . . . . 3858

9.6.3.7.1 Motion-request queue of the RCCL

system . . . . . . . . . . . . . . . . . . . . . . . . . 3858

9.6.3.7.2 Path blending in V+ and RCCL systems . . . 3860

9.6.3.8 Hand control instructions . . . . . . . . . . . . . . . . . . 3866

9.6.3.8.1 Hand control instructions of V+ . . . . . . . . 3866

9.6.3.8.2 Gripper control functions of RCCL . . . . . . . 3869

9.6.3.8.3 Creating new motion requests for

hand control . . . . . . . . . . . . . . . . . . . . . 3879

9.6.3.8.4 Side effect of the motion requests

for hand control . . . . . . . . . . . . . . . . . . 3887

9.6.3.8.5 Extension of the existing motion

requests for hand control . . . . . . . . . . . . . 3892

43

TABLE OF CONTENTS

9.6.3.9 Digital I/O instructions . . . . . . . . . . . . . . . . . . . 3903

9.6.3.9.1 Digital IO instructions of V+ . . . . . . . . . . 3903

9.6.3.9.2 Digital IO functions of RCCL . . . . . . . . . . 3905

9.6.3.9.3 Forward processing implemented

in V+ and RCCL . . . . . . . . . . . . . . . . . . 3908

9.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3911

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3912

10 Dynamics of Serial Robots 3913

10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3913

10.2 Motion of a point mass in a moving frame . . . . . . . . . . . . . . . . 3915

10.3 Link velocities and accelerations . . . . . . . . . . . . . . . . . . . . . . . 3917

10.3.1 Link velocities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3917

10.3.2 Link accelerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3921

10.4 Propagation of link velocities and accelerations . . . . . . . . . . . . . 3922

10.4.1 Propagation of link velocities . . . . . . . . . . . . . . . . . . . . . 3922

10.4.2 Propagation of link accelerations . . . . . . . . . . . . . . . . . . . 3924

10.5 Lagrange formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3925

10.5.1 Computation of the kinetic energy . . . . . . . . . . . . . . . . . . 3926

10.5.2 Computation of the potential energy . . . . . . . . . . . . . . . . 3934

10.5.3 Joint-space dynamic model . . . . . . . . . . . . . . . . . . . . . . 3934

10.6 Newton-Euler formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 3938

10.6.1 Newton and Euler equations of a link . . . . . . . . . . . . . . . 3938

10.6.2 Recursive Newton-Euler algorithm . . . . . . . . . . . . . . . . . . 3943

10.6.2.1 Coordinate-free algorithm . . . . . . . . . . . . . . . . . 3943

10.6.2.2 Link-coordinates algorithm . . . . . . . . . . . . . . . . . 3947

10.7 Computation of the PUMA 560 dynamics using a C++ package . . . 3951

10.7.1 Implementation of Recursive Newton-Euler algorithms . . . . . 3952

10.7.2 Preparation of kinematic and dynamic parameters . . . . . . . 3959

10.7.2.1 Kinematic parameters . . . . . . . . . . . . . . . . . . . . 3959

10.7.2.2 Dynamic parameters . . . . . . . . . . . . . . . . . . . . . 3963

10.7.3 Robot configuration input file . . . . . . . . . . . . . . . . . . . . . 3964

10.7.4 3D visualization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3967

10.8 Symbolic computation of inverse dynamics . . . . . . . . . . . . . . . . 3968

44

TABLE OF CONTENTS

10.8.1 PUMA 560 robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3968

10.8.2 SCARA robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3998

10.9 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4003

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4004

11 Robot Motion Control 4007

11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4008

11.2 Robot control in joint and Cartesian space . . . . . . . . . . . . . . . . 4011

11.2.1 Robot control in joint space . . . . . . . . . . . . . . . . . . . . . . 4011

11.2.2 Robot control in Cartesian space . . . . . . . . . . . . . . . . . . . 4013

11.3 Digital control and system identification . . . . . . . . . . . . . . . . . . 4019

11.3.1 Continuous-time and discrete-time transfer functions . . . . . . 4019

11.3.1.1 Continuous-time systems . . . . . . . . . . . . . . . . . . 4019

11.3.1.2 Discrete-time systems . . . . . . . . . . . . . . . . . . . . 4023

11.3.2 Approaches to digital controller design . . . . . . . . . . . . . . . 4027

11.3.3 System identification . . . . . . . . . . . . . . . . . . . . . . . . . . . 4035

11.3.3.1 Parameter identification of the ARX model . . . . . . 4036

11.3.3.2 Frequency response estimation via spec-

tral analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 4040

11.4 AC servomotor control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4048

11.4.1 motor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4049

11.4.2 PWM inverter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4054

11.4.3 Design of current controllers . . . . . . . . . . . . . . . . . . . . . 4061

11.4.3.1 PI-type current controller . . . . . . . . . . . . . . . . . . 4061

11.4.3.2 P-type current controllers . . . . . . . . . . . . . . . . . . 4065

11.4.4 Intergrator Anti-windup scheme . . . . . . . . . . . . . . . . . . . 4069

11.4.5 Design of velocity controllers . . . . . . . . . . . . . . . . . . . . . 4072

11.4.6 Design of position controllers . . . . . . . . . . . . . . . . . . . . . 4075

11.4.7 Servo stiffness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4078

11.4.8 Feedforward control . . . . . . . . . . . . . . . . . . . . . . . . . . . 4079

11.4.9 Command velocity and velocity feedback . . . . . . . . . . . . . 4084

11.4.10 Bandwidth and sampling rate of commercial servo drives . . . 4087

11.4.11 Two-degree-of-freedom PID controllers . . . . . . . . . . . . . . . 4089

11.5 Analysis of the PUMA servo system . . . . . . . . . . . . . . . . . . . . . 4100

45

TABLE OF CONTENTS

11.5.1 Mark I servo system . . . . . . . . . . . . . . . . . . . . . . . . . . . 4100

11.5.1.0.1 Controller structure . . . . . . . . . . . . . . . . 4100

11.5.1.0.2 Digital servo board firmware . . . . . . . . . . 4109

11.5.2 UNIVAL servo system . . . . . . . . . . . . . . . . . . . . . . . . . . 4130

11.5.2.1 Controller structure . . . . . . . . . . . . . . . . . . . . . 4130

11.5.2.2 Velocity feedback filter . . . . . . . . . . . . . . . . . . . 4138

11.5.2.3 Details of the current controller . . . . . . . . . . . . . 4142

11.5.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4145

11.6 Servo controller design for robots . . . . . . . . . . . . . . . . . . . . . . 4147

11.6.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4147

11.6.2 Control of a single link robot . . . . . . . . . . . . . . . . . . . . . 4151

11.6.2.1 Steady state servo errors . . . . . . . . . . . . . . . . . . 4155

11.6.2.2 Velocity errors . . . . . . . . . . . . . . . . . . . . . . . . . 4157

11.6.2.3 Acceleration errors . . . . . . . . . . . . . . . . . . . . . . 4159

11.6.3 Control of a multiple link robot . . . . . . . . . . . . . . . . . . . 4161

11.6.3.1 Dynamics-based control . . . . . . . . . . . . . . . . . . . 4161

11.6.3.2 Gravity compensation . . . . . . . . . . . . . . . . . . . . 4164

11.6.3.2.1 Gravity compensation for a two-

link planar robot . . . . . . . . . . . . . . . . . . 4166

11.6.3.2.2 Computation of gravity loading for

PUMA 560 robots . . . . . . . . . . . . . . . . . 4177

11.6.3.2.3 Implementation of gravity compen-

sation in the Unimation controllers. . . . . . . 4188

11.6.3.2.4 Gravity compensation examples in

RCCL v1.0 and v5.0 . . . . . . . . . . . . . . . . 4192

11.6.3.3 Servo control with joint coupling . . . . . . . . . . . . 4198

11.7 Vibration suppression . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4201

11.7.1 Causes of vibration . . . . . . . . . . . . . . . . . . . . . . . . . . . 4201

11.7.2 Suppression of tortional vibration . . . . . . . . . . . . . . . . . . 4202

11.7.2.1 Tortional vibration . . . . . . . . . . . . . . . . . . . . . . 4202

11.7.2.2 Vibration suppression schemes (I) . . . . . . . . . . . . 4207

11.7.2.2.1 Filtering methods . . . . . . . . . . . . . . . . . . 4211

11.7.2.2.2 Observer as sensor replacements . . . . . . . . 4214

11.7.2.2.3 Observer-based anti-resonance methods . . . . 4217

46

TABLE OF CONTENTS

11.7.2.3 Vibration suppression schemes (II) . . . . . . . . . . . . 4220

11.7.3 Input shaping for vibration reduction . . . . . . . . . . . . . . . . 4224

11.7.3.1 Causes of the vibration in industrial robots . . . . . . 4224

11.7.3.2 Input shaping . . . . . . . . . . . . . . . . . . . . . . . . . 4225

11.7.3.3 Input shaping for wafer transfer robots . . . . . . . . . 4231

11.7.3.4 Implementation issues . . . . . . . . . . . . . . . . . . . . 4245

11.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4249

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4250

12 Force control 4253

12.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4253

12.2 Static forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4254

12.2.1 Transformation of forces between coordinate frames . . . . . . 4255

12.2.2 Forces, moments, and equivalent joint torgues . . . . . . . . . . 4261

12.2.3 Estimation of load mass . . . . . . . . . . . . . . . . . . . . . . . . 4263

12.2.3.1 Estimation of load mass by joint torques . . . . . . . . 4263

12.2.3.2 Estimation of load mass by a wrist force sensor . . . 4268

12.3 Force control methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4268

12.3.1 Classification of force control algorithms . . . . . . . . . . . . . . 4268

12.3.2 Free joint method . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4271

12.3.3 Modified free joint method . . . . . . . . . . . . . . . . . . . . . . 4274

12.3.4 Impedance and admittance control . . . . . . . . . . . . . . . . . 4278

12.3.4.1 Inverse dynamics control scheme for mo-

tion control . . . . . . . . . . . . . . . . . . . . . . . . . . 4278

12.3.4.2 Mechanical impedance and admittance . . . . . . . . . 4282

12.3.4.3 Impedance control . . . . . . . . . . . . . . . . . . . . . . 4285

12.3.4.4 Admittance control . . . . . . . . . . . . . . . . . . . . . . 4290

12.3.4.5 Various structures of impedance/admittance

controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . 4296

12.3.4.5.1 Impedance control with inner ve-

locity servo . . . . . . . . . . . . . . . . . . . . . . 4296

12.3.4.5.2 Position-based admittance control . . . . . . . 4298

12.3.4.5.3 Velocity-based admittance control . . . . . . . 4303

12.3.4.5.4 Position-based impedance control . . . . . . . 4306

47

TABLE OF CONTENTS

12.3.4.6 Admittance/impedance control examples . . . . . . . . 4312

12.3.4.6.1 Human-robot collaboration for car-

rying heavy objects . . . . . . . . . . . . . . . . . 4312

12.3.4.6.2 Manipulator performance constraints

in Cartesian admittance control . . . . . . . . . 4315

12.3.4.6.3 Impedance control of a two-link manipulator 4317

12.4 Force control method of the RCCL v1.0 system . . . . . . . . . . . . . 4331

12.4.1 Functions for force control . . . . . . . . . . . . . . . . . . . . . . . 4331

12.4.2 Examples of force control . . . . . . . . . . . . . . . . . . . . . . . 4338

12.4.3 Incorpolating force control in a trajectory generator . . . . . . 4345

12.5 Force control method of the MultiRCCL system . . . . . . . . . . . . . 4397

12.5.1 Limit specification routines . . . . . . . . . . . . . . . . . . . . . . . 4398

12.5.2 Compiliance specification routines . . . . . . . . . . . . . . . . . . 4400

12.5.3 Position-based impedance control scheme implemented

in MultiRCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4401

12.5.4 Compliance control demo: comply . . . . . . . . . . . . . . . . . . 4404

12.6 Force sensing implemented in the Adept robot systems . . . . . . . . 4418

12.6.1 Basic concepts of force sensing . . . . . . . . . . . . . . . . . . . . 4418

12.6.1.1 Force measurement . . . . . . . . . . . . . . . . . . . . . . 4418

12.6.1.2 Force operating modes . . . . . . . . . . . . . . . . . . . 4420

12.6.2 V+ programming for force sensing . . . . . . . . . . . . . . . . . 4422

12.6.2.1 Functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4422

12.6.2.2 Program instructions . . . . . . . . . . . . . . . . . . . . . 4424

12.6.2.3 Programming example: peg-in-hole demo . . . . . . . 4429

12.7 Force/torque control without a force sensor . . . . . . . . . . . . . . . . 4432

12.7.0.1 External force estimation . . . . . . . . . . . . . . . . . . 4432

12.7.0.2 Cartesian admittance control . . . . . . . . . . . . . . . 4439

12.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4442

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4443

13 Program Execution in a Robot Controller 4449

13.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4449

13.2 hardware/software structure of a robot controller . . . . . . . . . . . . 4449

13.3 Interpretation of a robot program . . . . . . . . . . . . . . . . . . . . . . 4451

48

TABLE OF CONTENTS

13.4 Execution of a robot program . . . . . . . . . . . . . . . . . . . . . . . . . 4454

13.4.1 Position description . . . . . . . . . . . . . . . . . . . . . . . . . . . 4455

13.4.2 Motion description . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4462

13.4.3 Synchronization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4463

13.5 Trajectory generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4464

13.6 Robot control by the servo controller . . . . . . . . . . . . . . . . . . . . 4470

13.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4473

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4474

14 Programming and control of multiple industrial robots 4477

14.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4477

14.1.1 Synchronization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4477

14.1.2 Coordination and cooperation . . . . . . . . . . . . . . . . . . . . . 4485

14.2 Hardware structures of multi-robot and multi-arm robot controllers 4487

14.2.1 Hardware structures of multi-robot controllers . . . . . . . . . . 4487

14.2.2 Hardware structures of dual-arm robot controllers . . . . . . . 4494

14.3 Industrial multi-robot systems . . . . . . . . . . . . . . . . . . . . . . . . . 4497

14.3.1 ABB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4497

14.3.1.1 Hardware configuration . . . . . . . . . . . . . . . . . . . 4498

14.3.1.2 Movement options . . . . . . . . . . . . . . . . . . . . . . 4498

14.3.1.3 Program instructions . . . . . . . . . . . . . . . . . . . . . 4502

14.3.1.4 Programming examples . . . . . . . . . . . . . . . . . . . 4507

14.3.2 KUKA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4515

14.3.2.1 Basic concepts of cooperating robots . . . . . . . . . . 4515

14.3.2.1.1 Synchronization . . . . . . . . . . . . . . . . . . . 4515

14.3.2.1.2 Geometric coupling . . . . . . . . . . . . . . . . 4517

14.3.2.1.3 Forms of cooperation . . . . . . . . . . . . . . . 4519

14.3.2.1.4 Control system for cooperating robots . . . . 4521

14.3.2.2 Hardware architecture of a CR controller . . . . . . . 4524

14.3.2.3 Software architecture of a CR controller . . . . . . . . 4526

14.3.2.4 Program instructions and functions of RoboTeam . . 4529

14.3.2.5 Programming examples . . . . . . . . . . . . . . . . . . . 4530

14.3.3 Yaskawa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4535

14.3.3.1 Coordination control of robots and stations . . . . . . 4535

49

TABLE OF CONTENTS

14.3.3.2 Coordinated and independent control functions . . . 4542

14.3.3.3 Programming examples . . . . . . . . . . . . . . . . . . . 4548

14.4 MultiRCCL systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4553

14.4.1 Hardware structures of MultiRCCL systems . . . . . . . . . . . . 4553

14.4.1.1 Hardware configuration of MutlRCCL v5.0

for a single robot . . . . . . . . . . . . . . . . . . . . . . . 4553

14.4.1.2 Hardware configuration of MultiRCCL for

two robots . . . . . . . . . . . . . . . . . . . . . . . . . . . 4554

14.4.2 Software structure of MultiRCCL . . . . . . . . . . . . . . . . . . . 4563

14.4.3 Important RCCL and RCI functions in executing

user programs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4565

14.4.4 Multi-robot capabilities . . . . . . . . . . . . . . . . . . . . . . . . . 4573

14.4.4.1 Motion description of a cooperative task. . . . . . . . 4573

14.4.4.2 Tracking example with two robot arms: “trackII.560”.4578

14.4.5 Data structures of MultiRCCL . . . . . . . . . . . . . . . . . . . . . 4587

14.4.5.1 MANIP structure . . . . . . . . . . . . . . . . . . . . . . . 4587

14.4.5.2 RCI_RBT structure . . . . . . . . . . . . . . . . . . . . . . 4590

14.4.6 Control level routines and the trajectory generator . . . . . . . 4602

14.4.6.1 Monitor functions . . . . . . . . . . . . . . . . . . . . . . . 4603

14.4.6.2 Computation sequence of a trajectory gen-

eration task . . . . . . . . . . . . . . . . . . . . . . . . . . 4603

14.4.7 Sensor integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4606

14.4.7.1 Presetting the world model. . . . . . . . . . . . . . . . . 4607

14.4.7.2 Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4611

14.4.7.3 Guarded motions and updating the world model . . 4615

14.4.8 Graphic simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4620

14.4.8.1 MultiRCCL v5.1 . . . . . . . . . . . . . . . . . . . . . . . . 4621

14.4.8.2 MultiRCCL v5.1.3 . . . . . . . . . . . . . . . . . . . . . . . 4627

14.4.8.2.1 “box.560” example . . . . . . . . . . . . . . . . . 4627

14.4.8.2.2 Teaching example . . . . . . . . . . . . . . . . . . 4630

14.4.8.2.3 Multirobot simulation example . . . . . . . . . 4633

14.4.9 Realtime simulation (dry run) . . . . . . . . . . . . . . . . . . . . 4640

14.4.9.1 Modification of the RWRCCL for realtime simulation 4640

14.4.9.2 RCI programming example . . . . . . . . . . . . . . . . . 4642

50

TABLE OF CONTENTS

14.4.9.3 RCCL programming example . . . . . . . . . . . . . . . 4646

14.4.10 Non-realtime simulation . . . . . . . . . . . . . . . . . . . . . . . . 4650

14.4.10.1 RCI programming examples . . . . . . . . . . . . . . . . 4651

14.4.10.1.1RWRCCL . . . . . . . . . . . . . . . . . . . . . . . 4651

14.4.10.1.2RCCL v5.1.3 . . . . . . . . . . . . . . . . . . . . . 4656

14.4.10.2 RCCL programming examples . . . . . . . . . . . . . . . 4662

14.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4684

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4685

Part II. Implementation of Industrial Robot Controllers 4687

15 Overview of robot controller implementations 4689

15.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4689

15.2 Hardware implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . 4690

15.2.1 Main controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4693

15.2.2 Servo controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4694

15.3 Software implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4696

15.3.1 Main controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4699

15.3.2 Servo controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4702

15.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4703

16 Implementation of a Main Controller 4705

16.1 Linux/PC as an embedded controller . . . . . . . . . . . . . . . . . . . . 4705

16.1.1 Linux boot process . . . . . . . . . . . . . . . . . . . . . . . . . . . 4706

16.1.1.1 Stages of a typical linux boot process . . . . . . . . . . 4707

16.1.1.2 Upstart: event-based init deamon . . . . . . . . . . . . 4715

16.1.1.3 Initial ram disks and file systems . . . . . . . . . . . . 4719

16.1.2 Post-installation setting . . . . . . . . . . . . . . . . . . . . . . . . . 4720

16.1.2.1 Xubuntu 11.04 . . . . . . . . . . . . . . . . . . . . . . . . 4720

16.1.2.2 Debian 6.0.6 . . . . . . . . . . . . . . . . . . . . . . . . . . 4725

16.1.3 Linux installation on a CompactFlash card and a

SSD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4727

16.1.3.1 Installation of Linux on a CompactFlash card . . . . . 4727

16.1.3.2 Installation of Linux on a SSD . . . . . . . . . . . . . . 4735

51

TABLE OF CONTENTS

16.1.4 Read-only root filesystem . . . . . . . . . . . . . . . . . . . . . . . . 4737

16.1.5 Installation or re-installation of GRUB 2 from a

Linux liveCD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4742

16.2 Main processor board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4743

16.2.1 Printer port interface . . . . . . . . . . . . . . . . . . . . . . . . . . 4749

16.2.2 IDE hard disk interface . . . . . . . . . . . . . . . . . . . . . . . . . 4749

16.2.3 SATA hard disk interface . . . . . . . . . . . . . . . . . . . . . . . . 4751

16.2.4 ISA parallel I/O card . . . . . . . . . . . . . . . . . . . . . . . . . . 4753

16.2.5 Serial interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4753

16.3 PCI boards . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4757

16.3.1 PCI multi-port serial cards . . . . . . . . . . . . . . . . . . . . . . . 4758

16.3.1.1 Oxford 16PCI954 . . . . . . . . . . . . . . . . . . . . . . . 4758

16.3.1.2 SUN1889 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4762

16.3.2 PCI Fieldbus cards . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4763

16.3.2.1 Hardware implementation . . . . . . . . . . . . . . . . . 4763

16.3.2.2 Software implementation . . . . . . . . . . . . . . . . . . 4765

16.3.2.2.1 Linux driver . . . . . . . . . . . . . . . . . . . . . 4765

16.3.2.2.2 RTDM driver . . . . . . . . . . . . . . . . . . . . . 4771

16.3.2.2.3 UIO driver . . . . . . . . . . . . . . . . . . . . . . 4782

16.3.3 PCI parallel I/O cards . . . . . . . . . . . . . . . . . . . . . . . . . . 4791

16.3.3.1 Hardware implemetation . . . . . . . . . . . . . . . . . . 4791

16.3.3.2 Software implementation I: Adlink PCI-7432 . . . . . 4795

16.3.3.2.1 Linux device driver . . . . . . . . . . . . . . . . . 4797

16.3.3.2.2 Direct accessing the I/O ports . . . . . . . . . 4818

16.3.3.2.3 RTDM device driver . . . . . . . . . . . . . . . . 4821

16.3.3.2.4 Linux device driver with an inter-

rupt handler . . . . . . . . . . . . . . . . . . . . . 4840

16.3.3.2.5 RTDM device driver with an in-

terrupt handler . . . . . . . . . . . . . . . . . . . 4861

16.3.3.3 Software implementation II: JS Automa-

tion DIO-3248 . . . . . . . . . . . . . . . . . . . . . . . . . 4874

16.3.3.3.1 Hardware configuration . . . . . . . . . . . . . . 4874

16.3.3.3.2 Software configuration . . . . . . . . . . . . . . 4876

16.3.3.3.3 Asynchronous notification mechanism . . . . . 4876

52

TABLE OF CONTENTS

16.3.3.3.4 User-space library . . . . . . . . . . . . . . . . . . 4879

16.3.3.3.5 Device driver . . . . . . . . . . . . . . . . . . . . 4892

16.3.3.3.6 User application . . . . . . . . . . . . . . . . . . . 4905

16.3.3.3.7 Qt GUI for user applications . . . . . . . . . . . 4909

16.3.4 FPGA-based PCI bus interface . . . . . . . . . . . . . . . . . . . . . 4914

16.3.4.1 PCI interface core . . . . . . . . . . . . . . . . . . . . . . 4915

16.3.4.2 Wishbone bus . . . . . . . . . . . . . . . . . . . . . . . . . 4916

16.3.4.2.1 wishbone signals . . . . . . . . . . . . . . . . . . 4917

16.3.4.2.2 Classic standard single READ/WRITE

cycles . . . . . . . . . . . . . . . . . . . . . . . . . 4924

16.3.4.2.3 Wishbone slave I/O port examples . . . . . . . 4927

16.3.4.2.4 MASTER/SLAVE interconnection examples . . 4939

16.3.4.3 PCI board design examples . . . . . . . . . . . . . . . . 4946

16.3.4.3.1 PCI serial board . . . . . . . . . . . . . . . . . . . 4946

16.3.4.3.2 PCI Digital oscilloscope . . . . . . . . . . . . . . 4964

16.3.4.3.3 PCI parallel IO board . . . . . . . . . . . . . . . 4986

16.4 PCI Express boards . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5030

16.4.1 Hardware design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5032

16.4.2 Software design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5033

16.5 Serial comminucation via USB . . . . . . . . . . . . . . . . . . . . . . . . 5080

16.5.1 USB-to-UART bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . 5080

16.5.2 USB-to-FIFO bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . 5092

16.5.2.1 Asynchronous FIFO mode . . . . . . . . . . . . . . . . . 5095

16.5.2.2 Synchronous FIFO mode . . . . . . . . . . . . . . . . . . 5114

16.5.3 Secure authentication . . . . . . . . . . . . . . . . . . . . . . . . . . 5137

16.5.3.1 1-Wire interface demo . . . . . . . . . . . . . . . . . . . . 5137

16.5.3.2 FPGA IFF copy protection with 1-wire

secure EEPROMs . . . . . . . . . . . . . . . . . . . . . . . 5154

16.6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5164

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5164

17 Implementation of a Servo Controller 5167

17.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5167

17.2 Single-axis servo drives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5172

53

TABLE OF CONTENTS

17.2.1 Texas Instruments’ reference design . . . . . . . . . . . . . . . . . 5172

17.2.1.1 Hardware setup . . . . . . . . . . . . . . . . . . . . . . . . 5173

17.2.1.2 Software structure . . . . . . . . . . . . . . . . . . . . . . 5177

17.2.2 Freescale’s reference design . . . . . . . . . . . . . . . . . . . . . . 5182

17.2.2.1 Hardware setup . . . . . . . . . . . . . . . . . . . . . . . . 5182

17.2.2.2 Software structure . . . . . . . . . . . . . . . . . . . . . . 5186

17.2.3 Commercial single-axis servo drives . . . . . . . . . . . . . . . . . 5188

17.3 Multi-axis servo controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . 5191

17.4 Servo processor board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5199

17.5 Power stage: rectifier, DC link, inverter . . . . . . . . . . . . . . . . . . . 5201

17.6 Electromagnetic Interference Issues . . . . . . . . . . . . . . . . . . . . . 5204

17.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5208

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5209

18 Implementation of ethercat-based robot controllers 5211

18.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5211

18.2 Hardware and software structures of an ethercat-based

robot controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5213

18.2.1 Hardware structure . . . . . . . . . . . . . . . . . . . . . . . . . . . 5213

18.2.2 Software structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5218

18.3 CiA402 drive profile of Panasonic A5B servo drives . . . . . . . . . . . 5223

18.3.1 Finite State Automation, Control word, and Status word . . . 5227

18.3.1.1 Finite State Automation . . . . . . . . . . . . . . . . . . . 5227

18.3.1.2 Controlword (6040h) . . . . . . . . . . . . . . . . . . . . 5229

18.3.1.3 Statusword (6041h) . . . . . . . . . . . . . . . . . . . . . 5229

18.3.2 Operation mode setting . . . . . . . . . . . . . . . . . . . . . . . . . 5234

18.3.3 Synchronous communication modes and related objects . . . . 5236

18.3.3.1 Synchronous communication modes . . . . . . . . . . . 5236

18.3.3.2 Sync Manager Communication Type (1C00h) . . . . . 5241

18.3.3.3 Sync Manager 2/3 Synchronization (1C32h,

1C33h) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5242

18.3.3.3.1 DC mode . . . . . . . . . . . . . . . . . . . . . . . 5247

18.3.3.3.2 SM2 mode . . . . . . . . . . . . . . . . . . . . . . 5249

18.3.3.3.3 Free RUN mode . . . . . . . . . . . . . . . . . . . 5251

54

TABLE OF CONTENTS

18.3.3.3.4 Input shift time . . . . . . . . . . . . . . . . . . . 5253

18.3.4 Motion functions in common among position, ve-

locity, and torque modes . . . . . . . . . . . . . . . . . . . . . . . . 5254

18.3.4.1 Option code (deceleration stop sequence) . . . . . . . 5254

18.3.4.1.1 Abort connection opition code (6007h) . . . . 5256

18.3.4.1.2 Quick stop option code (605Ah) . . . . . . . . 5261

18.3.4.1.3 Shutdown option code (605Bh) . . . . . . . . 5263

18.3.4.1.4 Disable operation option code (605Ch) . . . . 5265

18.3.4.1.5 Halt option code (605Dh) . . . . . . . . . . . . 5267

18.3.4.1.6 Fault reaction option code (605Eh) . . . . . . 5269

18.3.4.1.7 Sequence at drive inhibition in-

put (POT, NOT) . . . . . . . . . . . . . . . . . . . 5271

18.3.4.2 Position information . . . . . . . . . . . . . . . . . . . . . 5271

18.3.4.2.1 Initialization of position information . . . . . 5271

18.3.4.2.2 Polarity (607Eh) . . . . . . . . . . . . . . . . . . 5272

18.3.4.2.3 Electronic gear . . . . . . . . . . . . . . . . . . . 5275

18.3.4.2.4 Home offset (607Ch) . . . . . . . . . . . . . . . 5277

18.3.4.2.5 Initialization of Absolute encoder . . . . . . . 5280

18.3.4.3 Interpolation time period (60C2h) . . . . . . . . . . . . 5284

18.3.5 Cyclic Synchronous Position mode . . . . . . . . . . . . . . . . . . 5284

18.3.5.1 Objects for command and setup . . . . . . . . . . . . . 5286

18.3.5.1.1 objects for PDS command, posi-

tion offset, and special setup . . . . . . . . . . 5286

18.3.5.1.2 objects common in position control . . . . . . 5288

18.3.5.1.3 objects common in motion control . . . . . . . 5291

18.3.5.2 Objects for monitoring . . . . . . . . . . . . . . . . . . . 5296

18.3.5.2.1 Statusword (6041h) . . . . . . . . . . . . . . . . 5296

18.3.5.2.2 Objects common in position control . . . . . . 5296

18.3.5.2.3 Objects common in motion control . . . . . . 5298

18.3.5.3 Other objects . . . . . . . . . . . . . . . . . . . . . . . . . 5300

18.3.5.3.1 Positional command filter . . . . . . . . . . . . 5300

18.3.5.3.2 Positioning complete output (INP/INP2) . . . 5301

18.3.6 PDO mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5303

18.4 Ethercat-based digital IO . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5308

55

TABLE OF CONTENTS

18.4.1 Sinking and sourcing in digital IO modules . . . . . . . . . . . . 5308

18.4.2 Hardware components . . . . . . . . . . . . . . . . . . . . . . . . . 5309

18.4.2.1 Ethercat bus couplers . . . . . . . . . . . . . . . . . . . . 5309

18.4.2.2 Digital input terminals . . . . . . . . . . . . . . . . . . . 5310

18.4.2.3 Digital output terminals . . . . . . . . . . . . . . . . . . . 5316

18.4.3 Digital IO programming with IgH ethercat master

for linux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5320

18.4.3.1 Programming with Xenomai dual kernel . . . . . . . . 5320

18.4.3.1.1 EtherCAT master stack installation . . . . . . . 5320

18.4.3.1.2 Free-run mode programming . . . . . . . . . . 5322

18.4.3.1.3 DC-mode (DC Synchronous oper-

ating mode) programming . . . . . . . . . . . . 5367

18.4.3.2 Programming with RT-preempt kernel . . . . . . . . . . 5435

18.4.3.2.1 EtherCAT master stack installation . . . . . . . 5435

18.4.3.2.2 DC-mode programming . . . . . . . . . . . . . . 5440

18.4.3.2.3 Free-run mode programming . . . . . . . . . . 5457

18.4.3.3 Programming with RTAI dual kernel . . . . . . . . . . . 5470

18.4.3.3.1 EtherCAT master stack installation . . . . . . . 5470

18.4.3.3.2 DC mode programming . . . . . . . . . . . . . . 5474

18.4.4 Digital IO programming with SOEM . . . . . . . . . . . . . . . . 5495

18.4.4.1 Programming with Linux kernel . . . . . . . . . . . . . 5495

18.4.4.2 Programming with Xenomai dual kernel . . . . . . . . 5510

18.5 Ethercat servo drives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5526

18.5.1 Hardware setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5526

18.5.1.1 Specification of servo drives and servo motors . . . . 5526

18.5.1.2 Connection of servo drives and servo motors . . . . . 5530

18.5.2 Test run . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5543

18.5.2.1 Hardware setup . . . . . . . . . . . . . . . . . . . . . . . . 5543

18.5.2.2 Panaterm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5545

18.5.3 Programming servo drives in pp and csv modes . . . . . . . . . 5552

18.5.3.1 Profile Position mode (pp mode) . . . . . . . . . . . . . 5552

18.5.3.2 Cyclic Synchronous Velocity mode (csv mode) . . . . 5571

18.5.4 Programming servo drives in csp modes . . . . . . . . . . . . . . 5614

18.5.4.1 DC synchronization activation . . . . . . . . . . . . . . . 5614

56

TABLE OF CONTENTS

18.5.4.2 A single servo drive . . . . . . . . . . . . . . . . . . . . . 5621

18.5.4.2.1 DC-mode programming (I) . . . . . . . . . . . . 5621

18.5.4.2.2 DC-mode programming (II) . . . . . . . . . . . 5635

18.5.4.2.3 User-space xenomai programming . . . . . . . 5646

18.5.4.2.4 Kernel-space xenomai programming . . . . . . 5663

18.5.4.3 Two servo drives . . . . . . . . . . . . . . . . . . . . . . . 5676

18.5.4.3.1 Free RUN mode programming . . . . . . . . . 5676

18.5.4.3.2 DC-mode programming . . . . . . . . . . . . . . 5689

18.5.4.3.3 User-space Xenomai programming . . . . . . . 5712

18.5.4.4 Eight digital IO terminals and two servo drives . . . 5722

18.5.4.4.1 Extracting PDO information . . . . . . . . . . . 5722

18.5.4.4.2 Non-realtime Free-RUN mode programming . 5729

18.5.4.4.3 Non-realtime bus-shift DC mode programming5751

18.5.4.4.4 Bus-shift DC mode programming

with Xenomai . . . . . . . . . . . . . . . . . . . . 5767

18.5.4.4.5 Master-shift DC mode programming

with Xenomai . . . . . . . . . . . . . . . . . . . . 5779

18.6 Ethercat framework for RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . 5822

18.6.1 Communication between RTC and MOPER in RCCL

systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5824

18.6.1.1 FIFO communication in RCCL v1.0 system . . . . . . 5824

18.6.1.2 FIFO and shared-memory communication

in RWRCCL system . . . . . . . . . . . . . . . . . . . . . . 5856

18.6.2 Detailed explanation of RCCL/RTC’s source code . . . . . . . . 5887

18.6.2.0.1 Operation mode of the RCCL . . . . . . . . . . 5888

18.6.2.0.2 Source code of the RCCL for PLAN/-

FAKE/REAL modes . . . . . . . . . . . . . . . . . 5889

18.6.2.0.3 Encoder calibration . . . . . . . . . . . . . . . . . 5905

18.6.2.0.4 Encoder-to-angle mapping in RCCL

v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . 5907

18.6.2.0.5 Encoder-to-angle mapping in RCCL

v5.0 . . . . . . . . . . . . . . . . . . . . . . . . . . 5914

18.6.2.0.6 Torque-to-current mapping . . . . . . . . . . . . 5927

18.6.2.0.7 Source files in rtc and sys directories . . . . . 5936

57

TABLE OF CONTENTS

18.6.2.0.8 RCCL execution in FAKE and REAL

modes . . . . . . . . . . . . . . . . . . . . . . . . . 5941

18.6.2.0.9 Error handling . . . . . . . . . . . . . . . . . . . . 5947

18.6.3 FIFO and shared-memory communication between

MOPERM and RCCL/RTC . . . . . . . . . . . . . . . . . . . . . . . 5950

18.6.3.1 RTC clock generation with FIFOs . . . . . . . . . . . . . 5951

18.6.3.1.1 Implemetation of moperm as a Xeno-

mai kernel module (I) . . . . . . . . . . . . . . 5956

18.6.3.1.2 Implemetation of moperm as a Xeno-

mai kernel module (II) . . . . . . . . . . . . . . 5970

18.6.3.1.3 Implementation of moperm as a

realtime user-space application . . . . . . . . . 5975

18.6.3.2 Sending set-points to MOPERM via shared-

memory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5980

18.6.4 Integration of RCCL/RTC with MOPERM using the

Xenomai API . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6022

18.6.4.1 Position/torque mappings between joints

and motors (rtc/cvae.c) . . . . . . . . . . . . . . . . . . . 6023

18.6.4.1.1 Position control of joint motors . . . . . . . . . 6023

18.6.4.1.2 Torque control of joint motors . . . . . . . . . 6034

18.6.4.1.3 Force/torque control . . . . . . . . . . . . . . . . 6042

18.6.4.1.4 Gravity compensation . . . . . . . . . . . . . . . 6044

18.6.4.2 Shared-memory interface (h/rtc.h and src/misc.c) . . 6047

18.6.4.3 Communication with MOPERM (rtc/pack.c

and src/main.c) . . . . . . . . . . . . . . . . . . . . . . . . 6052

18.6.4.4 MOPERM for real-time simulation (dry-

run mode) . . . . . . . . . . . . . . . . . . . . . . . . . . . 6061

18.6.4.5 MOPERM for ethercat servo drives and

digital IO terminals . . . . . . . . . . . . . . . . . . . . . 6082

18.6.5 Integration of RCCL/RTC with MOPERM using the

Posix realtime extensions . . . . . . . . . . . . . . . . . . . . . . . . 6122

18.6.5.1 FIFO and shared-memory implementation . . . . . . . 6123

18.6.5.2 Servo controller implementation . . . . . . . . . . . . . 6134

18.6.5.3 MOPERM implementation . . . . . . . . . . . . . . . . . 6152

58

TABLE OF CONTENTS

18.7 Ethercat framework for MultiRCCL . . . . . . . . . . . . . . . . . . . . . . 6174

18.7.1 Software architectures of the MultiRCCL systems . . . . . . . . 6174

18.7.2 Threads vs. processes . . . . . . . . . . . . . . . . . . . . . . . . . . 6176

18.7.3 Clock devices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6181

18.7.4 Robot communication devices . . . . . . . . . . . . . . . . . . . . . 6206

18.7.5 IPC between JLS and planning-control processes . . . . . . . . 6232

18.7.6 Data types of encoder counters . . . . . . . . . . . . . . . . . . . . 6265

18.7.6.1 sim robot communication device and the robotsim . 6266

18.7.6.2 tri robot communication device and a PUMA

560 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6319

18.7.7 JLS process for EtherCAT servo drives . . . . . . . . . . . . . . . 6333

18.7.7.1 Encoder-to-angle mapping . . . . . . . . . . . . . . . . . 6333

18.7.7.2 Servo interface for the Trident TRC004 board . . . . 6337

18.7.7.3 Servo interface for EtherCAT servo drives . . . . . . . 6355

18.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6405

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6406

Part III. CNC and Motion Control Systems 6407

19 Computer Numerical Control (CNC) 6409

19.1 Overview of CNC systems . . . . . . . . . . . . . . . . . . . . . . . . . . . 6409

19.2 Interpolator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6413

19.2.1 Linear interpolator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6413

19.2.2 Circular interpolator . . . . . . . . . . . . . . . . . . . . . . . . . . . 6416

19.2.3 Cubic spline interpolator . . . . . . . . . . . . . . . . . . . . . . . . 6425

19.2.4 NURBS interpolator . . . . . . . . . . . . . . . . . . . . . . . . . . . 6430

19.2.5 Fine interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6439

19.3 Hardware implementation of linear interpolators . . . . . . . . . . . . . 6446

19.3.1 Binary rate multiplier (BRM) . . . . . . . . . . . . . . . . . . . . . 6446

19.3.1.1 Pulse generation . . . . . . . . . . . . . . . . . . . . . . . 6446

19.3.1.2 Linear interpolation . . . . . . . . . . . . . . . . . . . . . 6446

19.3.1.3 Hardware implementation . . . . . . . . . . . . . . . . . 6448

19.3.2 Digital differential analyzer (DDA) . . . . . . . . . . . . . . . . . . 6451

19.3.2.1 Numerical integration . . . . . . . . . . . . . . . . . . . . 6451

59

TABLE OF CONTENTS

19.3.2.2 Hardware implementation of a DDA . . . . . . . . . . . 6453

19.3.2.3 DDA for command pulse generation . . . . . . . . . . . 6456

19.4 Acceleration/deceleration control . . . . . . . . . . . . . . . . . . . . . . . 6461

19.4.1 ADCAI (Acc/Dec Control After Interpolation) . . . . . . . . . . . 6463

19.4.1.1 Velocity profiles . . . . . . . . . . . . . . . . . . . . . . . . 6463

19.4.1.2 G-codes to set path control mode . . . . . . . . . . . . 6466

19.4.2 ADCBI (Acc/Dec Control Before Interpolation) . . . . . . . . . . 6469

19.4.2.1 Trapezoidal and triangular velocity profiles . . . . . . 6470

19.4.2.2 Block overlap control of two consecutive

linear blocks . . . . . . . . . . . . . . . . . . . . . . . . . . 6473

19.4.2.3 Block overlap control of linear and circu-

lar block pairs . . . . . . . . . . . . . . . . . . . . . . . . . 6502

19.4.3 S-curve velocity profiles . . . . . . . . . . . . . . . . . . . . . . . . . 6505

19.4.3.1 Generic S-curve velocity profile . . . . . . . . . . . . . . 6505

19.4.3.2 Blending two consecutive S-curve veloc-

ity profiles . . . . . . . . . . . . . . . . . . . . . . . . . . . 6508

19.4.3.3 S-curve velocity profiles with/without lin-

ear velocity regions . . . . . . . . . . . . . . . . . . . . . 6512

19.5 CNC controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6517

19.6 5-axis CNC machines and their interpolators . . . . . . . . . . . . . . . 6519

19.7 LinuxCNC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6524

19.7.1 LinuxCNC on Ubuntu 12.04/PC . . . . . . . . . . . . . . . . . . . 6524

19.7.1.1 Installation . . . . . . . . . . . . . . . . . . . . . . . . . . . 6524

19.7.1.2 Running the “5axis” example . . . . . . . . . . . . . . . 6528

19.7.2 LinuxCNC on Raspberry Pi . . . . . . . . . . . . . . . . . . . . . . . 6532

19.7.3 Programming examples with RTAPI . . . . . . . . . . . . . . . . . 6540

19.7.3.1 FIFO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6541

19.7.3.2 Shared-memory . . . . . . . . . . . . . . . . . . . . . . . . 6547

19.8 Case study: milling machine control with LinuxCNC, Ether-

cat, and Xenomai . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6553

19.8.1 Hardware configuration . . . . . . . . . . . . . . . . . . . . . . . . . 6554

19.8.2 Installation of Machinekit/LinuxCNC . . . . . . . . . . . . . . . . 6557

19.8.3 Configuration of Machinekit/LinuxCNC . . . . . . . . . . . . . . . 6566

19.8.4 Running Machinekit/LinuxCNC . . . . . . . . . . . . . . . . . . . . 6567

60

TABLE OF CONTENTS

19.9 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6576

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6577

20 Motion Control 6579

20.1 PC-based motion control systems . . . . . . . . . . . . . . . . . . . . . . . 6579

20.1.1 Overview of PC-based motion control systems . . . . . . . . . . 6579

20.1.2 Comparison of PC-based motion control systems

with CNC controllers . . . . . . . . . . . . . . . . . . . . . . . . . . 6581

20.1.3 Pulse-type and network-type motion control systems . . . . . . 6582

20.1.4 Design of a PC-based pulse-type motion control system . . . . 6584

20.1.5 Internals of the pulse-type motion control system . . . . . . . . 6591

20.1.5.1 Motion control card . . . . . . . . . . . . . . . . . . . . . 6592

20.1.5.2 Terminal board . . . . . . . . . . . . . . . . . . . . . . . . 6599

20.1.5.3 Servo drive . . . . . . . . . . . . . . . . . . . . . . . . . . . 6601

20.2 PCI motion control cards . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6603

20.2.1 NPM PCL6045B . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6603

20.2.1.1 Registers of the PCL6045B . . . . . . . . . . . . . . . . . 6603

20.2.1.2 Interrupt and its handling . . . . . . . . . . . . . . . . . 6609

20.2.1.3 Timing considerations in accessing a PCL6045B . . . 6614

20.2.1.4 Speed patterns and their setting . . . . . . . . . . . . . 6616

20.2.1.5 FH correction . . . . . . . . . . . . . . . . . . . . . . . . . 6639

20.2.1.6 Input/output interfaces . . . . . . . . . . . . . . . . . . . 6652

20.2.2 Hivertec HPCI-CPD534 . . . . . . . . . . . . . . . . . . . . . . . . . 6657

20.2.2.1 Hardware configuration . . . . . . . . . . . . . . . . . . . 6657

20.2.2.2 Software configuration . . . . . . . . . . . . . . . . . . . 6662

20.2.2.3 Porting DOS library to Linux . . . . . . . . . . . . . . . 6671

20.2.3 JS Automation MPC-3050 . . . . . . . . . . . . . . . . . . . . . . . 6681

20.2.3.1 Hardware configuration . . . . . . . . . . . . . . . . . . . 6681

20.2.3.2 PCI encoder boards . . . . . . . . . . . . . . . . . . . . . 6685

20.2.3.3 Software configuration . . . . . . . . . . . . . . . . . . . 6692

20.2.4 Adlink PCI-8164 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6704

20.2.4.1 Reverse-engineering of the PCI-8164 card . . . . . . . 6705

20.2.4.2 Development of a Linux device driver . . . . . . . . . 6712

61

TABLE OF CONTENTS

20.2.4.3 Porting the device driver of MPC-3035 to

PCI-8164 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6746

20.2.4.4 Development of an user-space library . . . . . . . . . . 6756

20.3 Case study: Construction of a PC-based motion control

system with Adlink PCI-8164 boards, Panasonic servo mo-

tors, and Panasonic servo drivers . . . . . . . . . . . . . . . . . . . . . . . 6776

20.3.1 Installation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6776

20.3.2 Test run . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6787

20.3.2.1 Setting the parameters of a Panasonic servo

driver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6788

20.3.2.2 Setting the parameters of a PCI-8164 board . . . . . 6805

20.3.2.3 Command pulse generation and output pulse

counting . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6807

20.3.2.4 Auto-tuning of a Panasonic servo driver . . . . . . . . 6808

20.4 Interfacing motion control ASICs with PCs . . . . . . . . . . . . . . . . . 6819

20.4.1 Serial interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6819

20.4.2 Network interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6821

20.5 Design of a PC-based motion control system with an in-

terpolation board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6822

20.5.1 Limitations of commercial motion control boards . . . . . . . . 6822

20.5.2 Linear interpolation in motion and robot control . . . . . . . . 6825

20.5.3 Implementation of an interpolation board with se-

rial interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6826

20.5.3.1 Structure of an interpolation board . . . . . . . . . . . 6826

20.5.3.2 Hardward and software resources . . . . . . . . . . . . 6828

20.5.3.2.1 DSP . . . . . . . . . . . . . . . . . . . . . . . . . . 6828

20.5.3.2.2 FPGA . . . . . . . . . . . . . . . . . . . . . . . . . 6829

20.5.4 Implementation of an interpolation board with USB

interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6837

20.5.4.1 Structure of a simple interpolation board

with USB interface . . . . . . . . . . . . . . . . . . . . . . 6838

20.5.4.2 Design of a motion-control FPGA . . . . . . . . . . . . 6840

20.5.4.2.1 Quadrature counter . . . . . . . . . . . . . . . . 6841

20.5.4.2.2 Command-pulse generator . . . . . . . . . . . . 6845

62

TABLE OF CONTENTS

20.5.4.3 Specification of the I/O signals . . . . . . . . . . . . . . 6862

20.5.4.4 Specification of power circuits . . . . . . . . . . . . . . 6866

20.5.4.5 Timing charts of the servo I/O signals . . . . . . . . . 6867

20.5.4.6 Scheduling PC and FPGA tasks . . . . . . . . . . . . . . 6875

20.5.4.7 Loopback test of the interpolation board . . . . . . . . 6876

20.5.4.7.1 FPGA code . . . . . . . . . . . . . . . . . . . . . . 6876

20.5.4.7.2 PC test code . . . . . . . . . . . . . . . . . . . . . 6897

20.5.5 Implementation of an interpolation board with PCI

interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6906

20.5.5.1 Hardware implementation . . . . . . . . . . . . . . . . . 6906

20.5.5.1.1 PCI interface . . . . . . . . . . . . . . . . . . . . . 6907

20.5.5.1.2 Servo-drive interface . . . . . . . . . . . . . . . . 6907

20.5.5.1.3 DC/DC converters . . . . . . . . . . . . . . . . . 6910

20.5.5.1.4 Multi-axis synchronization . . . . . . . . . . . . 6910

20.5.5.2 Software implementation . . . . . . . . . . . . . . . . . . 6914

20.5.5.2.1 Structure of the motion control FPGA . . . . 6914

20.5.5.2.2 Input and output ports . . . . . . . . . . . . . . 6917

20.5.5.2.3 Encoder pulse counter . . . . . . . . . . . . . . . 6942

20.5.5.2.4 Command pulse generator (CPG) . . . . . . . 6977

20.5.5.2.5 Linux device driver . . . . . . . . . . . . . . . . . 6983

20.6 Interpolation-board design tips . . . . . . . . . . . . . . . . . . . . . . . . 6983

20.6.1 Optocoupler input drive circuits . . . . . . . . . . . . . . . . . . . 6983

20.6.2 D flip-flop with a clock-enable input . . . . . . . . . . . . . . . . 6986

20.6.3 Clock domain crossing . . . . . . . . . . . . . . . . . . . . . . . . . 6987

20.6.4 Asynchronous vs. synchronous reset . . . . . . . . . . . . . . . . 6991

20.6.4.0.1 Reset-pulse generation via a con-

trol register . . . . . . . . . . . . . . . . . . . . . 6993

20.6.4.0.2 Clock-domain-crossing reset pulses . . . . . . . 6997

20.6.5 Digital noise filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6998

20.6.6 Verilog implementation of a DDA . . . . . . . . . . . . . . . . . . 7000

20.6.7 Clock synchronization between TG and DDA . . . . . . . . . . . 7013

20.6.7.1 Synchronization of TG execution to DDA’s

clock . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7014

63

TABLE OF CONTENTS

20.6.7.2 Synchronization of TG execution to main

processor’s timer . . . . . . . . . . . . . . . . . . . . . . . 7025

20.7 Design of a 6-axis robot controller with an interpolation board . . . 7032

20.7.1 Hardware structure . . . . . . . . . . . . . . . . . . . . . . . . . . . 7032

20.7.2 Software structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7034

20.7.2.1 Software structure of the RCCL v1.0 . . . . . . . . . . 7034

20.7.2.2 Pros and Cons of the commercial servo drives . . . . 7038

20.7.2.3 Modification of the RCCL v1.0 for the in-

terpolation board . . . . . . . . . . . . . . . . . . . . . . . 7040

20.7.2.3.1 Structures chg and how . . . . . . . . . . . . . 7040

20.7.2.3.2 Representation of joint and mo-

tor angles . . . . . . . . . . . . . . . . . . . . . . . 7041

20.7.2.3.3 Setting the initial values of encoder

counters . . . . . . . . . . . . . . . . . . . . . . . . 7042

20.7.2.3.4 Replacing the FIFO device driver

with the USB device driver . . . . . . . . . . . 7046

20.8 Ethercat servo drives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7048

20.8.1 EtherCAT comminication . . . . . . . . . . . . . . . . . . . . . . . . 7049

20.8.2 EtherCAT master stacks . . . . . . . . . . . . . . . . . . . . . . . . . 7054

20.8.2.1 Commercial EtherCAT master stacks . . . . . . . . . . . 7054

20.8.2.2 Open-source EtherCAT master stacks . . . . . . . . . . 7058

20.8.3 Kollmorgen AKD Servo Drive . . . . . . . . . . . . . . . . . . . . . 7061

20.8.3.1 PDO mappings . . . . . . . . . . . . . . . . . . . . . . . . 7061

20.8.3.2 Torque, velocity, cyclic sync position modes . . . . . . 7068

20.8.4 Yaskawa Sigma-V SGDV SERVOPACK . . . . . . . . . . . . . . . . 7082

20.8.4.1 Cyclic sync position mode programming . . . . . . . . 7082

20.8.4.2 Synchronization with Distributed Clocks . . . . . . . . 7091

20.8.5 Synapticon SOMANET modules . . . . . . . . . . . . . . . . . . . . 7098

20.9 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7144

20.9.1 Comparison of CNC systems with robot systems . . . . . . . . . 7144

20.9.2 Considerations in applying PC-based motion con-

trollers to CNC and robot systems . . . . . . . . . . . . . . . . . . 7147

20.10 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7150

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7151

64

TABLE OF CONTENTS

Index 7153

Appendices 7153

A Hardware and software platforms of industrial robot controllers 7155

A.1 Hardware and software platforms for industrial robot con-

trollers (slides) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7155

A.2 Modelling, Trajectory Calculation and Servoing of a Com-

puter Controlled Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7173

A.3 Robot Manipulators: Mathematics, Programming, and Control . . . . 7268

B Programming and control of the PUMA robots 7559

B.1 Programming and control of PUMA robot arms (slides) . . . . . . . . 7559

B.2 Robot manipulator control under Unix. RCCL: A robot

control “C” library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7626

B.3 Multi-RCCL: Extending the RCCL Programming Environ-

ment to Multiple Robots and Processors . . . . . . . . . . . . . . . . . . 7645

B.4 An Overview of KALI: a System to Program and Control

Cooperative Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7671

B.5 RWRCCL: A New RCCL Implementation Using Real-Time

Linux And A Single CPU . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7684

B.6 The ARCL robot programming system . . . . . . . . . . . . . . . . . . . . 7690

B.7 Multiprocessor based control for multiple robots: Soft-

ware and kinematic programming methodology . . . . . . . . . . . . . 7701

B.8 QRobot - A Multitasking PC Based Robot Control System . . . . . . . 7891

B.9 Tracking the PUMA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8024

B.10 Software for Control and Dynamic Simulation of Uni-

mate PUMA 560 Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8045

B.11 Coordinates, Conversions, and Kinematics for the Rochester

Robotics Lab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8298

C Kinematics and differential kinematics 8319

C.1 Denavit-Hartenberg parameters . . . . . . . . . . . . . . . . . . . . . . . . 8319

C.1.1 Denavit-Hartenberg parameters – Wikipedia . . . . . . . . . . . . 8319

C.1.2 A kinematic notation for lower pair mechanisms

based on matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8329

65

TABLE OF CONTENTS

C.1.3 A simple and systematic approach to assigning Denavit-

Hartenberg Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 8337

C.1.4 Visualization of arm poses according to the DH parameters . 8343

C.2 Forward, Inverse and Differential Kinematics of PUMA

560 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8345

C.2.1 Kinematic control equations for simple manipulators . . . . . . 8345

C.2.2 A Geometric Approach in Solving the Inverse Kine-

matics of PUMA Robots . . . . . . . . . . . . . . . . . . . . . . . . . 8353

C.2.3 Differential Kinematic Control Equations for Sim-

ple Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8366

C.2.4 Efficient Jacobian inversion for the control of sim-

ple robot manipulators . . . . . . . . . . . . . . . . . . . . . . . . . 8372

C.2.5 An Exact Kinematic Model of PUMA 560 robot . . . . . . . . . 8382

C.2.6 Efficient PUMA Manipulator Jacobian Calculation

and Inversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8391

C.3 Robotics Toolbox for Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . 8405

C.4 Kinematic singularity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8848

C.4.1 Singularity robust algorithm in serial manipulators . . . . . . . 8848

C.4.2 Six-Axis Robot Configuration Singularities . . . . . . . . . . . . . 9067

C.4.3 Singularity-Robust Trajectory Generation . . . . . . . . . . . . . . 9079

C.4.3.1 Journal paper . . . . . . . . . . . . . . . . . . . . . . . . . 9079

C.4.3.2 Dissertation . . . . . . . . . . . . . . . . . . . . . . . . . . . 9099

C.4.3.3 Source code for simulation . . . . . . . . . . . . . . . . . 9313

C.4.3.4 Installation and configuration of KVM (Kernel-

based Virtual Machine) on Ubuntu 18.04 . . . . . . . 9316

C.4.4 Extension of Usable Workspace of Rotational Axes

in Robot Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9321

C.4.5 Avoiding of the Kinematic Singularities of Contem-

porary Industrial Robots . . . . . . . . . . . . . . . . . . . . . . . . 9331

C.4.6 Symbolic Singular Value Decomposition for a PUMA

Robot and Its Application to a Robot Operation

Near Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9344

C.4.7 Living with the singularity of robot wrists . . . . . . . . . . . . . 9358

C.4.8 Dynamic scaling of manipulator trajectories . . . . . . . . . . . . 9364

66

TABLE OF CONTENTS

C.4.9 Study and Resolution of Singularities for a 6-DOF

PUMA Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9370

C.4.10 Manipulator control at kinematic singularities: a

dynamically consistent strategy . . . . . . . . . . . . . . . . . . . . 9383

C.4.11 Robot Control in the Neighborhood of Singular Points . . . . . 9397

C.4.12 An efficient pseudo-inverse solution to the inverse

kinematic problem for six-joint manipulators . . . . . . . . . . . 9405

C.4.13 On the calculation of pseudoinverses . . . . . . . . . . . . . . . . 9434

C.4.14 A framework for singularity-robust manipulator con-

trol during physical human-robot interaction . . . . . . . . . . . 9437

C.4.15 Kinematic Control With Singularity Avoidance for

Teaching-Playback Robot Manipulator System . . . . . . . . . . . 9454

C.5 Screw theory and dual quaternions for robot kinematics . . . . . . . . 9469

C.5.1 3D kinematics using dual quaternions: theory and

applications in neuroscience . . . . . . . . . . . . . . . . . . . . . . 9469

C.5.2 A Comparative Study of Three Inverse Kinematic

Methods of Serial Industrial Robot Manipulators in

the Screw Theory Framework . . . . . . . . . . . . . . . . . . . . . 9502

C.5.3 Kinematics of Robot Manipulators via Line Transformations . 9615

C.5.4 Robot Kinematic Modeling and Control Based on

Dual Quaternion Algebra . . . . . . . . . . . . . . . . . . . . . . . . 9642

C.5.5 Dual Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9691

C.5.6 A comparison between the Denavit-Hartenberg and

the screw-based methods used in kinematic mod-

eling of robot manipulators . . . . . . . . . . . . . . . . . . . . . . 9707

C.5.7 On Homogeneous Transforms, Quaternions, and Com-

putational Efficiency . . . . . . . . . . . . . . . . . . . . . . . . . . . 9714

C.5.8 Algorithmic approach to geometric solution of gen-

eralized Paden-Kahan subproblem and its extension . . . . . . 9722

C.5.9 On the Comparisons of Unit Dual Quaternion and

Homogeneous Transformation Matrix . . . . . . . . . . . . . . . . 9734

C.5.10 A comparison between the Denavit–Hartenberg and

the screw-based methods used in kinematic mod-

eling of robot manipulators . . . . . . . . . . . . . . . . . . . . . . 9752

67

TABLE OF CONTENTS

C.6 Guidelines for the control of a multi-axial planar robot

with ST10F276 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9759

C.7 Survey of Robotic Arm and Parameters . . . . . . . . . . . . . . . . . . . 9801

C.8 Inverse kinematics of 6-axis industrial robots . . . . . . . . . . . . . . . 9808

C.8.1 Computationally Efficient Kinematics for Manipu-

lators with Spherical Wrists Based on the Homo-

geneous Transformation Representation . . . . . . . . . . . . . . 9808

C.8.2 Application of Dual-Number Matrices to the inverse

Kinematics Problem of Robot Manipulators . . . . . . . . . . . . 9822

C.8.3 An Analytical Solution of the Inverse Kinematics

Problem of Industrial Serial Manipulators with an

Ortho-parallel Basis and a Spherical Wrist . . . . . . . . . . . . . 9831

C.9 Math for Game Programmers . . . . . . . . . . . . . . . . . . . . . . . . . 9844

C.9.1 Dual Numbers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9844

C.9.2 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9878

C.10 Representing Robot Poses . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9909

C.11 Product of Exponentials Formula . . . . . . . . . . . . . . . . . . . . . . . 9989

C.11.1 Robotic manipulators and the product of exponen-

tials formula . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9989

C.11.2 An analytic approach to converting POE parame-

ters into D-H parameters for serial-link robots . . . . . . . . . .10000

C.12 Jacobian-based inverse kinematics . . . . . . . . . . . . . . . . . . . . . .10007

C.12.1 Jacobian methods for inverse kinematics . . . . . . . . . . . . . .10007

C.12.2 Introduction to Inverse Kinematics with Jacobian

Transpose, Pseudo inverse and Damped Least Squares

methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10017

C.12.2.1 Report . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10017

C.12.2.2 Accompanying software . . . . . . . . . . . . . . . . . . .10037

C.12.3 A Comparison of Jacobian-based Methods of In-

verse Kinematics for Serial Robot Manipulators . . . . . . . . .10041

C.12.4 Singularity Avoidance Algorithms for Controlling

Robot Manipulator: A Comparative Study . . . . . . . . . . . . .10052

C.12.5 Inverse Kinematics Techniques in Computer Graph-

ics: A Survey . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10066

68

TABLE OF CONTENTS

C.12.6 Damped Least Squares Methods . . . . . . . . . . . . . . . . . . .10158

C.12.6.1 A Comparison of Damped Least Squares

Algorithms for Inverse Kinematics of Robot

Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . .10158

C.12.6.2 Review of the Damped Least-Squares In-

verse Kinematics with Experiments on an

Industrial Robot Manipulator . . . . . . . . . . . . . . .10165

C.12.6.3 Estimate of the Two Smallest Singular Val-

ues of the Jacobian Matrix: Application

to Damped Least-Squares Inverse Kinemat-

ics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10178

C.12.6.4 Selectively Damped Least Squares for In-

verse Kinematics . . . . . . . . . . . . . . . . . . . . . . .10191

C.12.6.5 Overview of Damped Least-Squares Meth-

ods for Inverse Kinematics of Robot Ma-

nipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . .10205

C.13 Efficient Computation of the Jacobian for Robot Manipulators . . . .10232

C.14 Manipulability of Robotic Mechanisms . . . . . . . . . . . . . . . . . . .10243

C.15 Kinematic redundancy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10251

C.15.1 Robots with kinematic redundancy . . . . . . . . . . . . . . . . .10251

C.15.2 Redundant inverse kinematics: Experimental com-

parative review and two enhancements . . . . . . . . . . . . . .10295

C.15.3 An overview of null space projections for redun-

dant, torque-controlled robots . . . . . . . . . . . . . . . . . . . . .10304

C.15.4 Dynamically consistent generalized inverse of Ja-

cobian matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10321

C.15.4.1 Inertial properties in robotic manipulation:

An object-level framework . . . . . . . . . . . . . . . . .10321

C.15.4.2 Load Independence of the Dynamically Con-

sistent Inverse of the Jacobian Matrix . . . . . . . . .10359

C.15.4.3 The relationship between controlled joint

torque and end-effector force in underac-

tuated robotic systems . . . . . . . . . . . . . . . . . . . .10363

69

TABLE OF CONTENTS

C.15.4.4 Design of inverse kinematics algorithms:

extended Jacobian approximation of the

dynamically consistent Jacobian inverse . . . . . . . .10368

C.16 Advanced Robotic Manipulation . . . . . . . . . . . . . . . . . . . . . . . .10385

D RTOS for Robot Programming 10523

D.1 Installation/configuration of real-time Linux systems . . . . . . . . . .10523

D.1.1 Linux kernel 2.6.32 and RTAI 3.8.1 . . . . . . . . . . . . . . . . .10523

D.1.2 Linux kernel 2.6.32 and Xenomai 2.5.5.2 . . . . . . . . . . . . .10606

D.1.3 Preempt-rt kernel 3.2.0-4-rt . . . . . . . . . . . . . . . . . . . . . .10688

D.1.4 Linux kernel 3.10.32 for RTAI 4.1 . . . . . . . . . . . . . . . . . .10786

D.1.5 Low-latency Linux system configuration recommended

by linuxaudio.org . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10896

D.1.6 Enable real-time capabilities of the mainline kernel . . . . . . .10913

D.1.7 Red Hat Enterprise Linux for Real Time 7 Tuning Guide . . .10922

D.1.8 Custom memory allocators/managers for real-time systems . .11035

D.1.9 Making Linux do Hard Real-time . . . . . . . . . . . . . . . . . . .11077

D.1.10 Porting a Linux application to Xenomai dual kernel . . . . . .11174

D.1.11 Linux Real Time Manual . . . . . . . . . . . . . . . . . . . . . . . .11189

D.1.12 Cyclictest . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .11243

D.1.13 High resolution timer and dynamic tick of Linex kernel . . . .11299

D.1.14 Preempt-rt kernel 4.9.98-rt76 . . . . . . . . . . . . . . . . . . . . .11303

D.1.15 Mastering Real-Time Linux . . . . . . . . . . . . . . . . . . . . . . .11306

D.1.16 Xenomai 2.6.5/3.0.5 on Ubuntu 14.04/16.04 . . . . . . . . . . .11344

D.2 EtherCAT experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .11358

D.2.1 TI AM335X ICEv2 boards . . . . . . . . . . . . . . . . . . . . . . .11358

D.2.1.1 Schematic diagram of the TI AM335X ICEv2 . . . . .11358

D.2.1.2 Configuring TwinCAT For AM335x . . . . . . . . . . . .11374

D.2.1.3 Running AM335x EtherCAT Application in

DC Mode . . . . . . . . . . . . . . . . . . . . . . . . . . . .11393

D.2.2 Running the IgH ethercat master stack on Ubuntu

14.04 + RTAI 4.1 . . . . . . . . . . . . . . . . . . . . . . . . . . . .11402

D.3 Linux, Solaris, and VxWorks documents . . . . . . . . . . . . . . . . . .11407

D.3.1 Mainline kernel building for Ubuntu distribution . . . . . . . .11407

70

TABLE OF CONTENTS

D.3.2 Porting 32-bit applications to 64-bit Linux/Solaris systems . .11409

D.3.2.1 Porting to 64-bit GNU/Linux Systems . . . . . . . . . .11409

D.3.2.2 Porting an application to 64-bit Linux on

HP Integrity servers . . . . . . . . . . . . . . . . . . . . .11424

D.3.2.3 Solaris 64-bit Developers’s Guide . . . . . . . . . . . . .11440

D.3.3 Multiarch - what is it and why should I care? . . . . . . . . . .11515

D.3.4 The end of time . . . . . . . . . . . . . . . . . . . . . . . . . . . . .11539

D.3.5 Solaris to Linux Porting Guide . . . . . . . . . . . . . . . . . . . .11569

D.3.6 An introduction to Linux IPC . . . . . . . . . . . . . . . . . . . . .11768

D.3.7 Red Hat Enterprise Linux to Oracle Solaris Porting Guide . . .11822

D.3.8 Porting VxWorks Applications to Linux Pthreads . . . . . . . . .11933

D.3.9 Multithreaded Programming Guide . . . . . . . . . . . . . . . . .11974

D.3.10 Linux kernel module programming guide . . . . . . . . . . . . .12245

D.3.10.1 The Linux kernel module programming guide,

Version 1.1.0 . . . . . . . . . . . . . . . . . . . . . . . . . .12245

D.3.10.2 The Linux kernel module programming guide,

Version 2.4.0 . . . . . . . . . . . . . . . . . . . . . . . . . .12376

D.3.10.3 The Linux kernel module programming guide,

Version 2.6.4 . . . . . . . . . . . . . . . . . . . . . . . . . .12472

D.3.11 Solaris Device Drivers . . . . . . . . . . . . . . . . . . . . . . . . . .12527

D.3.11.1 Writing Device Drivers, 2008 . . . . . . . . . . . . . . .12527

D.3.11.2 Writing Device Drivers, 2012 . . . . . . . . . . . . . . .13144

D.3.11.3 Device Driver Tutorial, 2012 . . . . . . . . . . . . . . . .13775

D.3.12 Linux Device Drivers . . . . . . . . . . . . . . . . . . . . . . . . . .13892

D.3.13 Linux Kernel and Driver Development Training . . . . . . . . .14519

D.3.14 Unreliable Guide To Locking . . . . . . . . . . . . . . . . . . . . .14758

D.3.14.1 Unreliable Guide To Locking for kernel 2.4 . . . . . .14758

D.3.14.2 Unreliable Guide To Locking for kernel 2.6 . . . . . .14783

D.3.15 Hrtimers and Beyond: Transforming the Linux Time

Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .14824

D.3.16 Over and over again: periodic tasks in Linux . . . . . . . . . .14853

D.3.17 Ubuntu wiki: security features . . . . . . . . . . . . . . . . . . . .14861

D.3.18 Groking the Linux SPI Subsystem . . . . . . . . . . . . . . . . . .14874

D.4 Real-time systems for robot applications . . . . . . . . . . . . . . . . . .14899

71

TABLE OF CONTENTS

D.4.1 Open Real-time Robotics Control - PC Hardware,

Windows/VxWorks Operating Systems and Communication . .14899

D.4.2 Real-Time Operating Systems for Robotic Applica-

tions: a Comparative Survey . . . . . . . . . . . . . . . . . . . . .14921

D.4.3 Real-time control in ROS and ROS 2.0 . . . . . . . . . . . . . . .14931

D.4.4 A Real-time Control System For Industrial Robots

and Control Applications Based on Real-time Linux . . . . . . .14959

D.4.5 An open architecture for sensory feedback control

of a dual-arm industrial robotic cell . . . . . . . . . . . . . . . . .14966

D.4.6 Standard Linux for embedded real-time robotics and

manufacturing control systems . . . . . . . . . . . . . . . . . . . .14975

D.4.7 Real-Time Performance Analysis in Linux-Based Robotic

Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .14989

D.4.8 Sensor-based Real-time Control of Industrial Robots . . . . . .15043

D.4.9 Real-Time Programming for Robotics . . . . . . . . . . . . . . . .15210

D.5 openPOWERLINK documents . . . . . . . . . . . . . . . . . . . . . . . . .15322

D.5.1 openPOWERLINK on Linux . . . . . . . . . . . . . . . . . . . . . .15322

D.5.2 openPOWERLINK over Xenomai . . . . . . . . . . . . . . . . . . .15329

D.5.3 openPOWERLINK stack implementation . . . . . . . . . . . . . .15348

D.6 Embedded boards for realtime USB experiments . . . . . . . . . . . . .15369

D.6.1 Schematic diagram of the FlashLink board . . . . . . . . . . . .15376

D.6.2 Schematic diagram of the Saturn board . . . . . . . . . . . . . .15383

D.7 PC motherboards for realtime Linux experiments . . . . . . . . . . . .15390

D.7.1 GIGABYTE GA-N3150N-D3V User’s Manual . . . . . . . . . . . .15390

D.7.2 Supermicro C2SBC-Q User’s Manual . . . . . . . . . . . . . . . . .15419

D.8 POSIX Programming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .15507

D.8.1 POSIX Threads Programming . . . . . . . . . . . . . . . . . . . . .15507

D.8.2 POSIX Shared Memory . . . . . . . . . . . . . . . . . . . . . . . . .15883

D.8.3 Real-Time POSIX: An Overview . . . . . . . . . . . . . . . . . . . .15914

D.8.4 The Use of POSIX in Real-time Systems, Assessing

its Effectiveness and Performance . . . . . . . . . . . . . . . . . .15923

D.8.5 Using the POSIX API: Threads, real-time and IPC . . . . . . . .15940

D.9 RTLinux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .15961

D.9.1 The RTLinux Manifesto . . . . . . . . . . . . . . . . . . . . . . . . .15961

72

TABLE OF CONTENTS

D.9.2 A Linux-based Real-Time Operating System . . . . . . . . . . . .15974

D.9.3 Analyzing RTLinux/GPL Source Code for Education . . . . . . .16018

D.9.4 RTLinux POSIX API for IO on Real-time FIFOs and

Shared Memory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .16025

D.10 Mitsubishi PA10 robot documents . . . . . . . . . . . . . . . . . . . . . .16036

D.10.1 PA10 Robot Programming Manual . . . . . . . . . . . . . . . . . .16036

D.10.2 PA10 Robot Operating Manual . . . . . . . . . . . . . . . . . . . .16433

D.11 RCCL documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .16535

D.11.1 RCCL/RCI Startup and Installation Guide . . . . . . . . . . . . .16535

D.11.2 XVME-203/293 Counter module with quadrature . . . . . . . .16599

D.11.3 XVME-240 Digital IO module . . . . . . . . . . . . . . . . . . . . .16677

D.11.4 Trident boards for PUMA robot control . . . . . . . . . . . . . . .16751

D.11.4.1 Trident TRC004 User’s Manual . . . . . . . . . . . . . .16751

D.11.4.2 Trident TRC006 User’s Manual . . . . . . . . . . . . . .16779

D.11.4.3 First Steps Towards an Open Control Ar-

chitecture for a PUMA 560 . . . . . . . . . . . . . . . .16785

E Design of a BASIC interpreter for embedded systems 16793

E.1 Source code of the tinybc BASIC interpreter . . . . . . . . . . . . . . .16793

E.2 BASIC language written in Lua . . . . . . . . . . . . . . . . . . . . . . . .16820

E.2.1 TBASIC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .16820

E.2.2 Interpreter design using LPeg . . . . . . . . . . . . . . . . . . . . .16837

F Design of a robot programming system 16875

F.1 AL User’s Manual, 1979 . . . . . . . . . . . . . . . . . . . . . . . . . . . . .16875

F.2 VAL/VAL-II/V+/VAL3 langauges . . . . . . . . . . . . . . . . . . . . . . . .17016

F.2.1 VAL: A Versatile Robot Programming and Control System . . .17016

F.2.2 VAL-II: A New Robot Control System for Automatic

Manufacturing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .17023

F.2.3 User’s guide to VAL 398H2A, 1980 . . . . . . . . . . . . . . . . .17039

F.2.4 Design of a robot programming system using the

VAL-compiler, 1984 . . . . . . . . . . . . . . . . . . . . . . . . . . .17162

F.2.5 Syntax checking of VAL-II programs . . . . . . . . . . . . . . . . .17233

F.2.6 V+ Reference Manual . . . . . . . . . . . . . . . . . . . . . . . . . .17268

F.2.7 VAL3 Reference Manual . . . . . . . . . . . . . . . . . . . . . . . . .18070

73

TABLE OF CONTENTS

F.2.8 PUMA100 VAL Emulation Library . . . . . . . . . . . . . . . . . .18231

F.3 RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .18279

F.3.1 RCCL User’s Manual Version 1.0, 1983 . . . . . . . . . . . . . . .18279

F.3.2 Data Stuructures of the RCCL Version 1.0 . . . . . . . . . . . . .18394

F.3.3 RTC User’s Manual, 1983 . . . . . . . . . . . . . . . . . . . . . . .19723

F.3.4 Source code of the RCCL v1.0 . . . . . . . . . . . . . . . . . . . .19741

F.3.5 A Multi-Microprocessor-Based Control Environment

for Industrial Robots . . . . . . . . . . . . . . . . . . . . . . . . . . .20328

F.3.6 Integrating Robot Manipulator Control Into PASCAL . . . . . .20441

F.4 Robot programming languages . . . . . . . . . . . . . . . . . . . . . . . .20448

F.4.1 On reverse-engineering the KUKA Robot Language . . . . . . .20448

F.4.2 Comparative study of robot languages . . . . . . . . . . . . . . .20455

F.4.3 Programming language SLIM . . . . . . . . . . . . . . . . . . . . .20471

F.4.3.1 Industrial robots – Programming language

SLIM, JIS B 8439-1992 . . . . . . . . . . . . . . . . . . .20471

F.4.3.2 Industrial robots – Programming language

SLIM, KS B 7097:2011 . . . . . . . . . . . . . . . . . . .20511

F.4.3.3 Industrial robots – Intermediate code STROLIC,

JIS B 8440-1995 . . . . . . . . . . . . . . . . . . . . . . .20562

F.4.4 A Survey on Domain-specific Modeling and Lan-

guages in Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . .20641

F.4.5 Robot Language from the Standpoint of FA System

Development – An Outline of FA-BASIC . . . . . . . . . . . . . .20667

F.4.6 Industrial Robot Programming Languages: A Com-

parative Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . .20682

F.4.7 The Roadmap and Challenges of Robot Program-

ming Languages . . . . . . . . . . . . . . . . . . . . . . . . . . . . .20689

F.5 Robot programming systems . . . . . . . . . . . . . . . . . . . . . . . . . .20696

F.5.1 Implementation of a RCCL/RCI system for PUMA

260 robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .20696

F.5.2 Kali: A computing architecture for a multiple robot

controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .20892

F.5.3 Robot Controller Case Study . . . . . . . . . . . . . . . . . . . . .21047

F.5.4 Robotics API . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .21091

74

TABLE OF CONTENTS

F.5.4.1 Robotics API: Object-Oriented Software De-

velopment for Industrial Robots . . . . . . . . . . . . .21091

F.5.4.2 Object-oriented Software for Industrial Robots . . . .21114

F.5.4.3 Embedding Real-Time Critical Robotics Ap-

plications in an Object-Oriented Language . . . . . . .21378

F.5.4.4 Using Java for Real-Time Critical Indus-

trial Robot Programming . . . . . . . . . . . . . . . . . .21618

F.5.5 Generic Educational Robot Control System – GERCS . . . . . .21632

F.5.6 MRROC++ based system description . . . . . . . . . . . . . . . .21754

F.6 Industrial Robot Programming . . . . . . . . . . . . . . . . . . . . . . . . .21853

F.6.1 Robot Programming . . . . . . . . . . . . . . . . . . . . . . . . . . .21853

F.6.2 Robotics Middleware: A Comprehensive Literature

Survey and Attribute-Based Bibliography . . . . . . . . . . . . . .21911

F.6.3 Robot Programming Methods . . . . . . . . . . . . . . . . . . . . .21928

F.6.4 Application Oriented Programming and Control of

Industrial Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . .22043

F.6.5 PLCopen for robot programming . . . . . . . . . . . . . . . . . . .22188

F.7 VAL language implementation using the RCCL v1.0 . . . . . . . . . . .22319

F.7.1 Source code of the VAL language implemented with

the RCCL v1.0 and the Lex/Yacc . . . . . . . . . . . . . . . . . . .22319

F.7.2 Source code of the VAL language implemented with

the RCCL v1.0 and the BIFES interpreter . . . . . . . . . . . . .22417

F.7.3 Source code of the VAL language implemented with

the RCCL v1.0 and the BIFERS interpreter . . . . . . . . . . . .22501

F.8 ROS (Robot Operating System) . . . . . . . . . . . . . . . . . . . . . . . .22659

F.8.1 Moveit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .22659

F.8.1.1 Moveit Tutorials, Release Indigo . . . . . . . . . . . . .22659

F.8.1.2 Moveit Tutorials, Release Kinetic . . . . . . . . . . . . .22739

F.8.1.3 Motion Planning for Industrial Robots us-

ing MoveIt! . . . . . . . . . . . . . . . . . . . . . . . . . . .22909

F.8.2 HOW to design ROS-powered robots . . . . . . . . . . . . . . . .22926

F.8.3 Catkin meta-buildsystem . . . . . . . . . . . . . . . . . . . . . . . .22945

F.8.3.1 Understanding and using catkin (and bloom) . . . . .22945

F.8.3.2 Migrating from catkin_make . . . . . . . . . . . . . . . .22964

75

TABLE OF CONTENTS

F.8.3.3 catkin_tools Documentation . . . . . . . . . . . . . . . .22973

F.8.4 ROS control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23068

F.8.4.1 ROS control, an overview . . . . . . . . . . . . . . . . .23068

F.8.4.2 ros_control: A generic and simple con-

trol framework for ROS . . . . . . . . . . . . . . . . . . .23131

F.8.5 ROS-based robot systems . . . . . . . . . . . . . . . . . . . . . . . .23137

F.8.5.1 ControlIt! . . . . . . . . . . . . . . . . . . . . . . . . . . . .23137

F.8.5.2 Franka Control Interface . . . . . . . . . . . . . . . . . .23225

F.9 Component-based software engineering in robotics . . . . . . . . . . .23269

F.9.1 Component-Based Robotic Engineering . . . . . . . . . . . . . . .23269

F.9.2 Component-based Robotics Middleware . . . . . . . . . . . . . . .23296

F.9.3 Comparison Study of Robotic Middleware for Robotic

Applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23315

F.9.4 Robotic frameworks, architectures and middleware comparison23325

F.10 OROCOS (Open RObot COntrol Software) . . . . . . . . . . . . . . . . .23345

F.10.1 An introduction to OROCOS . . . . . . . . . . . . . . . . . . . . .23345

F.10.2 Component-based modelling for generating robotic

arm applications running under OROCOS middleware . . . . .23357

F.10.3 Design and Development of a Reusable Component-

based Architecture for Surgical Robotics . . . . . . . . . . . . . .23364

F.10.4 Open-source real-time robot operation and control

system for highly dynamic, modular machines . . . . . . . . . .23485

F.10.5 OROCOS/ROS Components for Light Weight Robots . . . . . .23559

F.10.6 An Architecture for Controlling the Barrett WAM

Robot Using ROS and OROCOS . . . . . . . . . . . . . . . . . . .23621

G Design of a Trajectory Generator 23661

G.1 Tutorials of trajectory generation . . . . . . . . . . . . . . . . . . . . . . .23661

G.1.1 Motion planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23661

G.1.2 Trajectory Planning for Robots: the Challenges of

Industrial Considerations . . . . . . . . . . . . . . . . . . . . . . . .23709

G.1.3 Trajectory Planning for Robot Manipulators . . . . . . . . . . . .23723

G.1.4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . .23794

G.2 Trajectory generation algorithms and implementations . . . . . . . . .23818

76

TABLE OF CONTENTS

G.2.1 Real-time trajectory generation in Multi-RCCL . . . . . . . . . .23818

G.2.2 Design and Implementation of a Robot Force and

Motion Server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23855

G.2.3 Design and implementation of a trajectory gener-

ator for arbitrarily moving targets and on-line sin-

gularity robustness . . . . . . . . . . . . . . . . . . . . . . . . . . . .24030

G.2.4 Real-Time Robot Trajectory Generation with Python . . . . . .24115

G.2.5 Identification of a trajectory generator model for

the PUMA-560 robot . . . . . . . . . . . . . . . . . . . . . . . . . . .24139

G.2.6 FIR filter-based online jerk-constrained trajectory generation .24154

G.2.7 Trajectory planning and feedforward design for elec-

tromechanical motion systems . . . . . . . . . . . . . . . . . . . .24167

G.2.8 The Reflexxes Motion Libraries . . . . . . . . . . . . . . . . . . . .24233

G.2.9 Jerk-Bounded Manipulator Trajectory Planning: De-

sign for Real-Time Applications . . . . . . . . . . . . . . . . . . . .24302

G.2.10 Task Space Velocity Blending for Real-Time Trajec-

tory Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24479

G.2.11 Spherical Parabolic Blends for Robot Workspace Trajectories .24531

G.2.12 Blending in pick and place applications . . . . . . . . . . . . . .24538

G.2.13 Planning and Execution of Straight Line Manipu-

lator Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24606

G.2.14 Real-time trajectory planning along a predefined path . . . . .24620

G.2.15 C2-continuous orientation trajectory planning for

robot based on spline quaternion curve . . . . . . . . . . . . . .24669

G.2.16 Manipulator Cartesian Path Control . . . . . . . . . . . . . . . . .24679

G.2.17 Motion Generation in the MRROC++ Robot Pro-

gramming Framework . . . . . . . . . . . . . . . . . . . . . . . . . .24690

G.2.18 Iterative path-accurate trajectory generation for fast

sensor-based motion of robot arms . . . . . . . . . . . . . . . . .24719

G.2.19 Accurate Interpolation of Machining Tool-paths Based

on FIR Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24735

G.3 Interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24749

G.3.1 Interpolating values . . . . . . . . . . . . . . . . . . . . . . . . . . .24749

G.3.2 Cubic spline algorithms for orientation interpolation . . . . . .24791

77

TABLE OF CONTENTS

G.3.3 Quaternion Cubic Spline . . . . . . . . . . . . . . . . . . . . . . . .24812

G.3.4 Geometric Skinning with Approximate Dual Quater-

nion Blending . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24827

G.3.5 Camera Keyframing Using Linear Interpolation of Matrices . .24851

G.3.6 Animating Rotation with Quaternion Curves . . . . . . . . . . .24867

G.3.7 Quaternions, interpolation, and animation . . . . . . . . . . . . .24878

G.3.8 Interpolation of Rigid Motions using a Matrix Representation24982

G.3.9 An Efficient Sub-optimal Motion Planning Method

for Attitude Manoeuvres . . . . . . . . . . . . . . . . . . . . . . . .25044

G.3.10 Rotation Interpolation Based on the Geometric Struc-

ture of Unit Quaternions . . . . . . . . . . . . . . . . . . . . . . . .25077

G.3.11 Interpolation and Splines . . . . . . . . . . . . . . . . . . . . . . . .25084

G.3.12 A general construction scheme for unit quaternion

curves with simple high order derivatives . . . . . . . . . . . . .25188

G.4 Rotations, quaternions, and dual quaternions . . . . . . . . . . . . . . .25197

G.4.1 Understanding rotations and quaternions . . . . . . . . . . . . .25197

G.4.1.1 Understanding rotations . . . . . . . . . . . . . . . . . . .25197

G.4.1.2 Understanding quaternions . . . . . . . . . . . . . . . . .25317

G.4.2 Euler angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25386

G.4.3 Using Rotations to Build Aerospace Coordinate Systems . . . .25469

G.4.4 Dual-Quaternions: From Classical Mechanics to Com-

puter Graphics and Beyond . . . . . . . . . . . . . . . . . . . . . .25507

G.4.5 Hand-Eye Calibration Using Dual Quaternions . . . . . . . . . .25519

G.4.6 Tutorial: Consistent representations of and conver-

sions between 3D rotations . . . . . . . . . . . . . . . . . . . . . .25533

G.4.7 Rodrigues’ rotation formula . . . . . . . . . . . . . . . . . . . . . .25556

G.4.8 Orientation, Rotation, Velocity and Acceleration, and

the SRM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25562

G.4.9 Dual-Number Transformation and Its Applications

to Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25632

G.4.10 A Comparison of Transforms and Quaternions in Robotics . .25642

G.4.11 Representing attitude: Euler angles, unit quater-

nions, and rotation vectors . . . . . . . . . . . . . . . . . . . . . .25649

G.5 Inverse trigonometric functions and atan2 function . . . . . . . . . . .25685

78

TABLE OF CONTENTS

G.5.1 Inverse trigonometric functions . . . . . . . . . . . . . . . . . . . .25685

G.5.2 Atan2 function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25698

G.6 Approximation algorithms of spherical linear interpola-

tion (Slerp) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25707

G.6.1 Hacking Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . .25707

G.6.2 PolySlerp: A fast and accurate polynomial approx-

imation of spherical linear interpolation (Slerp) . . . . . . . . .25718

G.6.3 A Fast and Accurate Algorithm for Computing SLERP . . . . .25725

H Design of a graphic robot simulator 25753

H.1 CAD models of the PUMA 560 robot . . . . . . . . . . . . . . . . . . . .25753

H.1.1 Inventor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25753

H.1.2 VRML model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25765

H.2 Source code of the PUMA 560 simulator . . . . . . . . . . . . . . . . . .25780

H.3 Tools for dynamics simulation of robots: a survey based

on user feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25864

H.4 V-REP documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25880

H.4.1 Virtual Robot Experimentation Platform V-REP: A

Versatile 3D Robot Simulator . . . . . . . . . . . . . . . . . . . . .25880

H.4.2 V-REP Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25893

H.4.3 V-REP Tutorials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .25916

H.4.4 Writing code in and around V-REP . . . . . . . . . . . . . . . . .26002

H.4.5 Remote API modus operandi . . . . . . . . . . . . . . . . . . . . .26007

H.4.6 V-REP User Manual . . . . . . . . . . . . . . . . . . . . . . . . . . .26015

H.5 Robotics Library: A Software Architecture for Robot Con-

trol and its Application to Social Robotics . . . . . . . . . . . . . . . . .26931

H.6 GAZEBO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .26941

H.7 Motion Planning for Real Robots . . . . . . . . . . . . . . . . . . . . . . .26963

H.8 Design of an Interactive Computer Graphics Simulator of VAL . . . .27006

H.9 OpenRAVE: A Planning Architecture for Autonomous Robotics . . . .27169

H.10 Open Source Framework for Real-Time Robot Simulation

and Collision Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . .27188

H.11 Python Computer Graphics Kit (cgkit) . . . . . . . . . . . . . . . . . . .27195

H.11.1 User manual . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .27195

79

TABLE OF CONTENTS

H.11.2 Tutorials and examples . . . . . . . . . . . . . . . . . . . . . . . . .27617

H.12 PyBullet . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .27702

H.12.1 yBullet Quickstart Guide . . . . . . . . . . . . . . . . . . . . . . . .27702

H.12.2 PyBullet installation and examples . . . . . . . . . . . . . . . . . .27770

I Python and Lua for Industrial Robot Programming 27775

I.1 Developing Python bindings of the RCCL . . . . . . . . . . . . . . . . .27775

I.2 pyRCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .27804

I.2.1 Source code of the pyRCCL . . . . . . . . . . . . . . . . . . . . . .27804

I.2.2 Source code of the graphic simulator for the pyR-

CCL demo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .27887

I.3 V+ Operating System and Language . . . . . . . . . . . . . . . . . . . .27965

I.3.1 V+ Language User’s Guide Version 12.1 . . . . . . . . . . . . . .27965

I.3.2 V+ Language Reference Guide Version 12.1 . . . . . . . . . . . .28370

I.3.3 V+ Operating System User’s Guide Version 12.1 . . . . . . . . .29159

I.3.4 V+ Operating System Reference Guide Version 12.1 . . . . . .29313

I.4 Python documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .29526

I.4.1 IPython Documentation Release 0.10.2 . . . . . . . . . . . . . . .29526

I.4.2 CFFI Documentation Release 1.9.1 . . . . . . . . . . . . . . . . . .30063

I.4.3 The Python Language Reference, Release 2.7.14 . . . . . . . . .30135

I.4.4 Regular Expression HOWTO, Release 2.7.14 . . . . . . . . . . .30270

I.4.5 Python re module . . . . . . . . . . . . . . . . . . . . . . . . . . . .30291

I.4.6 Python parser module . . . . . . . . . . . . . . . . . . . . . . . . . .30311

I.4.7 Numpy Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . .30318

I.5 Lua documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .31732

I.5.1 Lua for Python Programmers . . . . . . . . . . . . . . . . . . . . .31732

I.5.2 Lua 5.1 Reference Manual . . . . . . . . . . . . . . . . . . . . . . .31747

I.5.3 LuaJIT FFI Library . . . . . . . . . . . . . . . . . . . . . . . . . . . .31838

I.5.4 Penlight Documentation . . . . . . . . . . . . . . . . . . . . . . . . .31872

I.5.5 Lrexlib Reference Manual . . . . . . . . . . . . . . . . . . . . . . .32074

I.5.6 LPeg documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32088

I.5.6.1 LPeg – Parsing Expression Grammars For Lua . . . . .32088

I.5.6.2 Lpeg Tutorial . . . . . . . . . . . . . . . . . . . . . . . . . .32139

I.5.6.3 Lpeg Recipes . . . . . . . . . . . . . . . . . . . . . . . . . .32152

80

TABLE OF CONTENTS

I.5.6.4 LPEG and regular expressions - compari-

son and tutorial . . . . . . . . . . . . . . . . . . . . . . . .32168

I.5.6.5 A text pattern-matching tool based on Pars-

ing Expression Grammars . . . . . . . . . . . . . . . . . .32189

I.5.6.6 Summary of Lpeg functions . . . . . . . . . . . . . . . .32228

I.5.7 Lua Metatables Tutorial . . . . . . . . . . . . . . . . . . . . . . . . .32254

I.5.8 Simple Lua Classes . . . . . . . . . . . . . . . . . . . . . . . . . . . .32260

I.5.9 Real Scripting with Lua . . . . . . . . . . . . . . . . . . . . . . . . .32274

I.5.10 Lua for Low-Level Programming . . . . . . . . . . . . . . . . . . .32344

I.6 Lua code formatter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32358

I.6.1 Lua 5.1 parser and grammar checker . . . . . . . . . . . . . . . .32358

I.6.2 Lua 5.1 code formatter . . . . . . . . . . . . . . . . . . . . . . . . .32366

I.7 V+ to Lua translator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32395

I.7.1 V+ to python translator written in Lua . . . . . . . . . . . . . .32395

I.7.2 V+ to Lua translator written in Lua . . . . . . . . . . . . . . . . .32419

J Dynamics of Serial Robots 32443

J.1 Comparison between Standard and Modified Denavit-Hartenberg

Methods in Robotics Modelling . . . . . . . . . . . . . . . . . . . . . . . .32443

J.2 ROBOOP: A Robotics Object Oriented Package in C . . . . . . . . . . .32454

J.3 PUMA 560 dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32605

J.3.1 The Explicit Dynamic Model and Inertial Parame-

ters of the PUMA 560 Arm . . . . . . . . . . . . . . . . . . . . . .32605

J.3.2 A search for consensus among model parameters

reported for the PUMA 560 robot . . . . . . . . . . . . . . . . . .32615

J.3.3 The Dynamics of the PUMA Manipulator . . . . . . . . . . . . .32622

J.4 Recursive Newton-Euler algorithms in standard and mod-

ified DH conventions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32629

J.4.1 Recursive Newton-Euler algorithm in standard DH convention32629

J.4.2 Recursive Newton-Euler algorithm in modified DH convention32632

J.5 Computational robot dynamics . . . . . . . . . . . . . . . . . . . . . . . .32634

J.6 Estimation of load and link inertial parameters . . . . . . . . . . . . .32920

J.7 Identification and control of Mitsubishi PA-10 robots . . . . . . . . . .33081

81

TABLE OF CONTENTS

J.7.1 Dynamic identification of a Mitsubishi PA-10 robotic

manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33081

J.7.2 Motion Control of Industrial Robots in Operational

Space: Analysis and Experiments with the PA10 Arm . . . . .33128

J.8 Physical feasibility of robot base inertial parameter iden-

tification: A linear matrix inequality approach . . . . . . . . . . . . . .33155

J.9 On-line computational scheme for mechanical manipulators . . . . .33175

J.10 Robot Dynamics: Equations and Algorithms . . . . . . . . . . . . . . . .33184

J.11 Geometric Algorithms for Robot Dynamics: A Tutorial Review . . . .33194

K Robot Motion Control 33219

K.1 PUMA robot control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33219

K.1.1 The Unimation PUMA servo system . . . . . . . . . . . . . . . . .33219

K.1.2 A Study on the Wrist Servo-Controller of PUMA-

760 Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33274

K.1.3 Estimation and Compensation of Gravity and Fric-

tion Forces for Robot Arms: Theory and Experiments . . . . .33379

K.1.4 Experimental Study on Direct Adaptive Control of

a PUMA 560 Industrial Robot . . . . . . . . . . . . . . . . . . . . .33396

K.1.5 Digital control for multiaxis robots . . . . . . . . . . . . . . . . .33422

K.2 Robot control systems: A survey . . . . . . . . . . . . . . . . . . . . . . .33579

K.3 The Interplay Between Robotics and Control Theory . . . . . . . . . .33603

K.4 Command shaping (input shaping) . . . . . . . . . . . . . . . . . . . . .33620

K.4.1 Vibration Suppression of Semiconductor Wafer Trans-

fer Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33620

K.4.2 A review of command shaping techniques for elim-

ination of residual vibrations in flexible joint ma-

nipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33647

K.4.3 Command Shaping for Flexible Systems: A Review

of the First 50 Years . . . . . . . . . . . . . . . . . . . . . . . . . . .33660

K.4.4 Intelligent Command Generation for Flexible Systems . . . . .33677

K.5 Resonant Load Control Methods for Industrial Servo Drives . . . . .33709

K.6 A review of torsional vibration control methods and a

proposal of disturbance observer-based new techniques . . . . . . . .33718

82

TABLE OF CONTENTS

K.7 Two-Degree-of-Freedom PID Controllers . . . . . . . . . . . . . . . . . .33725

K.8 Analysis of the Computed Torque Drive Method and Com-

parison With Conventional Position Servo for a Computer-

Controlled Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33737

K.9 Sampling of discontinuous voltage and current signals in

electrical drives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33798

K.10 Jacobian-based Algorithms: A Bridge Between Kinemat-

ics And Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33807

K.11 Conventional Controller Design for Industrial Robots–A Tutorial . . .33840

K.12 Resolved motion control . . . . . . . . . . . . . . . . . . . . . . . . . . . .33860

K.12.1 Resolved motion rate control . . . . . . . . . . . . . . . . . . . . .33860

K.12.1.1 Resolved Motion Rate Control of Manip-

ulators and Human Prostheses . . . . . . . . . . . . . .33860

K.12.1.2 The Mathematics of Coordinated Control

of Prosthetic Arms and Manipulators . . . . . . . . . .33868

K.12.1.3 A Comparison of Position and Rate Con-

trol for Telemanipulations with Consider-

ation of Manipulator System Dynamics . . . . . . . . .33876

K.12.2 Resolved motion acceleration control . . . . . . . . . . . . . . . .33888

K.12.3 Resolved motion force control . . . . . . . . . . . . . . . . . . . .33896

K.13 PD Control with Desired Gravity Compensation of Robotic

Manipulators: A Review . . . . . . . . . . . . . . . . . . . . . . . . . . . .33907

K.14 Control of robot manipulators . . . . . . . . . . . . . . . . . . . . . . . . .33921

L Force control 33965

L.1 Impedance and admittance control . . . . . . . . . . . . . . . . . . . . . .33965

L.1.1 Experiments in force control of robotic manipulators . . . . . .33965

L.1.2 A Shared Position/Force Control Methodology for

Teleoperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .33974

L.1.3 Implementation of robotic force control with posi-

tion accommodation . . . . . . . . . . . . . . . . . . . . . . . . . . .33999

L.1.4 A comparison of force control algorithms for robots

in contact with flexible environments . . . . . . . . . . . . . . . .34096

83

TABLE OF CONTENTS

L.1.5 Manipulator performance constraints in human-robot

cooperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .34251

L.1.6 Admittance control for physical human-robot interaction . . .34270

L.1.7 Nonlinear and Adaptive Control of Force and Com-

pliance in Manipulators . . . . . . . . . . . . . . . . . . . . . . . . .34543

L.1.8 Position Accommodation and Compliance Control

for Robotic Excavation . . . . . . . . . . . . . . . . . . . . . . . . .34562

L.1.9 Impedance control: An approach to manipulation . . . . . . . .34571

L.1.10 Stable execution of contact tasks using impedance control . .34596

L.1.11 Impedance control, lecture note . . . . . . . . . . . . . . . . . . .34605

L.1.12 On the Implementation and Performance of Impedance

Control on Position Controlled Robots . . . . . . . . . . . . . . .34614

L.1.13 Categorizing Admittance Control Parameters for the

Ranger 8-dof Tele-operated Space Manipulator . . . . . . . . . .34621

L.1.14 Force Tracking Impedance Control with Variable Tar-

get Stiffness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .34804

L.1.15 Understanding the Implementation of Impedance

Control in Industrial Robots . . . . . . . . . . . . . . . . . . . . . .34811

L.1.16 Whole-body impedance control of wheeled mobile

manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .34818

L.1.17 Accuracy/Robustness Dilemma in Impedance Control . . . . . .35012

L.2 Stability issues in force control . . . . . . . . . . . . . . . . . . . . . . . .35023

L.2.1 Stability and Performance Limits of Interaction Controllers . .35023

L.2.2 Impedance control stability properties in common

implementations . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35032

L.2.3 Contact stability issues in position based impedance

control: theory and experiments . . . . . . . . . . . . . . . . . . .35039

L.2.4 Modelling robot dynamic performance for end point

control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35046

L.3 Benchmarking Robot Force Control Capabilities: Experi-

mental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35186

L.4 Force Sensors in Robotics Research . . . . . . . . . . . . . . . . . . . . .35202

L.5 Robot Collision Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . .35238

L.5.1 Towards Safe Robots: Approaching Asimov’s 1st Law . . . . .35238

84

TABLE OF CONTENTS

L.5.2 A Control Architecture for Human-Robot Collaboration . . . .35612

L.5.3 Robot Collisions: A Survey on Detection, Isolation,

and Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35653

L.5.4 Sensorless Robot Collision Detection and Hybrid

Force/Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . .35675

L.6 Compliance and control . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35682

L.7 Robot Hand-Arm Co-operated Motion Planning . . . . . . . . . . . . . .35696

L.8 OpenPHRI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35911

L.8.1 In Pursuit of Safety: An Open-Source Library for

Physical Human-Robot Interaction . . . . . . . . . . . . . . . . . .35911

L.8.2 An introduction to OpenPHRI . . . . . . . . . . . . . . . . . . . . .35924

L.8.3 Installation of the OpenPHRI . . . . . . . . . . . . . . . . . . . . .35934

L.9 Tutorial/survey of robot force control . . . . . . . . . . . . . . . . . . . .35936

L.9.1 A Survey of Robot Interaction Control Schemes with

Experimental Comparison . . . . . . . . . . . . . . . . . . . . . . .35936

L.9.2 Force control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .35950

L.9.2.1 Chapter 7. Force control . . . . . . . . . . . . . . . . . .35950

L.9.2.2 Chapter 9. Force control . . . . . . . . . . . . . . . . . .35976

L.9.3 Best Practices and Performance Metrics Using Force

Control for Robotic Assembly . . . . . . . . . . . . . . . . . . . . .36002

L.9.4 Implicit Force Control of a Position Controlled Robot

– A Comparison with Explicit Algorithms . . . . . . . . . . . . .36020

L.9.5 Robot Manipulator Control and Computational Cost . . . . . .36028

L.9.6 Robot force control . . . . . . . . . . . . . . . . . . . . . . . . . . .36068

L.9.7 An overview of robot force control . . . . . . . . . . . . . . . . .36262

L.9.8 Impedance and Interaction Control . . . . . . . . . . . . . . . . .36273

L.9.9 Control of Robotic Systems in Contact Tasks . . . . . . . . . . .36298

L.9.10 Active compliant motion: a survey . . . . . . . . . . . . . . . . .36351

L.9.11 Historical Perspective and State of the Art in Robot

Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .36373

L.9.12 Active Impedance Control of Bioinspired Motion

Robotic Manipulators: An Overview . . . . . . . . . . . . . . . . .36386

L.9.13 A review of algorithms for compliant control of

stiff and fixed-compliance robots . . . . . . . . . . . . . . . . . . .36407

85

TABLE OF CONTENTS

L.10 Control Strategies for Soft Robotic Manipulators: A Survey . . . . . .36447

L.11 Sensorless force control . . . . . . . . . . . . . . . . . . . . . . . . . . . . .36463

L.11.1 Sensorless force control for ABB robots . . . . . . . . . . . . . .36463

L.11.1.1 Motor-Current-Based Estimation of Carte-

sian Contact Forces and Torques for Robotic

Manipulators and Its Application to Force

Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . .36463

L.11.1.2 Cartesian Contact Force and Torque Esti-

mation for Redundant Manipulators . . . . . . . . . . .36472

L.11.1.3 Cartesian contact force estimation for robotic

manipulators using Kalman filters and the

generalized momentum . . . . . . . . . . . . . . . . . . .36484

L.11.1.4 MPC-based Admittance Control for Robotic

Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . .36491

L.11.1.5 Force controlled robotic assembly without

a force sensor . . . . . . . . . . . . . . . . . . . . . . . . .36499

L.11.1.6 On Robotic Assembly using Contact Force

Control and Estimation . . . . . . . . . . . . . . . . . . .36506

L.11.1.7 On Robotic Work-Space Sensing and Control . . . . .36514

L.11.1.8 On Robotic Assembly using Contact Force

Control and Estimation . . . . . . . . . . . . . . . . . . .36727

L.11.1.9 Sensorless Kinesthetic Teaching of Robotic

Manipulators Assisted by an Observer-based

Force Control . . . . . . . . . . . . . . . . . . . . . . . . .36953

L.11.1.10 ABB’s patents on force control . . . . . . . . . . . . . .37048

L.11.1.11 Sensorless Null-Space Admittance Control

for Redundant Manipulators . . . . . . . . . . . . . . . .37066

L.11.2 Sensorless force control for Tohiba robots . . . . . . . . . . . . .37074

L.11.2.1 Tohiba’s US patents on force control . . . . . . . . . .37074

L.11.2.2 Tohiba’s sensorless compliance control . . . . . . . . .37190

L.11.3 Sensorless and adaptive admittance control of in-

dustrial robot in physical human-robot interaction . . . . . . .37193

L.11.4 Force Estimation in Robotic Manipulators: Model-

ing, Simulation and Experiments . . . . . . . . . . . . . . . . . . .37220

86

TABLE OF CONTENTS

L.12 Operational Space Control . . . . . . . . . . . . . . . . . . . . . . . . . . .37355

L.12.1 A Unified Approach for Motion and Force Control

of Robot Manipulators: The Operational Space Formulation .37355

L.12.2 Operational Space Control: A Theoretical and Em-

pirical Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . .37367

L.13 Compliance and Force Control for Computer Controlled Manipulators37389

L.14 force sensing and control in industrial robot systems . . . . . . . . . .37475

L.14.1 Adept Intelligent Force Sensing System User’s Guide . . . . . .37475

L.14.2 KUKA Force Torque Control 3.1 . . . . . . . . . . . . . . . . . . .37548

L.14.3 Mitsubishi CR750/CR751 Controller Instruction Man-

ual: Force Sense Function . . . . . . . . . . . . . . . . . . . . . . .37638

M Program Execution in a Robot Controller 37825

N Programming and control of multiple industrial robots 37827

N.1 Multi-RCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .37827

N.1.1 Multi-RCCL User’s Guide . . . . . . . . . . . . . . . . . . . . . . . .37827

N.1.2 Multi-RCCL Reference Manual . . . . . . . . . . . . . . . . . . . .38039

N.1.3 RCI User’s Guide . . . . . . . . . . . . . . . . . . . . . . . . . . . . .38193

N.1.4 RCI Reference Manual . . . . . . . . . . . . . . . . . . . . . . . . .38274

N.1.5 RCCL/RCI Hardware Installation Notes . . . . . . . . . . . . . . .38472

N.1.6 A multithreaded implementation of a robot con-

trol C library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .38526

N.1.7 Data Stuructures of the RCCL Version 5.0 . . . . . . . . . . . . .38600

N.1.8 Data Stuructures of the RCCL Version 5.1.3 . . . . . . . . . . . .48822

N.1.9 The research and development of the general-purpose

robot control environment based on UNIX/C . . . . . . . . . . .56362

N.1.10 Development of a Real-time Control System for Fara

Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .56432

N.1.11 Implementation of RCCL on Universal Controller

and Controlling The Multi-robot . . . . . . . . . . . . . . . . . . .56510

N.1.12 RCCL-based Robot Controller and Simulator . . . . . . . . . . .56573

N.1.13 RCCL for Mitsubishi PA-10 Robots . . . . . . . . . . . . . . . . . .56625

N.1.14 RCCL 5.1.4 HowTo . . . . . . . . . . . . . . . . . . . . . . . . . . . .57012

N.2 Dual arm manipulation–A survey . . . . . . . . . . . . . . . . . . . . . . .57019

87

TABLE OF CONTENTS

N.3 Experiments in force control of robotic manipulators . . . . . . . . . .57034

N.4 Off-Line Programming Techniques for Multirobot Coop-

eration System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57043

N.5 Multi-Robot Systems: A classification focused on coordination . . . .57061

N.6 Constrained relations between two coordinated industrial

robots for motion control . . . . . . . . . . . . . . . . . . . . . . . . . . . .57076

N.7 A work-piece based approach for programming cooper-

ating industrial robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57094

N.8 Two-arm Manipulation: From Manipulators to Enhanced

Human-Robot Collaboration . . . . . . . . . . . . . . . . . . . . . . . . . .57271

N.9 Kinematics Programming for Two Cooperating Robots Per-

forming Tasks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57426

N.10 MRROC++ robot programming framework . . . . . . . . . . . . . . . .57437

O Overview of robot controller implementations 57621

O.1 Open Robotic Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . .57621

O.2 Universal RT-middleware robot controller . . . . . . . . . . . . . . . . .57628

O.3 A generic controller architecture for intelligent robotic systems . . .57635

O.4 Open Controller Architecture - Past, Present and Future . . . . . . . .57650

O.5 An FPGA-Based Open Architecture Industrial Robot Controller . . . .57659

O.6 The Structured Design of an Industrial Robot Controller . . . . . . . .57671

O.7 Development of an Industrial Robot Controller with Open

Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57683

O.8 Retrofitting of the IRB6-S2 robotic manipulator using Com-

puter Numerical Control-based controllers . . . . . . . . . . . . . . . . .57688

P Implementation of a Main Controller 57711

P.1 Implementation of a FPGA + USB board . . . . . . . . . . . . . . . . . .57711

P.2 Xilinx FPGA boards with Microblaze processors . . . . . . . . . . . . .57724

P.2.1 Saturn Spartan-6 FPGA module . . . . . . . . . . . . . . . . . . .57725

P.2.1.1 Creation of an EDK test project . . . . . . . . . . . . . .57726

P.2.1.2 Running Linux on Saturn Spartan-6 FPGA

module . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57737

P.2.2 ELS SB1600 board . . . . . . . . . . . . . . . . . . . . . . . . . . . .57774

P.2.2.1 Bulding and running a “hello world” application . . .57775

88

TABLE OF CONTENTS

P.2.2.2 Bulding and running a “lwip echo server” application57781

P.3 Altera FPGA boards with NIOS-II processors . . . . . . . . . . . . . . .57784

P.3.1 Bulding and running a “Hello World” application . . . . . . . .57784

P.3.2 Bulding and running a web server application . . . . . . . . . .57865

P.4 PCI/PCI Express board design . . . . . . . . . . . . . . . . . . . . . . . . .57874

P.4.1 Schematics of the Enterpoint Raggedstone 1 board . . . . . . .57874

P.4.2 Schematics of the Altera MAX II development board . . . . . .57886

P.4.3 PCI reference design of the Altera MAX II devel-

opment kit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .57892

P.4.3.1 Installation and test-run of the PCI refer-

ence design . . . . . . . . . . . . . . . . . . . . . . . . . .57892

P.4.3.2 Building the PCI reference design . . . . . . . . . . . .57914

P.4.4 PCI board design tips . . . . . . . . . . . . . . . . . . . . . . . . . .57925

P.4.4.1 Open-drain output signals: INTB and SERR . . . . . .57925

P.4.4.2 The PCI slave to WB master IP-core (pci_to_wb)

and its modification . . . . . . . . . . . . . . . . . . . . .57927

P.4.4.3 Sending signals from the kernel space to

the user space . . . . . . . . . . . . . . . . . . . . . . . . .57964

P.4.5 Device drivers of Hilscher PCI CC-Link slave boards . . . . . .57970

P.4.5.1 Kernel-space device driver . . . . . . . . . . . . . . . . .57970

P.4.5.2 UIO device driver . . . . . . . . . . . . . . . . . . . . . . .58293

P.4.5.3 RTAI/RTDM device driver . . . . . . . . . . . . . . . . .58329

P.4.6 Schematics of the Lattice Semiconductor ECP5 Versa

development development board . . . . . . . . . . . . . . . . . . .58695

P.5 Linux documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .58706

P.5.1 Protecting the Root Filesystem on Ubuntu with Overlayroot .58706

P.5.2 Creation of a bootable Compact Flash . . . . . . . . . . . . . . .58711

P.5.3 aufs root filesystem on a USB flash . . . . . . . . . . . . . . . . .58731

P.5.4 Minimal root filesystem for Debian or Ubuntu . . . . . . . . . .58743

P.5.4.1 Ubuntu Base . . . . . . . . . . . . . . . . . . . . . . . . . .58743

P.5.4.2 Creating a minimal rootfs with debootstrap . . . . . .58749

P.5.5 Getting your system to boot with initrd and initramfs . . . . .58774

P.5.6 Understanding systemd . . . . . . . . . . . . . . . . . . . . . . . . .58781

P.5.7 SSD Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . .58812

89

TABLE OF CONTENTS

P.5.8 Debian Packaging Tutorial . . . . . . . . . . . . . . . . . . . . . . .58834

P.6 Embedded Linux system development . . . . . . . . . . . . . . . . . . . .58878

Q Implementation of a Servo Controller 59139

Q.1 Hardware schematics and source code of the TI’s refer-

ence design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .59139

Q.1.1 Schematics of the ezDSP F2812 board . . . . . . . . . . . . . . .59139

Q.1.2 Schematics of the DMC550 digital motion controller . . . . . .59146

Q.1.3 Schematics of the DMC1500 digital motion controller . . . . .59158

Q.1.4 PMSM3_4 System Document C2000 Foundation Software . . .59172

Q.1.5 Source code of the PMSM3-4 software module . . . . . . . . .59208

Q.2 Hardware schematics and source code of the Freescale’s

reference design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .59226

Q.2.1 Schematics of the TWR-56F8400 . . . . . . . . . . . . . . . . . . .59226

Q.2.2 Schematics of the TWR-MC-LV3PH . . . . . . . . . . . . . . . . .59235

Q.2.3 PMSM FOC of Industrial Drives using the 56F84789 . . . . . .59242

Q.2.4 Source code of the PMSM_FOC_ENCODER_MC56F84789_TWR_DEMO59257

Q.2.5 PMSM FOC of Industrial Drives using the 56F84789 . . . . . .59274

Q.3 EMI emissions of modern PWM AC drives . . . . . . . . . . . . . . . . .59331

Q.4 Multi-axis Servo and System Drives Comparison . . . . . . . . . . . . .59416

Q.5 Current limiting algorithms . . . . . . . . . . . . . . . . . . . . . . . . . .59502

Q.6 Servo parameter tuning example . . . . . . . . . . . . . . . . . . . . . . .59511

Q.7 Panasonic A4-series gain tuning guide . . . . . . . . . . . . . . . . . . .59536

Q.8 Servo gain tuning for vertical articulated robots . . . . . . . . . . . . .59571

Q.9 Motion system tuning, Allen-Bradley . . . . . . . . . . . . . . . . . . . .59577

Q.10 High-Precision Position Control of Permanent Magnet Syn-

chronous Motor for Semiconductor Equipments . . . . . . . . . . . . .59702

Q.11 Servo Gain Tuning Technique of a 6-Axes Articulated Robot

Manipulator Considering Dynamic Characteristics . . . . . . . . . . . .59710

Q.12 Automatic Control Loop Tuning for Permanent-Magnet AC

Servo Motor Drives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .59775

R Implementation of EtherCAT-based robot controllers 59785

R.1 Beckhoff documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .59785

R.1.1 Beckhoff I/O systems . . . . . . . . . . . . . . . . . . . . . . . . . .59785

90

TABLE OF CONTENTS

R.1.2 Datasheets of Beckhoff EK1100, EL1008, EL1018, EL2008 . .59793

R.2 EtherCAT realtime networking . . . . . . . . . . . . . . . . . . . . . . . . .59799

R.2.1 Realtime networking with RT-preempt kernel . . . . . . . . . . .59799

R.2.2 Multi-axis position control by EtherCAT real-time networking 59817

R.2.3 IgH EtherCAT Master for Linux . . . . . . . . . . . . . . . . . . . .59833

R.2.3.1 EtherLab’s Open-Source EtherCAT Master . . . . . . .59833

R.2.3.2 IgH EtherCAT Master 1.5.2 Documentation . . . . . .59847

R.2.3.3 Igh Ethercat Master, Xenomai, EPOS3 Tutorial . . . .59942

R.2.3.4 Patches for the IgH EtherCAT Master for Linux . . . .59979

R.2.3.4.1 ec-debianize . . . . . . . . . . . . . . . . . . . . .59979

R.2.3.4.2 Etherlab master patchset 20160804 . . . . . .59980

R.2.3.4.3 rtleader . . . . . . . . . . . . . . . . . . . . . . . .59983

R.2.3.4.4 Etherlab master patchset 20180622 . . . . . .59985

R.2.4 SOEM (Simple Open EtherCAT Master) . . . . . . . . . . . . . .59988

R.2.5 Implementation and Performance Evaluation of Preempt-

RT Based Multi-core Motion Controller for Indus-

trial Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .60360

R.2.6 EtherCAT Tutorial, an introduction for real-time hard-

ware communication on Windows . . . . . . . . . . . . . . . . . .60475

R.2.7 Accurate Synchronization of EtherCAT Systems Us-

ing Distributed Clocks . . . . . . . . . . . . . . . . . . . . . . . . . .60543

R.3 Panasonic documents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .60560

R.3.1 MINAS-A5BA1, A5B01 Series AC servo driver Ref-

erence Specifications, SX-DSV02471 . . . . . . . . . . . . . . . . .60560

R.3.2 MINAS-A5B series (EtherCAT communication/rota-

tion type) Functional Specifications, SX-DSV02472 . . . . . . .60648

R.3.3 MINAS-A5B series (EtherCAT communication/rota-

tion type) Communication Specifications, SX-DSV02473 . . . .60836

R.3.4 MINAS A5-series, AC Servo Motor & Driver, Oper-

ating Instraction (Overall) . . . . . . . . . . . . . . . . . . . . . . .61131

R.3.5 PANATERM Ver. 6.0 Operation Manual . . . . . . . . . . . . . . .61610

R.4 Motor circuit design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .61814

R.4.1 Introduction to Motor Circuit Design . . . . . . . . . . . . . . . .61814

R.4.2 Molded case circuit breakers, technical note . . . . . . . . . . .61895

91

TABLE OF CONTENTS

R.4.3 Metal oxide varistors, application note . . . . . . . . . . . . . . .62079

R.4.4 Transient Overvoltage Protection. technical note . . . . . . . . .62100

R.5 EtherCAT-based robot controllers . . . . . . . . . . . . . . . . . . . . . . .62142

R.5.1 EtherCAT Based Robot Modular Joint Controller . . . . . . . . .62142

R.5.2 Development of a Portable Welding Robot with Ether-

CAT Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .62149

R.5.3 EtherCAT-based Industrial Manipulator . . . . . . . . . . . . . . .62162

R.5.4 An EtherCAT-Based Real-Time Control System Ar-

chitecture for Humanoid Robots . . . . . . . . . . . . . . . . . . .62168

R.5.5 ROS-Industrial Focused Community Meeting: Kuka

LBR iiwa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .62177

R.5.6 Motor Controller Designed for Robotics Based on

Microcontroller with Integrated EtherCAT . . . . . . . . . . . . .62206

R.5.7 Motion Control of 6-DOF Manipulator Based on EtherCAT . .62213

S Computer Numerical Control (CNC) 62229

S.1 Little Tutorial for the use of Bosch Rexroth IndraDrives

with LinuxCNC over EtherCAT . . . . . . . . . . . . . . . . . . . . . . . .62229

S.2 Installation of Debian Machinekit images on Beaglebone

Black boards . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .62241

S.3 Reference-Pulse Circular Interpolators for CNC Systems . . . . . . . .62256

S.4 Machinekit/Debian Jessie on Thinkpad X200 . . . . . . . . . . . . . . .62263

S.5 3D 6-DOF Serial Arm Robot - Kinematics and Implemen-

tation in LinuxCNC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .62282

T Motion Control 62321

T.1 Pule-type motion control . . . . . . . . . . . . . . . . . . . . . . . . . . . .62321

T.1.1 Encoder interface circuits of the NPM PCL6045B . . . . . . . .62321

T.1.2 Programming examples of the NPM PCL6045B . . . . . . . . . .62323

T.1.3 PPCI7443 (PCi-8164) Motion Control Card User’s Manual . .62347

T.1.4 Panasonic AC Servo Motor and Driver MINAS A4

Series Instruction manual . . . . . . . . . . . . . . . . . . . . . . .62514

T.1.5 Datasheet of Avago HCTL-20XX Quadrature Decoder/-

Counter Interface ICs . . . . . . . . . . . . . . . . . . . . . . . . . .62879

T.2 FPGA programming with Xilinx ISE . . . . . . . . . . . . . . . . . . . . .62896

92

TABLE OF CONTENTS

T.3 Ethercat-based motion control . . . . . . . . . . . . . . . . . . . . . . . . .62964

T.3.1 CiA 402: CANopen device profile for drives and

motion control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .62964

T.3.2 EtherCAT Slave Controllers, Section II – Register Description .63164

T.3.3 EtherCAT Slave Implementation Guide . . . . . . . . . . . . . . .63263

T.3.4 Kollmorgen AKD EtherCat Communication Manual . . . . . . .63323

T.3.5 Kollmorgen AKD CAN-BUS Communication Manual . . . . . . .63484

T.3.6 Yaskawa EthereCAT (CoE) Network Module Manual . . . . . .63661

T.3.7 EtherCAT Master Architecture, Yaskawa . . . . . . . . . . . . . .63871

T.4 Stepper motor control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .63890

T.4.1 Generate stepper-motor speed profiles in real time . . . . . . .63890

T.4.2 AVR446: Linear speed control of stepper motor . . . . . . . . .63896

93

List of Figures

1.1 Block diagram of a simplified robot controller. . . . . . . . . . . . . . . . 25

1.2 Robot controller structure (hardware platform). . . . . . . . . . . . . . . . 30

1.3 Structure of a single-axis servo drive. . . . . . . . . . . . . . . . . . . . . . 31

1.4 Type-1 Servo Controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

1.5 Type-2 Servo Controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

1.6 Type-3 Servo Controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

1.7 An example of robot controller structures. . . . . . . . . . . . . . . . . . . 35

2.1 Components of the Mark I PUMA system. . . . . . . . . . . . . . . . . . . 45

2.2 Mark I controller’s peripherals. . . . . . . . . . . . . . . . . . . . . . . . . . 45

2.3 Internals of the MK I controllers. . . . . . . . . . . . . . . . . . . . . . . . . 46

2.4 “Individual joint control” implemented in the MK I system. . . . . . . . 47

2.5 Position and current sensors of the MK I system. . . . . . . . . . . . . . 48

2.6 Structure of the Digital servo board. . . . . . . . . . . . . . . . . . . . . . . 48

2.7 Structure of the Analog servo board. . . . . . . . . . . . . . . . . . . . . . 49

2.8 Position, velocity and current loops of the Mark I servo system. . . . . 50

2.9 Block diagram of the Mark I servo system. . . . . . . . . . . . . . . . . . 50

2.10 Components of the Mark III PUMA system. . . . . . . . . . . . . . . . . . 53

2.11 Internals of the Mark III controller. . . . . . . . . . . . . . . . . . . . . . . 53

2.12 Control card set of the Mark III controller. . . . . . . . . . . . . . . . . . . 54

2.13 Power card set of the Mark III controller. . . . . . . . . . . . . . . . . . . 55

2.14 Block diagram of the Mark III controller. . . . . . . . . . . . . . . . . . . . 56

2.15 Electronic flow of Mark IIII system. . . . . . . . . . . . . . . . . . . . . . . 56

2.16 Comparison between Mark I and Mark III controllers. . . . . . . . . . . 58

2.17 Mark III digital servo board code. . . . . . . . . . . . . . . . . . . . . . . . 58

2.18 Comparion between PUMA 560 and PUMA 562 robots. . . . . . . . . . 59

2.19 Block diagram of the Unival controller. . . . . . . . . . . . . . . . . . . . . 59

94

LIST OF FIGURES

2.20 Analog current control implemented in the Mark III controller. . . . . . 60

2.21 Digital current control implemented in the Unival controller. . . . . . . 60

2.22 Position and current loops of the Unival controller. . . . . . . . . . . . . 61

2.23 Current controller and power amplifiers of the Unival servo system. . 61

2.24 Block diagram of the Unival servo system. . . . . . . . . . . . . . . . . . . 62

2.25 Family tree of the robot programming languages. . . . . . . . . . . . . . 62

2.26 Information flow in the VAL programming system. . . . . . . . . . . . . . 63

2.27 Six rotary joints of a PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . 64

2.28 Example of setting coordinate frames W, B, and T and trans-

forms BASE, T6, and TOOL. . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

2.29 World coordinate system of the PUMA 560 robot. . . . . . . . . . . . . . 66

2.30 Tool coordinate system of the PUMA 560 robot with the

null tool. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

2.31 Dimensions of the PUMA 560 robot with the null tool. . . . . . . . . . . 69

2.32 Tool coordinate system and [n s a p]. . . . . . . . . . . . . . . . . . . . . . 69

2.33 Right-handed coordinate system. . . . . . . . . . . . . . . . . . . . . . . . . 70

2.34 Definition of OAT angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

2.35 Initial alignment of the tool coordinate system. . . . . . . . . . . . . . . . 72

2.36 Pointer table and linked-lists. . . . . . . . . . . . . . . . . . . . . . . . . . . 87

2.37 A simple VAL program and its location data. . . . . . . . . . . . . . . . . 89

2.38 Pointer table stored in the memory. . . . . . . . . . . . . . . . . . . . . . . 90

2.39 Program and data stored in the memory. . . . . . . . . . . . . . . . . . . . 91

2.40 Family tree of the RCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93

2.41 Hardware configuration of the RCCL system. . . . . . . . . . . . . . . . . 94

2.42 Serial and parallel (FIFO) links to the host computer. . . . . . . . . . . 95

2.43 Software structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 95

2.44 RCCL routines. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

2.45 Execution of real-time tasks by an interrupt handler. . . . . . . . . . . . 98

2.46 Excution cycle of real-time tasks in a RCCL v1.0 system. . . . . . . . . 99

2.47 Real-time communication between the LSI-11 CPU and the

host system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

2.48 Task positions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

2.49 Task frames P, H and Z. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

2.50 Standard RCCL/RCI system. . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

95

LIST OF FIGURES

2.51 CPUs and their tasks in a Multi-RCCL system. . . . . . . . . . . . . . . . 107

2.52 Software structure of Multi-RCCL. . . . . . . . . . . . . . . . . . . . . . . . 108

2.53 MultiRCCL implemented on a µVAX-II with two CPUs. . . . . . . . . . . 109

2.54 Ring structures for spatial relationships. . . . . . . . . . . . . . . . . . . . 118

2.55 Robosim: the graphic simulator of Multi-RCCL. . . . . . . . . . . . . . . . 119

2.56 Robotsim (Robot + TP simulator) of the Multi-RCCL. . . . . . . . . . . . 120

2.57 Block diagram of the RCCL stystem. . . . . . . . . . . . . . . . . . . . . . . 120

2.58 Four levels of a robot programming environment. . . . . . . . . . . . . . 122

2.59 Hardware configuration of the Level-II system. . . . . . . . . . . . . . . . 123

2.60 Kali, a goddess in Hindu mythology. . . . . . . . . . . . . . . . . . . . . . . 124

2.61 Directory tree of the Kali software. . . . . . . . . . . . . . . . . . . . . . . . 126

2.62 “src” directory of the Kali software. . . . . . . . . . . . . . . . . . . . . . . 126

2.63 Hardware configuration of the Kali system. . . . . . . . . . . . . . . . . . 127

2.64 Processes mapped to CPUs of the Kali system. . . . . . . . . . . . . . . . 127

2.65 Graphic simulator of the Kali system (solid-filled model). . . . . . . . . 128

2.66 Graphic simulator of the Kali system (wire-framed model). . . . . . . . 129

2.67 Specification comparison between the ALVIN system and

other systems. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130

2.68 ALVIN system based on the vxworks. . . . . . . . . . . . . . . . . . . . . . 130

2.69 ALVIN system based on the QNX. . . . . . . . . . . . . . . . . . . . . . . . . 131

2.70 Software architecture of the ALVIN system. . . . . . . . . . . . . . . . . . 132

2.71 RWRCCL system with TRC boards. . . . . . . . . . . . . . . . . . . . . . . . 133

2.72 Software architecture of the RWRCCL system. . . . . . . . . . . . . . . . . 134

2.73 Detailed software architecture of the RWRCCL system. . . . . . . . . . . 135

2.74 QRobot system with the MultiQ board. . . . . . . . . . . . . . . . . . . . . 137

2.75 Configuration of the ARCL system. . . . . . . . . . . . . . . . . . . . . . . . 139

3.1 Rotation and translation of a local frame with respect to a

global frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146

3.2 Representation of a point P in coordinate frames B and G. . . . . . . . 146

3.3 A rigid body with an attached coordinate frame B(oxyz)

moving freely in a global coordinate frame G(OXYZ). . . . . . . . . . . . 152

3.4 Position vectors GrP , GdB, and GB rP . . . . . . . . . . . . . . . . . . . . . . . 153

3.5 Geometric interpretation of a rigid body velocity. . . . . . . . . . . . . . . 154

96

LIST OF FIGURES

3.6 Screw motions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169

3.7 General screw motion (with nonzero rotation). . . . . . . . . . . . . . . . 170

3.8 Numbering the links and joints of a robot manipulator. . . . . . . . . . 173

3.9 Forward and inverse kinematics of a manipulator. . . . . . . . . . . . . . 174

3.10 Definition of link parameters for a rotational joint (Paul’s method). . . 175

3.11 Dimensions of the PUMA 560 robot (in inches). . . . . . . . . . . . . . . 176

3.12 The coordinate frames of the PUMA 560 in the zero position (Paul81). 177

3.13 The coordinate frames of the PUMA 560 arm with a hand (Fijany88). 178

3.14 Zero position setting for the “righty” PUMA 560 robot (Paul86). . . . . 180

3.15 Ready position of the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . 181

3.16 Definition of link parameters for a rotational joint (Craig’s method). . 182

3.17 The coordinate frames of the PUMA 560 in the zero-angles

position (Craig86). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184

3.18 The 4-th, 5-th and 6-th coordinate frames of the PUMA

560 arm (Craig86). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185

3.19 Zero-angles pose of the PUMA 560 arm in RCCL v1.0. . . . . . . . . . . 187

3.20 Ready position of the PUMA 560 arm. . . . . . . . . . . . . . . . . . . . . 188

3.21 Solutions of θ1 using the geometric method (Lee84). . . . . . . . . . . . 199

3.22 Left/right and up/down configurations of the PUMA 560 arm . . . . . 208

3.23 Left and right configurations of the Adept Viper 560 robot . . . . . . . 210

3.24 Above and below configurations of the Adept Viper 560 robot . . . . . 210

3.25 Flip and no-flip configurations of the Adept Viper 560 robot . . . . . . 211

3.26 A SCARA-type robot with its coordinate frames shown. . . . . . . . . . . 221

3.27 2 DOF planar robot with rotational joints . . . . . . . . . . . . . . . . . . 222

3.28 Geometrical interpretation of the cos(α− θ1) if θ2 > 0. . . . . . . . . 224

3.29 Geometrical interpretation of the cos(θ1 −α) if θ2 < 0. . . . . . . . . . 224

3.30 Coordinate frames of a 2-DOF planar robot. . . . . . . . . . . . . . . . . . 227

3.31 Geometric interpretation of the column vectors J1, J2 of

the Jacobian matrix J . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231

3.32 Singular configurations of the 2-DOF planar robot. . . . . . . . . . . . . 232

3.33 Singular configurations of the 2-DOF planar robot with θ2

= 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235

3.34 Singular configurations of the 2-DOF planar robot with θ2

= π. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235

97

LIST OF FIGURES

3.35 Trajectory tracking in the singularity regions . . . . . . . . . . . . . . . . 237

3.36 Joint velocities profiles for tracking the trajectory in Fig. 3.35. . . . . . 238

3.37 End-effector trajectory of the 2-DOF planar robot. . . . . . . . . . . . . . 243

3.38 Cartesian velocities (vx , vy) for tracking the trajectory shown

in Figure 3.37 with the constant speed of 1.6m/s. . . . . . . . . . . . . . 243

3.39 Joint angles (θ1,θ2) for tracking the trajectory shown in Figure 3.37. 244

3.40 Joint velocities (.θ1,

.θ2) calculated from joint angles. . . . . . . . . . . . 245

3.41 Joint velocities (.θ1,

.θ2) calculated from Cartesian velocities

using Jacobian J . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245

3.42 Joint velocities (.θ1,

.θ2) with inverse kinematics (IK) and

Jacobian J , respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246

3.43 A simple robot controller consisting of an IK solver and

joint position servo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246

3.44 A simple robot controller with a rate limiter. . . . . . . . . . . . . . . . . 247

3.45 Effect of the rate limiter in tracking the Cartesian path. . . . . . . . . . 252

3.46 Joint velocities in tracking the Cartesian path. . . . . . . . . . . . . . . . 252

3.47 Cartesian path specified by 220 via points. . . . . . . . . . . . . . . . . . . 257

3.48 Joint velocities calculated by the trajectory time-scaling method. . . . . 262

3.49 Traveling times calculated by the trajectory time-scaling method. . . . 262

3.50 Closed-loop resolved motion rate control. . . . . . . . . . . . . . . . . . . 263

3.51 Eigenvalues of the Jacobian matrix J while tracking the

Cartesian path specified in Figure 3.47. . . . . . . . . . . . . . . . . . . . . 264

3.52 Joint velocities.θ1,

.θ2 while tracking the Cartesian path

specified in Figure 3.47. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265

3.53 Eigenvalues used for calculating J−1. . . . . . . . . . . . . . . . . . . . . . 271

3.54 Joint velocities calculated by clrmrc-svd.py. . . . . . . . . . . . . . . . . . 271

3.55 Cartesian path calculated by clrmrc-svd.py. . . . . . . . . . . . . . . . . . . 272

3.56 Eigenvalues of 2J and |det(2J)| while tracking the Carte-

sian path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274

3.57 An example of type 1 singularity, on a 2-DOF planar robot. . . . . . . 279

3.58 End-effector trajectory passing through Type I singularity region. . . . 283

3.59 Cartesian path calculated by collapsed-jacobian.py. . . . . . . . . . . . . . 289

3.60 Joint angles calculated by collapsed-jacobian.py. . . . . . . . . . . . . . . 289

3.61 Joint velocities calculated by collapsed-jacobian.py. . . . . . . . . . . . . 290

98

LIST OF FIGURES

3.62 Cartesian path calculated by similarity-avoidance.py. . . . . . . . . . . . . 298

3.63 Joint velocities calculated by similarity-avoidance.py. . . . . . . . . . . . 298

3.64 Joint velocities calculated by similarity-avoidance.py with

the singularity avoidance scheme disabled. . . . . . . . . . . . . . . . . . . 299

3.65 Realationship between wrist and tool points. . . . . . . . . . . . . . . . . 307

3.66 Singular direction at the shoulder singularity of the PUMA 560. . . . . 312

3.67 Singular direction at the elbow singularity of the PUMA 560. . . . . . 312

3.68 Singular direction at the wrist singularity of the PUMA 560. . . . . . . 313

3.69 Jacobian-based inverse kinematics for calculating desired

joint angles qd . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314

3.70 Desired end-effector position xd(t) along a straight line. . . . . . . . . . 315

3.71 Desired joint angles qd(t) calculated by solving inverse kinematics. . . 319

3.72 Desired joint velocities.qd(t) calculated from joint angles

qd(t) and qd(t −∆t). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320

3.73 Desired joint angles qd(t) calculated using the Jacobian-

based inverse kinematics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320

3.74 Comparison of the desired joint angles which are calcu-

lated using inverse kinematics and the Jacobian-based in-

verse kinematics, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . 321

3.75 Inverse kinematics algorithm with Jacobian inverse. . . . . . . . . . . . . 322

3.76 Joint angles calculated by the Jacobian inverse kinematics

algorithm with ∆t = 10ms and K = (1,1). . . . . . . . . . . . . . . . . . 328

3.77 Joint angles calculated by the Jacobian inverse kinematics

algorithm with ∆t = 1ms and K = (1, 1). . . . . . . . . . . . . . . . . . . 329

3.78 Joint angles calculated by the Jacobian inverse kinematics

algorithm with ∆t = 1ms and K = (100, 100). . . . . . . . . . . . . . . . 329

3.79 Joint angles calculated by the Jacobian inverse kinematics

algorithm with ∆t = 1ms and K = (2005, 2005). . . . . . . . . . . . . . 330

3.80 Mapping between the joint velocity space and the end-effector

velocity space. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 332

3.81 Geometric view of Eq. (3.607):.q = J†ve +

�

In − J†J� .

q0 . . . . . . . . . 334

3.82 Manipulator ellipsoid and manipulability measure of a two-

link planar robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 338

3.83 Manipulator motion in the presence of some obstacles. . . . . . . . . . . 339

99

LIST OF FIGURES

3.84 End-effector trajectory generated by the closed-loop DLS

algorithm with ∆t = 1ms, K = (1000,1000), and λ = 0,1,5,10 . . . . 351

3.85 Revolute and prismatic joints. . . . . . . . . . . . . . . . . . . . . . . . . . . 352

3.86 A SCARA robot at its zero pose. . . . . . . . . . . . . . . . . . . . . . . . . 355

3.87 A SCARA robot at its zero pose, with base frame coincident

with tool frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 359

3.88 A 6R elbow robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361

3.89 AdeptThree robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 381

3.90 Link coordinates frames of the AdeptThree robot. . . . . . . . . . . . . . 382

3.91 Representation of a point P in coordinate frames B and G. . . . . . . . 415

3.92 Representation of a line with two vectors a and d. . . . . . . . . . . . . 428

3.93 Plücker coordinates (l, m) of a line. . . . . . . . . . . . . . . . . . . . . . . 429

3.94 The geometry of a screw motion. . . . . . . . . . . . . . . . . . . . . . . . . 433

3.95 A general screw motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 438

3.96 A SCARA robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 439

3.97 Top view of the SCARA robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 446

3.98 Kinematic singularity of the SCARA robot at θ2 = 0. . . . . . . . . . . . 449

3.99 Kinematic singularity of the SCARA robot at θ2 = π. . . . . . . . . . . . 449

3.100 LEFT/RIGHT configurations of the SCARA robot. . . . . . . . . . . . . . . 450

3.101 Link coordinate frames of the PUMA 560 in the zero pose (Paul81). . 468

3.102 Result of x.plot(). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 481

3.103 A SCARA robot with coordinate frames shown. . . . . . . . . . . . . . . . 482

3.104 ScorBot ER-4u robot with 5 degrees of freedom. . . . . . . . . . . . . . . 492

3.105 Comau SMART SiX robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494

3.106 Robot pose displayed by comau_kinematic_control.m. . . . . . . . . . . . 511

3.107 EPSON E2L SCARA Robots. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 512

3.108 A SCARA robot at its zero pose. . . . . . . . . . . . . . . . . . . . . . . . . 586

3.109 Arm types of the wafer transfer robots (Brooks Automation

의 MagnaTran series) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 589

3.110 A SCARA robot with its coordinates. . . . . . . . . . . . . . . . . . . . . . . 590

3.111 A SCARA-type wafer transfer robot. . . . . . . . . . . . . . . . . . . . . . . 591

3.112 Coordinate setting of the SCARA-type wafer transfer robot. . . . . . . . 592

3.113 A working environment of the SCARA-type wafer transfer robot. . . . 593

3.114 Belt-driven cylinderical coordinate robot. . . . . . . . . . . . . . . . . . . . 594

100

LIST OF FIGURES

3.115 Typical working environment of the 3-axis cylinderical co-

ordinate WTR. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 595

3.116 Singular direction of the 3-axis cylinderical coordinate WTR

(θ3 = 0). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 596

3.117 Singular direction of the 3-axis cylinderical coordinate WTR

(θ3 = π). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 597

3.118 Infeasible move segment of the 3-axis cylinderical coordi-

nate WTR. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 597

3.119 Configuration change for avoiding the kinematic sinularity. . . . . . . . 599

3.120 Motion planning for the 3-axis cylinderical coordinate WTR. . . . . . . 600

3.121 4-axis horizontal and multi-joint type WTR. . . . . . . . . . . . . . . . . . 601

3.122 Adept Viper s650 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 602

3.123 Link coordinate frames of the Adept Viper s650 robot. . . . . . . . . . . 603

3.124 Zero pose of the Adept Viper s650 robot. . . . . . . . . . . . . . . . . . . 607

3.125 Zero and ready poses of the Viper s650 robot. . . . . . . . . . . . . . . . 608

3.126 A typical serial robot shown with its OPW parameters. . . . . . . . . . . 619

3.127 Side view of the serial robot (elbow-down configuration). . . . . . . . . 621

3.128 Top view of the serial robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . 623

3.129 Top view of the serial robot in shoulder-front configuration. . . . . . . 623

3.130 Top view of the serial robot in shoulder-back configuration. . . . . . . 624

3.131 Zero pose of a serial robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . 627

3.132 LEFTY/RIGHTY configurations. . . . . . . . . . . . . . . . . . . . . . . . . . 630

3.133 Determination of the LEFTY/RIGHTY configurations. . . . . . . . . . . . 631

3.134 ABOVE/BELOW configurations. . . . . . . . . . . . . . . . . . . . . . . . . . 631

3.135 Determination of the ABOVE/BELOW configurations. . . . . . . . . . . . 632

3.136 FLIP/NOFLIP configurations. . . . . . . . . . . . . . . . . . . . . . . . . . . . 633

3.137 Determination of the FLIP/NOFLIP configurations. . . . . . . . . . . . . . 633

4.1 Linux PC for robot programming and control. . . . . . . . . . . . . . . . . 685

4.2 C2SBC-Q board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 688

4.3 VIA PC-2500E board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 688

4.4 Adlink PCI 7432 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 692

4.5 Viscom 400H UPCI board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 694

4.6 PICMG 1.0 full-size SBC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 702

101

LIST OF FIGURES

4.7 PICMG 1.0 backplane. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 702

4.8 PCI motion control card. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 703

4.9 Linux real-time extensions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 728

4.10 Layering in the Xenomai. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 729

4.11 Layering in the RTAI. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 729

4.12 Xenomai architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 730

4.13 Beagleboard-xM. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 743

4.14 Beaglebone Black. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 752

4.15 Specification of the Beaglebone Black board. . . . . . . . . . . . . . . . . 753

4.16 task execution in non-preemptive kernels. . . . . . . . . . . . . . . . . . . 809

4.17 task execution in preemptive kernels. . . . . . . . . . . . . . . . . . . . . . 810

4.18 Dual kernel approach to RTOS. . . . . . . . . . . . . . . . . . . . . . . . . . 811

4.19 Native kernel approach to RTOS. . . . . . . . . . . . . . . . . . . . . . . . . 811

4.20 GIGABYTE GA-N3150N-D3V Mini-ITX motherboard. . . . . . . . . . . . . 836

4.21 Writing the Xubuntu 16.04 ISO image to a USB memory

with Rufus. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 837

4.22 Supermicro C2SBC-Q motherboard. . . . . . . . . . . . . . . . . . . . . . . 838

4.23 Configurating kernel 4.8.11-rt7. . . . . . . . . . . . . . . . . . . . . . . . . . 841

4.24 Gkrellm window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 844

4.25 USB to RS232C converter with a FTDI’s FT232R USB UART

IC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 873

4.26 Overview of Linux USB subsystem. . . . . . . . . . . . . . . . . . . . . . . . 873

4.27 Layers of Linux serial subsystem. . . . . . . . . . . . . . . . . . . . . . . . . 874

4.28 Data flow and function calls in accessing a TTY device. . . . . . . . . . 875

4.29 TTY layers hooked to a USB-serial driver. . . . . . . . . . . . . . . . . . . 875

4.30 USB commication wiith a USB-to-UART bridge. . . . . . . . . . . . . . . . 889

4.31 Hardware setup of a FTDI serial converter for loopback test. . . . . . . 938

4.32 Screenshot of the usbview window. . . . . . . . . . . . . . . . . . . . . . . 1028

4.33 USB commication wiith a USB-to-FIFO bridge. . . . . . . . . . . . . . . . 1099

4.34 Block diagram of the FT2232H usb-to-FIFO bridge. . . . . . . . . . . . . 1099

4.35 Flashlink board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1100

4.36 Saturn FPGA board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1142

4.37 A single-robot controller composed of a trajectory-generation

processor and multiple servo processors. . . . . . . . . . . . . . . . . . . . 1180

102

LIST OF FIGURES

4.38 A multiple-robot controller composed of a supervisory con-

troller and individual controllers (bus-type connection). . . . . . . . . . 1180

4.39 STMicroelectronics STM3210C-EVAL. . . . . . . . . . . . . . . . . . . . . . . 1181

4.40 STMicroelectronics STEVAL-PCC010V1. . . . . . . . . . . . . . . . . . . . . 1182

4.41 Structure of the source code directory (STM32F107_LwIP_PTP

_V1.0.3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1182

4.42 Boot-related switches of the STM3210C-EVAL board. . . . . . . . . . . . 1183

4.43 Correctly displayed characters on the LCD with the new firmware. . . 1185

4.44 Hardware setup for the PTPd demo. . . . . . . . . . . . . . . . . . . . . . . 1186

4.45 STM320F4Discovery board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1188

4.46 STM32F4Discovery extension board (STM32F4DIS-BB). . . . . . . . . . . 1189

4.47 LCD module for the STM320F4Discovery board. . . . . . . . . . . . . . . 1189

4.48 Assembly of the STM32F4DISCOVERY board, STM32F4DIS-

BB board, and LCD module. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1189

4.49 Hardware setup for an IEEE 1588 ptpd test. . . . . . . . . . . . . . . . . 1190

4.50 STM320F4Discovery board as a DHCP slave. . . . . . . . . . . . . . . . . 1191

4.51 STM320F4 Discovery board with a static IP address. . . . . . . . . . . . 1192

4.52 telet connection to the STM320F4 Discovery board. . . . . . . . . . . . . 1192

4.53 Shell commands for ptpd test. . . . . . . . . . . . . . . . . . . . . . . . . . . 1192

4.54 ptpd slave in listening state. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1193

4.55 Networking two PCs for the openPOWERLINK demo. . . . . . . . . . . . 1194

4.56 Initial windows of the powerlink demo on the MN PC. . . . . . . . . . . 1195

4.57 Selection of the LAN card for the powerlink demo on the

MN PC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1196

4.58 Initial windows of the powerlink demo on the CN PC. . . . . . . . . . . 1196

4.59 Disply of the digital IO on the CN PC. . . . . . . . . . . . . . . . . . . . . 1197

4.60 Disply of the digital IO of the CN PC on the MN PC. . . . . . . . . . . 1198

4.61 Linux PCs networked for the openpowerlink demo. . . . . . . . . . . . . 1199

4.62 openPOWERLINK libraries. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1201

4.63 Connecting two PCs with a crossover cable for the open-

POWERLINK demo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1207

4.64 Digital IO panel displayed on the MN PC’s screen. . . . . . . . . . . . . 1210

4.65 AM3359 Industrial Communications Engine. . . . . . . . . . . . . . . . . . 1212

4.66 AM437x Industrial Development Kit. . . . . . . . . . . . . . . . . . . . . . . 1213

103

LIST OF FIGURES

4.67 Graphical view of the slave7 system. . . . . . . . . . . . . . . . . . . . . . 1219

4.68 Structure of an EtherCAT slave controller. . . . . . . . . . . . . . . . . . . 1236

4.69 AM3359 ICE running the ethercat slave demo. . . . . . . . . . . . . . . . 1251

4.70 Two AM3359 ICEs used for the the ethercat slave demo. . . . . . . . . 1251

4.71 Network connection of a PC and two AM3359 ICEs. . . . . . . . . . . . 1252

4.72 Topology graph created with the “ethercat graph” command. . . . . . . 1253

4.73 LED blinking on the AM3359 ICEs. . . . . . . . . . . . . . . . . . . . . . . 1260

4.74 Raspberry Pi 3 Model B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1302

4.75 Infineon XMC4800 Relax EtherCAT kit. . . . . . . . . . . . . . . . . . . . . 1302

4.76 Functional block diagram of the Raspberry Pi 3 Model B. . . . . . . . . 1303

4.77 Functional block diagram of the Microchip LAN9514. . . . . . . . . . . . 1303

4.78 Functional block diagram of the WIZnet W5500. . . . . . . . . . . . . . . 1304

4.79 Reference schematic of the WIZnet W5500. . . . . . . . . . . . . . . . . . 1305

4.80 PiCAT board for interfacing the WIZnet W5500 to a Rasp-

berry Pi 3 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1306

4.81 Hardware setup for digital IO demos. . . . . . . . . . . . . . . . . . . . . . 1310

4.82 Digital IOs of the EtherCAT slave. . . . . . . . . . . . . . . . . . . . . . . . 1311

4.83 Hardware architecture of the RCCL v1.0 system. . . . . . . . . . . . . . . 1335

4.84 Software architecture of the RCCL v1.0 system. . . . . . . . . . . . . . . 1335

4.85 Hardware achitecture of the Multirccl system for a single

robot arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1336

4.86 Parallel communication link in the Multirccl system with

two robot arms. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1337

4.87 Software architecture of the MultiRCCL system. . . . . . . . . . . . . . . 1338

4.88 Hardware archirecture of the RWRCCL system. . . . . . . . . . . . . . . . 1339

4.89 Software architecture of the RWRCCL system. . . . . . . . . . . . . . . . . 1340

4.90 A MultiRCCL system consisting of a VMEbus-based work-

station and a PUMA robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1341

4.91 Hardware architecture of the RCCL v5.13 system for a PA10

robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1342

4.92 Software architecture of the RCCL 5.1.4 system for a PA10 robot. . . 1342

4.93 Robotsim: PUMA 560 robot at the rcclpark position. . . . . . . . . . . . 1355

4.94 Robotsim: PUMA 560 robot after setting the angle of joint

1 to zero. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1356

104

LIST OF FIGURES

4.95 Simderella as a graphic simulator. . . . . . . . . . . . . . . . . . . . . . . . 1361

4.96 Graphic simulation with the Simderella. . . . . . . . . . . . . . . . . . . . 1363

4.97 Software architecture of RCCL. . . . . . . . . . . . . . . . . . . . . . . . . . 1364

4.98 Software architecture of RCCL/RTAI. . . . . . . . . . . . . . . . . . . . . . . 1364

4.99 Communication between the JLS (joint level servo) task

and the TC (trajectory control) task in RWRCCL. . . . . . . . . . . . . . 1392

4.100 PUMA 560 robot displayed on the robotsim. . . . . . . . . . . . . . . . . 1422

4.101 Hardware structure of a robot controller consisting of main

and servo controllers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1469

4.102 Hardware structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 1469

4.103 Hardware structure of a “new” RCCL system. . . . . . . . . . . . . . . . . 1470

4.104 Servo controller simulator connected to the master controller. . . . . . 1499

4.105 Hardware architecture of the RCCL v1.0 system. . . . . . . . . . . . . . . 1518

4.106 Hardware architecture of the ALVIN system. . . . . . . . . . . . . . . . . . 1518

4.107 PA10-6C robot of the Mitsuibishi Heavy Industries. . . . . . . . . . . . . 1537

4.108 Mobile service robot TASER. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1538

4.109 Hardware structure of the TASER robot. . . . . . . . . . . . . . . . . . . . 1539

4.110 Hardware configuration of a RCCL v5.1.4 system with a

PUMA robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1540

4.111 Hardware configuration of a RCCL v5.1.4 system with a

PA10-6C robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1540

5.1 Compiler construction using Flax and Bison. . . . . . . . . . . . . . . . . . 1572

5.2 Gambas architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1651

5.3 Building Hello application (1). . . . . . . . . . . . . . . . . . . . . . . . . . . 1652

5.4 Building Hello application (2). . . . . . . . . . . . . . . . . . . . . . . . . . . 1653

5.5 Building Hello application (3). . . . . . . . . . . . . . . . . . . . . . . . . . . 1654

5.6 Building Hello application (4). . . . . . . . . . . . . . . . . . . . . . . . . . . 1655

5.7 Building Hello application (5). . . . . . . . . . . . . . . . . . . . . . . . . . . 1656

5.8 Building Hello application (6). . . . . . . . . . . . . . . . . . . . . . . . . . . 1657

5.9 Building Hello application (7). . . . . . . . . . . . . . . . . . . . . . . . . . . 1658

5.10 Building Hello application (8). . . . . . . . . . . . . . . . . . . . . . . . . . . 1659

6.1 DOS and BASIC interpreter in the early PCs. . . . . . . . . . . . . . . . . 1735

6.2 VAL operating system and VAL interpreter in Unimation controllrts. . 1735

105

LIST OF FIGURES

6.3 Structure of compilers and interpreters. . . . . . . . . . . . . . . . . . . . . 1740

6.4 Execution of PacScript programs and subroutines. . . . . . . . . . . . . . 1743

6.5 Writing a user program in VAL or C language. . . . . . . . . . . . . . . . 1752

6.6 Parallel FIFO link between VAX-11/780 and Unimation Controller. . . 1754

6.7 Serial FIFO link between main processor and servo controller. . . . . . 1754

6.8 Servo Controller Emulator for testing RCCL REAL mode. . . . . . . . . . 1755

6.9 Parse tree corresponding to “x = 3 * 4 + 5 * 6”. . . . . . . . . . . . . . 1775

6.10 Tree representation of a BASIC program. . . . . . . . . . . . . . . . . . . . 1775

6.11 Data strustures for storing “x = 3 * 4 + 5 * 6”. . . . . . . . . . . . . . . 1777

6.12 Compilation sequence. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1778

6.13 Parse-tree construction for the ASSIGN statement. . . . . . . . . . . . . . 1781

6.14 Linked-list representation of a program. . . . . . . . . . . . . . . . . . . . . 1818

6.15 Coordinate frame {H}. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1830

6.16 Axis specification of the frame {W} with points AW , BW ,

and CW . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1831

6.17 Target transform 0POIN T with its origin at the tool center point. . . 1833

6.18 Tool frame when target transform W POIN T0 = I4x4. . . . . . . . . . . . 1840

6.19 Flowchart of writing a VAL program. . . . . . . . . . . . . . . . . . . . . . 1846

6.20 Teach pendant of the Unimation Mark I controllers. . . . . . . . . . . . . 1847

6.21 Teach pendant of the Unimation MK II controller. . . . . . . . . . . . . . 1863

6.22 Teach pendant simulator of the Robotsim. . . . . . . . . . . . . . . . . . . 1865

6.23 Operating system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1866

6.24 Hardware structure of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . 1867

6.25 System tasks of V+. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1871

6.26 Scheduling algorithm of V+. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1873

6.27 Hardware structure of the RCCL v1.0 + interpreter. . . . . . . . . . . . . 1876

6.28 Tasks assigned to multiple CPUS in the Kali system. . . . . . . . . . . . 1877

6.29 User and TG tasks running on the same CPU in a single-

CPU multi-rccl system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1878

6.30 User, TG and servo tasks assigned to a signle CPU in the

RWRCCL system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1879

6.31 Architecture of the ROS-Industrial software. . . . . . . . . . . . . . . . . . 1880

6.32 Architecture of the MoveIt!. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1881

6.33 UR5 robot of the Universal Robots. . . . . . . . . . . . . . . . . . . . . . . 1884

106

LIST OF FIGURES

6.34 Initial Rviz window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1885

6.35 demo_descartes running on a Rviz window. . . . . . . . . . . . . . . . . . 1886

6.36 Motion planning of the TRA1 robot using the MoveIt!. RViz

plugin. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1901

6.37 Motion planning and execution of the TRA1 robot using

the MoveIt!. RViz plugin. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1902

6.38 fake_joint_launch.rviz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1903

6.39 rqt joint trajectory controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1903

6.40 Motion planning using the MoveIt! RViz and fake joint driver. . . . . . 1904

6.41 Opening a RViz window for jog control. . . . . . . . . . . . . . . . . . . . 1905

6.42 Adding a jog control panel to the RViz window. . . . . . . . . . . . . . . 1906

6.43 jogjointPanel added to the RViz window. . . . . . . . . . . . . . . . . . . . 1906

6.44 jogjoint panel on the RViz window. . . . . . . . . . . . . . . . . . . . . . . 1907

7.1 Trajectory generation for a pick-and-place operation using

4-3-4 polynomials. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1915

7.2 Two consecutive trapezoidal velocity profiles for moves from

point A and point C via point C . . . . . . . . . . . . . . . . . . . . . . . . 1916

7.3 A new velocity profiles after velocity blending. . . . . . . . . . . . . . . . 1916

7.4 Linear function with polynomial blends (I). . . . . . . . . . . . . . . . . . 1917

7.5 Linear function with polynomial blends (II). . . . . . . . . . . . . . . . . . 1917

7.6 Linear function with polynomial blends (III). . . . . . . . . . . . . . . . . 1918

7.7 parabola construction by repeated linear interpolation. . . . . . . . . . . 1919

7.8 Linear segments connecting points pi , p j , and pk . . . . . . . . . . . . . 1920

7.9 Velocity profiles V Pi j and V Pjk corresponding to the mo-

tions Mi j and M jk, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . 1921

7.10 A blend between two line segments. . . . . . . . . . . . . . . . . . . . . . . 1921

7.11 Path blending using a quadratic Bézier curve. . . . . . . . . . . . . . . . . 1924

7.12 Velocity and acceleration in the quadratic Bézier curve. . . . . . . . . . 1926

7.13 Interpolation with a fifth-order polynomial function. . . . . . . . . . . . 1930

7.14 Velocity, acceleration, and jerks during the interpolation with

a fifth-order polynomial function. . . . . . . . . . . . . . . . . . . . . . . . . 1932

7.15 Interpolation with a seventh-order polynomial function. . . . . . . . . . 1936

107

LIST OF FIGURES

7.16 Velocity, acceleration, and jerks during the interpolation with

a seventh-order polynomial function. . . . . . . . . . . . . . . . . . . . . . 1938

7.17 Direct blending two paths x1 and x2. . . . . . . . . . . . . . . . . . . . . . 1939

7.18 Path blending with a convex average of two line segments. . . . . . . . 1949

7.19 Blend p(t) according to different κ values. . . . . . . . . . . . . . . . . . 1949

7.20 Velocity and acceleration during the path blending with a

convex average. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1952

7.21 Path blending using a convex average for continuous jerk. . . . . . . . 1960

7.22 Velocity and acceleration during the path blending with a

convex average. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1963

7.23 Jerk during the path blending with a convex average. . . . . . . . . . . 1963

7.24 Blending two extrapolated paths. . . . . . . . . . . . . . . . . . . . . . . . . 1972

7.25 Velocity and acceleration while blending two extrapolated paths. . . . 1973

7.26 Blending two extrapolated paths for continuous jerk. . . . . . . . . . . . 1974

7.27 Velocity and acceleration while blending two extrapolated

paths for continuous jerk. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1975

7.28 The direction of a rotation according to right hand rule. . . . . . . . . 1978

7.29 Right-handed coordinate system. . . . . . . . . . . . . . . . . . . . . . . . . 1978

7.30 Basis vectors and their cross products in left-handed and

right-handed coordinate systems. . . . . . . . . . . . . . . . . . . . . . . . . 1978

7.31 Pitch, yaw and roll associated with a wrist (I). . . . . . . . . . . . . . . . 1993

7.32 Pitch, yaw and roll associated with a wrist (II). . . . . . . . . . . . . . . 1993

7.33 Tool orientation represented with n, s, a vectors. . . . . . . . . . . . . . 1994

7.34 Tool orientation represented with roll, pitch, yaw angles. . . . . . . . . 1995

7.35 Body frame attached to an airplane. . . . . . . . . . . . . . . . . . . . . . . 1996

7.36 Interpolation of two quaternions. . . . . . . . . . . . . . . . . . . . . . . . . 2008

7.37 Discontinuity in angular velocities due to the spherical lin-

ear interpolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2018

7.38 Overlapping two angular velocity profiles. . . . . . . . . . . . . . . . . . . 2019

7.39 Interpolation between the points P1 and P2 with a cubic

Bézier curve. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2030

7.40 Interpolation parameters ui j and u jk without blending. . . . . . . . . . . 2039

7.41 Interpolation parameters ui j , u jk, and uk with blending. . . . . . . . . . 2041

7.42 Plot of −1+ 2h− 2h3 + h4 for h ∈ [0,1]. . . . . . . . . . . . . . . . . . . . 2047

108

LIST OF FIGURES

7.43 Plot of 2h3 − h4 for h ∈ [0,1]. . . . . . . . . . . . . . . . . . . . . . . . . . . 2049

7.44 Plot of u j(t) = 10h3 − 15h4 + 6h5 for h ∈ [0,1]. . . . . . . . . . . . . . . 2051

7.45 Extrapolation of interpolation parameters ui j and u jk for blending. . . 2053

7.46 Rotation blending with the convex average. . . . . . . . . . . . . . . . . . 2067

7.47 Angular Velocity and angular acceleration during the rota-

tion blending with a convex average. . . . . . . . . . . . . . . . . . . . . . 2071

7.48 Velocity and acceleration during the path blending with a

convex average. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2072

7.49 Rotation blending with the convex average for continuous jerk. . . . . 2083

7.50 Angular Velocity and angular acceleration during the rota-

tion blending with continuous jerk. . . . . . . . . . . . . . . . . . . . . . . 2087

7.51 Blending two extrapolated rotations. . . . . . . . . . . . . . . . . . . . . . . 2094

7.52 Angular velocity and acceleration while blending two ex-

trapolated rotations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2095

7.53 Blending two extrapolated rotations for continuous angular jerk. . . . 2102

7.54 Angular velocity and acceleration while blending two ex-

trapolated rotations for continuous angular jerk. . . . . . . . . . . . . . . 2102

7.55 Paths F(H0, H1; t) and FL(H0, H1; t) connecting H0 and H1. . . . . . . . 2111

7.56 Unbuffered mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2146

7.57 Velocity profile in the unbuffered mode. . . . . . . . . . . . . . . . . . . . 2146

7.58 Buffer (queue) for storing motion requests. . . . . . . . . . . . . . . . . . 2147

7.59 Buffered mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2147

7.60 Overlapping consecutive velocity profiles. . . . . . . . . . . . . . . . . . . . 2148

7.61 Path blending. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2148

7.62 Path-blending disabled. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2149

7.63 Conveyot task of picking up pins on a moving conveyor

and inserting them into a hole. . . . . . . . . . . . . . . . . . . . . . . . . . 2151

7.64 End-effector positions specified for picking and inserting a pin. . . . . 2151

7.65 Coordinate frames of the Stanford manipulator. . . . . . . . . . . . . . . 2153

7.66 T6 frame and end-effector frame (E) of the Stanford manipulator. . . 2154

7.67 Frames W, Z, P, H defined for the conveyor task. . . . . . . . . . . . . . . 2154

7.68 Transformation PG with respect to frame P. . . . . . . . . . . . . . . . . . 2155

7.69 Transformations HR1 and HR2 with respect to frame H. . . . . . . . . . 2156

7.70 Transformations PHA, PCH and PAL with respect to frame HRi . . . . . 2156

109

LIST OF FIGURES

7.71 Transformation PN with respect to frame HRi . . . . . . . . . . . . . . . . 2156

7.72 Path transitions (Paul 1981). . . . . . . . . . . . . . . . . . . . . . . . . . . . 2169

7.73 An industrial robot mounted on a linear track. . . . . . . . . . . . . . . . 2175

7.74 Path transitions for a moving frame. . . . . . . . . . . . . . . . . . . . . . . 2178

7.75 n,o,a and p vectors of a tool frame. . . . . . . . . . . . . . . . . . . . . . . 2184

7.76 Cartesian path transitions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2187

7.77 Cartesian motion example (Example 5.1 from Ref. [Paul81]). . . . . . 2191

7.78 Cartesian path transition. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2196

7.79 TG state transitions in RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . . 2208

7.80 Path transitions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2208

7.81 Constant velocity profiles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2212

7.82 Linear velocity transition. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2212

7.83 Transition between two path segments. . . . . . . . . . . . . . . . . . . . . 2214

7.84 Velocity profile specified by ts and tacc . . . . . . . . . . . . . . . . . . . . . 2219

7.85 Velocity profile specified by ts, ta1 and ta2. . . . . . . . . . . . . . . . . . 2220

7.86 Motion-request queue linking the planning and trajectory tasks. . . . . 2222

7.87 Velocity profile corresponding to a normal motion request. . . . . . . . 2223

7.88 Velocity profiles corresponding to two consecutive normal

motion requests. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2223

7.89 Velocity profile corresponding to a null motion request. . . . . . . . . . 2224

7.90 Velocity profiles corresponding to a sequence of normal,

null and normal motion requests. . . . . . . . . . . . . . . . . . . . . . . . . 2225

7.91 Velocity profile corresponding to a stop motion request. . . . . . . . . . 2225

7.92 Velocity profiles corresponding to a sequence of normal,

stop and normal motion requests. . . . . . . . . . . . . . . . . . . . . . . . 2226

7.93 Transition between path segments. . . . . . . . . . . . . . . . . . . . . . . . 2228

7.94 Hardware architecture of the RFMS system. . . . . . . . . . . . . . . . . . 2278

7.95 State transitions in the RFMS’ TG (I). . . . . . . . . . . . . . . . . . . . . . 2287

7.96 Path segments in joint space. . . . . . . . . . . . . . . . . . . . . . . . . . . 2290

7.97 Transition and straight-line segments in joint space. . . . . . . . . . . . . 2290

7.98 Path segments in Cartesian space. . . . . . . . . . . . . . . . . . . . . . . . 2296

7.99 Transition and straight-line segments in Cartesian space. . . . . . . . . . 2297

7.100 Motion queue for storing motion requests. . . . . . . . . . . . . . . . . . . 2308

7.101 State transitions in the RFMS’ TG (II). . . . . . . . . . . . . . . . . . . . . 2310

110

LIST OF FIGURES

7.102 Velocity profile in the stopping state (state S5). . . . . . . . . . . . . . . 2313

7.103 Kinematic loop for describing a target position . . . . . . . . . . . . . . . 2325

7.104 Straight-line path segments of the TCP between target positions. . . . 2326

7.105 Blend S(t) between two straight-line segments. . . . . . . . . . . . . . . . 2326

7.106 Estimation of T6(tb). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2337

7.107 Trajectories of TCP and T6 in Cartesian space. . . . . . . . . . . . . . . . 2340

7.108 TG state transitions in the Multi-RCCL. . . . . . . . . . . . . . . . . . . . . 2355

7.109 Blending two path segment together. . . . . . . . . . . . . . . . . . . . . . 2358

7.110 Computing the drive parameters for fixed targets. . . . . . . . . . . . . . 2364

7.111 Computing the drive parameters for moving targets. . . . . . . . . . . . 2367

7.112 Velocity profile with a cruise phase. . . . . . . . . . . . . . . . . . . . . . . 2369

7.113 Velocity profile without a cruise phase. . . . . . . . . . . . . . . . . . . . . 2370

7.114 Execution of monitor functions at each trajectory genera-

tion task. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2374

7.115 Rendezvous operations between trajectory generation tasks

executed on separate CPUs. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2375

7.116 Constant velocity profile vs. trapezoidal velocity profile. . . . . . . . . . 2407

7.117 A trapezoidal velocity profile with equal acceleration and

deceleration intervals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2409

7.118 A trapezoidal velocity profile with no constant velocity interval. . . . . 2409

7.119 A trapezoidal velocity profile with a small moving distance. . . . . . . 2410

7.120 Motion from point pA to point pC via point pB. . . . . . . . . . . . . . . 2410

7.121 Two trapezoidal velocity profiles for a point-to-point motion. . . . . . . 2411

7.122 P-to-P motion with a smooth transition at point pB. . . . . . . . . . . . . 2412

7.123 Two consecutive trapezoidal velocity profiles without ve-

locity blending. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2413

7.124 Two consecutive trapezoidal velocity profiles after velocity blending. . 2413

7.125 Trapezoidal velocity profiles of multiple joints. . . . . . . . . . . . . . . . 2415

7.126 Multiple trapezoidal velocity profiles with simultaneously stops. . . . . 2415

7.127 Trajectory generator of the profile position mode. . . . . . . . . . . . . . 2417

7.128 Trajectory generator of the interpolated position mode. . . . . . . . . . . 2417

7.129 Trapezoidal velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2418

7.130 S-curve velocity profile (sinusoidal). . . . . . . . . . . . . . . . . . . . . . . 2419

7.131 S-curve velocity profile (trapezoidal). . . . . . . . . . . . . . . . . . . . . . 2421

111

LIST OF FIGURES

7.132 Parabolic velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2421

7.133 Transitions of the states defined by Table 7.21. . . . . . . . . . . . . . . . 2423

7.134 Representation of trajectory regions with Alphabet letters. . . . . . . . . 2466

7.135 Position, velocity and acceleration profiles between points

A and B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2472

7.136 Range and null space of the manipulator jacobian J. . . . . . . . . . . . 2476

7.137 A single processor executing both TG and servo tasks. . . . . . . . . . . 2484

7.138 Main and servo processors for executing TG and servo tasks. . . . . . . 2485

7.139 A TG processor and multiple servo processors. . . . . . . . . . . . . . . . 2485

7.140 Sampling intervals of TG and servo. . . . . . . . . . . . . . . . . . . . . . . 2486

7.141 Implementation of a linear interpolator in a servo processor. . . . . . . 2486

7.142 A task execution timing example of TG-to-Servo communication. . . . 2487

7.143 Clock synchronization in the Unimation robot controllers. . . . . . . . . 2489

7.144 Position and velocity commands without a linear interpolation. . . . . 2490

7.145 Position and velocity commands with a linear interpolation. . . . . . . 2491

7.146 Trajectory errors due to the linear interpolatiom. . . . . . . . . . . . . . . 2494

7.147 Quadratic extrapolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2499

7.148 Communication between TG and Servo tasks in RWRCCL. . . . . . . . . 2504

7.149 TG and Servo CPUs in Kali. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2504

7.150 PCI bus with a DPRAM for TG-to-Servo Interface. . . . . . . . . . . . . . 2505

7.151 Custom parallel I/F board for TG-Servo communication. . . . . . . . . . 2506

7.152 Serial Interface between TG and Servo procecssors. . . . . . . . . . . . . 2507

7.153 Input/output of pulse-type servo drives. . . . . . . . . . . . . . . . . . . . 2507

7.154 pulse-type input/output interface with BRMs and QCs. . . . . . . . . . . 2508

7.155 Distributed clocks in an EtherCAT network. . . . . . . . . . . . . . . . . . 2509

7.156 Hardware structure of the Unimation MK I system. . . . . . . . . . . . . 2510

7.157 Clock signals generated in the Unimation MK I controller. . . . . . . . . 2511

7.158 Trajectory error due to the execution period mismatch be-

tween TG and control tasks. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2512

7.159 Hardware structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 2513

7.160 Ideal communication sequence between the Unimation con-

troller and the VAX 11/780 for the RCCL v1.0 system. . . . . . . . . . . 2525

7.161 Communication sequence implemented in the RCCL v1.0 system. . . . 2526

7.162 Hardware structure of the Multi-RCCL system. . . . . . . . . . . . . . . . 2527

112

LIST OF FIGURES

7.163 Hardware setup with a VME-based host computer. . . . . . . . . . . . . . 2528

7.164 Interrupt signals generated by the XVME240 board. . . . . . . . . . . . . 2528

7.165 Hardware structure of the RWRCCL system. . . . . . . . . . . . . . . . . . 2534

7.166 PC-based motion controller with an interpolation board. . . . . . . . . . 2545

7.167 Communication sequence between a PC and an interpola-

tion board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2545

7.168 Timing constraints on the interrupt and start signals. . . . . . . . . . . . 2546

8.1 Robot arm and teach pendant as the peripherals of a robot controller. 2552

8.2 Configuration of an offline robot programming system (I). . . . . . . . 2552

8.3 Configuration of an offline robot programming system (II). . . . . . . . 2553

8.4 Hadndling of TG outputs at each operating mode of the RCCL. . . . . 2553

8.5 kits of the Qt Creator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2557

8.6 Setting a default kit for Qt Creator. . . . . . . . . . . . . . . . . . . . . . . 2558

8.7 Building SoQt with Visual Studio 2008. . . . . . . . . . . . . . . . . . . . . 2559

8.8 Retained-mode graphics library. . . . . . . . . . . . . . . . . . . . . . . . . . 2560

8.9 Immediate-mode graphics library. . . . . . . . . . . . . . . . . . . . . . . . . 2560

8.10 Graphics APIs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2562

8.11 FeeCAD loading the Invertor-format file, “cone.iv”. . . . . . . . . . . . . . 2563

8.12 Example of an Inventor program: 02.1.HelloCone. . . . . . . . . . . . . . 2564

8.13 Architecture of the Open Inventor. . . . . . . . . . . . . . . . . . . . . . . . 2565

8.14 Scene graph diagram corresponding to the cone.iv. . . . . . . . . . . . . 2566

8.15 Scene graph diagram drawn by gview. . . . . . . . . . . . . . . . . . . . . 2567

8.16 Result of executing the soshapekit application. . . . . . . . . . . . . . . . 2568

8.17 An example of trackball manipulator: 02.3.Trackball. . . . . . . . . . . . 2570

8.18 An example showing the Xt utility functions of the Open Inventor. . . 2572

8.19 Open Inventor classes for Win32 and for Unix. . . . . . . . . . . . . . . . 2573

8.20 Wiggly example. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2575

8.21 Creating a project file with Qt Creator (1). . . . . . . . . . . . . . . . . . 2579

8.22 Creating a project file with Qt Creator (2). . . . . . . . . . . . . . . . . . 2580

8.23 Creating a project file with Qt Creator (3). . . . . . . . . . . . . . . . . . 2580

8.24 Creating a project file with Qt Creator (4). . . . . . . . . . . . . . . . . . 2581

8.25 Creating a project file with Qt Creator (5). . . . . . . . . . . . . . . . . . 2581

8.26 Creating a project file with Qt Creator (6). . . . . . . . . . . . . . . . . . 2582

113

LIST OF FIGURES

8.27 Creating a project file with Qt Creator (7). . . . . . . . . . . . . . . . . . 2582

8.28 mainwindow.ui after the test1 project is created. . . . . . . . . . . . . . . 2585

8.29 Qt Creator plugins for input and display widgets. . . . . . . . . . . . . . 2585

8.30 mainwindow.ui with QLabel and QLineEdit objects. . . . . . . . . . . . . 2586

8.31 Changing the frameShape property of the label to Box. . . . . . . . . . 2587

8.32 Application test1 built with Qt Creator (wiggly example). . . . . . . . . 2591

8.33 SoQt: binding bwtween Coin and Qt. . . . . . . . . . . . . . . . . . . . . . 2591

8.34 An example of the Inventor: 02.4.Examiner. . . . . . . . . . . . . . . . . . 2594

8.35 An example of Coin3D and SoQt: 02.4.Examiner. . . . . . . . . . . . . . 2596

8.36 Inventor-file viewer using the class SoQtExaminerViewer. . . . . . . . . 2598

8.37 Robot control panel and view windows for the RX90 robot arm. . . . 2600

8.38 Display of PUMA 560 with instant Player. . . . . . . . . . . . . . . . . . . 2602

8.39 Display of PUMA-560.iv using ivview. . . . . . . . . . . . . . . . . . . . . . 2603

8.40 Control panel of the Qilex. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2605

8.41 PUMA 560 display. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2610

8.42 Axes of ABB IRB 2600 robots. . . . . . . . . . . . . . . . . . . . . . . . . . . 2612

8.43 Dimensions of the ABB IRB2600/20(12)/1.65 robot. . . . . . . . . . . . 2612

8.44 Links of KUKA KR 16 robots. . . . . . . . . . . . . . . . . . . . . . . . . . . 2613

8.45 Coordinate system of the link 0. . . . . . . . . . . . . . . . . . . . . . . . . 2617

8.46 Coordinate system of the link 1. . . . . . . . . . . . . . . . . . . . . . . . . 2617

8.47 Coordinate system of the link 2. . . . . . . . . . . . . . . . . . . . . . . . . 2617

8.48 Coordinate system of the link 3. . . . . . . . . . . . . . . . . . . . . . . . . 2618

8.49 Coordinate system of the link 4. . . . . . . . . . . . . . . . . . . . . . . . . 2618

8.50 Coordinate system of the link 5. . . . . . . . . . . . . . . . . . . . . . . . . 2618

8.51 Coordinate system of the link 6. . . . . . . . . . . . . . . . . . . . . . . . . 2618

8.52 Global coordinate frame set in the IRB2600 CAD model. . . . . . . . . 2619

8.53 An example of setting the origins of link frames. . . . . . . . . . . . . . 2620

8.54 Setting the origins of link frames for the IRB 2600 robot. . . . . . . . . 2620

8.55 Frame rotation for link 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2621

8.56 VRML model of the IRB 2600 robot (irb2600.wrl) loaded

on the FeeeCAD. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2626

8.57 Arm pose coresponding to joint angles (0,+90◦, -90◦, 0,0,0). . . . . . . 2627

8.58 FeeCAD window at startup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2631

8.59 FeeCAD window after selecting the robot example project. . . . . . . . 2632

114

LIST OF FIGURES

8.60 FeeCAD window after loading the robot workbench. . . . . . . . . . . . 2633

8.61 FreeCAD window after executing the robot example. . . . . . . . . . . . 2633

8.62 Combo view widget after the simulation ended. . . . . . . . . . . . . . . 2634

8.63 Qslider example. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2642

8.64 Setting the Qslider value via a shared-memory. . . . . . . . . . . . . . . . 2646

8.65 VRML model of IRB 4400 robot (irb4400_60_m2000_rev1_vrml_c.zip). 2647

8.66 Parts in the VRML model of IRB 4400 robot. . . . . . . . . . . . . . . . . 2648

8.67 Global coordinate system used in the VRML model of IRB

4400 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2648

8.68 Opening the project file IRB400 from qtcreator. . . . . . . . . . . . . . . 2649

8.69 Selecting the form file mainWidgetform.ui. . . . . . . . . . . . . . . . . . . 2649

8.70 Qt-designer for editing mainWidgetform.ui. . . . . . . . . . . . . . . . . . 2650

8.71 Initial screen of the IRB4440 simulator. . . . . . . . . . . . . . . . . . . . . 2651

8.72 Initial screen of the “Tower of Honoi” demo. . . . . . . . . . . . . . . . . 2652

8.73 Final screen of the “Tower of Honoi” demo. . . . . . . . . . . . . . . . . . 2653

8.74 Axes of the IRB 4400 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2654

8.75 Joint angle input using sliders and spin boxes. . . . . . . . . . . . . . . . 2655

8.76 Work envelope of the IRB 4400 robot. . . . . . . . . . . . . . . . . . . . . 2659

8.77 A VRMl model (SCARAcontinuo.wrl) of the IBM 7540 robot. . . . . . . 2663

8.78 SCARAcontinuo.wrl viewed by qiew after clenups. . . . . . . . . . . . . . 2665

8.79 Global coordinate system in the SCARAcontinuo.wrl. . . . . . . . . . . . 2666

8.80 Rorating the forearm link by +90◦ by modifying the SCARA-

continuo.wrl file. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2667

8.81 SCARA robot simulator with a control panel. . . . . . . . . . . . . . . . . 2670

8.82 Global coordinate system of the PUMA 560 VRML model. . . . . . . . . 2680

8.83 Display of the PUMA 560 VRML model. . . . . . . . . . . . . . . . . . . . 2680

8.84 Display of the PUMA 560 rotated about x-axis by −90◦. . . . . . . . . . 2681

8.85 Display of the PUMA 560 with a SoQt viewer . . . . . . . . . . . . . . . 2682

8.86 Local coordinate system of the base link (link 0). . . . . . . . . . . . . . 2682

8.87 Qsliders and QDoubleSpinBox widgets for entering and dis-

playing joint angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2686

8.88 Editing slots for QDoubleSpinBox widgets. . . . . . . . . . . . . . . . . . . 2687

8.89 PUMA 560 at the ready position. . . . . . . . . . . . . . . . . . . . . . . . . 2691

8.90 PUMA 560 at the zero-angle position. . . . . . . . . . . . . . . . . . . . . . 2691

115

LIST OF FIGURES

8.91 TCP (tool center position) representation with a position equation. . . 2692

8.92 coord0.wrl for displaying the axes of local frame {0}. . . . . . . . . . . 2693

8.93 coord6.wrl for displaying the axes of local frame {6}. . . . . . . . . . . 2693

8.94 Visualization of the link frames {0} and {6}. . . . . . . . . . . . . . . . . 2695

8.95 Link 0 frame set by the VAL programming system. . . . . . . . . . . . . 2696

8.96 Running the puma560 simulator on Windows 7 x64. . . . . . . . . . . . 2698

8.97 Teach pendant of the Unimation Mark I controllers. . . . . . . . . . . . . 2699

8.98 Staubli Unimation 3D32488G02 teach pendant. . . . . . . . . . . . . . . . 2700

8.99 Adept Staubli MCP (Manual Control Pendant). . . . . . . . . . . . . . . . 2700

8.100 Layout of the Adept Staubli MCP. . . . . . . . . . . . . . . . . . . . . . . . . 2701

8.101 Adept T1 Pendant. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2702

8.102 MCP of the Staubli CS8 controller. . . . . . . . . . . . . . . . . . . . . . . . 2702

8.103 Teach pendant simulator of the Staubli Robotics Studio. . . . . . . . . . 2703

8.104 Robotsim (Robot + TP simulator) of the Multi-RCCL. . . . . . . . . . . . 2704

8.105 User inferface example of a teach pendant simulator. . . . . . . . . . . . 2705

8.106 Local mode of the teach pendant simulator. . . . . . . . . . . . . . . . . . 2705

8.107 Remote mode of the teach pendant simulator. . . . . . . . . . . . . . . . 2706

8.108 QLineEdit widget for entering teach commands. . . . . . . . . . . . . . . 2707

8.109 Shared-memory inteface for a robot arm + TP simulator . . . . . . . . 2709

8.110 TP simulator layout. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2710

8.111 Button backgound color on the click event. . . . . . . . . . . . . . . . . . 2711

8.112 A simple example of FIFO: writer and reader. . . . . . . . . . . . . . . . . 2717

8.113 FIFO inteface for a robot arm + TP simulator. . . . . . . . . . . . . . . . 2721

8.114 TCP socket interface for the robot and TP simulators. . . . . . . . . . . 2739

8.115 CRT terminal connected to the RCCL v1.0 system for teaching. . . . . 2740

8.116 Robt arm simulator and terminal emulator connected to

the RCCL v1.0 system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2741

8.117 Robot arm simulator, terminal emulator and MK I TP sim-

ulator connected to the RCCL v1.0 system. . . . . . . . . . . . . . . . . . 2742

8.118 FIFO interface to robot and TP simulators. . . . . . . . . . . . . . . . . . . 2742

8.119 Interfacing a robot simulator to the RCCL v1.0 system. . . . . . . . . . 2744

8.120 PUMA560 robot simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2744

8.121 GUI of a teach pendant simulator designed for the RCCL v1.0. . . . . 2747

116

LIST OF FIGURES

8.122 Displaying both a scene (unimation-puma560_boxes.xml)

and a robot (unimation-puma560.xml) with rlCoachKin. . . . . . . . . . 2758

8.123 Displaying robot motion with rlCoachKin. . . . . . . . . . . . . . . . . . . 2760

8.124 A planar robot with two degree-of-freedoms. . . . . . . . . . . . . . . . . 2763

8.125 Structure of the planer2.xml. . . . . . . . . . . . . . . . . . . . . . . . . . . 2764

8.126 Structure of the planer2.xml with element ids. . . . . . . . . . . . . . . . 2764

8.127 Link coordinate frames of the PUMA 560 robot. . . . . . . . . . . . . . . 2771

8.128 A robot of tree structure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2774

8.129 “visual” robot on the Rviz window. . . . . . . . . . . . . . . . . . . . . . . . 2779

8.130 rqt_graph window during displaying the “visual” robot. . . . . . . . . . 2780

8.131 “flexiable” robot on the Rviz window. . . . . . . . . . . . . . . . . . . . . . 2786

8.132 “Joint State Publisher” window for the “flexiable” robot. . . . . . . . . . 2787

8.133 Viewing the r2d2 robot with the V-REP. . . . . . . . . . . . . . . . . . . . . 2792

8.134 r2d2 robot moving on the rviz window. . . . . . . . . . . . . . . . . . . . 2795

8.135 rqt_graph window when the r2d2 robot is moving. . . . . . . . . . . . . 2796

8.136 RRBot robot displayed on the rviz window. . . . . . . . . . . . . . . . . . 2798

8.137 Joint Satet Publisher window. . . . . . . . . . . . . . . . . . . . . . . . . . . 2798

8.138 New silder positions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2798

8.139 New pose of the RRBot robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 2799

8.140 RRBot robot displayed on the gazebo window. . . . . . . . . . . . . . . . 2799

8.141 Joint-controlled RRBot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2800

8.142 RQT window for sending joint commands. . . . . . . . . . . . . . . . . . . 2801

8.143 Sinusoidal rotation of the link 1 with respect to base link. . . . . . . . 2801

8.144 Software architecture of the openrave. . . . . . . . . . . . . . . . . . . . . 2802

8.145 Hanoi example of the OpenRave. . . . . . . . . . . . . . . . . . . . . . . . . 2809

8.146 Adding python.exe to Path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2814

8.147 Setting the properties of the ode solution. . . . . . . . . . . . . . . . . . . 2815

8.148 Boxes falling from above after the tutorial2.py starts. . . . . . . . . . . . 2822

8.149 Boxes are dropping when the falling-boxes.py runs. . . . . . . . . . . . . 2835

8.150 An explosion blows boxes away when the falling-boxes.py runs. . . . . 2836

8.151 Hanoi demo of the roboti-sandbox. . . . . . . . . . . . . . . . . . . . . . . . 2837

8.152 Belt demo of the roboti-sandbox. . . . . . . . . . . . . . . . . . . . . . . . . 2838

8.153 Drawing demo of the roboti-sandbox. . . . . . . . . . . . . . . . . . . . . . 2838

8.154 Mistake demo of the roboti-sandbox. . . . . . . . . . . . . . . . . . . . . . 2839

117

LIST OF FIGURES

8.155 Adept Viper S650. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2839

8.156 User interface of the V-REP. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2875

8.157 Toolbar 1 (top toolbar) of the V-REP. . . . . . . . . . . . . . . . . . . . . . 2875

8.158 Toolbar 2 (left toolbar) of the V-REP. . . . . . . . . . . . . . . . . . . . . . 2876

8.159 The 6 possible types of control methods in and around V-REP. . . . . . 2878

8.160 V-REP framework. Colored areas are custom or can be customized. . 2879

8.161 Scen view after drag-and-dropping the ABB IRB 140 from

Model brower. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2880

8.162 New robot pose after executing command “arm.setq([3.14

0 0 0 0 0])”. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2881

8.163 Loading IRB 140 robot model. . . . . . . . . . . . . . . . . . . . . . . . . . . 2882

8.164 Script icon of the IRB 140 robot model. . . . . . . . . . . . . . . . . . . . 2883

8.165 Lua script associated with the IRB 140 robot model. . . . . . . . . . . . 2883

8.166 IRB140 user interface window. . . . . . . . . . . . . . . . . . . . . . . . . . 2890

8.167 Loading IRB 140 robot model. . . . . . . . . . . . . . . . . . . . . . . . . . . 2892

8.168 New robot pose after executing client.py. . . . . . . . . . . . . . . . . . . . 2893

8.169 Scene remoteApiCommandServerExample.ttt. . . . . . . . . . . . . . . . . 2897

8.170 Display a message from the C++ client complexCommandTest. . . . . 2900

8.171 Graphic simulation of an IRB 140 robot with V-REP and

irb140Client (C++ client). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2903

8.172 External client application controlling the red robot via ROS. . . . . . . 2912

8.173 Initial V-REP window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2920

8.174 Initial pose of the Poppy humanoid robot. . . . . . . . . . . . . . . . . . . 2921

8.175 Poppy humanoid robot at the goal position. . . . . . . . . . . . . . . . . . 2922

8.176 Opening 0_Baxter_IK_felt_pen_cheating_recording.ttt. . . . . . . . . . . . 2923

8.177 Display the handwriting script (hello.csv). . . . . . . . . . . . . . . . . . . 2924

8.178 Writing the word “hello” during simulation. . . . . . . . . . . . . . . . . . 2924

8.179 The fnal word written after simulation. . . . . . . . . . . . . . . . . . . . . 2925

8.180 Scene robotLanguageControl.ttt. . . . . . . . . . . . . . . . . . . . . . . . . . 2928

8.181 Objects in the scene hierachy. . . . . . . . . . . . . . . . . . . . . . . . . . . 2928

8.182 Objects in the scene view. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2929

8.183 Running the first mtbserver application on Windows OS. . . . . . . . . 2929

8.184 Running the second mtbserver application on Windows OS. . . . . . . . 2930

8.185 Running the Robot language interpreter integration tutorial. . . . . . . 2930

118

LIST OF FIGURES

8.186 Qt-based custom UI for each MTB robot. . . . . . . . . . . . . . . . . . . . 2932

8.187 Dialog for displaying robot’s inputs and outputs. . . . . . . . . . . . . . . 2933

8.188 Link 2 and its reference frame. . . . . . . . . . . . . . . . . . . . . . . . . . 2956

8.189 Visualization of a URDF model of the PUMA 560 robot

with V-REP. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2960

8.190 PUMA 560 robot at joint angles (0, 0, 0, 0, 0, 0). . . . . . . . . . . . . 2961

8.191 PUMA 560 robot displayed after executing the shell script

launch_vrep.bash. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2964

8.192 PUMA 560 robot at joint angles (90◦, 0, 0, 0, 0, 0). . . . . . . . . . . . 2965

8.193 Scene hieracrhy after importing puma560-vrep.urdf. . . . . . . . . . . . . 2967

8.194 Bounding box of the puma560_link0_visual. . . . . . . . . . . . . . . . . . 2967

8.195 Scene hieracrhy with new link names. . . . . . . . . . . . . . . . . . . . . 2968

8.196 Properties of the “puma560” object. . . . . . . . . . . . . . . . . . . . . . . 2968

8.197 Modified properties of the “puma560” object. . . . . . . . . . . . . . . . . 2969

8.198 Bounding box of the “puma560” object. . . . . . . . . . . . . . . . . . . . 2969

8.199 Running a V-REP simulation with scene forwardAndInverseKine-

matics2.ttt. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2970

8.200 Building a custom user interface for the puma560.tfm. . . . . . . . . . . 2970

8.201 A control panel added to Scene view. . . . . . . . . . . . . . . . . . . . . . 2971

8.202 Simulation running with the control panel. . . . . . . . . . . . . . . . . . 2972

8.203 Work envirinment of a PUMA 560 robot. . . . . . . . . . . . . . . . . . . . 2975

8.204 Adding a dummy object to the robot model. . . . . . . . . . . . . . . . . 2975

8.205 Scene object properties of the dummy object. . . . . . . . . . . . . . . . . 2976

8.206 Scene object properties of the link 0. . . . . . . . . . . . . . . . . . . . . . 2976

8.207 Scene object properties of the link 1. . . . . . . . . . . . . . . . . . . . . . 2977

8.208 Definining Collection M_robot. . . . . . . . . . . . . . . . . . . . . . . . . . . 2978

8.209 Definining Collection Environment. . . . . . . . . . . . . . . . . . . . . . . . 2978

8.210 Definining a collision object. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2979

8.211 Attaching a script to the puma560 object. . . . . . . . . . . . . . . . . . . 2979

8.212 A collision between the PUMA 560 robot and the box is detected. . . 2981

8.213 Zero pose of the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . . . 2985

8.214 “Joint State Publisher” window for the PUMA 560 robot. . . . . . . . . 2985

8.215 PUMA 560 robot moving on the rviz window. . . . . . . . . . . . . . . . . 2988

8.216 Visualization of the PUMA 560 robot’s DH parameters. . . . . . . . . . . 2992

119

LIST OF FIGURES

8.217 Visualization of the PUMA 560 robot’s VRML model. . . . . . . . . . . . 2994

8.218 Visualization of link2.wrl. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2996

8.219 Visualization of the PUMA 560 robot’s VRML model using

the VREP. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3006

8.220 Link 3 transparently displayed by the rviz. . . . . . . . . . . . . . . . . . . 3006

8.221 Holes in the mesh. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3007

8.222 Wrong orientation of normal vectors. . . . . . . . . . . . . . . . . . . . . . 3007

8.223 Reparing the link0.wrl (1). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3008

8.224 Reparing the link0.wrl (2). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3009

8.225 Reparing the link0.wrl (3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3010

8.226 Reparing the link0.wrl (4). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3010

8.227 Reparing the link0.wrl (5). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3011

8.228 Reparing the link0.wrl (7). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3011

8.229 Reparing the link0.wrl (6). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3012

8.230 Link3 correctly displayed by the rviz. . . . . . . . . . . . . . . . . . . . . . 3012

8.231 Convex visual model of the PUMA 560 robot. . . . . . . . . . . . . . . . . 3016

9.1 Workflow of executing hello.pyx. . . . . . . . . . . . . . . . . . . . . . . . . 3272

9.2 RCCL without a interpreter. . . . . . . . . . . . . . . . . . . . . . . . . . . . 3345

9.3 RCCL with a robot interpreter written in C. . . . . . . . . . . . . . . . . . 3345

9.4 RCCL with a Python interpreter. . . . . . . . . . . . . . . . . . . . . . . . . 3346

9.5 RCCL written in Python. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3346

9.6 Compiling and linking rccl programs. . . . . . . . . . . . . . . . . . . . . . 3347

9.7 Circular doubly-linked list the position equation p2. . . . . . . . . . . . . 3354

9.8 Shared-memory interface between a pyRCCL program and

a graphic simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3357

9.9 Am example of the interprocesss communication using a

shared-memory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3360

9.10 Qilex robot simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3373

9.11 Left/right and up/down configurations of a PUMA 560 robot . . . . . . 3379

9.12 Initial lun configuration of the PUMA 560 robot while run-

ning pcny.py. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3379

9.13 Initial lun configuration of the PUMA 560 robot after chang-

ing the view point with the Roty wheel. . . . . . . . . . . . . . . . . . . . 3380

120

LIST OF FIGURES

9.14 The luf configuration of the PUMA 560 robot. . . . . . . . . . . . . . . . 3381

9.15 The ldn configuration of the PUMA 560 robot. . . . . . . . . . . . . . . . 3381

9.16 ensiie-irb4440: ABB IRB4440 simulator. . . . . . . . . . . . . . . . . . . . . 3394

9.17 PUMA 560 robot simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3395

9.18 Teach pendant simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3395

9.19 Running the pcnf.py (a pyRCCL program) in a command

prompt window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3398

9.20 Coodinate values displayed on the teach pendant simulator. . . . . . . 3400

9.21 Button and command input for teaching. . . . . . . . . . . . . . . . . . . . 3401

9.22 Ready position of the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . 3405

9.23 Base frame at the ready position of the PUMA 560 robot. . . . . . . . . 3405

9.24 t6 frame a the ready position of the PUMA 560 robot. . . . . . . . . . . 3406

9.25 Socket comminucation between host and target machines

during remdebug’s debugging session. . . . . . . . . . . . . . . . . . . . . . 3606

9.26 Socket comminucation between host and target machines

during mobdebug’s debugging session. . . . . . . . . . . . . . . . . . . . . 3647

9.27 Debugging test.lua with debug.lua (1). . . . . . . . . . . . . . . . . . . . . 3653

9.28 Debugging test.lua with debug.lua (2). . . . . . . . . . . . . . . . . . . . . 3654

9.29 Debugging test.lua with debug.lua (3). . . . . . . . . . . . . . . . . . . . . 3654

9.30 Debugging test.lua with ZeroBrane Studio (1). . . . . . . . . . . . . . . . 3656

9.31 Debugging test.lua with ZeroBrane Studio (2). . . . . . . . . . . . . . . . 3656

9.32 Debugging test.lua with ZeroBrane Studio (3). . . . . . . . . . . . . . . . 3657

9.33 Debugging test.lua with ZeroBrane Studio (4). . . . . . . . . . . . . . . . 3658

9.34 Debugging test.lua with ZeroBrane Studio (5). . . . . . . . . . . . . . . . 3658

9.35 Debugging test.lua with ZeroBrane Studio (6). . . . . . . . . . . . . . . . 3659

9.36 Debugging test.lua with ZeroBrane Studio (7). . . . . . . . . . . . . . . . 3659

9.37 Building a robot application in the RCCL system. . . . . . . . . . . . . . 3728

9.38 Reorganization of main.c and RCCL/RTC libraries. . . . . . . . . . . . . . 3740

9.39 Robot programming in Lua. . . . . . . . . . . . . . . . . . . . . . . . . . . . 3742

9.40 Running a robot application in the LuaRCCL system. . . . . . . . . . . . 3772

9.41 Debugging the spher.lua (1). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3775

9.42 Debugging the spher.lua (2). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3775

9.43 Debugging the spher.lua (3). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3776

9.44 Debugging the spher.lua (4). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3776

121

LIST OF FIGURES

9.45 Debugging the spher.lua (5). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3777

9.46 Debugging the spher.lua (6). . . . . . . . . . . . . . . . . . . . . . . . . . . . 3777

9.47 ZeroBrane Studio running on Windows OS (1). . . . . . . . . . . . . . . 3779

9.48 ZeroBrane Studio running on Windows OS (2). . . . . . . . . . . . . . . 3779

9.49 ZeroBrane Studio running on Windows OS (3). . . . . . . . . . . . . . . 3780

9.50 A simplified software structure of a robot controller. . . . . . . . . . . . 3788

9.51 World coordinate and local reference frames with Adept

RPY angles shown. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3796

9.52 XYZ Elements of the transformation (x = 30, y = 100, z = 125). . . 3797

9.53 Yaw angle of the transformation (θyaw = 30◦). . . . . . . . . . . . . . . . 3798

9.54 Pitch angle of the transformation (θpitch = 40◦). . . . . . . . . . . . . . . 3799

9.55 Roll angle of the transformation (θrol l = 20◦). . . . . . . . . . . . . . . . 3800

9.56 Axis identification of the Adept Viper s650 robot. . . . . . . . . . . . . . 3804

9.57 Side dimensions and work envelope of the Adept Viper

s650 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3805

9.58 Top dimensions and work envelope of the Adept Viper s650

robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3806

9.59 Link-coordinate-frame origins specified by DK(). . . . . . . . . . . . . . . 3808

9.60 Link coordinate frames of the Adept Viper s650 robot. . . . . . . . . . . 3811

9.61 Gripper model of the Viper S650 robot used in robot-sandbox. . . . . 3815

9.62 Link-coordinate-frame origins specified by changeRobotpos(). . . . . . . 3815

9.63 Zero pose of the Viper s650 robot with the gripper fully open. . . . . 3818

9.64 Zero pose of the Viper s650 robot with the gripper fully closed. . . . . 3819

9.65 Zero pose of the Viper s650 robot with the gripper fully

open (corrected). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3822

9.66 Motion parameters of the V+ system. . . . . . . . . . . . . . . . . . . . . . 3854

9.67 Motion parameters of the RCCL system. . . . . . . . . . . . . . . . . . . . 3856

9.68 Queue for storing motion requests in the RCCL system. . . . . . . . . . 3858

9.69 Implementation of the motion-request queue in the RCCL system. . . . 3859

9.70 CP switch setting for path blending. . . . . . . . . . . . . . . . . . . . . . . 3862

9.71 OPEN operation in blending mode. . . . . . . . . . . . . . . . . . . . . . . . 3869

9.72 Hand opens before reaching P1 in blending mode (OPEN). . . . . . . . 3869

9.73 OPEN operation in non-blending mode. . . . . . . . . . . . . . . . . . . . . 3870

9.74 Hand opens at P1 in non-blending mode (OPEN). . . . . . . . . . . . . . 3870

122

LIST OF FIGURES

9.75 OPENI operation in blending mode. . . . . . . . . . . . . . . . . . . . . . . 3871

9.76 Hand opens at P1 in blending mode (OPENI). . . . . . . . . . . . . . . . 3871

9.77 OPENI operation in non-blending mode. . . . . . . . . . . . . . . . . . . . 3872

9.78 Hand opens at P1 in non-blending mode (OPENI). . . . . . . . . . . . . 3872

9.79 Pick-and-place task. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3874

9.80 Hand opening while executing the RCCL program hand_open . . . . . 3878

9.81 Hand operation in a short deceleration region . . . . . . . . . . . . . . . 3879

9.82 Hand operation in a long deceleration region . . . . . . . . . . . . . . . . 3879

9.83 Synchronization among motion and hand operations when

hand_open() is used. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3883

9.84 Synchronization among motion and hand operations when

hand_openi() is used. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3883

9.85 Stop duration due to a hand_openi() call. . . . . . . . . . . . . . . . . . . 3884

9.86 movejnts_cp(part1,200,1000) places stop(0) operation be-

fore hccl_openi() operation. . . . . . . . . . . . . . . . . . . . . . . . . . . . 3890

9.87 Trajectory generator discards stop(0) motion request be-

fore openi()/closei() motion request. . . . . . . . . . . . . . . . . . . . . . 3891

9.88 stop(0) opertation before hccl_openi() operation is removed. . . . . . . 3892

9.89 Hardware structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 3905

9.90 Digital IO board connected to the main processor. . . . . . . . . . . . . . 3906

9.91 Robot controller with network-type servo drives and digital

IO modules. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3907

10.1 Dynamic model of a robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3913

10.2 Representation of a point P in different coordinate frames. . . . . . . . 3915

10.3 Denavit-Hartenberg kinematic parameters according to R.

P. Paul’s method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3918

10.4 Characterization of generic link i of a manipulator. . . . . . . . . . . . . 3918

10.5 Kinetic description of link i for Lagrange formulation. . . . . . . . . . . 3927

10.6 Kinetic description of motor i for Lagrange formulation. . . . . . . . . . 3932

10.7 Force F acting on a body. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3938

10.8 Moment N acting on a body. . . . . . . . . . . . . . . . . . . . . . . . . . . 3938

10.9 Free-body diagram of the augmented link i for Newton-

Euler formulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3939

123

LIST OF FIGURES

10.10 Forward and backward computations in the Newton-Euler formulation. 3944

10.11 Representation of a point P in different coordinate frames. . . . . . . . 3950

10.12 Zero position setting for the “lefty” PUMA 560 robot (Paul81). . . . . 3960

10.13 Zero position setting for the “righty” PUMA 560 robot (Paul86). . . . . 3961

10.14 The coordinate frames of the PUMA 560 in the zero-angles

position (Armstrong86). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3962

10.15 Screenshot of the GLroboop running PUMA 560 examples. . . . . . . . 3968

10.16 Screenshot of the JRoBoOp running PUMA 560 examples. . . . . . . . . 3968

10.17 SCARA robot shown with its coordinates frames. . . . . . . . . . . . . . . 3999

11.1 Servo controller structure for multiple joints. . . . . . . . . . . . . . . . . 4009

11.2 Structure of the semi-closed loop control. . . . . . . . . . . . . . . . . . . 4010

11.3 Robot control in joint space. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4012

11.4 Independent joint control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4013

11.5 Resolved motion rate control for telemanipulation. . . . . . . . . . . . . 4014

11.6 Resolved motion rate control with a trajectory generator. . . . . . . . . 4015

11.7 Resolved motion force control. . . . . . . . . . . . . . . . . . . . . . . . . . . 4019

11.8 Input, output, transfer function of a continuous-time system. . . . . . . 4019

11.9 Input, output, transfer function of a discrete-time system. . . . . . . . . 4023

11.10 Integrator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4028

11.11 Numerical integrations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4029

11.12 Sampled-data system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4033

11.13 Identification procedure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4035

11.14 System identification from input/output data. . . . . . . . . . . . . . . . . 4039

11.15 Distrubance v and output y. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4041

11.16 Correlated distrubance v and output y. . . . . . . . . . . . . . . . . . . . . 4048

11.17 Block diagram of a SMPMSM in the d-q coordinate system. . . . . . . 4053

11.18 Block diagram of a SMPMSM control system in the d-q co-

ordinate system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4053

11.19 Structure of 3-phase inverters. . . . . . . . . . . . . . . . . . . . . . . . . . . 4055

11.20 Representation of the 3-phase inverter operations with switch-

ing functions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4055

11.21 Voltage modulation using offset voltage. . . . . . . . . . . . . . . . . . . . 4057

11.22 Calculation of gate-firing time. . . . . . . . . . . . . . . . . . . . . . . . . . 4059

124

LIST OF FIGURES

11.23 Structure of a gate circuit. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4060

11.24 Command phase-volatege limiter and PWM-inverter model. . . . . . . . 4060

11.25 Current controller with counter-emf compensation and feed-

forward cross-coupling compensation in the d-q coordinate syste. . . . 4063

11.26 Fluctuation of the armature current. . . . . . . . . . . . . . . . . . . . . . . 4065

11.27 Block diagram of a current controller. . . . . . . . . . . . . . . . . . . . . . 4066

11.28 Bandwidth of a current loop according to current sampling

methods and PWM methods.. . . . . . . . . . . . . . . . . . . . . . . . . . . 4066

11.29 P-type current contoller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4066

11.30 PI-type current contoller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4068

11.31 General control scheme with saturation. . . . . . . . . . . . . . . . . . . . 4069

11.32 Anti-windup scheme with back calculation. . . . . . . . . . . . . . . . . . 4070

11.33 Anti-windup scheme for a current controller. . . . . . . . . . . . . . . . . 4071

11.34 Block diagram of a velocity control system. . . . . . . . . . . . . . . . . . 4072

11.35 Open-loop transfer function of the velocity control system. . . . . . . . 4073

11.36 Block diagram of a position control system. . . . . . . . . . . . . . . . . . 4075

11.37 Feedback and feedforward controllers. . . . . . . . . . . . . . . . . . . . . . 4080

11.38 Acceleration feedforward for velocity control. . . . . . . . . . . . . . . . . 4081

11.39 Torque feedforward for velocity control. . . . . . . . . . . . . . . . . . . . 4082

11.40 Torque feedforward for position control. . . . . . . . . . . . . . . . . . . . 4082

11.41 PID-type position controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4083

11.42 PID-type position controller using velocity feedback. . . . . . . . . . . . . 4083

11.43 PID-type position controller with torque feedforward. . . . . . . . . . . . 4084

11.44 Command velocity as the the output of a position controller. . . . . . . 4085

11.45 Command velocity input to a position control system. . . . . . . . . . . 4085

11.46 Conventional one-degree-of-freedom PID controller. . . . . . . . . . . . . 4089

11.47 Simplified Block diagram with the 1-DOF PID controller. . . . . . . . . . 4089

11.48 Feedforward-type two-degree-of-freedom PID controller. . . . . . . . . . 4090

11.49 Simplified Block diagram with the feedfordware-type 2-DOF

PID controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4091

11.50 Feedback-type two-degree-of-freedom PID controller. . . . . . . . . . . . . 4092

11.51 Simplified Block diagram with the feedback-type 2-DOF PID

controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4093

11.52 Setpoint-filter-type two-degree-of-freedom PID controller. . . . . . . . . . 4094

125

LIST OF FIGURES

11.53 Simplified Block diagram with the setpoint-filter-type 2-DOF

PID controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4094

11.54 Filter-and-preceded-derivative type two-degree-of-freedom PID

controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4097

11.55 Feedback-type two-degree-of-freedom PID controller with a

setpoint filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4098

11.56 Component-seperated-type two-degree-of-freedom PID controller. . . . . 4100

11.57 Block diagram of the Mark I controller. . . . . . . . . . . . . . . . . . . . . 4101

11.58 Friction model of the DC motor + gear + load. . . . . . . . . . . . . . . 4102

11.59 Friction versus velocity charateristics. . . . . . . . . . . . . . . . . . . . . . 4102

11.60 Block diagram of the current controller. . . . . . . . . . . . . . . . . . . . . 4103

11.61 Voltage feedback for power amplifiers. . . . . . . . . . . . . . . . . . . . . 4104

11.62 Simulink model of the current loop + motor + friction. . . . . . . . . . 4105

11.63 Block diagram of the velocity controller. . . . . . . . . . . . . . . . . . . . 4106

11.64 Block diagram of the position controller. . . . . . . . . . . . . . . . . . . . 4107

11.65 Simulink model of the position controller. . . . . . . . . . . . . . . . . . . 4107

11.66 Block diagram of the UNIVAL controller. . . . . . . . . . . . . . . . . . . . 4131

11.67 Block diagram of the UNIVAL servo system. . . . . . . . . . . . . . . . . . 4132

11.68 Detailed block diagram of the position and velocity controllers. . . . . 4132

11.69 Detailed block diagram of the current controller. . . . . . . . . . . . . . . 4133

11.70 Structure of a conventional servo controller. . . . . . . . . . . . . . . . . . 4133

11.71 Structure of the position and velocity controllers in the UNI-

VAL system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4134

11.72 PID position controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4134

11.73 Current control system with counter-emf compensation. . . . . . . . . . 4135

11.74 Current control system after the counter-emf compensation. . . . . . . 4136

11.75 PID + acceleration feedforward control in the Unival system. . . . . . 4137

11.76 Tachometer with a low-pass filter. . . . . . . . . . . . . . . . . . . . . . . . 4139

11.77 Low-pass filter as a velocity feedback filter. . . . . . . . . . . . . . . . . . 4139

11.78 Lead-lag and low-pass filter as a velocity feedback filter. . . . . . . . . . 4140

11.79 Current controller of the UNIVAL system. . . . . . . . . . . . . . . . . . . . 4143

11.80 H-bridge circuit for DC motors. . . . . . . . . . . . . . . . . . . . . . . . . . 4143

11.81 Structure of a generic position conroller. . . . . . . . . . . . . . . . . . . . 4145

11.82 4th-order model of motor + load. . . . . . . . . . . . . . . . . . . . . . . . 4149

126

LIST OF FIGURES

11.83 Block diagram of a power amplifier and a DC servomotor. . . . . . . . 4151

11.84 Block diagram of a power amplifier and a DC servomotor

when L ≈ 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4152

11.85 Block diagram of a position control system. . . . . . . . . . . . . . . . . . 4152

11.86 Disturbance torque T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4155

11.87 Joint servo with the compensation of Coulomb friction, static

friction, and gravity toque. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4157

11.88 Joint servo with velocity compensation. . . . . . . . . . . . . . . . . . . . . 4159

11.89 Joint servo with acceleration compensation. . . . . . . . . . . . . . . . . . 4160

11.90 Block diagram of a DC servomotor. . . . . . . . . . . . . . . . . . . . . . . 4162

11.91 Two-link planar robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4166

11.92 Link frames of the two-link planar robot (standard DH convention). . 4167

11.93 Link frames of the two-link planar robot (modified DH convention). . 4173

11.94 Zero-angle position of a PUMA robot (Armstrong86). . . . . . . . . . . . 4179

11.95 Block diagram of a servo controller with gravity compensation. . . . . 4188

11.96 ADC setup for reading motor current. . . . . . . . . . . . . . . . . . . . . . 4189

11.97 Installation of an ADC board in an Unimation controller. . . . . . . . . 4189

11.98 Joint servo in current control mode. . . . . . . . . . . . . . . . . . . . . . . 4189

11.99 Position control mode of the Unimation MK I controller. . . . . . . . . . 4190

11.100 Torque control mode of the Unimation MK I controller. . . . . . . . . . 4190

11.101 Example of a joint controller without joint coupling. . . . . . . . . . . . 4199

11.102 Example of a joint controller with joint coupling. . . . . . . . . . . . . . 4200

11.103 Two-inertia mechanical system. . . . . . . . . . . . . . . . . . . . . . . . . . 4202

11.104 Motor torque Te(s)에 대한 motor position θrm(s)의 주파수

특성. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4206

11.105 Block diagram of a two-inertia mechanical system. . . . . . . . . . . . . 4206

11.106 Motor and load with a compliant coupling. . . . . . . . . . . . . . . . . . 4208

11.107 Block diagram of compliantly coupled load. . . . . . . . . . . . . . . . . . 4208

11.108 Resonant and anti-resonant frequencies in the open-loop

frquency response. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4210

11.109 Control system with compliantly coupled load. . . . . . . . . . . . . . . . 4210

11.110 Effect of a low-pass filter on the opn loop frequency response. . . . . 4212

11.111 Effect of a notch filter on the opn loop frequency response. . . . . . . 4212

127

LIST OF FIGURES

11.112 Motor damping and load oscillation with a properly tuned

bi-quad filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4213

11.113 Rigid-body Luenberger observer. . . . . . . . . . . . . . . . . . . . . . . . . . 4214

11.114 Extended rigid-body Luenberger observer. . . . . . . . . . . . . . . . . . . 4215

11.115 Compliant-body observer. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4216

11.116 Frequency response of observed velocity error vs. distur-

bance torque. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4216

11.117 Disturbance torque observer. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4217

11.118 Acceleration feedback. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4218

11.119 Typical configuration of steel rolling mill system. . . . . . . . . . . . . . . 4220

11.120 Two-inertia system model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4220

11.121 Block diagram of the two-inertia system. . . . . . . . . . . . . . . . . . . . 4221

11.122 Bode plot of ωM/TM when R0 = 1. . . . . . . . . . . . . . . . . . . . . . . 4221

11.123 Speed derivative compensation. . . . . . . . . . . . . . . . . . . . . . . . . . 4221

11.124 Simulator following control system. . . . . . . . . . . . . . . . . . . . . . . 4222

11.125 Model following Two-DOF control system. . . . . . . . . . . . . . . . . . . 4222

11.126 State feedback and load acceleration control (SFLAC). . . . . . . . . . . 4222

11.127 Disturbance torque observer. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4223

11.128 Resonance ratio control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4223

11.129 Effect of the resonance ratio control. . . . . . . . . . . . . . . . . . . . . . 4224

11.130 Robot hand viewed as a Cantilever beam. . . . . . . . . . . . . . . . . . . 4225

11.131 Input shaper for vibration reduction. . . . . . . . . . . . . . . . . . . . . . . 4225

11.132 Response of the two input impulses. . . . . . . . . . . . . . . . . . . . . . . 4227

11.133 Generation of a convolved shaper for two vibration modes. . . . . . . . 4230

11.134 A example of wafer transfering wafers. . . . . . . . . . . . . . . . . . . . . 4231

11.135 3-DOF planer robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4232

11.136 Two inverse kinematic solutions of a 3R robot. . . . . . . . . . . . . . . . 4235

11.137 Joint angles for r-φ robots. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4236

11.138 Input, output and transfer function of a plant. . . . . . . . . . . . . . . . 4237

11.139 Command inputs to the WTR for model identification. . . . . . . . . . . 4238

11.140 Sensors used for vibration measurement. . . . . . . . . . . . . . . . . . . . 4240

11.141 Amplitude of the transfer function (|G(w)|). . . . . . . . . . . . . . . . . . 4240

11.142 g j(t) and ln g j(t) for the estimation of a j . . . . . . . . . . . . . . . . . . . 4242

11.143 Input shaper for orientation control. . . . . . . . . . . . . . . . . . . . . . . 4243

128

LIST OF FIGURES

11.144 Input shaper for servo control. . . . . . . . . . . . . . . . . . . . . . . . . . 4244

11.145 Effect of the input shaping. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4244

11.146 Input shaper for a digital controller. . . . . . . . . . . . . . . . . . . . . . . 4245

11.147 Impulse generated at time t i . . . . . . . . . . . . . . . . . . . . . . . . . . . 4245

11.148 Impulse generated at time t i . . . . . . . . . . . . . . . . . . . . . . . . . . . 4246

11.149 Sensitivity curves for various input shapers. . . . . . . . . . . . . . . . . . 4248

12.1 Force control options of industrial robots. . . . . . . . . . . . . . . . . . . 4255

12.2 The end effector of a robot is in contact with a rigid surface. . . . . . 4256

12.3 Disturbance torque T due to the gravity loading of a pay-

load or an external forces. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4267

12.4 Basic classification of robot compliance. . . . . . . . . . . . . . . . . . . . . 4269

12.5 Classification of passive compliance. . . . . . . . . . . . . . . . . . . . . . . 4269

12.6 Classification of active compliance control methods. . . . . . . . . . . . . 4270

12.7 An implementation of the Free joint method. . . . . . . . . . . . . . . . . 4273

12.8 Modified free joint method. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4276

12.9 Basic concept of operational-space motion control. . . . . . . . . . . . . . 4279

12.10 Exact linearization performed by inverse dynamics control. . . . . . . . 4280

12.11 Operational space inverse dynamics control system. . . . . . . . . . . . . 4281

12.12 Impedance and admittance of common mechanical elements. . . . . . . 4282

12.13 External force applied on the robot’s end-effector. . . . . . . . . . . . . . 4283

12.14 Impedance-controlled manipulator. . . . . . . . . . . . . . . . . . . . . . . . 4284

12.15 Admittance-controlled manipulator making contact with a

rigid wall. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4285

12.16 Concept of impedance control. . . . . . . . . . . . . . . . . . . . . . . . . . 4285

12.17 Block diagram of a impedance control system with force/-

torque measurements. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4286

12.18 Concept of admittance control. . . . . . . . . . . . . . . . . . . . . . . . . . 4291

12.19 Block diagram of an admittance control system with force/-

torque measurements. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4291

12.20 Robot end-effector contacting with a wall. . . . . . . . . . . . . . . . . . . 4294

12.21 Target stiffness k versus desired trajectory modification δx

at a given steady-state contact force 10N. . . . . . . . . . . . . . . . . . . 4296

12.22 Impedance control with inner velocity loop. . . . . . . . . . . . . . . . . . 4297

129

LIST OF FIGURES

12.23 Computation of xd − xc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4299

12.24 Computation of xd − xc in case M = 0. . . . . . . . . . . . . . . . . . . . . 4299

12.25 Admittance control system with inner velocity servo (I). . . . . . . . . . 4300

12.26 Compliance control system with inner velocity servo. . . . . . . . . . . . 4301

12.27 Admittance control system with inner joint velocity servo (I). . . . . . 4302

12.28 Admittance control system with inner joint position servo. . . . . . . . 4303

12.29 Computation of.xd −

.xc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4304

12.30 Computation of.xd −

.xc in case M = 0. . . . . . . . . . . . . . . . . . . . . 4304

12.31 Admittance control system with inner velocity servo (II). . . . . . . . . 4305

12.32 Admittance control system with inner joint velocity servo (II). . . . . . 4306

12.33 Position model-error impedance control scheme. . . . . . . . . . . . . . . 4307

12.34 Position model-error impedance control scheme without force

feedback. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4307

12.35 Force model error impedance control scheme. . . . . . . . . . . . . . . . . 4309

12.36 Force model-error impedance control scheme without force feedback. 4310

12.37 Human-robot collaboration for carrying heavy objects. . . . . . . . . . . 4312

12.38 Admittance control of the mobile-base robot’s end-effector. . . . . . . . 4313

12.39 Simulation run of the ridgeback_ur5_controller. . . . . . . . . . . . . . . . 4315

12.40 Cartesian admittance control system for handling manipu-

lator performance constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . 4316

12.41 Moving the end-effector with a translation towards its workspace

boundaries. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4317

12.42 Effects of the virtual force Fv on joint velocities during the

translational linear movement. . . . . . . . . . . . . . . . . . . . . . . . . . . 4318

12.43 A planar robot with two degrees-of-freedom. . . . . . . . . . . . . . . . . 4319

12.44 Block diagram of the impedance control demo (I). . . . . . . . . . . . . 4320

12.45 Initial configuration of the two-link robot. . . . . . . . . . . . . . . . . . . 4320

12.46 Impedance control the two-link robot in position control mode. . . . . 4321

12.47 Impedance control the two-link robot with non-zero distur-

bance force. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4322

12.48 Block diagram of the impedance control demo (II). . . . . . . . . . . . . 4323

12.49 Robot end-effector contacting with a wall. . . . . . . . . . . . . . . . . . . 4324

12.50 Two-link robot at the init position x(0). . . . . . . . . . . . . . . . . . . . 4328

12.51 Two-link robot at the equilibrium position xeq. . . . . . . . . . . . . . . . 4329

130

LIST OF FIGURES

12.52 Block diagram of an admittance control system. . . . . . . . . . . . . . . 4329

12.53 Block diagram of a simple admittance control system (I). . . . . . . . . 4330

12.54 Block diagram of a simple admittance control system (II). . . . . . . . . 4331

12.55 Positions P1, P2, P3, P4 and P5 specified for the manex8 demo. . . . 4340

12.56 Positions “align”, “touch” and “in” defined for the manex10 demo. . . 4343

12.57 Relationship between motor current Ii and joint torque τi . . . . . . . . 4373

12.58 Path transition occurs if forcedtrans = 1 at point P. . . . . . . . . . . . . 4384

12.59 Change of a velocity profile if forcedtrans = 1 at point P. . . . . . . . . 4385

12.60 Path transitions (Paul 1981). . . . . . . . . . . . . . . . . . . . . . . . . . . . 4389

12.61 n,o,a and p vectors of the end-effector frame W E. . . . . . . . . . . . . . 4394

12.62 T6 and end-effector frame E. . . . . . . . . . . . . . . . . . . . . . . . . . . 4395

12.63 Force control scheme implemented in MultiRCCL. . . . . . . . . . . . . . 4402

12.64 Robot end frame (wrist frame) and tool-tip frame. . . . . . . . . . . . . 4403

12.65 Sensor frame and tool tip frame: (a) mouting of a force/-

torque sensor, (b) sensor and tool-tip frames. . . . . . . . . . . . . . . . . 4404

12.66 Lead-through teaching. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4405

12.67 Walk-through teaching. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4405

12.68 Examples of natural and artificial constraints. . . . . . . . . . . . . . . . . 4407

12.69 Installation of a force/torque sensor and a Force Inteface

Card on an Adept robot system. . . . . . . . . . . . . . . . . . . . . . . . . 4418

12.70 Effect of loading position and forces and moments in force

reference frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4420

12.71 Bit settings of parameters parm0 and parm3. . . . . . . . . . . . . . . . . 4426

12.72 Structure of a “simple” virtual force sensor. . . . . . . . . . . . . . . . . . 4435

12.73 Structure of a virtual force sensor based on the state-space

model and the Kalman filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4439

12.74 Cartesian admittance control with a virtual force sensor. . . . . . . . . . 4440

12.75 Cartesian admittance control with zero force feedback. . . . . . . . . . . 4441

13.1 Hardware structure of a robot controller. . . . . . . . . . . . . . . . . . . . 4450

13.2 FIFO communication between main processor and servo controller. . . 4450

13.3 Software structure of the main processor. . . . . . . . . . . . . . . . . . . 4451

13.4 Monitor commands for program execution. . . . . . . . . . . . . . . . . . 4452

13.5 Lex spec. and Yacc grammar for the move instruction. . . . . . . . . . . 4453

131

LIST OF FIGURES

13.6 C-language routine for executing “move point” program statement. . . 4454

13.7 Diagram of the workspace for a grasp task. . . . . . . . . . . . . . . . . . 4456

13.8 Frame transformation graph for the grasp task. . . . . . . . . . . . . . . . 4456

13.9 Frames and transforms for the grasp task superimposed on

the workspace diagram. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4457

13.10 Ring data structure for a position equation. . . . . . . . . . . . . . . . . . 4459

13.11 Ring structure for the position equation p2. . . . . . . . . . . . . . . . . . 4461

13.12 Straight-line motions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4465

13.13 Constant velocity profiles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4466

13.14 Transition between two path segments. . . . . . . . . . . . . . . . . . . . . 4466

13.15 Hardware structure of a servo controller. . . . . . . . . . . . . . . . . . . . 4470

13.16 Software structure of a servo controller (in pseudo codes). . . . . . . . 4471

13.17 Reference positions without the linear interpolation. . . . . . . . . . . . . 4472

13.18 Reference positions with the linear interpolation. . . . . . . . . . . . . . . 4472

14.1 Visualization of the barrirer synchronization mechanism. . . . . . . . . . 4478

14.2 Electronic gearing. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4479

14.3 Linear and circular interpolation for sealant dispensing. . . . . . . . . . 4480

14.4 Linear interpolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4481

14.5 Approximation of chord Pi Pi+1 with line segment Pi Pi+1. . . . . . . . . 4481

14.6 Typical XY robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4482

14.7 Single-belt XY robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4483

14.8 Motion control system composed of a conveyor, a label feeder,

and sensors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4484

14.9 Synchronization of multi-robot motions. . . . . . . . . . . . . . . . . . . . . 4484

14.10 A typical configuration of industrial robot system. . . . . . . . . . . . . . 4488

14.11 Multiple-robot controller composed of a supervisory con-

troller and individual robot controllers (star-type connection). . . . . . 4488

14.12 Hardware configuration of the MultiRCCL for two PUMA robots. . . . 4489

14.13 Multiple-robot controller composed of a supervisory con-

troller and individual controllers (bus-type connection). . . . . . . . . . 4490

14.14 Hardware configuration of the MultiRCCL for two PA-10 robots. . . . . 4490

14.15 Multiple-robot controller composed of a single main con-

troller and multiple servo controllers. . . . . . . . . . . . . . . . . . . . . . 4491

132

LIST OF FIGURES

14.16 Hardware configuration of the ABB’s multi-robot controllers. . . . . . . 4491

14.17 Single-robot controller with multiple axis controllers. . . . . . . . . . . . 4492

14.18 Unimation MK I robot controller composed of a single main

controller and six axis controllers. . . . . . . . . . . . . . . . . . . . . . . . 4493

14.19 Pulse-type motion control system. . . . . . . . . . . . . . . . . . . . . . . . 4493

14.20 Network-type motion control systems (SSNET). . . . . . . . . . . . . . . . 4494

14.21 Multiple-robot controller composed of a main controller and

multiple axis controllers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4495

14.22 ABB FRIDA dual-arm robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4495

14.23 Yaskawa SDA20D dual-arm robot. . . . . . . . . . . . . . . . . . . . . . . . 4496

14.24 Twin-arm wafer transfer robot. . . . . . . . . . . . . . . . . . . . . . . . . . 4496

14.25 Hardware structure of a dual-arm robot controller. . . . . . . . . . . . . 4497

14.26 Dual- and single-cabinet-type IRC5 controller. . . . . . . . . . . . . . . . . 4498

14.27 Hardware configuration for the MultiMove. . . . . . . . . . . . . . . . . . 4499

14.28 UnsyncArc example of independent movements. . . . . . . . . . . . . . . 4499

14.29 SyncArc example in which two robots work on the same

object held by a positioner. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4500

14.30 SyncArc example of semi-coordinated movements. . . . . . . . . . . . . . 4501

14.31 SyncArc example of coordinated synchronized movements. . . . . . . . 4501

14.32 SyncSpot example in which two robots work on the same

moving object. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4502

14.33 SyncSpot example of coordinated synchronized movements. . . . . . . . 4502

14.34 UnsyncArc example with two robots. . . . . . . . . . . . . . . . . . . . . . 4508

14.35 Synchronized motion start. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4516

14.36 Synchronized motion time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4517

14.37 Geometric coupling of two robots. . . . . . . . . . . . . . . . . . . . . . . . 4518

14.38 Direct geometric coupling of two robots. . . . . . . . . . . . . . . . . . . . 4519

14.39 Cooperation: Loading sharing. . . . . . . . . . . . . . . . . . . . . . . . . . . 4520

14.40 Cooperation: Process-dependent procedure. . . . . . . . . . . . . . . . . . 4520

14.41 Cooperation: Extended process-dependent procedure. . . . . . . . . . . . 4520

14.42 Cooperation: Combined procedure. . . . . . . . . . . . . . . . . . . . . . . . 4521

14.43 Cooperation: Extended master-slave principle. . . . . . . . . . . . . . . . . 4522

14.44 Shared workspace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4522

14.45 Uncontrolled user of a shared workspace. . . . . . . . . . . . . . . . . . . 4523

133

LIST OF FIGURES

14.46 Controlled use of a shared workspace. . . . . . . . . . . . . . . . . . . . . 4523

14.47 Hardware Components of a KUKA robot system. . . . . . . . . . . . . . . 4524

14.48 Overview of a KR C2 robot controller. . . . . . . . . . . . . . . . . . . . . . 4525

14.49 Hardware architecture of a CR (Cooperating Robots) controller. . . . . 4525

14.50 Network cabling for a CR (Cooperating Robots) controller. . . . . . . . 4526

14.51 CR cabling for a CR (Cooperating Robots) controller. . . . . . . . . . . . 4527

14.52 Software architecture of a CR controller. . . . . . . . . . . . . . . . . . . . 4528

14.53 Function diagram of the Safety Selection Board. . . . . . . . . . . . . . . 4528

14.54 Types of cooperation created by PROSYNC, GEOLINK, SYNC

commands. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4531

14.55 A station defined as a positioner holding a workpiece for

a manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4536

14.56 Coordinated positioner system. . . . . . . . . . . . . . . . . . . . . . . . . . 4536

14.57 Jigless system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4537

14.58 Cooordinated and individual operations in a jigless system. . . . . . . . 4538

14.59 Twin synchronous system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4539

14.60 Subtask jobs of a twin synchronous system created by teach-

ing and copying. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4539

14.61 Subtask jobs of a twin synchronous system created by teach-

ing for each manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4540

14.62 Triple synchronous system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4541

14.63 Subtask jobs of a triple synchronous system created by teach-

ing and copying. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4541

14.64 Subtask jobs of a triple synchronous system created by teach-

ing for each manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4542

14.65 Example of job teaching in a jigless system. . . . . . . . . . . . . . . . . . 4549

14.66 Synchronized mode in teaching. . . . . . . . . . . . . . . . . . . . . . . . . . 4550

14.67 Single mode in teaching. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4550

14.68 Example of non-synchronous operations. . . . . . . . . . . . . . . . . . . . 4551

14.69 Example of synchronous operations. . . . . . . . . . . . . . . . . . . . . . . 4552

14.70 Hardware configuration of the MultiRCCL with a MicroVax-

II host. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4553

14.71 Interface between a MicroVAX-II host and an Unimation controller. . . 4554

134

LIST OF FIGURES

14.72 Hardware configuration of the Multirccl with a SUN work-

station host. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4555

14.73 Interface between a SUN workstation host and an Unima-

tion controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4555

14.74 Hardware configuration of the MultiRCCL with a MicroVax-

II host and two PUMA robots. . . . . . . . . . . . . . . . . . . . . . . . . . . 4556

14.75 JPL telerobotic S&P and MCM subsystems (I). . . . . . . . . . . . . . . . 4557

14.76 Hardware configuration of the Multirccl with a SUN work-

station host and two PUMA robots. . . . . . . . . . . . . . . . . . . . . . . 4558

14.77 JPL telerobotic S&P and MCM subsystems (II). . . . . . . . . . . . . . . . 4559

14.78 Hardware configuration of the RWRCCL for two PUMA robots. . . . . 4561

14.79 Mitsuibishi PA10-6C robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4561

14.80 Mitsuibishi PA10-7C robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4562

14.81 Hardware configuration of the MultiRCCL for two PA-10 robots. . . . . 4562

14.82 CPUs and their tasks in a Multi-RCCL system. . . . . . . . . . . . . . . . 4564

14.83 Software structure of Multi-RCCL. . . . . . . . . . . . . . . . . . . . . . . . 4565

14.84 Kinematic diagram of a master-slave robot system. . . . . . . . . . . . . 4575

14.85 Kinematic diagram of a master-slave robot system with com-

pliance control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4576

14.86 Two robots holding a common object. . . . . . . . . . . . . . . . . . . . . . 4577

14.87 Kinematic diagram for two robots holding a common object. . . . . . . 4578

14.88 Kinematic diagram of the trackII.560 demo. . . . . . . . . . . . . . . . . . 4579

14.89 Kinematic diagram of the trackII.560 demo for the manip-

ulator 1 and a virtual manipulator. . . . . . . . . . . . . . . . . . . . . . . . 4579

14.90 here and lastTC transforms in a transition region. . . . . . . . . . . . . . 4589

14.91 Execution of monitor functions at each trajectory genera-

tion task. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4604

14.92 Rendezvous operations between trajectory generation tasks

executed on separate CPUs. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4605

14.93 Comminucation between the trajectory generator and the

servo controller with no control lag. . . . . . . . . . . . . . . . . . . . . . . 4606

14.94 Default comminication sequence between the trajectory gen-

erator and the servo controller. . . . . . . . . . . . . . . . . . . . . . . . . . 4607

135

LIST OF FIGURES

14.95 robotsim graphic simulator waiting for incoming set-points

or commands. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4625

14.96 robotsim graphic simulator with a teach pendant panel. . . . . . . . . . 4634

14.97 robotsim window displaying two robots. . . . . . . . . . . . . . . . . . . . 4637

15.1 Hardware structure of a robot controller. . . . . . . . . . . . . . . . . . . . 4691

15.2 Adept teach pendant: Model 133. . . . . . . . . . . . . . . . . . . . . . . . 4691

15.3 Implementation of the main controller with a x86 processor. . . . . . . 4693

15.4 Implementation of the main controller with an ARM processor. . . . . 4694

15.5 Hardware structure of a single-axis servo drive. . . . . . . . . . . . . . . 4695

15.6 Implementation of a single-axis servo drive with a DSP. . . . . . . . . . 4695

15.7 Block diagram of a servo processor board. . . . . . . . . . . . . . . . . . . 4697

15.8 Implementation example of a servo controller. . . . . . . . . . . . . . . . 4697

15.9 Hardware implementation of a robot controller. . . . . . . . . . . . . . . 4698

15.10 Architecture of a generic Linux system. . . . . . . . . . . . . . . . . . . . . 4700

15.11 Architecture of a RTAI/Linux system. . . . . . . . . . . . . . . . . . . . . . 4701

15.12 Robot-specific application layer. . . . . . . . . . . . . . . . . . . . . . . . . . 4702

16.1 Boot process of Linux. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4707

16.2 Settings menu of the Xubuntu 11.04. . . . . . . . . . . . . . . . . . . . . . 4724

16.3 Adding Korean input method in the ibus setting. . . . . . . . . . . . . . . 4724

16.4 CompactFlash-to-IDE adapter. . . . . . . . . . . . . . . . . . . . . . . . . . . 4727

16.5 CompactFlash-to-SATA adapter. . . . . . . . . . . . . . . . . . . . . . . . . . . 4728

16.6 Block diagram of the Intel P53 chipset. . . . . . . . . . . . . . . . . . . . . 4744

16.7 Block diagram of the C2SBC-Q Motherboard. . . . . . . . . . . . . . . . . 4745

16.8 Block diagram of the Beagleboard. . . . . . . . . . . . . . . . . . . . . . . . 4747

16.9 An example of the printer port isolation. . . . . . . . . . . . . . . . . . . . 4749

16.10 A typical isolation scheme for DC input. . . . . . . . . . . . . . . . . . . . 4750

16.11 A typical isolation scheme for DC output. . . . . . . . . . . . . . . . . . . 4750

16.12 A typical isolation scheme for AC input. . . . . . . . . . . . . . . . . . . . 4750

16.13 A typical isolation scheme for AC output. . . . . . . . . . . . . . . . . . . 4750

16.14 IDE-to-CompactFlash adapter. . . . . . . . . . . . . . . . . . . . . . . . . . . 4751

16.15 Schematic diagram of an IDE-to-CompactFlash adapter. . . . . . . . . . . 4752

16.16 SATA-to-CompactFlash adapter. . . . . . . . . . . . . . . . . . . . . . . . . . . 4752

16.17 Block diagram of the SATA-to-CompactFlash adapter. . . . . . . . . . . . 4752

136

LIST OF FIGURES

16.18 8255 parallel I/O card for the 8-bit ISA bus. . . . . . . . . . . . . . . . . 4753

16.19 Two 8255 connection to a 16-bit data bus. . . . . . . . . . . . . . . . . . 4754

16.20 Block diagram of an ISA parallel I/O card. . . . . . . . . . . . . . . . . . 4754

16.21 MOXA TCC-82 port-powered RS232 4-channel isolator. . . . . . . . . . . 4755

16.22 Block diagram of a RS232 isolator. . . . . . . . . . . . . . . . . . . . . . . . 4755

16.23 Schematic diagram of a RS232 isolator (I). . . . . . . . . . . . . . . . . . 4756

16.24 Schematics of a RS232C isolator (II). . . . . . . . . . . . . . . . . . . . . . 4756

16.25 USB-to-RS232 converter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4757

16.26 Schematic diagram of an USB-to-RS232 converter. . . . . . . . . . . . . . 4757

16.27 OXuPCI954 evaluation board. . . . . . . . . . . . . . . . . . . . . . . . . . . 4759

16.28 Isolation scheme of a serial port. . . . . . . . . . . . . . . . . . . . . . . . . 4759

16.29 Advantech PCI-1612AU multi-port serial card. . . . . . . . . . . . . . . . . 4760

16.30 SUNIX 5037T multi-port serial card. . . . . . . . . . . . . . . . . . . . . . . 4762

16.31 Hilscher CC-link slave card. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4763

16.32 Block diagram of the Hilscher CC-link slave card. . . . . . . . . . . . . . 4764

16.33 Hilscher Profibus master card. . . . . . . . . . . . . . . . . . . . . . . . . . . 4764

16.34 Block diagram of the PLXtech 9030RDK board. . . . . . . . . . . . . . . . 4765

16.35 Block diagram of the PLXtech 9054RDK-860 board. . . . . . . . . . . . . 4765

16.36 Software structure of the Hilscher cc-link driver. . . . . . . . . . . . . . . 4766

16.37 DPM layout of the Hilscher cc-link driver. . . . . . . . . . . . . . . . . . . 4768

16.38 RTDM skin of the Xenomai. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4772

16.39 RTDM module of the RTAI. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4772

16.40 API of the RTDM. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4773

16.41 Conventional kernel-mode device driver. . . . . . . . . . . . . . . . . . . . 4782

16.42 UIO device driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4783

16.43 Hilscher CIFX driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4783

16.44 Amplicon PCI272 digital I/O card. . . . . . . . . . . . . . . . . . . . . . . . 4791

16.45 Schematic diagram of the PCI272 card (I). . . . . . . . . . . . . . . . . . 4792

16.46 Schematic diagram of the PCI272 card (II). . . . . . . . . . . . . . . . . . 4793

16.47 Block diagram of a PCI parallel I/O card. . . . . . . . . . . . . . . . . . . 4794

16.48 NI PCI-6518 digital I/O card. . . . . . . . . . . . . . . . . . . . . . . . . . . 4794

16.49 JS Automation DIO-3248 digital I/O card. . . . . . . . . . . . . . . . . . . 4795

16.50 Local bus signals of the PCI9052 according to its modes. . . . . . . . . 4796

16.51 PCI digital I/O board with input buffers and output latches. . . . . . . 4797

137

LIST OF FIGURES

16.52 Adlink PCI-7432 isolated 32-CH DI and 32-CH DO card. . . . . . . . . . 4797

16.53 PCI configuration registor of the PCI9052. . . . . . . . . . . . . . . . . . . 4804

16.54 Interrupt latch circuit of the PCI7432 card. . . . . . . . . . . . . . . . . . 4841

16.55 Local control registers of the PCI9052. . . . . . . . . . . . . . . . . . . . . 4842

16.56 Interrupt control and status register of the PCI9052. . . . . . . . . . . . 4844

16.57 I/O port-point mapping table of DIO-3248. . . . . . . . . . . . . . . . . . 4875

16.58 Index-mode addressing of DIO-3248 ports. . . . . . . . . . . . . . . . . . . 4875

16.59 Operation of the 16-bit index-mode addressing. . . . . . . . . . . . . . . 4875

16.60 Flow of a DIO-3248 application program. . . . . . . . . . . . . . . . . . . 4881

16.61 Interrupt routing in DIO-3248. . . . . . . . . . . . . . . . . . . . . . . . . . 4882

16.62 Interrupt setting sequence in a DIO-3248 application program. . . . . . 4882

16.63 Interrupt mask register in the CPLD of DIO-3248. . . . . . . . . . . . . . 4890

16.64 DIO-3248 card and its accessories. . . . . . . . . . . . . . . . . . . . . . . . 4909

16.65 Wiring board for a DIO-3248 card. . . . . . . . . . . . . . . . . . . . . . . . 4910

16.66 Example of a control panel for DIO-3248 cards. . . . . . . . . . . . . . . 4915

16.67 Hardware structure of a generic PCI board. . . . . . . . . . . . . . . . . . 4916

16.68 Hardware structure of a FPGA-based PCI board. . . . . . . . . . . . . . . 4916

16.69 PCI/Wishbone system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4917

16.70 Single MASTER/Single SLAVE interconnection example. . . . . . . . . . 4918

16.71 Classic standard single READ cycle of the Wishbone bus. . . . . . . . . 4925

16.72 Classic standard single WRITE cycle of the Wishbone bus. . . . . . . . . 4926

16.73 A simple 8-bit WISHBONE slave output port. . . . . . . . . . . . . . . . . 4927

16.74 Simulation result of the wboport08.v. . . . . . . . . . . . . . . . . . . . . . 4934

16.75 Data flow interconnection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4939

16.76 Crossbar interconnection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4940

16.77 Shared-bus interconnection. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4940

16.78 Example of the shared-bus interconnection. . . . . . . . . . . . . . . . . . 4941

16.79 RTL schematic of the wb_intercon.vhd. . . . . . . . . . . . . . . . . . . . . 4947

16.80 Altera MAX-II development board. . . . . . . . . . . . . . . . . . . . . . . . 4948

16.81 Implementation of a PCI serial board with pci32tlite_oc. . . . . . . . . . 4948

16.82 RTL view of the top-module maxii_uart. . . . . . . . . . . . . . . . . . . . 4958

16.83 16550A registers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4962

16.84 minicom setup for /dev/ttyS4. . . . . . . . . . . . . . . . . . . . . . . . . . . 4963

16.85 Loopback test with minicom. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4963

138

LIST OF FIGURES

16.86 Enterpoint RaggedStone 1 board. . . . . . . . . . . . . . . . . . . . . . . . . 4964

16.87 Custom ADC daughter board. . . . . . . . . . . . . . . . . . . . . . . . . . . 4964

16.88 Top-level RTL schematic of the lpcd-scope. . . . . . . . . . . . . . . . . . . 4966

16.89 Hardware structure of a PCI parallel IO board. . . . . . . . . . . . . . . . 4986

16.90 Standard registers of PCI Type 0 (non-bridge) configura-

tion space header. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5006

16.91 Hardware structure of a generic PCIe board. . . . . . . . . . . . . . . . . 5032

16.92 Hardware structure of a FPGA-based PCIe board. . . . . . . . . . . . . . 5033

16.93 ECP5 Versa Development board. . . . . . . . . . . . . . . . . . . . . . . . . . 5034

16.94 Software directory structure of the ECP5 Versa Develop-

ment board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5035

16.95 Opening the programmer prokect file of the PCIeBasic demo. . . . . . 5036

16.96 Opening the programmer prokect file of the PCIeBasic demo. . . . . . 5037

16.97 Setting up the device properties for SPI flash programming. . . . . . . 5038

16.98 USB commication wiith a USB-to-UART bridge. . . . . . . . . . . . . . . . 5081

16.99 Block diagram of the FTDI FT232H. . . . . . . . . . . . . . . . . . . . . . . 5082

16.100 RS232 configuration the FTDI FT232H. . . . . . . . . . . . . . . . . . . . . 5083

16.101 Hardware setting of the FTDI FT232H for an USB-to-serial converter. 5084

16.102 Driver setting of the FTDI FT232H for an USB-to-serial converter. . . . 5085

16.103 UM232H: single channel USB hi-speed FT232H develop-

ment module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5086

16.104 UART-related pins of the FT232H. . . . . . . . . . . . . . . . . . . . . . . . 5087

16.105 Hardware setup of the UM232H for loopback test. . . . . . . . . . . . . 5087

16.106 Loopback test with FTDI terminal.exe. . . . . . . . . . . . . . . . . . . . . . 5088

16.107 USB commication wiith a USB-to-FIFO bridge. . . . . . . . . . . . . . . . 5092

16.108 Block diagram of the FTDI USB FIFO IC. . . . . . . . . . . . . . . . . . . . 5093

16.109 Hardware setup of the FT2232H as a 245-style USB-to-

FIFO bridge. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5094

16.110 FT2232H’s pins used in the FT245-style asynchronous FIFO

interface mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5095

16.111 Timing diagram of a read operation in the FT245-style asyn-

chronous FIFO interface mode. . . . . . . . . . . . . . . . . . . . . . . . . . 5096

16.112 Timing diagram of a write operation in the FT245-style

asynchronous FIFO interface mode. . . . . . . . . . . . . . . . . . . . . . . 5096

139

LIST OF FIGURES

16.113 FT2232H’s pins used in the FT245-style synchronous FIFO

interface mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5097

16.114 Timing diagram of a read operation in the FT245-style syn-

chronous FIFO interface mode. . . . . . . . . . . . . . . . . . . . . . . . . . 5098

16.115 Timing diagram of a write operation in the FT245-style

synchronous FIFO interface mode. . . . . . . . . . . . . . . . . . . . . . . . 5098

16.116 USB arbiter for a target CPU to access the FT2232H. . . . . . . . . . . . 5098

16.117 I/Os of the USB arbiter for loopback test. . . . . . . . . . . . . . . . . . . 5099

16.118 Hardware setup for loopback test: FPGA + FT2232H. . . . . . . . . . . 5106

16.119 Connection between a FT2232H and a FPGA for the FT245-

style synchronous FIFO interface mode. . . . . . . . . . . . . . . . . . . . . 5114

16.120 Hardware setup for the ds18b20_seg7.qpf project. . . . . . . . . . . . . . 5138

16.121 Loading the ds18b20_seg7.qpf project file. . . . . . . . . . . . . . . . . . . 5138

16.122 Compilation report of the ds18b20_seg7 project. . . . . . . . . . . . . . . 5148

16.123 “FILE->Convert Programming Files”. . . . . . . . . . . . . . . . . . . . . . . 5149

16.124 “Convert Programming Files” window. . . . . . . . . . . . . . . . . . . . . . 5149

16.125 “Select Devices” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5150

16.126 “Add File”. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5150

16.127 “Select Input File” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5151

16.128 “Select Devices” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5152

16.129 “Programmer” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5152

16.130 Running the ds18b20_seg7 demo. . . . . . . . . . . . . . . . . . . . . . . . 5153

16.131 Design security solution setup using IFF concept. . . . . . . . . . . . . . 5154

16.132 Design security flow using IFF concept. . . . . . . . . . . . . . . . . . . . . 5155

16.133 Top Level of the CIII_Design_Security_Enabler. . . . . . . . . . . . . . . . 5156

16.134 Top Level of the SHA-1 IFF engine. . . . . . . . . . . . . . . . . . . . . . . 5156

16.135 OpenEP3C16-C Altera development board. . . . . . . . . . . . . . . . . . . 5157

16.136 CoreEP3C16 Altera core board. . . . . . . . . . . . . . . . . . . . . . . . . . 5158

16.137 DVK600 FPGA/CPLD core board. . . . . . . . . . . . . . . . . . . . . . . . . 5158

16.138 Connecting a DS28E01 to the OpenEP3C16-C FPGA board. . . . . . . . 5158

16.139 Altera’s IFF demo running on the OpenEP3C16-C FPGA board. . . . . . 5164

17.1 Hardware structure of the Unimation MK I system. . . . . . . . . . . . . 5167

17.2 Robot controller structure (hardware platform). . . . . . . . . . . . . . . . 5168

140

LIST OF FIGURES

17.3 Structure of an electric drive. . . . . . . . . . . . . . . . . . . . . . . . . . . 5169

17.4 Structure of a servo drive. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5170

17.5 Block diagram of a typical DSP-based servo drive. . . . . . . . . . . . . . 5171

17.6 Servo controller in a robot controller. . . . . . . . . . . . . . . . . . . . . . 5171

17.7 TMDSHVMTRPFCKIT. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5173

17.8 Hardware setup for the PMSM3-4 demo. . . . . . . . . . . . . . . . . . . . 5173

17.9 ezDSP F2812 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5174

17.10 Block diagram of the ezDSP F2812 board. . . . . . . . . . . . . . . . . . . 5174

17.11 DMC550 Digital Motor Controller. . . . . . . . . . . . . . . . . . . . . . . . 5175

17.12 Permanent-magnet AC motor drive system. . . . . . . . . . . . . . . . . . . 5176

17.13 Block diagram of the DMC550 digital motor controller. . . . . . . . . . . 5176

17.14 DMC1500 Digital Motor Controller with a DSP board. . . . . . . . . . . 5177

17.15 Hardware setup of the PMSM3-4 demo with an external emulator. . . 5178

17.16 Input and outputs between the ezDSP F2812 board and

the DMC550 module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5178

17.17 Block diagram of a “soft servo controller” for the control

of a PMSM motor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5179

17.18 Implementation of a “soft servo controller” using the TI’s

DMC libarary (Phase 6 demo of the PMSM3-4). . . . . . . . . . . . . . . 5180

17.19 Flowchart of the pmsm3_4.c. . . . . . . . . . . . . . . . . . . . . . . . . . . 5181

17.20 Hardware setup of the Freescale RD56F84789 reference design. . . . . 5182

17.21 TWR-56F8400 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5183

17.22 Block diagram of the TWR-56F8400 board. . . . . . . . . . . . . . . . . . 5183

17.23 TWR-MC-LV3PH motor control module. . . . . . . . . . . . . . . . . . . . . 5184

17.24 Block diagram of the TWRMCLV3PH module. . . . . . . . . . . . . . . . . 5185

17.25 Signal connection between the TWR-56F8400 tower board

and the TWR-MC-LV3PH module. . . . . . . . . . . . . . . . . . . . . . . . . 5186

17.26 Software and hardware blocks of the TWR-56F8400 tower

board for PMSM motor control. . . . . . . . . . . . . . . . . . . . . . . . . . 5187

17.27 Software structure of the TWR-56F8400 tower board. . . . . . . . . . . 5187

17.28 Panasinic servomotor and servo drive (MINAS A4 series). . . . . . . . . 5188

17.29 Wiring example of Panasonic MINAS A4 servo drives. . . . . . . . . . . 5189

17.30 Pin map of the Panasonic A4 servo drive’s CX-5 connector. . . . . . . . 5190

17.31 Block diagram of the Panasonic Minas A4 servo drives. . . . . . . . . . 5192

141

LIST OF FIGURES

17.32 Hardware structure of a single-axis servo drive. . . . . . . . . . . . . . . 5193

17.33 Type-1 multi-axis servo controller. . . . . . . . . . . . . . . . . . . . . . . . 5193

17.34 Pulse-type motion control systems. . . . . . . . . . . . . . . . . . . . . . . . 5194

17.35 A example of network-type motion control systems: SSNET. . . . . . . 5194

17.36 Type-2 multi-axis servo controller. . . . . . . . . . . . . . . . . . . . . . . . 5196

17.37 Performance of TI C2000 Series DSPs. . . . . . . . . . . . . . . . . . . . . 5197

17.38 Type-3 multi-axis servo controller. . . . . . . . . . . . . . . . . . . . . . . . 5199

17.39 A hybid of type-1 and type-2 servo controller. . . . . . . . . . . . . . . . 5200

17.40 Block diagram of a servo processor board. . . . . . . . . . . . . . . . . . . 5200

17.41 Implementation of a servo processor board. . . . . . . . . . . . . . . . . . 5201

17.42 Mitsuibishi DIPIPM PS21A79. . . . . . . . . . . . . . . . . . . . . . . . . . . 5202

17.43 Block diagram of Mitsuibishi DIP-IPM PS21A7x. . . . . . . . . . . . . . . 5203

17.44 Inteface cuircuit of DIP-IPM PS21A7x with optocouplers. . . . . . . . . . 5203

17.45 Mitsuibishi EVBPS2xA7x evaluation board. . . . . . . . . . . . . . . . . . . 5204

17.46 Schematic diagram of the EVBPS2xA7x evaluation board. . . . . . . . . 5205

17.47 Conducted EMI generated by spray capacitance. . . . . . . . . . . . . . . 5206

17.48 Conducted EMI generated by mutual inductance. . . . . . . . . . . . . . . 5206

17.49 Common-mode current path in an AC drive system. . . . . . . . . . . . . 5207

17.50 Better wiring of the AC drive system shown in Fig.17.49. . . . . . . . . 5208

18.1 Hardware structure of a robot controller. . . . . . . . . . . . . . . . . . . . 5214

18.2 Hardware structure of Unimation MK I system. . . . . . . . . . . . . . . . 5215

18.3 Servo controller implemented with multiple single-axis servo

drives. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5216

18.4 Servo controller implemented with network-based servo drives. . . . . 5216

18.5 Robot controller with network-based servo drives (I). . . . . . . . . . . . 5217

18.6 Robot controller with network-based servo drives (II). . . . . . . . . . . 5217

18.7 Implementation example of an ethercat-based robot controller. . . . . . 5218

18.8 Hardware configuration of the RCCL v1.0 system. . . . . . . . . . . . . . 5219

18.9 Main and servo controllers in the RCCL v1.0 system. . . . . . . . . . . . 5219

18.10 Software structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 5220

18.11 Hardware structure of an ethercat-based robot controller. . . . . . . . . 5221

18.12 Main and servo processes in an ethercat-based robot controller. . . . . 5221

18.13 Software structure of an ethercat-based robot controller. . . . . . . . . . 5222

142

LIST OF FIGURES

18.14 Position control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5224

18.15 Position control block diagram of Panasonic A5B servo drives. . . . . . 5225

18.16 Position control block diagram of Omron G5-series servo drives. . . . . 5226

18.17 CiA 402 Finite State Automation. . . . . . . . . . . . . . . . . . . . . . . . . 5227

18.18 Bit assigment in the control word. . . . . . . . . . . . . . . . . . . . . . . . 5229

18.19 PDS commands in the control word. . . . . . . . . . . . . . . . . . . . . . . 5230

18.20 Mode-dependent bits in the control word. . . . . . . . . . . . . . . . . . . 5230

18.21 Bit assigment in the status word. . . . . . . . . . . . . . . . . . . . . . . . . 5231

18.22 States of the state machine. . . . . . . . . . . . . . . . . . . . . . . . . . . . 5231

18.23 Mode-dependent bits in the status word. . . . . . . . . . . . . . . . . . . . 5232

18.24 Internal limit factors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5233

18.25 Supported drive modes (6502h). . . . . . . . . . . . . . . . . . . . . . . . . 5234

18.26 Modes of operation (6060h). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5235

18.27 Modes of operation display (6061h). . . . . . . . . . . . . . . . . . . . . . 5235

18.28 Synchronous modes of Panasonic A5B servo drives. . . . . . . . . . . . . 5236

18.29 EtherCAT Slave Controller (ESC). . . . . . . . . . . . . . . . . . . . . . . . . 5237

18.30 Timing diagram of the DC mode. . . . . . . . . . . . . . . . . . . . . . . . . 5238

18.31 Timing diagram of the SM2 mode. . . . . . . . . . . . . . . . . . . . . . . . 5239

18.32 Timing diagram of the Free RUN mode. . . . . . . . . . . . . . . . . . . . 5240

18.33 Sync Manager Communication Type (1C00h). . . . . . . . . . . . . . . . . 5241

18.34 Sync Manager 2 Synchronization (1C32h) (I). . . . . . . . . . . . . . . . 5243

18.35 Sync Manager 2 Synchronization (1C32h) (II). . . . . . . . . . . . . . . . 5244

18.36 Sync Manager 3 Synchronization (1C33h) (I). . . . . . . . . . . . . . . . 5245

18.37 Sync Manager 3 Synchronization (1C33h) (II). . . . . . . . . . . . . . . . 5246

18.38 DC mode specification. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5247

18.39 Setting Sync manager 2 for the DC synchronous mode. . . . . . . . . . 5248

18.40 Setting Sync manager 3 for the DC synchronous mode. . . . . . . . . . 5248

18.41 SM2 mode specification. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5249

18.42 Setting Sync manager 2 for the SM2 mode. . . . . . . . . . . . . . . . . . 5250

18.43 Setting Sync manager 3 for the SM2 mode. . . . . . . . . . . . . . . . . . 5250

18.44 Free RUN mode specification. . . . . . . . . . . . . . . . . . . . . . . . . . . 5251

18.45 Setting Sync manager 2 for the Free RUN mode. . . . . . . . . . . . . . 5252

18.46 Setting Sync manager 3 for the Free RUN mode. . . . . . . . . . . . . . 5252

18.47 Effects of the Input shift time. . . . . . . . . . . . . . . . . . . . . . . . . . . 5253

143

LIST OF FIGURES

18.48 CiA 402 option code list. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5254

18.49 Realted option code. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5255

18.50 Objects for determining the operation sequence when main

power is off. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5256

18.51 Objects related to the Abort connection option code (6007h). . . . . . 5257

18.52 Deceleration method upon main power off (3509h = 2000). . . . . . . 5257

18.53 Deceleration method upon main power off (3509h ̸= 2000). . . . . . . 5258

18.54 Deceleration and stop example upon main power shutdown. . . . . . . 5259

18.55 PDS state during deceleration and stop before time progress

reaches the 3509h value or if 3509h = 2000. . . . . . . . . . . . . . . . 5260

18.56 PDS state during deceleration and stop after the time progress

reaches the 3509h value. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5260

18.57 Quick stop option code (605Ah). . . . . . . . . . . . . . . . . . . . . . . . . 5261

18.58 Objects related to the Quick stop option code (605Ah). . . . . . . . . . 5261

18.59 Example of deceleration and stop due to Quick stop command. . . . . 5262

18.60 Shutdown option code (605Bh). . . . . . . . . . . . . . . . . . . . . . . . . 5263

18.61 Objects related to the Shutdown option code (605Bh). . . . . . . . . . . 5263

18.62 Example of deceleration and stop due to Shutdown command. . . . . 5264

18.63 Disable operation option code (605Ch). . . . . . . . . . . . . . . . . . . . . 5265

18.64 Objects related to the Disable operation option code (605Ch). . . . . . 5265

18.65 Example of deceleration and stop due to Disable operation command. 5266

18.66 Halt option code (605Dh). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5267

18.67 Objects related to the Halt option code (605Dh). . . . . . . . . . . . . . 5267

18.68 Example of deceleration and stop due to the Halt function. . . . . . . . 5268

18.69 Fault reaction option code (605Eh). . . . . . . . . . . . . . . . . . . . . . . 5269

18.70 Objects related to the Falut reaction option code (605Eh). . . . . . . . 5269

18.71 Example of deceleration and stop due to alarm occurance. . . . . . . . 5270

18.72 Decelerate and stop due to drive inhibition input (POT, NOT). . . . . . 5271

18.73 Objects related to the deceleration and stop sequence due

to drive inhibition input. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5271

18.74 Pr0.00: Rotational direction setup. . . . . . . . . . . . . . . . . . . . . . . . 5272

18.75 Object 607Eh: Polarity. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5273

18.76 Parameter Pr0.00 and Object 607Eh in Panasonic A5B drives. . . . . . 5274

18.77 Electronic gear. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5275

144

LIST OF FIGURES

18.78 Examining the values of objects 608Fh, 6001h, and 6092h

with Panaterm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5278

18.79 Modifying the value of 6092h-01h with Panaterm. . . . . . . . . . . . . . 5278

18.80 Definition of Home offset. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5279

18.81 Object 607Ch (Home offset). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5279

18.82 Single-turn data and multi-turn data of absolute encoders. . . . . . . . 5280

18.83 Pr0.15 Absolute encoder setup. . . . . . . . . . . . . . . . . . . . . . . . . . 5280

18.84 Calculation of Position actual internal value (6063h) and

Position actual value (6064h) from absolute encoder data. . . . . . . . 5281

18.85 Position actual internal value (6063h) determined by multi-

turn and single-turn data. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5281

18.86 Diplaying multi-turn data with Panaterm. . . . . . . . . . . . . . . . . . . . 5282

18.87 Multi-turn data after clearance. . . . . . . . . . . . . . . . . . . . . . . . . . 5283

18.88 Objects 4D00h and 4D01h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5283

18.89 6062h (Interpolation time period) setup accroding to com-

munication cycle time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5284

18.90 6062h (Interpolation time period). . . . . . . . . . . . . . . . . . . . . . . . 5284

18.91 Block diagram of the cyclic synchronous position mode. . . . . . . . . . 5285

18.92 Objects for csp mode (I). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5286

18.93 Amount of change saturation function of command posi-

tion for csp mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5287

18.94 Objects for csp mode (II). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5288

18.95 Object 607Dh (Software position limit). . . . . . . . . . . . . . . . . . . . . 5289

18.96 Movable direction in Software limit range. . . . . . . . . . . . . . . . . . . 5289

18.97 Objects for csp mode (III). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5291

18.98 Object 607Bh. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5292

18.99 Example of the wraparound operation. . . . . . . . . . . . . . . . . . . . . 5293

18.100 Gear ratio and feed constant for creating user-defined units. . . . . . . 5295

18.101 Objects for csp mode (IV). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5296

18.102 Objects for csp mode (V). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5297

18.103 Functional overview of following error. . . . . . . . . . . . . . . . . . . . . 5297

18.104 Objects for csp mode (Vi) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5299

18.105 Parameters for positional command filters. . . . . . . . . . . . . . . . . . . 5300

18.106 Step response of the positional command smoothing filters. . . . . . . . 5300

145

LIST OF FIGURES

18.107 Step response of the positional command FIR filters. . . . . . . . . . . . 5302

18.108 Parameters for setting positioning complete outputs. . . . . . . . . . . . 5302

18.109 Example of setting the PDO mapping object RxPDO_1. . . . . . . . . . . 5304

18.110 Example of setting the PDO assign object 1C13h. . . . . . . . . . . . . . 5305

18.111 Default PDO mapping 1 for position control. . . . . . . . . . . . . . . . . 5305

18.112 Default PDO mapping 2 for position, velocity, and torque control. . . . 5306

18.113 Default PDO mapping 3 for position and velocity control. . . . . . . . . 5306

18.114 Default PDO mapping 4 for position, velocity, and torque control. . . . 5307

18.115 Interfacing sensors with PLCs. . . . . . . . . . . . . . . . . . . . . . . . . . . 5308

18.116 Sinking/sourcing inputs and outputs of PLCs. . . . . . . . . . . . . . . . . 5309

18.117 Beckhoff Ethercat coupler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5310

18.118 Connectors and terminal pins of the Beckhoff EK1100 ether-

cat bus coupler. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5310

18.119 Side view of the EK1100. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5311

18.120 Power circuit the EK1100. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5311

18.121 Wiring of gounds and PE in the EK1100. . . . . . . . . . . . . . . . . . . . 5312

18.122 Four sinking inputs of a PLC. . . . . . . . . . . . . . . . . . . . . . . . . . . 5313

18.123 Sharing the “common pin” among four sinking inputs. . . . . . . . . . . 5313

18.124 Sharing the power source among four input devices and

the “common pin” among four sinking inputs. . . . . . . . . . . . . . . . 5314

18.125 Beckhoff EL1008 8-channel digital input terminal. . . . . . . . . . . . . . 5315

18.126 Side view of the Beckhoff EL1008. . . . . . . . . . . . . . . . . . . . . . . . 5315

18.127 Beckhoff EL1100 coupled with an EL1008. . . . . . . . . . . . . . . . . . . 5316

18.128 Current path for an input device connected to the EL1008. . . . . . . . 5316

18.129 Beckhoff EL1004 4-channel digital input terminal. . . . . . . . . . . . . . 5317

18.130 Current path for an input device connected to the EL1004. . . . . . . . 5317

18.131 Beckhoff EL2008 8-channel digital output terminal. . . . . . . . . . . . . 5318

18.132 Current path for a load connected to the EL2008. . . . . . . . . . . . . . 5318

18.133 Beckhoff EL2004 4-channel digital output terminal. . . . . . . . . . . . . 5319

18.134 Current path for a load connected to the EL2004. . . . . . . . . . . . . . 5319

18.135 Hardware setup for a digital IO test. . . . . . . . . . . . . . . . . . . . . . 5323

18.136 Ethercat clocks. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5367

18.137 Hardware setup for ethercat digital IO tests in DC mode. . . . . . . . . 5375

146

LIST OF FIGURES

18.138 Hardware setup for ethercat 32-bit digital inputs and 32-

bit digital outputs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5406

18.139 Hardware setup for SOEM programming examples. . . . . . . . . . . . . 5495

18.140 Panasonic MADHT1505BA1 servo drive. . . . . . . . . . . . . . . . . . . . . 5526

18.141 Name plate of the MADHT1505BA1 servo drive. . . . . . . . . . . . . . . 5527

18.142 Model designation of Panasonic A5B servo drives. . . . . . . . . . . . . . 5528

18.143 Panasonic MSME012S1T servo motor. . . . . . . . . . . . . . . . . . . . . . 5528

18.144 Name plate of the MSME012S1T servo motor. . . . . . . . . . . . . . . . 5529

18.145 Model designation of Panasonic A5 servo motors. . . . . . . . . . . . . . 5529

18.146 Connection example of Panasonic A5 servo drives (A to D-frame). . . 5531

18.147 Panasonic A5B servo drives (A to B frame). . . . . . . . . . . . . . . . . . 5532

18.148 Peripheral equipments for the Panasonic MADHT1505BA1

servo drive. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5533

18.149 Wiring the power connector and terminal block of MADHT1505BA1

servo drives. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5535

18.150 Wiring diagram of servo power and emergency stop switches. . . . . . 5536

18.151 Diode rectifier and DC-link capacitor of a servo drive. . . . . . . . . . . 5537

18.152 Brake connection to the servo drive. . . . . . . . . . . . . . . . . . . . . . . 5538

18.153 Digital input pins of Connector X4. . . . . . . . . . . . . . . . . . . . . . . 5540

18.154 Changing the logic of POT and NOT to a-contact. . . . . . . . . . . . . . 5541

18.155 Installation of noise filters for signal lines. . . . . . . . . . . . . . . . . . . 5542

18.156 Wiring diagram for test run. . . . . . . . . . . . . . . . . . . . . . . . . . . . 5543

18.157 Hardware setup for test run. . . . . . . . . . . . . . . . . . . . . . . . . . . . 5544

18.158 Hardware setup for running two Panasonic A5B motors. . . . . . . . . . 5544

18.159 Test run with panaterm (1). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5546

18.160 Test run with panaterm (2). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5547

18.161 Test run with panaterm (3). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5547

18.162 Test run with panaterm (4). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5548

18.163 Test run with panaterm (5). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5548

18.164 Test run with panaterm (6). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5549

18.165 Test run with panaterm (7). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5549

18.166 Test run with panaterm (8). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5550

18.167 Test run with panaterm (9). . . . . . . . . . . . . . . . . . . . . . . . . . . . 5550

18.168 Test run with panaterm (10). . . . . . . . . . . . . . . . . . . . . . . . . . . 5551

147

LIST OF FIGURES

18.169 Profile position mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5553

18.170 Node addressing (Station alias). . . . . . . . . . . . . . . . . . . . . . . . . . 5554

18.171 EtherCAT State Machine Transitions. . . . . . . . . . . . . . . . . . . . . . . 5555

18.172 Synchronization setting for Sync manager 2/3 during the

DC synchronous mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5556

18.173 An example of the velocity profile setup for pp control. . . . . . . . . . 5557

18.174 State transitions of the power drive system. . . . . . . . . . . . . . . . . . 5568

18.175 Test run with Twincat 3.1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5570

18.176 Cyclic Synchronous Velocity mode. . . . . . . . . . . . . . . . . . . . . . . . 5571

18.177 Architecture of the LinuxCNC. . . . . . . . . . . . . . . . . . . . . . . . . . . 5572

18.178 EMCMOT: LinuxCNC motion controller. . . . . . . . . . . . . . . . . . . . . 5574

18.179 Delta ASDA A2-E servo drive. . . . . . . . . . . . . . . . . . . . . . . . . . . 5575

18.180 SWM Maschinen FM45A DRO Profi. . . . . . . . . . . . . . . . . . . . . . . 5613

18.181 Changing the Sync mode of Sync manager 2 with ESC reg-

ister 0x0981. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5615

18.182 Changing the Sync mode of Sync manager 3 with ESC reg-

ister 0x0981. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5616

18.183 DC Timing Signals in relation to Communication. . . . . . . . . . . . . . 5617

18.184 SyncSignal generation modes. . . . . . . . . . . . . . . . . . . . . . . . . . . 5620

18.185 Cyclic synchronous position mode. . . . . . . . . . . . . . . . . . . . . . . . 5622

18.186 Speed-torque characteristics of MSME012S1T motor. . . . . . . . . . . . 5634

18.187 Linux and Xenomai tasks for a Panasonic servo drive. . . . . . . . . . . 5647

18.188 Internal states of the servo process. . . . . . . . . . . . . . . . . . . . . . . 5647

18.189 Hardware setup for eight digital IO terminals and two servo

drives. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5722

18.190 Graphical representation of the bus topology. . . . . . . . . . . . . . . . . 5728

18.191 moper with FIFO interface. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5822

18.192 moper with FIFO and ethercat interface. . . . . . . . . . . . . . . . . . . . 5823

18.193 moperm with fifo and ethercat interface. . . . . . . . . . . . . . . . . . . . 5823

18.194 FIFO communication between RTC and MOPER. . . . . . . . . . . . . . . 5824

18.195 Hardware structure of the RWRCCL system. . . . . . . . . . . . . . . . . . 5856

18.196 Software structure of the RWRCCL system. . . . . . . . . . . . . . . . . . 5857

18.197 FIFO and shared-memory communication between RCI and MOPERX. 5857

18.198 FIFO communication between RTC and MOPER in RCCL v1.0. . . . . . 5888

148

LIST OF FIGURES

18.199 Ready pose of PUMA robots. . . . . . . . . . . . . . . . . . . . . . . . . . . . 5906

18.200 PUMA 560’s pose at joint angles (0, 270, 180, 0, 90, 0). . . . . . . . . 5913

18.201 Relationship between joint torque and motor current in PUMA

robots. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5928

18.202 FIFO and shared-memory communication between RTC and

MOPERM in an ethercat-based robot controller. . . . . . . . . . . . . . . . 5951

18.203 Default PDO mapping 1 of Panasonic A5B servo drives. . . . . . . . . . 5982

18.204 Shared-memory interface of the MOPERM. . . . . . . . . . . . . . . . . . . 5982

18.205 RTC, MOPERM, and ethercat master module. . . . . . . . . . . . . . . . . 5983

18.206 Sampling intervals of TG and servo. . . . . . . . . . . . . . . . . . . . . . . 6021

18.207 Fine interpolator placed between TG and servo. . . . . . . . . . . . . . . 6021

18.208 Position control of joint motors. . . . . . . . . . . . . . . . . . . . . . . . . . 6023

18.209 Torque control of joint motors. . . . . . . . . . . . . . . . . . . . . . . . . . 6034

18.210 Position mode of the Unimation MK I controller. . . . . . . . . . . . . . . 6042

18.211 Torque mode of the Unimation MK I controller. . . . . . . . . . . . . . . 6043

18.212 Block diagram of the CSP (Cyclic Synchronous Position) mode. . . . . 6043

18.213 Block diagram of the CST (Cyclic Synchronous Torque) mode. . . . . . 6044

18.214 Block diagram of a servo controller with gravity compensation. . . . . 6044

18.215 Occurent of Error Err27.4 due to the excessive amount of

position change. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6103

18.216 Objects 3722h and 6080h related to the amount of change

saturation function of command position. . . . . . . . . . . . . . . . . . . 6104

18.217 Effect of the amount-of-change saturation function of com-

mand position. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6105

18.218 Software architecture of the RCCL 5.1.4 system. . . . . . . . . . . . . . . 6175

18.219 Software architecture of the RWRCCL system. . . . . . . . . . . . . . . . . 6175

18.220 Software architecture of the MultiRCCL system with a generic

robot interface. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6176

18.221 Software architecture of the RCCL system for EtherCAT servo

drives. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6176

18.222 IPC between JLS and planning-control processes. . . . . . . . . . . . . . . 6232

18.223 Typical configuration of the MultiRCCL system for a PUMA robot. . . 6266

18.224 Configuration of the MultiRCCL system in simulation mode. . . . . . . 6292

149

LIST OF FIGURES

18.225 Hardware architecture of the Unimation controller for a

PUMA robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6338

18.226 Hardware architecture of the RWRCCL system using Tri-

dent boards. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6338

18.227 Status input register of the Trident TRC004 board: bit fields. . . . . . 6339

18.228 Discrete output register of the Trident TRC004 board: bit fields. . . . 6339

18.229 Memory map of the Trident TRC004 board. . . . . . . . . . . . . . . . . . 6340

18.230 Hardware setup for developing an EtherCAT-based RWR-

CCL system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6357

19.1 Basic motion of machine tools. . . . . . . . . . . . . . . . . . . . . . . . . . 6410

19.2 Axes of a milling machine. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6411

19.3 CNC machining system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6411

19.4 Point-to-point control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6412

19.5 Continuous-path control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6413

19.6 Computation steps in a 5-axis CNC system. . . . . . . . . . . . . . . . . . 6414

19.7 Linear interpolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6415

19.8 Velocities of X and Y axes by the linear interpolation. . . . . . . . . . . 6416

19.9 Circular interpolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6416

19.10 Approximation of chord Pi Pi+1 with line segment Pi Pi+1. . . . . . . . . 6417

19.11 Radial and chord heights error in the circular interpolation. . . . . . . 6419

19.12 Circular interpolation with the improved Tustin method. . . . . . . . . . 6424

19.13 Linear interpolation through three points. . . . . . . . . . . . . . . . . . . 6426

19.14 Circular interpolation through three points. . . . . . . . . . . . . . . . . . 6427

19.15 Cubic spline interpolation through three points. . . . . . . . . . . . . . . 6427

19.16 Real-time NURBS interpolation. . . . . . . . . . . . . . . . . . . . . . . . . . 6431

19.17 NURBS curve representation. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6432

19.18 Interpolation error on a free-form curve. . . . . . . . . . . . . . . . . . . . 6433

19.19 Curvature of a free-form curve. . . . . . . . . . . . . . . . . . . . . . . . . . 6434

19.20 Curvature of the circular arc pssing through P(ui−2), P(ui−1),

and P(ui). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6435

19.21 p(t) calculated by the rough interpolator. . . . . . . . . . . . . . . . . . . . 6440

19.22 Reference-pulse interpolator and a servo controller. . . . . . . . . . . . . 6440

19.23 θre f (t) computed from the p(t) without fine interpolation. . . . . . . . 6441

150

LIST OF FIGURES

19.24 fine interpolator located within a servo controller. . . . . . . . . . . . . . 6441

19.25 fine interpolator located outside of a servo controller. . . . . . . . . . . 6442

19.26 θre f (t) computed from the p(t) with linear fine interpolation. . . . . . 6442

19.27 a(t) computed from the p(t) with linear interpolation. . . . . . . . . . . 6443

19.28 b′′(t) obtained with the moving-average fine interpolation. . . . . . . . 6444

19.29 θre f (t) computed from p(t) with the moving-average fine interpolation.6445

19.30 θre f (t) computed by linear and moving-average fine inter-

polators, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6445

19.31 Inputs and output of a binary rate multipler. . . . . . . . . . . . . . . . . 6446

19.32 Trajectories generated by the TG and BRM. . . . . . . . . . . . . . . . . . 6447

19.33 Functional diagram of the CD4089 CMOS BRM. . . . . . . . . . . . . . . 6448

19.34 Logic diagram of the CD4089 CMOS BRM. . . . . . . . . . . . . . . . . . 6449

19.35 Timing diagram of the CD4089 CMOS BRM (I). . . . . . . . . . . . . . . 6449

19.36 Timing diagram of the CD4089 CMOS BRM (II). . . . . . . . . . . . . . . 6450

19.37 Backward Euler Integration. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6452

19.38 The symbol of a DDA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6453

19.39 Block diagram of a DDA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6453

19.40 Command pulse generation using a DDA. . . . . . . . . . . . . . . . . . . 6456

19.41 Command pulse generation for a robot controller. . . . . . . . . . . . . . 6458

19.42 An example of ADCAI (Acc/Dec Control After Interpolation). . . . . . . 6462

19.43 Control loop for the ADCAI. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6463

19.44 An example of ADCBI (Acc/Dec Control Before Interpolation). . . . . . 6464

19.45 Control loop for the ADCBI. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6464

19.46 Velocity profiles generated by three different acc/dec filters. . . . . . . 6465

19.47 A tool path specified by G-code G61. . . . . . . . . . . . . . . . . . . . . . 6466

19.48 A tool path specified by G-code G64. . . . . . . . . . . . . . . . . . . . . . 6467

19.49 Output of the Acc/Dec filter for two consecutive blocks (G64). . . . . 6468

19.50 Output of the Acc/Dec filter for two consecutive blocks (G61). . . . . 6468

19.51 ADCBI and linear interpolation for x and y axes. . . . . . . . . . . . . . 6469

19.52 Normal and short blocks. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6470

19.53 Trapezoidal velocity profile of a normal block. . . . . . . . . . . . . . . . 6470

19.54 Triangular velocity profile of a short block. . . . . . . . . . . . . . . . . . 6472

19.55 Blended velocity profile of Case 1. . . . . . . . . . . . . . . . . . . . . . . . 6475

19.56 Blended velocity profile of Case 2. . . . . . . . . . . . . . . . . . . . . . . . 6476

151

LIST OF FIGURES

19.57 Blended velocity profile of Case 3. . . . . . . . . . . . . . . . . . . . . . . . 6478

19.58 Velocity profile of a short block with only a deceleration region. . . . 6481

19.59 Blended velocity profile of Case 4. . . . . . . . . . . . . . . . . . . . . . . . 6481

19.60 Blended velocity profile of Case 5. . . . . . . . . . . . . . . . . . . . . . . . 6483

19.61 Velocity profile of a short block with only constant-velocity

and deceleration regions (case 6). . . . . . . . . . . . . . . . . . . . . . . . 6484

19.62 Blended velocity profile of Case 6. . . . . . . . . . . . . . . . . . . . . . . . 6485

19.63 Velocity profile of a short block with only an aceleration region. . . . 6486

19.64 Velocity profile of a short block with only acceleration and

constant-velocity regions (case 7). . . . . . . . . . . . . . . . . . . . . . . . 6487

19.65 Blended velocity profile of Case 7. . . . . . . . . . . . . . . . . . . . . . . . 6487

19.66 Blended velocity profile of Case 8. . . . . . . . . . . . . . . . . . . . . . . . 6489

19.67 Blended velocity profile of Case 9. . . . . . . . . . . . . . . . . . . . . . . . 6490

19.68 Transformation of the velocity profile of short blocks for blending. . . 6492

19.69 Blended velocity profile of Case 10. . . . . . . . . . . . . . . . . . . . . . . 6493

19.70 Blended velocity profile of Case 11. . . . . . . . . . . . . . . . . . . . . . . 6494

19.71 Blended velocity profile of Case 12. . . . . . . . . . . . . . . . . . . . . . . 6495

19.72 Corner velocity between two linear blocks (I). . . . . . . . . . . . . . . . 6497

19.73 Corner velocity between two linear blocks (II). . . . . . . . . . . . . . . . 6499

19.74 Circular motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6502

19.75 Directions of velocity and acceleration vectors in circular motion. . . . 6504

19.76 Overlapping of linear and circular blocks. . . . . . . . . . . . . . . . . . . 6505

19.77 Blending the velocity profiles of linear and circular blocks. . . . . . . . 6505

19.78 A generic S-curve velocity profile. . . . . . . . . . . . . . . . . . . . . . . . 6506

19.79 Moving distance during the acceleration region. . . . . . . . . . . . . . . 6508

19.80 Two consecutive S-curve velocity profiles with Fi−1 > Fi . . . . . . . . . . 6509

19.81 Blending two consecutive S-curve velocity profiles for the

case of Fi−1 > Fi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6509

19.82 Acceleration between points P and Q before blending two

S-curve velocity profiles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6510

19.83 Acceleration between points P and Q after blending two

S-curve velocity profiles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6511

19.84 S-curve velocity profile without linear regions. . . . . . . . . . . . . . . . 6513

19.85 S-curve velocity profile with linear regions. . . . . . . . . . . . . . . . . . 6514

152

LIST OF FIGURES

19.86 Architecture of the U-M M&C (University of Michigan Mea-

surement and Control) controller. . . . . . . . . . . . . . . . . . . . . . . . . 6518

19.87 Hierarchical control levels in a CNC system. . . . . . . . . . . . . . . . . . 6518

19.88 Types of 5-axis milling machines: (a) table-rotating/tilting

type; (b) spindle-rotating/tilting type; (c)table/spindle-tilting

type. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6520

19.89 CC point and CL point of a ball-end milling cutter. . . . . . . . . . . . . 6521

19.90 Tool path in a 3-axis milling operation. . . . . . . . . . . . . . . . . . . . . 6521

19.91 Tool position and orientation in a 5-axis milling operation. . . . . . . . 6522

19.92 3-axis, 5-axis and 3+2 axis machining. . . . . . . . . . . . . . . . . . . . . 6523

19.93 Definition of the tool path using the 5xNurbs format. . . . . . . . . . . 6523

19.94 Architecture of the LinuxCNC. . . . . . . . . . . . . . . . . . . . . . . . . . . 6525

19.95 EMCMOT structure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6526

19.96 LinuxCNC for a stepping motor-based CNC machine. . . . . . . . . . . . 6526

19.97 LinuxCNC Configuration Selector. . . . . . . . . . . . . . . . . . . . . . . . . 6529

19.98 Axis GUI window for the “5axis” machine. . . . . . . . . . . . . . . . . . . 6529

19.99 “Machine” menu of the Axis GUI. . . . . . . . . . . . . . . . . . . . . . . . 6530

19.100 “View” menu of the Axis GUI. . . . . . . . . . . . . . . . . . . . . . . . . . . 6531

19.101 Setting the Z-axis offset of G54. . . . . . . . . . . . . . . . . . . . . . . . . 6533

19.102 Raspberry Pi board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6533

19.103 Controller components. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6554

19.104 Beckhoff Ethercat components of the milling machine system. . . . . . 6555

19.105 Wiring the Beckhoff Ethercat components. . . . . . . . . . . . . . . . . . . 6555

19.106 Components of a milling machine. . . . . . . . . . . . . . . . . . . . . . . . 6556

19.107 Resetting the Thinkpad X200 BIOS. . . . . . . . . . . . . . . . . . . . . . . 6557

19.108 Disabling “USB BIOS Support” in the Thinkpad X200 BIOS setting. . . 6558

19.109 Logitec Rumblepad 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6568

19.110 AXIS window of the milling machine system. . . . . . . . . . . . . . . . . 6569

19.111 Button clamp for Z-axis correction. . . . . . . . . . . . . . . . . . . . . . . . 6570

19.112 Z-axis correction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6571

19.113 Monitoring the status of stepper motors. . . . . . . . . . . . . . . . . . . . 6573

19.114 HAL-graph visualization. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6574

19.115 HAL-graph data logger. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6575

153

LIST OF FIGURES

20.1 Architecture of motion control systems. . . . . . . . . . . . . . . . . . . . . 6580

20.2 motion control card and servo drive in a PC-based motion

control system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6580

20.3 Spindle and feed axis control in a CNC lathe machine. . . . . . . . . . . 6581

20.4 Motion control board and servo drive in a PC-based mo-

tion control system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6582

20.5 Pulse-type motion control systems. . . . . . . . . . . . . . . . . . . . . . . . 6583

20.6 Network-type motion control systems (SSNET). . . . . . . . . . . . . . . . 6583

20.7 Panasinic servomotor and drive (MINAS A4 series). . . . . . . . . . . . . 6585

20.8 Adlink motion control card (PCI-8164). . . . . . . . . . . . . . . . . . . . . 6585

20.9 Adlink terminal board (DIN-814P-A4). . . . . . . . . . . . . . . . . . . . . . 6585

20.10 Connectors of the Adlink terminal board (DIN-814P-A4). . . . . . . . . . 6586

20.11 Wiring the PCI-8164, the MINAS A4 drive, and the DIN-

814P-A4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6587

20.12 Pin map of the PCI-8164 CN2 connector. . . . . . . . . . . . . . . . . . . . 6587

20.13 Pin map of the DIN-814P-A4 connectors. . . . . . . . . . . . . . . . . . . . 6588

20.14 Pin map of the MINAS A4 CX-5 connector. . . . . . . . . . . . . . . . . . 6589

20.15 Wiring example of the Panasonic MINAS A4 drive. . . . . . . . . . . . . 6590

20.16 brake circuit connection for the MINAS A4 drive. . . . . . . . . . . . . . 6591

20.17 Serial connection of the MINAS A4 drives. . . . . . . . . . . . . . . . . . . 6591

20.18 Block diagram of the PCI 8164 board. . . . . . . . . . . . . . . . . . . . . 6592

20.19 Block diagram of the PCL 6045B. . . . . . . . . . . . . . . . . . . . . . . . 6593

20.20 Sanken SI-8401L. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6594

20.21 DC-DC converter of the PCI 8164 card. . . . . . . . . . . . . . . . . . . . . 6594

20.22 Pulser. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6595

20.23 Simultaneous start/stop control of multiple PCI 8164 cards. . . . . . . 6595

20.24 Pulse outputs of the PCI 8164 board. . . . . . . . . . . . . . . . . . . . . . 6596

20.25 Pulse input interface of the PCI 8164 board (line driver outputs). . . 6597

20.26 Sensor input interface of the PCI 8164 board. . . . . . . . . . . . . . . . 6597

20.27 LTC input interface of the PCI 8164 board. . . . . . . . . . . . . . . . . . 6598

20.28 CMP output interface of the PCI 8164 board. . . . . . . . . . . . . . . . . 6598

20.29 LED display of the ORG1 status. . . . . . . . . . . . . . . . . . . . . . . . . 6600

20.30 INP input interface of the PCI 8164 card (designed). . . . . . . . . . . . 6600

20.31 INP input interface of the PCI 8164 card (implemented). . . . . . . . . 6601

154

LIST OF FIGURES

20.32 INP input interface of the PCI 8164 card with a diode. . . . . . . . . . 6601

20.33 Block diagram of the MINAS A4 servo drive. . . . . . . . . . . . . . . . . 6602

20.34 Registers of the PCL6045B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6605

20.35 Register control commands of the PCL6045B. . . . . . . . . . . . . . . . . 6606

20.36 Flowchart of the PCL6045B interrupt handling. . . . . . . . . . . . . . . . 6610

20.37 Delay needed between two consecutive command writes to PCL6045B. 6614

20.38 Delay insertion using WRQ. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6615

20.39 Local address space 0 bus region descriptors (LAS0BRD; 28h). . . . . . 6617

20.40 Control procedure of the PCL6045B. . . . . . . . . . . . . . . . . . . . . . . 6618

20.41 Trapezoidal velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6619

20.42 Position, acceleration, jerk profiles according to a trape-

zoidal velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6620

20.43 S-curve velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6620

20.44 Position, acceleration, jerk profiles according to an S-curve

velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6621

20.45 Acceleration a(t) in the acceleration interval of an S-curve

velocity profile. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6622

20.46 Speed pattern parameters of PCL6045B. . . . . . . . . . . . . . . . . . . . 6623

20.47 Speed patterns of PCL6045B. . . . . . . . . . . . . . . . . . . . . . . . . . . 6624

20.48 Speed in the acceleration interval of a T-curve speed pattern. . . . . . 6626

20.49 Ramping-down points of a T-curve speed pattern (I). . . . . . . . . . . . 6631

20.50 Ramping-down point of a T-curve speed pattern (II). . . . . . . . . . . . 6632

20.51 Ramping-down point of a S-curve speed pattern without a

linear range. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6633

20.52 T-curve speed pattern with a positive offset written to PRDP. . . . . . . 6635

20.53 T-curve speed pattern with a negativeoffset written to PRDP. . . . . . . 6636

20.54 Automatic FH correction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6639

20.55 FH correction for T-curve speed patterns. . . . . . . . . . . . . . . . . . . . 6640

20.56 S-curve speed pattern with PRUS = 0 and PRDS = 0. . . . . . . . . . . 6641

20.57 Interface of nomal input signals. . . . . . . . . . . . . . . . . . . . . . . . . 6652

20.58 Interface of high-speed input signals. . . . . . . . . . . . . . . . . . . . . . 6654

20.59 Interface of line-driver input signals. . . . . . . . . . . . . . . . . . . . . . . 6654

20.60 Interface of line-driver input signals (corrected). . . . . . . . . . . . . . . 6655

20.61 Interface of output signals. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6655

155

LIST OF FIGURES

20.62 Interface of output signals (corrected). . . . . . . . . . . . . . . . . . . . . 6656

20.63 Interface of line-driver output signals. . . . . . . . . . . . . . . . . . . . . . 6656

20.64 Interface of line-driver output signals (corrected). . . . . . . . . . . . . . 6656

20.65 Hivertec HPCI-CPD532. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6657

20.66 Hivertec HPCI-CPD534. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6658

20.67 Hivertec HPCI-CPD574N. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6658

20.68 Hivertec HPCI-CPD578. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6658

20.69 Hivertec HPCI-CPD578N. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6659

20.70 Hardware configuration of a motion control system using

a Hivertec CPD530 series board. . . . . . . . . . . . . . . . . . . . . . . . . 6660

20.71 Wiring diagram between a HPCI-CPD530 series board and

a Panasonic A4 servo drive. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6660

20.72 Block diagram of Hivertec HPCI-CPD534. . . . . . . . . . . . . . . . . . . . 6661

20.73 Address map of Hivertec HPCI-CPD534. . . . . . . . . . . . . . . . . . . . . 6662

20.74 PCI configuration registers of Hivertec HPCI-CPD534. . . . . . . . . . . . 6663

20.75 Device driver routines for accessing the I/O ports of HCPI=CPD534. . 6663

20.76 Interrupt routing in HPCI-CPD534. . . . . . . . . . . . . . . . . . . . . . . . 6664

20.77 Software configuration of Hivertec CPD530 series boards. . . . . . . . . 6665

20.78 JC Automation MPC-3034 motion control card. . . . . . . . . . . . . . . . 6681

20.79 JS Automation MPC-3035 motion control card. . . . . . . . . . . . . . . . 6681

20.80 Wiring boards for MPC-3025. . . . . . . . . . . . . . . . . . . . . . . . . . . 6685

20.81 JC Automation LSI-3134 4-axis quadrature encoder counter

card. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6686

20.82 Layout of the LSI-3134 card. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6686

20.83 Block diagram of the Counter function (LSI-3144a). . . . . . . . . . . . . 6687

20.84 Accessing resisters of the LSI-3134 card (index-mode addressing). . . . 6688

20.85 Indices and their associated registers of Axis 0 (LSI-3134). . . . . . . . 6689

20.86 Adlink PCI-8124-C: advanced 4-CH encoder card with high-

speed triggering function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6690

20.87 Block diagram of the Adlink PCI-8124-C card. . . . . . . . . . . . . . . . 6690

20.88 Function blocks of the FPGA on the Adlink PCI-8124-C card. . . . . . . 6691

20.89 Adlink motion control card (PCI-8164). . . . . . . . . . . . . . . . . . . . . 6704

20.90 Block diagram of the PCI 8164 card (partial). . . . . . . . . . . . . . . . 6706

20.91 Pinmap of the IDT72V255LA FIFO. . . . . . . . . . . . . . . . . . . . . . . . 6706

156

LIST OF FIGURES

20.92 8086 CPU interface mode of the PCL6045B. . . . . . . . . . . . . . . . . . 6707

20.93 An example of EEPROM programmers. . . . . . . . . . . . . . . . . . . . . 6708

20.94 Connection of the PCI9052 I/O pins. . . . . . . . . . . . . . . . . . . . . . 6710

20.95 Interrupt sources of PCI9052. . . . . . . . . . . . . . . . . . . . . . . . . . . 6751

20.96 T-curve (trapezoidal) speed pattern for point-to-point mo-

tion control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6759

20.97 Synopsis of MPC3035_T_curve_position_move(). . . . . . . . . . . . . . . 6759

20.98 Synopsis of MPC3035_S_curve_position_move(). . . . . . . . . . . . . . . 6767

20.99 Construction of a single-axis motion control system for demo. . . . . . 6777

20.100 Jumpers and switches on the PCI-8164 board. . . . . . . . . . . . . . . . 6777

20.101 Determination of the output pulse signal levels with jumpers. . . . . . 6778

20.102 Default jumper setting of the PCI-8164 board. . . . . . . . . . . . . . . . 6778

20.103 EL input signals generated by EL sensors. . . . . . . . . . . . . . . . . . . 6778

20.104 Default switch setting of the PCI-8164 board. . . . . . . . . . . . . . . . . 6779

20.105 Device manager window after installing PCI 8164 board

and its driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6779

20.106 Cabling the Panasonic servo motor and driver. . . . . . . . . . . . . . . . 6782

20.107 Regenerative registor circuit of the Minas servo drivers. . . . . . . . . . 6783

20.108 Wiring diagram to connect a backup battery to the abso-

lute encoder. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6783

20.109 DIN-814P-A40: terminal board for Panasonic A4 series servo

driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6784

20.110 Pin assignments of the DIN-814P-A40 terminal board. . . . . . . . . . . 6785

20.111 Cabling and setting of the DIN-814P-A40 terminal board

for a single axis demo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6786

20.112 Connecting multiple MINAS A4 servo serivers to a PC via

RS232 and RS485 comminication lines. . . . . . . . . . . . . . . . . . . . . 6786

20.113 DV0P1960 serial cable. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6787

20.114 RS232 isolator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6787

20.115 Test run of the PCI-8164 and Panasonic Minas A4 servo driver. . . . . 6788

20.116 Communication protocol selection. . . . . . . . . . . . . . . . . . . . . . . . 6789

20.117 Communication port selection. . . . . . . . . . . . . . . . . . . . . . . . . . . 6789

20.118 Servo driver type selection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6789

20.119 Connection conformation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6790

157

LIST OF FIGURES

20.120 Menu window of the Panaterm. . . . . . . . . . . . . . . . . . . . . . . . . . 6790

20.121 Parameter loading window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6790

20.122 Parameter window of the panaterm. . . . . . . . . . . . . . . . . . . . . . . 6800

20.123 IO monitoring window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6800

20.124 Encoder monitoring window. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6801

20.125 Alarm window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6802

20.126 Calculation of an electronic gear ratio. . . . . . . . . . . . . . . . . . . . . 6803

20.127 I/O configuration of the Axis 0. . . . . . . . . . . . . . . . . . . . . . . . . . 6805

20.128 Interrupt configuration of the Axis 0. . . . . . . . . . . . . . . . . . . . . . 6806

20.129 Positive rotation direction of the Parasonic servo motor. . . . . . . . . . 6806

20.130 1-axis operation with the MotionCreator Pro. . . . . . . . . . . . . . . . . 6807

20.131 Auto-tuning window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6809

20.132 Servo parameters set after auto-tuning. . . . . . . . . . . . . . . . . . . . . 6809

20.133 Block diagram of the PCL6045BL evaluation kit. . . . . . . . . . . . . . . 6819

20.134 Base module of the PCL6045BL evaluation kit. . . . . . . . . . . . . . . . 6820

20.135 CPU module of the PCL6045BL evaluation kit. . . . . . . . . . . . . . . . 6820

20.136 GPIO module of the PCL6045BL evaluation kit. . . . . . . . . . . . . . . . 6821

20.137 Modification of the CPU module. . . . . . . . . . . . . . . . . . . . . . . . . 6821

20.138 Configuration of the Motionnet. . . . . . . . . . . . . . . . . . . . . . . . . . 6822

20.139 Typical structure of a motion control board. . . . . . . . . . . . . . . . . . 6823

20.140 Structure of an advanced motion control board. . . . . . . . . . . . . . . 6824

20.141 Function blocks of a robot controller corresponding to a

motion control board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6825

20.142 Linear interpolation board for robot or motion control. . . . . . . . . . . 6826

20.143 Structure of an interpolation board (UART-interface type). . . . . . . . . 6827

20.144 Spectrum Digital eZdsp F2812 board. . . . . . . . . . . . . . . . . . . . . . 6829

20.145 RealSYS DSP2812EZ board. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6830

20.146 Changing the write permission of modelsim.ini . . . . . . . . . . . . . . . 6832

20.147 Creating such a new environment variable as modelsim. . . . . . . . . . 6832

20.148 Adding the modelsim’s excutables directory to Path. . . . . . . . . . . . . 6833

20.149 Selecting a simulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6833

20.150 Selecting HDL(s) for simulation. . . . . . . . . . . . . . . . . . . . . . . . . 6834

20.151 Selecting device families. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6834

20.152 Selecting libraries for functiopnal and timing simulation. . . . . . . . . . 6834

158

LIST OF FIGURES

20.153 Specifying output directory for compiles libraries. . . . . . . . . . . . . . 6835

20.154 Compilation summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6835

20.155 XFC-S3P01: Hitec Logic’s FPGA training board. . . . . . . . . . . . . . . . 6835

20.156 Block diagram of the XFC-S3P01 board. . . . . . . . . . . . . . . . . . . . 6836

20.157 Bloack diagram of a 6-axis serial motion board. . . . . . . . . . . . . . . 6837

20.158 Structure of a interpolation board (UART-interface type). . . . . . . . . 6837

20.159 Structure of a interpolation board without a DSP (UART-

interface type). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6838

20.160 Structure of an interpolation board (USB-interface type). . . . . . . . . 6839

20.161 Bloack diagram of a 6-axis USB motion board. . . . . . . . . . . . . . . . 6839

20.162 Function blocks of the motion-control FPGA. . . . . . . . . . . . . . . . . 6840

20.163 Block diagram of the HCTL-2032 Quadrature Decoder/Counter

Interface IC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6841

20.164 Detecting states of the quarature signals. . . . . . . . . . . . . . . . . . . . 6843

20.165 Valid state transitions of the quadrature signals. . . . . . . . . . . . . . . 6843

20.166 Input and output of a 6-bit binary rate multiplier. . . . . . . . . . . . . . 6845

20.167 Logic diagram of the SN7497 synchronous 6-bit binary rate multiplier. 6846

20.168 synchronous 18-bit binary rate multiplier with cascading

three SN7497s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6846

20.169 Functional diagram of the CD4527B. . . . . . . . . . . . . . . . . . . . . . 6847

20.170 Logic diagram of the CD4527B. . . . . . . . . . . . . . . . . . . . . . . . . . 6848

20.171 Two CD4527B cascaded in the add mode. . . . . . . . . . . . . . . . . . . 6849

20.172 Two CD4527B cascaded in the multiply mode. . . . . . . . . . . . . . . . 6849

20.173 input and output signals of a programmabel one-shot circuit. . . . . . 6852

20.174 Simulation result of the programmable one-shot circuit. . . . . . . . . . 6853

20.175 BRM cascaded by a programmable one-shot. . . . . . . . . . . . . . . . . 6854

20.176 Command pulse formats. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6855

20.177 Generation of a sign signal and a pulse train. . . . . . . . . . . . . . . . 6855

20.178 Generation of CW and CCW pulse trains. . . . . . . . . . . . . . . . . . . 6856

20.179 Conversion of direction and step signals to CW and CCW pulses. . . . 6856

20.180 Simulation of the quadrature encoder with Xilinx ISim. . . . . . . . . . 6857

20.181 Conversion of direction and step signals to A-phase and B-

phase pulses. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6858

20.182 Generation of A-phase and B-Phase pulse trains (I). . . . . . . . . . . . . 6859

159

LIST OF FIGURES

20.183 Generation of A-phase and B-Phase pulse trains (II). . . . . . . . . . . . 6859

20.184 Simulation of a command pulse generator consisting of a

4-bit BRM and a quadrature encoder. . . . . . . . . . . . . . . . . . . . . . 6861

20.185 Connectores of the DIN-814P-A4 terminal board. . . . . . . . . . . . . . . 6862

20.186 Pin assignments of the DIN-814P-A4 terminal board (I). . . . . . . . . . 6863

20.187 Pin assignments of the DIN-814P-A4 terminal board (II). . . . . . . . . 6864

20.188 Power Curcuits of the PCI-8164 board. . . . . . . . . . . . . . . . . . . . . 6867

20.189 Power inputs of a usb motion board. . . . . . . . . . . . . . . . . . . . . . 6868

20.190 Timing diagram: From AC power-on to command entry. . . . . . . . . . 6869

20.191 Connecting example of the BRKOFF signal. . . . . . . . . . . . . . . . . . 6870

20.192 Routing the BRKOFF signal in the DIN-814P-A4 terminal board. . . . . 6870

20.193 Routing the BRKOFF signal in the interpolation board (I). . . . . . . . . 6871

20.194 Reading the BRKOFF signal with an optocoupler. . . . . . . . . . . . . . . 6871

20.195 Routing the BRKOFF signal in the interpolation board (II). . . . . . . . 6872

20.196 Timing diagram: When an error (alarm) has occurred (at

servo-on command). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6872

20.197 Timing diagram: When an alarm has been cleared (at servo-

on command). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6873

20.198 Timing diagram: Servo-on/off action while the motor is at

stall (servo-lock). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6874

20.199 Timing diagram: servo-on/off action while the motor is in motion. . . 6874

20.200 Operation-execution sequences on the PC and the FPGA. . . . . . . . . 6876

20.201 Hardware structure of a PCI interpolation board. . . . . . . . . . . . . . 6906

20.202 Interpolation board with PCI interface. . . . . . . . . . . . . . . . . . . . . 6908

20.203 Motion-control FPGA with PCI interface. . . . . . . . . . . . . . . . . . . . 6908

20.204 Terminal board for connecting various servo drives to a

motion control board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6908

20.205 Adlink PCI-8164 motion control board. . . . . . . . . . . . . . . . . . . . . 6909

20.206 PCI-8164 PCB layout. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6909

20.207 PCI-8164 face plate. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6910

20.208 CN2 pin assignments: Main connector. . . . . . . . . . . . . . . . . . . . . 6911

20.209 Implementation of the servo-drive interface on the PCI-8164

board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6912

160

LIST OF FIGURES

20.210 Cascade connection of the STA and STP signals generated

by a PCL6045. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6912

20.211 Cascade connection of the externally generated STA and

STP signals. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6913

20.212 Internal connection of the CSTA# signals. . . . . . . . . . . . . . . . . . . 6913

20.213 Clocks used for generating set points and command pulses. . . . . . . . 6914

20.214 Synchronized clocks in generating set points and command pulses. . . 6915

20.215 Software structure of the motion control FPGA. . . . . . . . . . . . . . . 6915

20.216 Loopback test of Wishbone input and output ports. . . . . . . . . . . . . 6922

20.217 Loopback test of Wishbone input and output ports. . . . . . . . . . . . . 6928

20.218 Interconnection module for connection four IO modules to

a Wishbone master. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6929

20.219 Loopback test of Wishbone four IO ports (see Figure 20.218). . . . . . 6942

20.220 Inputs and outputs of the quad_decoder module. . . . . . . . . . . . . . 6974

20.221 Simplified block diagram of the Avago HCPL-20XX. . . . . . . . . . . . . 6975

20.222 Simplified digital noise filter logic of the Avago HCPL-20XX. . . . . . . 6976

20.223 Signal propagation through the digital noise filter of the

Avago HCPL-20XX. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6977

20.224 Command pulse generation using a DDA. . . . . . . . . . . . . . . . . . . 6978

20.225 Command pulse generator with input and output latches. . . . . . . . . 6979

20.226 Command pulse generator with Wishbone-bus slave inter-

face (I). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6979

20.227 Command pulse generator with Wishbone-bus slave inter-

face (II). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6980

20.228 An implementation of the “read on the fly” function. . . . . . . . . . . . 6981

20.229 Command pulse generator with Wishbone-bus slave inter-

face (III). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6982

20.230 Generation of TG start and DDA load clocks. . . . . . . . . . . . . . . . . 6983

20.231 Characteristics of IR LEDs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6984

20.232 Typical LED drive circuit. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6984

20.233 LED Threshold Adjustment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6984

20.234 Isolated input channels of MPC-8164 (I). . . . . . . . . . . . . . . . . . . . 6985

20.235 “Current transfer ratio vs. forward current” chracteristics

of PS2805. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6986

161

LIST OF FIGURES

20.236 Isolated input channels of MPC-8164 (II). . . . . . . . . . . . . . . . . . . 6986

20.237 Logic symbol for a positive-triggered D flip-flop with a clock-

enable input. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6987

20.238 Crossing clock domains. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6987

20.239 Input timing constraints on the operation of D flip-flops. . . . . . . . . 6988

20.240 Synchronizer for a clock-domain-crossing signal. . . . . . . . . . . . . . . 6988

20.241 2DFF synchronizer. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6989

20.242 3Dff synchronizer for fast-to-slow clock domain crossing. . . . . . . . . 6989

20.243 Regeneration of a wide enable signal. . . . . . . . . . . . . . . . . . . . . . 6990

20.244 Regeneration of a narrow enable signal. . . . . . . . . . . . . . . . . . . . 6990

20.245 Pulse regeneration with level alternation. . . . . . . . . . . . . . . . . . . . 6991

20.246 Structure of a reset pulse generator. . . . . . . . . . . . . . . . . . . . . . . 6993

20.247 Circuit diagram of the reset pulse generator. . . . . . . . . . . . . . . . . 6993

20.248 Simulation result of the reset pulse generator (I). . . . . . . . . . . . . . 6995

20.249 Simulation result of the reset pulse generator (II). . . . . . . . . . . . . . 6997

20.250 Simulation result of the cdc_reset_pulse_generator_fsm.v. . . . . . . . . 6998

20.251 Digital noise filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6998

20.252 Block diagram of a DDA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7001

20.253 Simulation result of the DDA with an external load signal. . . . . . . . 7007

20.254 Simulation result of the DDA (dda16_ext_sync_short.v). . . . . . . . . . 7013

20.255 Clock generation for TG and DDA. . . . . . . . . . . . . . . . . . . . . . . . 7014

20.256 Generation of ClockT G and ClockDDA−load by the first in-

terpolation board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7015

20.257 Phase difference between ClockT G and ClockDDA−load for

TG execution. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7015

20.258 Generation of ClockT G pulse. . . . . . . . . . . . . . . . . . . . . . . . . . . 7020

20.259 Generation of ClockDDA−load pulse. . . . . . . . . . . . . . . . . . . . . . . . 7020

20.260 Generation of 65533 pulses (I). . . . . . . . . . . . . . . . . . . . . . . . . . 7025

20.261 Generation of ClockT G and ClockDDA−load by the main pro-

cessor board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7025

20.262 Generation of 65535 pulses (II). . . . . . . . . . . . . . . . . . . . . . . . . 7032

20.263 Hardware structure of a PC-based 6-axis robot controller. . . . . . . . . 7033

20.264 Hardware structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 7033

20.265 Software structure of the RCCL v1.0 system. . . . . . . . . . . . . . . . . 7034

162

LIST OF FIGURES

20.266 RCCL routines. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7035

20.267 Setup for calculating the initial value of the encoder count. . . . . . . 7043

20.268 Kollmorgen AKD Servo Drive . . . . . . . . . . . . . . . . . . . . . . . . . . . 7049

20.269 Yaskawa Σ-V Series SGDV SERVOPACK . . . . . . . . . . . . . . . . . . . . 7050

20.270 CoE structure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7051

20.271 EtherCAT state machine. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7052

20.272 RxPDOs and TxPDOs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7053

20.273 Example of a PDO mapping. . . . . . . . . . . . . . . . . . . . . . . . . . . . 7053

20.274 Yaskawa Σ-V Series SGDV SERVOPACK . . . . . . . . . . . . . . . . . . . . 7054

20.275 Atchtecture of an EtherCAT master stack. . . . . . . . . . . . . . . . . . . . 7055

20.276 Architecture of the Igh EtherCAT Master. . . . . . . . . . . . . . . . . . . . 7059

20.277 Block diagram of the cyclic synchronous position mode. . . . . . . . . . 7089

20.278 A SOMANET node consisting of core, COM, and IFM modules. . . . . 7099

20.279 Importing SOMANET Libraries into xTIMEcomposer (1). . . . . . . . . . 7101

20.280 Importing SOMANET Libraries into xTIMEcomposer (2). . . . . . . . . . 7101

20.281 Importing SOMANET Libraries into xTIMEcomposer (3). . . . . . . . . . 7102

20.282 Importing SOMANET Libraries into xTIMEcomposer (4). . . . . . . . . . 7102

20.283 Importing SOMANET Libraries into xTIMEcomposer (5). . . . . . . . . . 7103

20.284 Importing SOMANET Libraries into xTIMEcomposer (6). . . . . . . . . . 7103

20.285 Project building options. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7104

20.286 Messages printed on the console during the project building. . . . . . . 7104

20.287 Cycle i consisting of receive, calculate, send, and idle operations. . . . 7143

20.288 Cycle i consisting of receive, send, calculate, and idle operations. . . . 7143

20.289 Resolution, accuracy and repeatability of a robot arm. . . . . . . . . . . 7146

20.290 An example of PC-based motion control systems with pulse interface. 7148

20.291 An example of PC-based motion control systems with net-

work interface. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7148

20.292 Architecture of the PUMA Mark I controller. . . . . . . . . . . . . . . . . . 7149

C.1 Dimensions of the KUKA KR 500 robot. . . . . . . . . . . . . . . . . . . . . 8343

C.2 Visualization of KR 500 robot poses. . . . . . . . . . . . . . . . . . . . . . . 8344

C.3 “Robot graphics and animation” demo of the Robotics Tool-

box for Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8405

C.4 Starting the demos of the Robotics Toolbox for Matlab . . . . . . . . . . 8406

163

LIST OF FIGURES

C.5 rtbdemo window of the Robotics Toolbox for Matlab . . . . . . . . . . . 8407

C.6 Initial pose of the human figure Bill . . . . . . . . . . . . . . . . . . . . . . 8409

C.7 View menu of the Virtual Machine Manager. . . . . . . . . . . . . . . . . 9320

C.8 Adding a new Filesystem for sharing directories. . . . . . . . . . . . . . . 9320

C.9 Control panel for setting simulation parameters. . . . . . . . . . . . . . . 10040

C.10 Simulation output window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10040

F.1 VAL parser window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17246

H.1 Invenor model of the PUMA 560 (puma560.iv). . . . . . . . . . . . . . . 25764

H.2 VRML model of the PUMA 560 (puma560.wrl). . . . . . . . . . . . . . . 25779

P.1 FlashLink module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57711

P.2 Saturn FPGA module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57712

P.3 Morphic-IC-II module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57712

P.4 PC-based motion control board with USB interface. . . . . . . . . . . . . 57712

P.5 XFC-XC3S50AN FPGA module (Xilinx Spartan 3AN). . . . . . . . . . . . 57713

P.6 FTDI FT2232H mini module. . . . . . . . . . . . . . . . . . . . . . . . . . . . 57713

P.7 Schematic diagram of the XFC-XC3S50AN module. . . . . . . . . . . . . 57714

P.8 Schematic diagram of the FTDI FT2232H mini module. . . . . . . . . . 57715

P.9 Wiring diagram of the XFC-XC3S50AN and FT2232H mini modules. . 57717

P.10 Reset circuit of the FT2232H mini module. . . . . . . . . . . . . . . . . . 57718

P.11 Hardware setup for the XFC-XC3S50AN and FT2232H modules. . . . . 57719

P.12 Wiring diagram of the Open3S3500E board and the FT2232H

module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57724

P.13 Hardware setup for the Open3S3500E board and the FT2232H

module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57725

P.14 Basic Embedded Design Process Flow. . . . . . . . . . . . . . . . . . . . . . 57726

P.15 Saturn FPGA board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57727

P.16 Initial screen of the Xilinx Platform Studio. . . . . . . . . . . . . . . . . . 57728

P.17 Creation of a new XPS project. . . . . . . . . . . . . . . . . . . . . . . . . . 57729

P.18 Board and system selection. . . . . . . . . . . . . . . . . . . . . . . . . . . . 57729

P.19 Processor, Cache, and Peripheral Configuration. . . . . . . . . . . . . . . . 57730

P.20 Xilinx Platform Studio window. . . . . . . . . . . . . . . . . . . . . . . . . . 57730

P.21 “Export to SDK/Launch SDK” window. . . . . . . . . . . . . . . . . . . . . 57731

164

LIST OF FIGURES

P.22 Configuration of the Xilinx SDK workspace. . . . . . . . . . . . . . . . . . 57731

P.23 Initial Xilinx SDK window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57732

P.24 “Application Project” window. . . . . . . . . . . . . . . . . . . . . . . . . . . 57732

P.25 Creation of a new hardware platform. . . . . . . . . . . . . . . . . . . . . . 57733

P.26 “New Hardware Project” window. . . . . . . . . . . . . . . . . . . . . . . . . 57733

P.27 “New Hardware Project” window. . . . . . . . . . . . . . . . . . . . . . . . . 57734

P.28 “Application Project” window. . . . . . . . . . . . . . . . . . . . . . . . . . . 57734

P.29 “Templates” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57735

P.30 “Project Explorer” panel displayed in the SDK window. . . . . . . . . . . 57736

P.31 “Program FPGA” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57736

P.32 Selection of the SaturnV3Test project in the Project Ex-

plorer panel. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57737

P.33 Messages displayed on the terminal emulator. . . . . . . . . . . . . . . . . 57738

P.34 “Creating New XPS Project Using BSB Wizaed” window. . . . . . . . . . 57739

P.35 “Base System Builder – AXI flow: Board and System Se-

lection” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57740

P.36 “Base System Builder – AXI flow: Processor, Cache, and

Peripheral Configuration” window. . . . . . . . . . . . . . . . . . . . . . . . 57741

P.37 “Xilinx Platform Studio” window after generating a new project. . . . 57741

P.38 Dragging “AXI SPI Interface” and dropping it to “System

Assembly View”. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57742

P.39 “Add IP Instance to Design” window. . . . . . . . . . . . . . . . . . . . . . . 57742

P.40 “XPS Core Config” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57743

P.41 “Instantiate and Connect IP” window. . . . . . . . . . . . . . . . . . . . . . 57743

P.42 “Microblaze configuration wizard” window. . . . . . . . . . . . . . . . . . . 57744

P.43 Selection of “Linux wit mmu”. . . . . . . . . . . . . . . . . . . . . . . . . . . 57744

P.44 “XPS Core Config” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57745

P.45 "Open interrupt Control Dialog" button. . . . . . . . . . . . . . . . . . . . . 57745

P.46 “Interrupt Connection Dialog” window. . . . . . . . . . . . . . . . . . . . . 57746

P.47 Modification of the Saturn module’s ucf file. . . . . . . . . . . . . . . . . 57746

P.48 Base addresses of the peripherals. . . . . . . . . . . . . . . . . . . . . . . . 57747

P.49 “Project Options” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57747

P.50 “Export to SDK/Launch SDK” window. . . . . . . . . . . . . . . . . . . . . 57748

P.51 Configuration of the Xilinx SDK workspace. . . . . . . . . . . . . . . . . . 57749

165

LIST OF FIGURES

P.52 Initial Xilinx SDK window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57749

P.53 “Application Project” window. . . . . . . . . . . . . . . . . . . . . . . . . . . 57750

P.54 Creation of a new hardware platform. . . . . . . . . . . . . . . . . . . . . . 57751

P.55 “New Hardware Project” window. . . . . . . . . . . . . . . . . . . . . . . . . 57751

P.56 “New Hardware Project” window. . . . . . . . . . . . . . . . . . . . . . . . . 57752

P.57 “Application Project” window. . . . . . . . . . . . . . . . . . . . . . . . . . . 57752

P.58 “Templates” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57753

P.59 “Project Explorer” panel displayed in the SDK window. . . . . . . . . . . 57753

P.60 “Program FPGA” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57754

P.61 “Run as” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57755

P.62 Messages displayed on the terminal emulator. . . . . . . . . . . . . . . . . 57755

P.63 Connecting the Microblaze processor on the FPGA. . . . . . . . . . . . . 57759

P.64 Downloading the linux system image to the DDR RAM on

the Saturn module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57760

P.65 Starting the linux system on the Saturn module. . . . . . . . . . . . . . . 57761

P.66 Messages displayed on the terminal emulator. . . . . . . . . . . . . . . . . 57762

P.67 Login as root. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57763

P.68 cpuinfo of the Microblaze processor. . . . . . . . . . . . . . . . . . . . . . . 57763

P.69 Default startup clock: JTAGCLK. . . . . . . . . . . . . . . . . . . . . . . . . 57764

P.70 New startup clock when booting from the SPI flash: CCLK. . . . . . . . 57765

P.71 Setting the bootloader’s workspace. . . . . . . . . . . . . . . . . . . . . . . 57766

P.72 Creating a hardware project for the bootloader. . . . . . . . . . . . . . . 57766

P.73 Creating an application project for the bootloader. . . . . . . . . . . . . . 57767

P.74 Selection of the HelloWorld application from templetes. . . . . . . . . . 57767

P.75 Items in Program Explorer. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57768

P.76 ldscript.ld of the HelliWorld application. . . . . . . . . . . . . . . . . . . . 57768

P.77 ldscript.ld of the HelloWorld application. . . . . . . . . . . . . . . . . . . . 57769

P.78 helloworld.c. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57769

P.79 Downloading the download.bin to the SPI flash. . . . . . . . . . . . . . . 57773

P.80 Boot messages of the Linux system. . . . . . . . . . . . . . . . . . . . . . . 57774

P.81 ELS SB1600 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57775

P.82 Xilinx Sp601 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57776

P.83 Replaing FPGAs in the XPS window. . . . . . . . . . . . . . . . . . . . . . . 57777

P.84 Projects in the “Project Explorer” window. . . . . . . . . . . . . . . . . . . 57777

166

LIST OF FIGURES

P.85 Selection of “Program FPGA” menu. . . . . . . . . . . . . . . . . . . . . . . 57778

P.86 Setting the hardware and software configuration in the “Pro-

gram FPGA” window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57778

P.87 Lauching the “hello world” application. . . . . . . . . . . . . . . . . . . . . 57779

P.88 Message displayed on the terminal emulator. . . . . . . . . . . . . . . . . 57780

P.89 Boot messages of the Linux system. . . . . . . . . . . . . . . . . . . . . . . 57781

P.90 Messages displayed on the terminal emulator when lwip_echo_server

starts. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57782

P.91 Configuration of telnet connection. . . . . . . . . . . . . . . . . . . . . . . . 57782

P.92 Messages sent by the echo server. . . . . . . . . . . . . . . . . . . . . . . . 57783

P.93 Terasic DE2-115 board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57784

P.94 OpenEP3C16-C ALTERA FPGA Development Board. . . . . . . . . . . . . 57865

P.95 ENC28J60 Ethernet Board. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57865

P.96 “Select a workspace” window. . . . . . . . . . . . . . . . . . . . . . . . . . . 57866

P.97 “NIOS II Application and BSP from Template” window. . . . . . . . . . . 57867

P.98 lan_test and lan_test_bsp projects in the Project Explorer. . . . . . . . . 57868

P.99 IP address of the FPGA board set in the simple_server.c. . . . . . . . . . 57868

P.100 Progressing of the lan_test project building. . . . . . . . . . . . . . . . . . 57869

P.101 Hardware setup for the web servo project. . . . . . . . . . . . . . . . . . . 57869

P.102 “Quartus II Programmer” window. . . . . . . . . . . . . . . . . . . . . . . . 57870

P.103 “Select Programming File” window. . . . . . . . . . . . . . . . . . . . . . . . 57871

P.104 Downloading the “LED_hello_world.sof” file to the FPGA . . . . . . . . 57872

P.105 NIOS II Console. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57872

P.106 Connecting 192.168.0.100 with Internet Explorer. . . . . . . . . . . . . . 57873

P.107 Incrementing the current data and sending it to the FPGA board. . . . 57873

P.108 Displaying the received data on the NIOS II console. . . . . . . . . . . . 57873

P.109 Altera MAX-II development board. . . . . . . . . . . . . . . . . . . . . . . . 57892

P.110 Powering on the MAX-II development board. . . . . . . . . . . . . . . . . 57893

P.111 Programming the MAX II CPLD with the pre-built PCI ref-

erence design image (PCI_RefDesign.pof). . . . . . . . . . . . . . . . . . . 57894

P.112 Main components of the Altera MAX-II development board. . . . . . . . 57913

P.113 Warning message of migration. . . . . . . . . . . . . . . . . . . . . . . . . . 57915

P.114 MegaWizard Plug-In Manager [page 1]. . . . . . . . . . . . . . . . . . . . . 57916

P.115 Select “Edit . . . ” item for migrating the PCI megafunction. . . . . . . . 57916

167

LIST OF FIGURES

P.116 MegaWizard Plug-In Manager [page 2b]. . . . . . . . . . . . . . . . . . . . 57917

P.117 PCI compiler window. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57917

P.118 Setting parameters of the PCI compiler (1). . . . . . . . . . . . . . . . . . 57918

P.119 Setting parameters of the PCI compiler (2). . . . . . . . . . . . . . . . . . 57918

P.120 Setting parameters of the PCI compiler (3). . . . . . . . . . . . . . . . . . 57919

P.121 Setting parameters of the PCI compiler (4). . . . . . . . . . . . . . . . . . 57919

P.122 Setting up simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57920

P.123 Setting up simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57920

P.124 Generation report. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57921

P.125 Setting devices. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57922

P.126 Setting device and pin options. . . . . . . . . . . . . . . . . . . . . . . . . . 57923

P.127 Flow Summary of the Compilation Report. . . . . . . . . . . . . . . . . . . 57923

P.128 Programmer window of Quartus II. . . . . . . . . . . . . . . . . . . . . . . 57924

P.129 Warning message of loading a time-limited pof file. . . . . . . . . . . . . 57924

P.130 Programming the pof, CFM, UFM files. . . . . . . . . . . . . . . . . . . . . 57925

P.131 Structure of a PCI DIO board. . . . . . . . . . . . . . . . . . . . . . . . . . . 57944

P.132 Configuration of a PCI DIO board for loopback test. . . . . . . . . . . . 57944

P.133 Hilscher CC-link slave card. . . . . . . . . . . . . . . . . . . . . . . . . . . . 57971

168

List of Tables

2.8 Data types and their scaling constants of the VAL language . . . . . . . 86

2.9 Basic type of records . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

2.10 Record types according to the attribute field. . . . . . . . . . . . . . . . . 87

2.11 Record fields according to their types. . . . . . . . . . . . . . . . . . . . . . 87

2.12 Structure of syntax records . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

2.13 Comparison between RCCL and Level-II system. . . . . . . . . . . . . . . 124

3.1 DH parameters of the PUMA 560 (Paul81). . . . . . . . . . . . . . . . . . 176

3.2 DH parameters of the PUMA 560 (Fijany88). . . . . . . . . . . . . . . . . 178

3.3 DH parameters of the PUMA 560 used in RCCL v1.0. . . . . . . . . . . 178

3.4 DH parameters of the PUMA 560 (Paul86). . . . . . . . . . . . . . . . . . 179

3.5 Modified DH parameters of the PUMA 560 (Craig86). . . . . . . . . . . 183

3.6 DH parameters of the 2-DOF planar robot. . . . . . . . . . . . . . . . . . 227

3.7 D-H parameters of the AdeptThree robot. . . . . . . . . . . . . . . . . . . 382

3.8 DH parameters of the PUMA 560 (Paul81). . . . . . . . . . . . . . . . . . 456

3.9 D-H parameters of the PUMA 560 according to the stan-

dard D-H convention (Paul81). . . . . . . . . . . . . . . . . . . . . . . . . . 467

3.10 DH parameters of the SCARA-type wafer transfer robot. . . . . . . . . . 591

3.11 DH parameters of the Adept Viper S650 robot. . . . . . . . . . . . . . . . 603

4.1 Version history of the SunOS. . . . . . . . . . . . . . . . . . . . . . . . . . . 673

4.2 SunOS and its POSIX support. . . . . . . . . . . . . . . . . . . . . . . . . . . 673

4.3 PC motherboards used for installing and testing RTAI 3.6-cv. . . . . . . 689

4.4 Latency test of the Xenomai . . . . . . . . . . . . . . . . . . . . . . . . . . . 733

4.5 Latency test of the Xenomai with the USB disabled. . . . . . . . . . . . . 734

4.6 Xenomai latency test results with the beagleboard-xm. . . . . . . . . . . 751

4.7 Comparison of the services provided by the native and POSIX

API. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 762

169

LIST OF TABLES

4.8 Network configuration for Powerlink demos. . . . . . . . . . . . . . . . . . 1194

6.1 Teach commands of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . 1848

6.2 Priorities of the system tasks in V+. . . . . . . . . . . . . . . . . . . . . . . 1874

6.3 Priorities of the user tasks in V+. . . . . . . . . . . . . . . . . . . . . . . . 1874

6.4 RCCL v1.0 system의 porting. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1875

7.1 Comparison of rotation representation methods. . . . . . . . . . . . . . . 1977

7.2 Possible combinations of TG operation modes. . . . . . . . . . . . . . . . 2149

7.3 Boundary conditions for velocity compensation . . . . . . . . . . . . . . . 2180

7.4 TG state definition in RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . . . 2207

7.5 Boundary conditions at h= 0,1. . . . . . . . . . . . . . . . . . . . . . . . . . 2210

7.6 Representation of path-segment regions with Alphabet letters . . . . . . 2221

7.7 Boundary conditions at h= 0,1, considering moving targets. . . . . . . 2226

7.8 Boundary conditions of q0(h) at h= 0,1. . . . . . . . . . . . . . . . . . . . 2238

7.9 Boundary conditions of qv(h) at h= 0, 1. . . . . . . . . . . . . . . . . . . . 2238

7.10 Boundary conditions of hmv at hv = 0,1. . . . . . . . . . . . . . . . . . . . 2239

7.11 Definitions of RMFS’ TG states (I) . . . . . . . . . . . . . . . . . . . . . . . 2278

7.12 Trajectory generation algorithm of the RFMS (I). . . . . . . . . . . . . . 2279

7.13 Boundary conditions of the θmch(t) in the transition segment. . . . . . 2292

7.14 Boundary conditions of the h. . . . . . . . . . . . . . . . . . . . . . . . . . . 2293

7.15 Definitions of RFMS’ TG states (II). . . . . . . . . . . . . . . . . . . . . . . 2309

7.16 Trajectory generation algorithm of the RFMS (II). . . . . . . . . . . . . . 2311

7.17 Boundary conditions of the h at the state S5. . . . . . . . . . . . . . . . . 2314

7.18 Boundary conditions of the r at the state S5. . . . . . . . . . . . . . . . . 2314

7.19 TG states of the Multi-RCCL . . . . . . . . . . . . . . . . . . . . . . . . . . . 2354

7.20 TG algorithm of the Multi-RCCL. . . . . . . . . . . . . . . . . . . . . . . . . 2356

7.21 Modified TG states of the RCCL. . . . . . . . . . . . . . . . . . . . . . . . . 2422

7.22 Sign changes of the acceleration due to region transitions . . . . . . . . 2501

7.23 Number of interpolation intervals (ninter) according to TG period . . 2511

8.1 Minimum, maximum, initial values of the QSlider widgets. . . . . . . . 2687

8.2 Minimum, maximum, initial values of the QSoubleSpinBox widgets. . 2688

8.3 Data transmitted via the FIFOs . . . . . . . . . . . . . . . . . . . . . . . . . 2743

170

LIST OF TABLES

8.4 Minimum values of the PUMA 560’s joint angles set by the

RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2745

8.5 Dynamic parameters of the PUMA 560 robot in the rlmdl/unimation-

puma560.xml. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2770

8.6 Center of gravity measured by B. Amstrong. . . . . . . . . . . . . . . . . . 2771

8.7 DH parameters extracted from the rlmdl/unimation-puma560.xml. . . 2772

8.8 Joint-angle ranges extracted from the rlmdl/unimation-puma560.xml. 2772

8.9 DH parameters of the PUMA 560 robot in Robotics Library. . . . . . . . 2991

8.10 Joing-angle ranges of the PUMA 560 robot in RCCL v1.0. . . . . . . . . 2995

8.11 Mamximum velocity, torque, acceleration of joints measured

by P. Corke. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3016

10.1 DH parameters of the PUMA 560 (Paul81). . . . . . . . . . . . . . . . . . 3960

10.2 DH parameters of the PUMA 560 (Paul86). . . . . . . . . . . . . . . . . . 3961

10.3 Modified DH parameters of the PUMA 560 (Armstrong86). . . . . . . . 3962

10.4 DH parameters of the SCARA robot. . . . . . . . . . . . . . . . . . . . . . . 3998

11.1 Comparison of the sinusoidal input uc(t) and output yc,SS(t). . . . . . 4023

11.2 Comparison of the sinusoidal input ud(k) and output yd,SS(k). . . . . . 4027

11.3 Elmo SimpleIQ Digital Servo Drives Technical Data. . . . . . . . . . . . . 4087

11.4 Special cases of the feedback-type 2-DOF PID controller. . . . . . . . . . 4094

11.5 Comparison of 1-DOF PID, feedforward-type 2-DOF PID,

setpoint-filter-type 2-DOF PID control systems. . . . . . . . . . . . . . . . 4095

11.6 Measured friction parameters. . . . . . . . . . . . . . . . . . . . . . . . . . . 4103

11.7 DH parameters of the two-link planar robot (Paul’s DH convention). . 4166

11.8 DH parameters of the two-link planar robot (Craig’s DH convention). 4174

11.9 DH parameters of the PUMA 560 (Armstrong86). . . . . . . . . . . . . . 4179

11.10 DH parameters of the PUMA 560 (Paul81). . . . . . . . . . . . . . . . . . 4181

11.11 DH parameters of the PUMA 560 (Paul86). . . . . . . . . . . . . . . . . . 4181

11.12 Coefficients for gravity compensation in RCCL v5.0 and v1.0. . . . . . 4187

11.13 Vibration sources and their suppression methods. . . . . . . . . . . . . . 4201

12.1 Coefficients for gravity compensation in RCCL v5.0 and v1.0. . . . . . 4421

12.2 ID() function usage. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4422

12.3 Return values of LATCHED() function and their meaning. . . . . . . . . 4423

171

LIST OF TABLES

12.4 Arguments of FORCE.FRAME instruction and their meanings. . . . . . . 4425

12.5 Mode parameter of FORCE.MODE instruction and its meaning. . . . . 4425

12.6 Parameters parm0, parm1, parm2 and parm3 and their mean-

ings in gauarded mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4426

12.7 FORCE.MODE instruction for changing the operating pa-

rameters of the force system. . . . . . . . . . . . . . . . . . . . . . . . . . . 4427

12.8 FORCE.MODE instruction for miscellaneous functions. . . . . . . . . . . 4427

12.9 Return values of FORCE.READ instruction according to mode

parameter settings. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4429

13.1 Specifying COORD, POS, and TOOL in makeposition(). . . . . . . . . . . 4460

13.2 Boundary conditions at h= 0,1. . . . . . . . . . . . . . . . . . . . . . . . . . 4468

14.1 Comparison of multirobot cooperative functions. . . . . . . . . . . . . . . 4497

16.1 Initialization files of shells. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4715

16.2 Types of the Hilscher CIF cards. . . . . . . . . . . . . . . . . . . . . . . . . . 4766

16.3 Mofication of the cif_api.c for the RTDM. . . . . . . . . . . . . . . . . . . 4775

16.4 PCI IDs of the Hilscher CC-link card. . . . . . . . . . . . . . . . . . . . . . 4784

16.5 PCI Base Addresses of the PCI7432 card. . . . . . . . . . . . . . . . . . . . 4804

16.6 Linux and RTDM device drivers and test programs for the

PCI7432 card. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4822

16.7 Read. write operations on the Local Address Space 0. . . . . . . . . . . 4842

16.8 PCI Express link performance. . . . . . . . . . . . . . . . . . . . . . . . . . . 5031

18.1 Events and actions of PDS transitions. . . . . . . . . . . . . . . . . . . . . . 5228

18.2 Connector and terminal-pin description of the EK1100. . . . . . . . . . . 5312

18.3 Communication sequence between LSI-11 and VAX-11/780. . . . . . . . 5847

18.4 Gear ratios of PUMA 560 robots. . . . . . . . . . . . . . . . . . . . . . . . . 5923

18.5 Joint torque calibration of PUMA 560. . . . . . . . . . . . . . . . . . . . . 5929

18.6 Motor specifications of PUMA 560 robots. . . . . . . . . . . . . . . . . . . 6035

18.7 DH parameters of the PUMA 560 (Paul81). . . . . . . . . . . . . . . . . . 6046

18.8 DH parameters of the PUMA 560 (Paul86). . . . . . . . . . . . . . . . . . 6046

19.1 Charateristics of the circular interpolation methods. . . . . . . . . . . . . 6425

19.2 Circular-path machining error due to Acc/Dec filter. . . . . . . . . . . . . 6465

172

LIST OF TABLES

19.3 Cases of velocity transitions in the block overlap control. . . . . . . . . 6474

19.4 Comparison of RTAPI implementations. . . . . . . . . . . . . . . . . . . . . 6541

19.5 Linux systems officially supported by LinuxCNC v2.7. . . . . . . . . . . . 6553

20.1 Address map of the PCL6045B. . . . . . . . . . . . . . . . . . . . . . . . . . 6603

20.2 PCL6045B registers for setting speed pattern parameters. . . . . . . . . 6623

20.3 Comparison of the Hivertec motion control cards. . . . . . . . . . . . . . 6659

20.4 Address map of the MPC-3035. . . . . . . . . . . . . . . . . . . . . . . . . . 6685

20.5 Content of the serial EEPROM (93LC46B). . . . . . . . . . . . . . . . . . . 6708

20.6 Signal numbers used by the Adlink device driver of PCI

8164 cards. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6728

20.7 Enabling and clearing PCL6045B interrupts. . . . . . . . . . . . . . . . . . 6731

20.8 PCI BAR values of the PCI-8164 card. . . . . . . . . . . . . . . . . . . . . . 6749

20.9 Local interrupt sorces of MPC-3035 and PCI-8164. . . . . . . . . . . . . 6751

20.10 Interpolation board as a stripped-down version of the ad-

vanced motion control board. . . . . . . . . . . . . . . . . . . . . . . . . . . 6826

20.11 State definition of the quadrature signals. . . . . . . . . . . . . . . . . . . 6842

20.12 Determination of the up/down# (direction) signal. . . . . . . . . . . . . 6844

20.13 Address ranges assigned to Wishbone slaves. . . . . . . . . . . . . . . . . 6935

20.14 TT G with respect to different Modulo M values (33.33MHz

PCI clock). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7017

20.15 Position interface supported fixed mappings. . . . . . . . . . . . . . . . . . 7062

20.16 Position interface supported fixed mappings. . . . . . . . . . . . . . . . . . 7063

20.17 Objects selected for the RxTDOs and TxTDOs. . . . . . . . . . . . . . . . 7087

20.18 Differences between CNC systems and robot systems. . . . . . . . . . . . 7144

C.1 DH parameters of the KR 500 robot (FreeCAD). . . . . . . . . . . . . . . 8343

P.1 Differences in hardware specification. . . . . . . . . . . . . . . . . . . . . . 57775

173

List of Programs

3.1 Forward kinematics of the PUMA 560 written in Mathematica. . . . . . 188

3.2 Inverse kinematics of the PUMA 560 written in Mathematica. . . . . . 192

3.3 The inverse kinematrics of the PUMA 560 implemented in

the RCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 205

3.4 Determination of the configuration indicators. . . . . . . . . . . . . . . . . 208

3.5 singularity-test.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 238

3.6 ik-with-limiter.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 247

3.7 via-points.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255

3.8 time-scaling.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257

3.9 clrmrc-svd.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265

3.10 collapsed-jacobian.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282

3.11 singularity-avoidance.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 292

3.12 Computation of det(T6 J) (I). . . . . . . . . . . . . . . . . . . . . . . . . . . . 303

3.13 Computation of det(T6 J) (II). . . . . . . . . . . . . . . . . . . . . . . . . . . 309

3.14 jacobian-based-ik-2dof-robot.py . . . . . . . . . . . . . . . . . . . . . . . . . 314

3.15 closed-loop-jacobian-method.py . . . . . . . . . . . . . . . . . . . . . . . . . 323

3.16 closed-loop-dls.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345

4.1 Example of real-time peiodic tasks linked with librt. . . . . . . . . . . . 675

4.2 Modification of a generic kernel for a lowlatency kernel

(config-3.13.0-43-lowletency). . . . . . . . . . . . . . . . . . . . . . . . . . . 682

4.3 Adding ICH9 chipsets to smi.c. . . . . . . . . . . . . . . . . . . . . . . . . . 740

4.4 Xenomai example: trivial-periodic. . . . . . . . . . . . . . . . . . . . . . . . 756

4.5 Xenomai example: trivial-periodic-1.c . . . . . . . . . . . . . . . . . . . . . 759

4.6 Xenomai example: sched-tp.c . . . . . . . . . . . . . . . . . . . . . . . . . . 762

4.7 Example of clock_nanosleep() . . . . . . . . . . . . . . . . . . . . . . . . . . 767

4.8 Example of native + POSIX skins: mq_select.c . . . . . . . . . . . . . . . 770

4.9 RTDM device driver for shared memory: rtdm_shm.c . . . . . . . . . . . 774

174

LIST OF PROGRAMS

4.10 Makefile for building the kernel-module satch.ko. . . . . . . . . . . . . . 781

4.11 Source file of the kernel-module satch.ko. . . . . . . . . . . . . . . . . . . 781

4.12 Makefile for building the user-space application satch . . . . . . . . . . . 785

4.13 Source file of the user-space application satch . . . . . . . . . . . . . . . 786

4.14 Message heap services example: local_heap.c for a kernel module. . . 791

4.15 Message heap service example: shared_mem.c for a user-

space application. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 792

4.16 Message pipe service example: pipe.c for a kernel module. . . . . . . . 795

4.17 Message pipe service example: reading data from a pipe

with rt_pipe_monitor(). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 797

4.18 Message pipe service example: pipe.c for a user-space application. . . 798

4.19 Message pipe service example:: reading data from a pipe

with select() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 800

4.20 Modification of the lowlatency kernel configuration for rt-

preempt kernel. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 814

4.21 Creating peiodic tasks with timerfd(). . . . . . . . . . . . . . . . . . . . . . 845

4.22 Creating periodic tasks with timerfd() and librt. . . . . . . . . . . . . . . 847

4.23 usb-skeletin.c: a USB device-driver skeleton. . . . . . . . . . . . . . . . . . 878

4.24 Serial programming example (cannonical input processing). . . . . . . . 893

4.25 struct usb_driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 895

4.26 struct usb_serial_driver. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 897

4.27 struct serial_ops. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 897

4.28 struct ftdi_sio_device. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 898

4.29 function serial_ioctl(). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 898

4.30 usb_control_msg() function to support USB control transfer. . . . . . . 899

4.31 usb_bulk_msg() function for synchronous bulk data transfer. . . . . . . 900

4.32 serial_write() function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 902

4.33 fixup_generic() function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 902

4.34 usb_serial_generic_write() function. . . . . . . . . . . . . . . . . . . . . . . 903

4.35 usb_serial_generic_write_start() function. . . . . . . . . . . . . . . . . . . . 904

4.36 ftdi_prepare_write_buffer() function. . . . . . . . . . . . . . . . . . . . . . . 905

4.37 usb_submit_urb() function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 906

4.38 Function ftdi_process_read_urb(). . . . . . . . . . . . . . . . . . . . . . . . . 913

4.39 Function usb_serial_generic_read_bulk_callback(). . . . . . . . . . . . . . 913

175

LIST OF PROGRAMS

4.40 Function usb_serial_probe(). . . . . . . . . . . . . . . . . . . . . . . . . . . . 914

4.41 Function libusb_control_transfer() of the libusb library. . . . . . . . . . . 915

4.42 Function libusb_bulk_transfer() of the libusb library. . . . . . . . . . . . 917

4.43 Function ftdi_read_data_submit() of the libftdi library. . . . . . . . . . . 920

4.44 Function ftdi_write_data_submit() of the libftdi library. . . . . . . . . . . 921

4.45 Function ftdi_stream() of the libftdi library. . . . . . . . . . . . . . . . . . 922

4.46 stream_test.c of the libftdi examples. . . . . . . . . . . . . . . . . . . . . . 927

4.47 Loopback test program for ftdi_sio.ko: wrteread.c. . . . . . . . . . . . . . 939

4.48 Loopback test program for libftdi: loopback-ftdi.c. . . . . . . . . . . . . . 941

4.49 Function ftdi_usb_open_dev(). . . . . . . . . . . . . . . . . . . . . . . . . . . 943

4.50 Patch for usb-skeleton.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 954

4.51 A simple kernel-space device driver of a FTDI serial converter. . . . . . 956

4.52 usb-skeleton.c taken from the source of kernel 2.6.31.14. . . . . . . . . 969

4.53 Modification of the usb-serial.c for a FTDI serial converter. . . . . . . . 977

4.54 usb-skeleton.c taken from the source of kernel 2.6.31.14. . . . . . . . . 978

4.55 usb_rtdm.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 991

4.56 usbRtdmSkeleton.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1010

4.57 usbRtdmTest.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1024

4.58 Displaying the vendor and device IDs of PCI devies with

“lspci -nn” command. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1029

4.59 Porting the source files in the core directory to Xenomai. . . . . . . . . 1032

4.60 Porting the source files in the host directory to Xenomai. . . . . . . . . 1045

4.61 rt_ftdi_sio.c: realtime FTDI device driver. . . . . . . . . . . . . . . . . . . . 1082

4.62 Loopback test program for the realtime FTDI device driver. . . . . . . . 1096

4.63 FT2232H loopback test program with the synchronous API

of libftdi. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1100

4.64 FPGA code for 1530-byte loopback test (I). . . . . . . . . . . . . . . . . . 1103

4.65 FPGA code for 1530-byte loopback test (II). . . . . . . . . . . . . . . . . . 1106

4.66 FT2232H loopback test program which sends and receives

1530 bytes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1109

4.67 FPGA code for 1530-byte loopback test (III). . . . . . . . . . . . . . . . . 1113

4.68 FT2232H loopback test program with the asynchronous API

of the libftdi. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1116

176

LIST OF PROGRAMS

4.69 Linux kernel-space device driver of the FT2232H (synchronous

API). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1120

4.70 Linux kernel-space device driver of the FT2232H (synchronous

API, LDD3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1132

4.71 Loopback test program for the FT2232H kernel-space de-

vice driver (synchronous API, LDD3). . . . . . . . . . . . . . . . . . . . . . 1139

4.72 Linux kernel-space device driver of the FT2232H (asyn-

chronous API, LDD3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1143

4.73 Loopback test program for the FT2232H RTDM device driver

(nonblocking IO). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1156

4.74 RTDM device driver of the FT2232H (blocking IO). . . . . . . . . . . . . 1160

4.75 Loopback test program for the FT2232H RTDM device driver

(blocking IO). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1172

4.76 “dc-user” example of the IgH EtherCAT Master. . . . . . . . . . . . . . . . 1237

4.77 am3359_test.c: user-space ethercat master program for the

AM3359 ICE. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1245

4.78 am3359_dual_test.c: user-space master program for two AM3359

ICEs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1253

4.79 am3359_dual_dc_user.c: user-space bus-shift mode program

for two AM3359 ICEs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1261

4.80 xenomai_dc_user.c: Xenomai example of the bus-shift mode

programming in user space. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1271

4.81 dc_rtai_sample.c: RTAI example of the bus-shift mode. . . . . . . . . . . 1279

4.82 dc_xenomai_sample.c: Xenomai example of the bus-shift

mode progamming in kernel space. . . . . . . . . . . . . . . . . . . . . . . 1286

4.83 xenomai_dc_kernel.c: Xenomai example of the bus-shift mode

programming of two AM3359 ICEs in kernel space. . . . . . . . . . . . . 1293

4.84 Source code directory structure of the rccl-rtai. . . . . . . . . . . . . . . . 1366

4.85 Source code of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . . . . 1367

4.86 An implementation of the RCCL’s main.c using timerfd()

and librt. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1384

4.87 simdevices.c of the RWRCCL ported from RTLinux to RTAI. . . . . . . . 1395

4.88 simdevices.c of the RWRCCL ported from RTAI to Xenomai. . . . . . . 1398

4.89 moperx_sim.c for realtime simulation of the RWRCCL. . . . . . . . . . . 1404

177

LIST OF PROGRAMS

4.90 pack.c modified for the ALVIN system. . . . . . . . . . . . . . . . . . . . . 1518

5.1 Source code of mfcalc – Makefile. . . . . . . . . . . . . . . . . . . . . . . . 1547

5.2 Source code of mfcalc – calc.h . . . . . . . . . . . . . . . . . . . . . . . . . 1547

5.3 Source code of mfcalc – mfcalc.l . . . . . . . . . . . . . . . . . . . . . . . . 1548

5.4 Source code of mfcalc – mfcalc.y . . . . . . . . . . . . . . . . . . . . . . . . 1549

5.5 Source code of mfcalc – Makefile (II) . . . . . . . . . . . . . . . . . . . . . 1551

5.6 Source code of mfcalc – main.c (II) . . . . . . . . . . . . . . . . . . . . . . 1551

5.7 Source code of mfcalc – mfcalc.y (II) . . . . . . . . . . . . . . . . . . . . . 1552

5.8 Source code of fb3-2 – makefile . . . . . . . . . . . . . . . . . . . . . . . . 1554

5.9 Source code of fb3-2 – fb3-2.h . . . . . . . . . . . . . . . . . . . . . . . . . 1555

5.10 Source code of fb3-2 – fb3-2.y . . . . . . . . . . . . . . . . . . . . . . . . . 1558

5.11 Source code of fb3-2 – fb3-2.l . . . . . . . . . . . . . . . . . . . . . . . . . . 1561

5.12 Source code of fb3-2 – fb3-2funcs.c . . . . . . . . . . . . . . . . . . . . . . 1563

5.13 Source code of purecalc – Makefile . . . . . . . . . . . . . . . . . . . . . . 1573

5.14 Source code of purecalc – purecalc.h . . . . . . . . . . . . . . . . . . . . . 1574

5.15 Modification made to the fb3-2.h . . . . . . . . . . . . . . . . . . . . . . . . 1576

5.16 Source code of purecalc – purecalc.y . . . . . . . . . . . . . . . . . . . . . 1578

5.17 Modification made to the fb3-2.y . . . . . . . . . . . . . . . . . . . . . . . . 1579

5.18 Source code of purecalc – purecalc.l . . . . . . . . . . . . . . . . . . . . . . 1581

5.19 Modification made to the fb3-2.l . . . . . . . . . . . . . . . . . . . . . . . . 1583

5.20 Source code of purecalc – precalcfuncs.c . . . . . . . . . . . . . . . . . . . 1584

5.21 Modification made to the fb3-2funcs.c . . . . . . . . . . . . . . . . . . . . . 1592

5.22 Source code of yabasic – scan.l . . . . . . . . . . . . . . . . . . . . . . . . . 1610

5.23 Source code of yabasic – parse.y . . . . . . . . . . . . . . . . . . . . . . . . 1616

5.24 Source code of tinybc – Makefile . . . . . . . . . . . . . . . . . . . . . . . . 1642

5.25 Source code of tinybc – main.c . . . . . . . . . . . . . . . . . . . . . . . . . 1643

5.26 Source code of tinybc - expr() . . . . . . . . . . . . . . . . . . . . . . . . . . 1650

5.27 Source code of tinybc - get_type() . . . . . . . . . . . . . . . . . . . . . . . 1685

5.28 Source code of tinybc – tinybc_start() . . . . . . . . . . . . . . . . . . . . . 1686

5.29 Source code of tinybc – tinybc_get() . . . . . . . . . . . . . . . . . . . . . . 1687

5.30 Source code of tinybc – tinybc_set() . . . . . . . . . . . . . . . . . . . . . . 1687

5.31 Modification of the tinybc – main.c . . . . . . . . . . . . . . . . . . . . . . 1688

5.32 Modification of the tinybc – wrapper functions . . . . . . . . . . . . . . . 1688

5.33 Modification of the tinybc – struct cttype . . . . . . . . . . . . . . . . . . . 1689

178

LIST OF PROGRAMS

5.34 Modification of the tinybc – tinybc_get() and tinybc_set() . . . . . . . . 1691

5.35 Modification of the tinybc – examples of call statements . . . . . . . . . 1692

5.36 Modification of the tinybc – struct mttype . . . . . . . . . . . . . . . . . . 1693

5.37 Modification of the tinybc – program_stmt() . . . . . . . . . . . . . . . . . 1694

5.38 Modification of the tinybc – call_stmt() . . . . . . . . . . . . . . . . . . . . 1696

5.39 Modification of the tinybc – tcall_stmt() . . . . . . . . . . . . . . . . . . . 1699

5.40 Modification of the tinybc – tinybc_trun() . . . . . . . . . . . . . . . . . . 1699

5.41 Cocurrently running programs using TCALL statements . . . . . . . . . . 1700

5.42 Cocurrently running programs using trun command . . . . . . . . . . . . 1701

5.43 Implementation of the trun command . . . . . . . . . . . . . . . . . . . . . 1702

5.44 Modification of the tokenize.h for adding sqrt() . . . . . . . . . . . . . . 1703

5.45 Modification of the tokenize.c for adding sqrt() . . . . . . . . . . . . . . . 1703

5.46 Modification of the factor() for adding sqrt() . . . . . . . . . . . . . . . . 1704

5.47 Script for testing sqrt() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1705

5.48 Script to test tracing and single-step execution . . . . . . . . . . . . . . . 1710

5.49 Modification of the tinybc.c for the implementation of CFUN() . . . . . 1712

5.50 Script for testing UFUN() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1714

5.51 Modification of the tinybc.c for the implementation of ufun() . . . . . 1716

5.52 Implementation of if statement . . . . . . . . . . . . . . . . . . . . . . . . . 1719

5.53 Implementation of else statement . . . . . . . . . . . . . . . . . . . . . . . . 1721

5.54 Implementation of endif statement . . . . . . . . . . . . . . . . . . . . . . . 1721

5.55 Implementation of for and endfor statements . . . . . . . . . . . . . . . . 1723

5.56 Implementation of while and endwhile statements . . . . . . . . . . . . . 1726

6.1 Example of pthread_create() . . . . . . . . . . . . . . . . . . . . . . . . . . . 1745

6.2 Source code of the val interpreter – Makefile . . . . . . . . . . . . . . . . 1759

6.3 Source code of the val interpreter – val.h . . . . . . . . . . . . . . . . . . 1760

6.4 Source code of the val interpreter – val.l . . . . . . . . . . . . . . . . . . . 1761

6.5 Source code of the val interpreter – val.y . . . . . . . . . . . . . . . . . . 1765

6.6 Implementation of the move statement . . . . . . . . . . . . . . . . . . . . 1770

6.7 Data types obj and SYMBOL for representing programs as trees . . . . 1776

6.8 Example of repreneting a parse tree with the obj and SYMBOL . . . . 1776

6.9 Bison input file parse.y of the gnbasic . . . . . . . . . . . . . . . . . . . . . 1778

6.10 Example of RCCL programs, manex1.c . . . . . . . . . . . . . . . . . . . . 1784

6.11 BASIC program correspoding to RCCL program, manex1.c . . . . . . . . 1786

179

LIST OF PROGRAMS

6.12 Implementation of the settr_stmt() function . . . . . . . . . . . . . . . . . 1787

6.13 The robot_start() function of the RCCL . . . . . . . . . . . . . . . . . . . . 1788

6.14 Source code of BIFES – robot_movetr_stmt() function . . . . . . . . . . 1789

6.15 Example 1 of dlopen() – Makefile . . . . . . . . . . . . . . . . . . . . . . . 1792

6.16 Example 1 of dlopen() – test_dlopen.c . . . . . . . . . . . . . . . . . . . . 1792

6.17 Example 1 of dlopen() – myadd.c . . . . . . . . . . . . . . . . . . . . . . . 1793

6.18 Example 1 of dlopen() – test_add.c . . . . . . . . . . . . . . . . . . . . . . 1795

6.19 Example 2 of dlopen() – test_add_1.c . . . . . . . . . . . . . . . . . . . . . 1798

6.20 malloc.c of the BSD UNIX 4.1c . . . . . . . . . . . . . . . . . . . . . . . . . 1800

6.21 sbrk() implemented in Mac OS X. . . . . . . . . . . . . . . . . . . . . . . . 1804

6.22 sbrk() implemented in RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . 1805

6.23 Cutomized dynamic memory allocators . . . . . . . . . . . . . . . . . . . . 1806

6.24 Thread-safe dynamic memory allocator . . . . . . . . . . . . . . . . . . . . 1810

6.25 manex1.c of the RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . 1811

6.26 manex1.pg of the BIFES corresponding to the manex1.c . . . . . . . . . 1812

6.27 Log file geneated by the test_move_2 program . . . . . . . . . . . . . . . 1813

6.28 Modification of the tokenize.h for BIFERS . . . . . . . . . . . . . . . . . . 1819

6.29 C code to test the BIFERS’ scanner . . . . . . . . . . . . . . . . . . . . . . . 1820

6.30 BIFERS program (test_lower) to test the scanner . . . . . . . . . . . . . . 1822

6.31 Tokenization result of the test_lower program . . . . . . . . . . . . . . . . 1822

6.32 Implementation of the compile_print() function . . . . . . . . . . . . . . . 1823

6.33 Implementation of the decompile_print() function . . . . . . . . . . . . . 1824

6.34 Implementation of the execute_print() function . . . . . . . . . . . . . . . 1825

6.35 manex1.c of the RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . 1826

6.36 manex1.pg of BIFERS corresponding to the manex1.c . . . . . . . . . . . 1827

6.37 teachex.c of the RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1849

6.38 Script of executing the teachex program . . . . . . . . . . . . . . . . . . . 1850

6.39 teach() function implemented in RCCL v1.0 . . . . . . . . . . . . . . . . . 1851

6.40 Standalone teach program (teachdemo.c) of MultiRCCL . . . . . . . . . 1856

6.41 trsf() function implemented in MultiRCCL . . . . . . . . . . . . . . . . . . 1860

7.1 blend-parabolic.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1922

7.2 blend-parabolic-acceleration.py . . . . . . . . . . . . . . . . . . . . . . . . . . 1924

7.3 blend-fifth-order.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1928

7.4 blend-fifth-order-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1930

180

LIST OF PROGRAMS

7.5 blend-seventh-order.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1934

7.6 blend-convex-average.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1947

7.7 blend-convex-average-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 1950

7.8 blend-convex-average-fifth.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 1958

7.9 blend-convex-average-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 1960

7.10 blend-convex-average-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 1962

7.11 blend-prediction.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1968

7.12 blend-prediction-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1969

7.13 blend-rotation.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2065

7.14 blend-rotation-vel-acc-1.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2067

7.15 blend-rotation-jerk.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2080

7.16 blend-rotation-jerk-acc.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2082

7.17 blend-rotation-prediction.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2095

7.18 blend-rotation-prediction-acc-1.py . . . . . . . . . . . . . . . . . . . . . . . . 2097

7.19 example-5-1.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2201

7.20 Source code of the trajectory generator of the RCCL v1.0 . . . . . . . . 2243

7.21 Trajectory generator of the RFMS with states S0, S1, S2,

S3, and S4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2281

7.22 Source code of the trajectory generator implemented in the RFMS . . 2316

7.23 Source code of the trajectory generator of the multi-RCCL . . . . . . . 2383

7.24 New implementation of the RCCL v1.0’s trajectory generator . . . . . . 2423

7.25 Source code of a quaternion-based trajectory generator . . . . . . . . . . 2442

7.26 moper.c - main program of the supervisor program “moper”. . . . . . . 2514

7.27 vec.s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2520

8.1 Example from the Iventor Mentor, Hello cone. . . . . . . . . . . . . . . . 2563

8.2 Example of the SoShapeKit . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2567

8.3 Cube.c++ implemented without using the SoShapeKit . . . . . . . . . . 2568

8.4 Example of the trackball manipulator from the Inventor Mentor. . . . . 2569

8.5 Xt utility functions for initialize Inventor applications. . . . . . . . . . . 2571

8.6 Source code of a new wiggly application – mainwindow.h . . . . . . . . 2587

8.7 Source code a new wiggly application – mainwindow.cpp . . . . . . . . 2588

8.8 SoXt example – 02.4.Examiner.c++. . . . . . . . . . . . . . . . . . . . . . . 2591

8.9 SoQt example – 02.4.Examiner. . . . . . . . . . . . . . . . . . . . . . . . . . 2594

8.10 Example of SoQtExaminerViewer – SoQtExaminerViewer.cpp . . . . . . 2596

181

LIST OF PROGRAMS

8.11 Implementation of Inventor/VRML viewer with Pivy. . . . . . . . . . . . 2599

8.12 server.cpp for shared-memory interface. . . . . . . . . . . . . . . . . . . . . 2607

8.13 VRML model of the KR 16 robot. . . . . . . . . . . . . . . . . . . . . . . . . 2611

8.14 VRML model of the TX40 robot used by FreeCAD. . . . . . . . . . . . . 2614

8.15 Template for generating robot models in VRML format. . . . . . . . . . 2622

8.16 Python script for generation robot models in VRMK format. . . . . . . 2625

8.17 TaskRobot6Axis.cpp of the FreeCAD to display a robot con-

trol panel. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2637

8.18 Source code of QProgressBar – main.cpp . . . . . . . . . . . . . . . . . . . 2640

8.19 Source code of QProgressBar – myWindow.h . . . . . . . . . . . . . . . . 2640

8.20 Qt project file (qslider.pro) for the QProgressBar example. . . . . . . . . 2641

8.21 Source code of the genangles module for shared-memory writes. . . . 2642

8.22 Source code of the qslider module – main.cpp. . . . . . . . . . . . . . . . 2644

8.23 Source code of the qslider module – myWindow.h. . . . . . . . . . . . . 2644

8.24 main.c of the irb4440 package. . . . . . . . . . . . . . . . . . . . . . . . . . 2654

8.25 mainWidget.cpp of the irb4440 package. . . . . . . . . . . . . . . . . . . . 2658

8.26 ui_mainWidgetform.h of the irb4440 package. . . . . . . . . . . . . . . . 2661

8.27 main.c of the SCARA simulator. . . . . . . . . . . . . . . . . . . . . . . . . . 2668

8.28 mainWidget.cpp of the SCARA simulator. . . . . . . . . . . . . . . . . . . . 2669

8.29 VRML code of the PUMA 560 robot from the robotlib. . . . . . . . . . . 2670

8.30 VRML model of the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . 2678

8.31 Implementation of a server for reading a shared-memory. . . . . . . . . 2711

8.32 Implemetation of a clinet for reading a shared-memory. . . . . . . . . . 2714

8.33 Implementation of a FIFO writer. . . . . . . . . . . . . . . . . . . . . . . . . 2718

8.34 Implementation of a FIFO reader. . . . . . . . . . . . . . . . . . . . . . . . . 2718

8.35 Example of a TCP/IP echo server – TCPEchoServer-Select.c . . . . . . . 2725

8.36 Example of a TCP/IP client – TCPEchoClient.c . . . . . . . . . . . . . . . 2727

8.37 makefile for building the TCP/IP server and clinet examples . . . . . . 2728

8.38 Modified TCPEchoServer-Select.c for polling incomming data. . . . . . . 2729

8.39 Modified TCPEchoClient.c for sending data. . . . . . . . . . . . . . . . . . 2730

8.40 Implementation a non-blocking TCP server. . . . . . . . . . . . . . . . . . 2732

8.41 Implementation of a non-blocing TCP client . . . . . . . . . . . . . . . . . 2737

8.42 Header to describe FIFOs – fifo_com.h . . . . . . . . . . . . . . . . . . . . 2741

8.43 printjn.c function of the RCCL v1.0 . . . . . . . . . . . . . . . . . . . . . . 2745

182

LIST OF PROGRAMS

8.44 Reading an input line in non-blocking mode. . . . . . . . . . . . . . . . . 2748

8.45 Periodically executed thread to read FIFOs. . . . . . . . . . . . . . . . . . 2750

8.46 unimation-puma560_boxes.xml: scene file. . . . . . . . . . . . . . . . . . . 2755

8.47 rlkin/unimation-puma560.xml for descibing DH parameters

of the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2755

8.48 rlmdl/planar2.xml for the kinematic and dynamic models

of a planar robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2761

8.49 rlmdl/unimation-puma560.xml for the dynamic model of

the PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2765

8.50 URDF example: 05-visual.urdf. . . . . . . . . . . . . . . . . . . . . . . . . . 2775

8.51 URDF example: 06-flexible.urdf. . . . . . . . . . . . . . . . . . . . . . . . . 2780

8.52 Python script for solving the Hanoi puzzle (hanoi.py). . . . . . . . . . . 2804

8.53 Tutorial 3 of the PyODE (tutorial3.py). . . . . . . . . . . . . . . . . . . . . 2817

8.54 Falling-boxes tutorial of the cgkit (falling-boxes.py). . . . . . . . . . . . . 2833

8.55 Falling-boxes tutorial of the cgkit (falling-boxes.py). . . . . . . . . . . . . 2840

8.56 Running robot-sandbox demos on Ubuntu 10.04. . . . . . . . . . . . . . . 2844

8.57 scene.py of the robot-sandbox (stripped). . . . . . . . . . . . . . . . . . . . 2857

8.58 console.py of the robot-sandbox. . . . . . . . . . . . . . . . . . . . . . . . . 2861

8.59 Lua script embedded in “ABB IRB 140.ttm”. . . . . . . . . . . . . . . . . . 2884

8.60 URDF model of the PUMA 560 robot (PUMA-560-ros.urdf). . . . . . . . 2982

8.61 unimation-puma560.wrl: VRML model of the PUMA 560 robot. . . . . 2993

9.1 pumatask function written in C language (tstmk.c). . . . . . . . . . . . . 3337

9.2 pumatask function written in Python (tstmk.py). . . . . . . . . . . . . . . 3337

9.3 tstmk.py written with the RTAI wrapper. . . . . . . . . . . . . . . . . . . . 3341

9.4 Makefile for building the libshm.so – Makefile . . . . . . . . . . . . . . . 3358

9.5 header for building the libshm.so – shm_com.h . . . . . . . . . . . . . . . 3358

9.6 C source code for building the libshm.so – shmlib.c . . . . . . . . . . . . 3358

9.7 server.pro for the server program. . . . . . . . . . . . . . . . . . . . . . . . 3360

9.8 shm_com.h of the server program. . . . . . . . . . . . . . . . . . . . . . . . 3360

9.9 main.cpp of the server program. . . . . . . . . . . . . . . . . . . . . . . . . 3361

9.10 server.h of the server program. . . . . . . . . . . . . . . . . . . . . . . . . . 3362

9.11 server.cpp of the server program. . . . . . . . . . . . . . . . . . . . . . . . . 3363

9.12 client.pro for the client program. . . . . . . . . . . . . . . . . . . . . . . . . 3366

9.13 shm_com.h of the client program. . . . . . . . . . . . . . . . . . . . . . . . 3366

183

LIST OF PROGRAMS

9.14 main.cpp of the client program. . . . . . . . . . . . . . . . . . . . . . . . . . 3366

9.15 client.h of the client program. . . . . . . . . . . . . . . . . . . . . . . . . . . 3367

9.16 client.cpp of the client program. . . . . . . . . . . . . . . . . . . . . . . . . 3368

9.17 client.py: python implementation of the client program. . . . . . . . . . 3370

9.18 Shared library dependencies of the qilex. . . . . . . . . . . . . . . . . . . . 3375

9.19 A sample of pyRCCL programs – pcnf.py . . . . . . . . . . . . . . . . . . . 3377

9.20 A sample of RCCL programs – pcnf.c . . . . . . . . . . . . . . . . . . . . . 3378

9.21 Execution of the pcnf.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3379

9.22 Set points generated while the pcnf.py is executed. . . . . . . . . . . . . 3380

9.23 teach_tp() of the rccl.py. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3401

9.24 main.py of the pyRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3407

9.25 rccl.py of the pyRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3407

9.26 manual.py: manual demo program. . . . . . . . . . . . . . . . . . . . . . . 3409

9.27 rccl.py modified for the pyinstaller. . . . . . . . . . . . . . . . . . . . . . . . 3409

10.1 Implementation of a recursive Newton-Euler algorithm. . . . . . . . . . 3954

10.2 Kinematic and dynamic parameters of the PUMA 560 used

by the ROBOOP. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3965

10.3 Maple script puma560 for computing the inverse dynamics

of a PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3969

10.4 Symbolic computation of the inverse dynamics of the PUMA

560 robot using Maple. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3973

10.5 Symbolic computation of the inverse dynamics of a SCARA

robot using Maple. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3998

10.6 Maple script to compute the inverse dynamics of a SCARA robot. . . . 4002

11.1 Firmware code of the PUMA 560’s digital servo. . . . . . . . . . . . . . . 4112

11.2 Computation of gravity terms in the RCCL v5.0. . . . . . . . . . . . . . . 4180

11.3 Computation of gravity terms in the RCCL v1.0. . . . . . . . . . . . . . . 4186

11.4 Gravity compensation demo of the RCCL v1.0. . . . . . . . . . . . . . . . 4192

11.5 Gravity compensation demo of the RCCL v5.0 – zerogDemo.c . . . . . 4195

11.6 Gravity compensation demo of the RCCL v5.0 – zerogDemoCtrl.c . . . 4196

12.1 Implementation of function limit(). . . . . . . . . . . . . . . . . . . . . . . . 4332

12.2 Implementation of function comply(). . . . . . . . . . . . . . . . . . . . . . 4335

12.3 Implementation of function lock(). . . . . . . . . . . . . . . . . . . . . . . . 4336

12.4 Implementation of function massis(). . . . . . . . . . . . . . . . . . . . . . 4338

184

LIST OF PROGRAMS

12.5 Force control example of RCCL v1.0: manex8. . . . . . . . . . . . . . . . 4339

12.6 Force control example of RCCL v1.0: manex10. . . . . . . . . . . . . . . 4341

12.7 Source code of the RCCL v1.0’s trajectory generator . . . . . . . . . . . 4345

12.8 Function forcetr() of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . 4367

12.9 Function getobst_n() of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . 4371

12.10 Function getobst_n() of the RCCL v1.0 (corrected). . . . . . . . . . . . . 4375

12.11 Source code of diffToTrsf() function. . . . . . . . . . . . . . . . . . . . . . . 4408

12.12 manex.560.h: header of the “comply” demo. . . . . . . . . . . . . . . . . 4410

12.13 comply.560.c: planning level module of the “comply” demo. . . . . . . 4411

12.14 complyCtrl.560.c: control level module (RCI task) of the

“comply” demo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4413

12.15 Peg-in-hole demo written in the V+ language. . . . . . . . . . . . . . . . 4429

13.1 C-language structure for transformation. . . . . . . . . . . . . . . . . . . . 4458

14.1 Source code of the rcclCreate(). . . . . . . . . . . . . . . . . . . . . . . . . . 4566

14.2 Source code of the rcclStart(). . . . . . . . . . . . . . . . . . . . . . . . . . . 4568

14.3 Source code of the rcclRelease(). . . . . . . . . . . . . . . . . . . . . . . . . 4569

14.4 Source code of the rciCtrlTask(). . . . . . . . . . . . . . . . . . . . . . . . . 4570

14.5 A simple dual-robot program of MultiRCCL. . . . . . . . . . . . . . . . . . 4573

14.6 manex.560.h: header of the trackII.560 demo. . . . . . . . . . . . . . . . 4581

14.7 Planning level module of the trackII.560 demo: trackII.560.c . . . . . . 4582

14.8 Control level module of the trackII.560 demo: trackIICtrl.560.c . . . . 4586

14.9 MANIP structure of MultiRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . 4587

14.10 RCI_RBT structure of MultiRCCL. . . . . . . . . . . . . . . . . . . . . . . . . 4590

14.11 JLS structure of MultiRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4592

14.12 VAR structure of a PUMA 560 robot. . . . . . . . . . . . . . . . . . . . . . 4594

14.13 HOW structure of MultiRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . 4595

14.14 CHG structure of MultiRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . 4600

14.15 Planning level sensor integration demo: pickAndDrop.560.c . . . . . . . 4608

14.16 manex.560.h: header of the tracking demo. . . . . . . . . . . . . . . . . . 4611

14.17 Planning level module of the tracking demo: track.560.c . . . . . . . . 4612

14.18 Control level module of the guarded demo: guardedCtrl.560.c . . . . . 4613

14.19 manex.560.h: header of the guarded demo. . . . . . . . . . . . . . . . . . 4616

14.20 Planning level module of the guarded demo: guarded.560.c . . . . . . 4617

14.21 Control level module of the guarded demo: guardedCtrl.560.c . . . . . 4620

185

LIST OF PROGRAMS

14.22 MultiRCCL v5.1 demo: box.560.c . . . . . . . . . . . . . . . . . . . . . . . . 4622

14.23 MultiRCCL v5.1.3 demo: box.560.c . . . . . . . . . . . . . . . . . . . . . . 4627

14.24 Utility program for teaching: teachdemo.c . . . . . . . . . . . . . . . . . . 4630

14.25 The source code of the trackII.560 demo (trackII.560.c)

modified for MultiRCCL v5.1.3. . . . . . . . . . . . . . . . . . . . . . . . . . 4633

14.26 box560.c of the RWRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4662

14.27 box560.c of the RCCL 5.1.3. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4665

14.28 simpuma560 of the RWRCCL . . . . . . . . . . . . . . . . . . . . . . . . . . 4670

14.29 puma560.jls of the RCCL 5.1.3 . . . . . . . . . . . . . . . . . . . . . . . . . 4673

14.30 defaultValues.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4680

16.1 /etc/inittab of Debian Wheezy . . . . . . . . . . . . . . . . . . . . . . . . . . 4710

16.2 Scripts in /etc/init.d . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4711

16.3 Scripts in /etc/rc2.d . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4712

16.4 Modification of the grub setup . . . . . . . . . . . . . . . . . . . . . . . . . 4721

16.5 grub.cfg after executing “update-grub”. . . . . . . . . . . . . . . . . . . . . 4722

16.6 Display of disk usuage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4740

16.7 Disk usuage with kernel parameter “fsprotect=1G”. . . . . . . . . . . . . 4740

16.8 ISR of the Xenomai’s RTDM serial driver. . . . . . . . . . . . . . . . . . . . 4761

16.9 Loading message of the cif module. . . . . . . . . . . . . . . . . . . . . . . 4767

16.10 Program for testing the DPM of a CC-Link board. . . . . . . . . . . . . . 4768

16.11 Correcting bugs in the rtdm.h of RTAI 3.8.1 . . . . . . . . . . . . . . . . . 4773

16.12 cif_test.c: DPRAM test program. . . . . . . . . . . . . . . . . . . . . . . . . 4776

16.13 Modification of uio_cif.c for Hilscher CC-link cards. . . . . . . . . . . . . 4784

16.14 uio.c: Base functions of the UIO . . . . . . . . . . . . . . . . . . . . . . . . 4784

16.15 uio_cif.c: UIO driver for Hilscher CC-link cards. . . . . . . . . . . . . . . 4784

16.16 cif_main.c: program for testing the Hilscher CC-link UIO driver. . . . . 4788

16.17 adl_pci7432.c of the Comedi package. . . . . . . . . . . . . . . . . . . . . . 4797

16.18 pci7432_driver.h of the Comedi package. . . . . . . . . . . . . . . . . . . . 4804

16.19 pci7432_driver.c of the Comedi package. . . . . . . . . . . . . . . . . . . . 4806

16.20 Makefile for building the kernel module pci7432.ko and

the application test_led. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4814

16.21 Script for loading pci7432.ko module. . . . . . . . . . . . . . . . . . . . . . 4815

16.22 Script for unloading the pci7432.ko module. . . . . . . . . . . . . . . . . 4815

16.23 Header file for PCi7432 board – pci7432.h . . . . . . . . . . . . . . . . . 4815

186

LIST OF PROGRAMS

16.24 Test program of the PCI7432 board – test_led.c . . . . . . . . . . . . . . 4816

16.25 User-space read/write of the PCI7432 board. . . . . . . . . . . . . . . . . 4818

16.26 Makefile for building a RTDM driver of the PCI7432 board. . . . . . . 4823

16.27 RTDM driver of the PCI7432 board – xen_pci7432_driver.h . . . . . . . 4823

16.28 RTDM driver of the PCI7432 board – xen_pci7432_driver.c . . . . . . . 4825

16.29 Makefile for building a user-space application, xen_test_led. . . . . . . 4833

16.30 Source code of the xen_test_led – xen_test_led.h. . . . . . . . . . . . . . 4834

16.31 Source code of the xen_test_led – xen_test_led.c. . . . . . . . . . . . . . 4834

16.32 Script for loading kernel modules and executing the xen_test_led. . . 4839

16.33 PCI7432 device driver with interrupt handling – pci7432_driver.h . . . 4846

16.34 Definition of the PCI9052. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4847

16.35 PCI7432 device driver with interrupt handling – pci7432_driver.c . . . 4848

16.36 User application of the PCI7432 with interrupt handling – pci7432.h . 4857

16.37 User application of the PCI7432 with interrupt handling – test_led.c . 4858

16.38 User application for receiving interrupts from a device driver. . . . . . 4859

16.39 RTDM driver with interrupt handling – xen_pci7432_driver.h . . . . . . 4862

16.40 RTDM driver with interrupt handling – xen_pci7432_driver.c . . . . . . 4863

16.41 Header file of the user-space application – xen_pci7432.h . . . . . . . . 4871

16.42 wait4irq.c with semaphore mechanism. . . . . . . . . . . . . . . . . . . . . 4872

16.43 dio3248_initial() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4883

16.44 dio3248_close() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4884

16.45 dio3248_info() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4886

16.46 dio3248_read_port() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4886

16.47 dio3248_enable() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4887

16.48 dio3248_disable_IRQ() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4888

16.49 dio3248_set_IRQ() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4890

16.50 dio3248_link_IRQ_process() . . . . . . . . . . . . . . . . . . . . . . . . . . . 4890

16.51 Device driver – dio3248_init() and dio3248_exit() . . . . . . . . . . . . . 4893

16.52 Device driver – find_3248() . . . . . . . . . . . . . . . . . . . . . . . . . . . 4894

16.53 Definition of file operations in linux/fs.h . . . . . . . . . . . . . . . . . . . 4895

16.54 Device driver – struct file_operations . . . . . . . . . . . . . . . . . . . . . 4896

16.55 Device driver – dio3248_open() . . . . . . . . . . . . . . . . . . . . . . . . . 4897

16.56 Device driver – dio3248_release() . . . . . . . . . . . . . . . . . . . . . . . 4897

16.57 Device driver – dio3248_fsync() . . . . . . . . . . . . . . . . . . . . . . . . . 4897

187

LIST OF PROGRAMS

16.58 Device driver – dio3248_ioctl() . . . . . . . . . . . . . . . . . . . . . . . . . 4898

16.59 Device driver – dio3248_interrupt() . . . . . . . . . . . . . . . . . . . . . . 4904

16.60 User-space program for testing DIO3248 device driver – test.cpp . . . 4905

16.61 Modification of dio3248demo.c . . . . . . . . . . . . . . . . . . . . . . . . . 4912

16.62 Script of buildiing dio3248demo. . . . . . . . . . . . . . . . . . . . . . . . . 4913

16.63 VHDL implementation of the 8-bit output port interface. . . . . . . . . . 4927

16.64 Verilog implementation of the 8-bit output port interface (wboport08). 4928

16.65 Wishbone Master model (wb_master_model.v). . . . . . . . . . . . . . . . 4929

16.66 Testbench code for the wboport08.v (tst_bench_top.v). . . . . . . . . . . 4932

16.67 Verilog implementation of the simple 8-bit GPIO. . . . . . . . . . . . . . . 4934

16.68 Verilog implementation of the simple 8-bit output port (sim-

ple_output.v). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4937

16.69 VHDL implementation of the wb_syscon (wb_syscon.vhd). . . . . . . . . 4941

16.70 VHDL implementation of the wb_intercon (wb_intercon.vhd). . . . . . . 4944

16.71 VHDL implementation of the max_uart module. . . . . . . . . . . . . . . 4950

16.72 Top-level VHDL implementation of the lcpd-scope. . . . . . . . . . . . . . 4965

16.73 lcpd_scope_user.h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4972

16.74 lcpd_scope_driver.h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4974

16.75 lcpd_scope_driver.c. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4976

16.76 Wishbone 32-bit IO module for test. . . . . . . . . . . . . . . . . . . . . . . 4987

16.77 Top-level module for a PCI parallel IO board. . . . . . . . . . . . . . . . . 4988

16.78 Top-level module modified for the pci_to_wb IP core (maxii_gpio.v). . 4991

16.79 Wishbone 32-bit IO module for the parallel IO board (wb_gpio.v). . . 4993

16.80 Linux device driver of the parallel IO board (alteramaxiipci.c). . . . . . 4994

16.81 User-space test program for the parallel IO board (alteramaxiipci_test.c).5003

16.82 Verilog implementation of an 128-bit PCI parallel IO board. . . . . . . 5007

16.83 GPIO module of the PCI parallel IO board with interrupt

input lines. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5011

16.84 Top-level module of the PCI parallel IO board with inter-

rupt input lines. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5012

16.85 maxii.h: header of the linux device driver maxii.c. . . . . . . . . . . . . 5016

16.86 maxii.c: linux device driver of the parallel IO board . . . . . . . . . . . 5016

16.87 maxii_test.c: user-space test program. . . . . . . . . . . . . . . . . . . . . . 5025

16.88 Source code directory structure of the lscpcie2 driver. . . . . . . . . . . 5041

188

LIST OF PROGRAMS

16.89 lscpcie2 driver of the ECP3 Versa develoipment board. . . . . . . . . . . 5041

16.90 Loopback test of the FT232H with 4 Mbaud. . . . . . . . . . . . . . . . . 5088

16.91 ft232h-test-12m.c for loopback test of the FT232H with 12 Mbaud. . 5090

16.92 USB arbiter implemented in Verilog. . . . . . . . . . . . . . . . . . . . . . . 5097

16.93 USB arbiter implemented in Verilog. . . . . . . . . . . . . . . . . . . . . . . 5101

16.94 Verilog code for 128-byte loopback test. . . . . . . . . . . . . . . . . . . . 5102

16.95 serial_test.c modified for loopback test. . . . . . . . . . . . . . . . . . . . . 5107

16.96 Implementation of ftdi_write_data() and ftdi_read_data(). . . . . . . . . 5110

16.97 Loopback-test result with the serial_test. . . . . . . . . . . . . . . . . . . . 5111

16.98 Loopback-test program for the FT245-style asynchronous

FIFO transfer mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5114

16.99 Loopback-test program for the FT245-style synchronous FIFO

transfer mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5118

16.100 Verilog code for the 510-bytes loopback test (I) (the FT2232H

runs in FT245-style synchronous FIFO transfer mode). . . . . . . . . . . 5122

16.101 Verilog code for the 510-bytes loopback test (II) (the FT2232H

runs in FT245-style synchronous FIFO transfer mode). . . . . . . . . . . 5126

16.102 Verilog code for the 510-bytes loopback test (III) (the FT2232H

runs in FT245-style synchronous FIFO transfer mode). . . . . . . . . . . 5131

16.103 DS18B20 driver in Verilog . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5138

16.104 DS18B20 driver in Verilog . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5139

16.105 CIII_Design_Security_Enabler.v converted from CIII_Design

_Security_Enabler.bdf . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5160

16.106 ds18b20_seg7.v modified for the CIII_Design_Security demo. . . . . . . 5162

18.1 User-space programming example: example/user/main.c . . . . . . . . . 5324

18.2 examples/user/main.c modified for the new hardware configuration . 5332

18.3 Xenomai programming example: example/xenomai/main.c . . . . . . . 5339

18.4 example/xenomai/main.c modified for the new hardware configuration5344

18.5 Xenomai posix programming example: example/xenomai_posix/main.c 5353

18.6 example/xenomai_posix/main.c modified for the new hard-

ware configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5357

18.7 example/dc_user/main.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5367

18.8 example/dc_user/main.c modified for the new hardware setup . . . . . 5375

18.9 example/dc_rtai/dc_rtai_sample.c . . . . . . . . . . . . . . . . . . . . . . . . 5383

189

LIST OF PROGRAMS

18.10 example/dc_xenomai_kernel/dc_xenomai_kernel.c . . . . . . . . . . . . . 5389

18.11 example/dc_xenomai_user/dc_xenomai_user.c . . . . . . . . . . . . . . . . 5398

18.12 example/dc_xenomai_user_8/dc_xenomai_user_8.c . . . . . . . . . . . . . 5406

18.13 example/xenomai_dc/main.c . . . . . . . . . . . . . . . . . . . . . . . . . . . 5419

18.14 example/xenomai_dc/main.c modified for EL2008 . . . . . . . . . . . . . 5426

18.15 DC-mode programming example with RT-preempt kernel: dc_user_8.c 5440

18.16 Free-run mode programming example with RT-preempt ker-

nel: user_8.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5457

18.17 Kernel-space DC-mode programming example with RTAI ker-

nel: dc_rtai_sample.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5474

18.18 User-space DC-mode programming example with RTAI ker-

nel: dc_rtai_user.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5484

18.19 lcec_deasda.h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5575

18.20 lcec_deasda.c. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5578

18.21 lcec_main.c. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5590

18.22 swm-fm45a.hal. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5611

18.23 ethercat_igh.h for two servo drives in Free RUN mode . . . . . . . . . . 5689

18.24 test_pana_2.c for two servo drives in Free RUN mode . . . . . . . . . . 5692

18.25 test_pana_dc_2.c for two servo drives in DC mode . . . . . . . . . . . . 5700

18.26 test_pana_dc_xeno_2.c for user-space xenomai programming . . . . . . 5713

18.27 ethercat_dio_servo.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5729

18.28 test_dio8_servo2.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5737

18.29 test_dio8_servo2_dc.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5752

18.30 test_dio8_servo2_dc_xeno.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5767

18.31 test_dio8_servo2_dc_xeno_master_shift.c . . . . . . . . . . . . . . . . . . . 5779

18.32 ethercat_dio_servo_domain.h . . . . . . . . . . . . . . . . . . . . . . . . . . . 5797

18.33 test_dio8_servo2_dc_xeno_master_shift_domain.c . . . . . . . . . . . . . . 5805

18.34 thread_test.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6177

18.35 Makefile for compiling thread_test.c . . . . . . . . . . . . . . . . . . . . . . 6181

18.36 Execution result of thread_test . . . . . . . . . . . . . . . . . . . . . . . . . 6181

18.37 genrci.c: driver of the clock and robot devices . . . . . . . . . . . . . . . 6182

18.38 genrci.c: driver of the clock device for kernel 4.9 and 4.16. . . . . . . 6192

18.39 Makefile for building the kernel module genrci.ko . . . . . . . . . . . . . 6199

18.40 testGenrci.c for testing the kernel module genrci.ko . . . . . . . . . . . . 6200

190

LIST OF PROGRAMS

18.41 Execution result of the testGenrci . . . . . . . . . . . . . . . . . . . . . . . . 6202

18.42 IOdriverTable.c of the RCCL 5.1.3. . . . . . . . . . . . . . . . . . . . . . . . 6207

18.43 lib/generic/robotIO.cfg of the RCCL 5.0 . . . . . . . . . . . . . . . . . . . . 6209

18.44 lib/generic/robotIO.cfg of the RCCL 5.1.3. . . . . . . . . . . . . . . . . . . 6209

18.45 drivers/IOdriverTable.c of the RWRCCL. . . . . . . . . . . . . . . . . . . . . 6211

18.46 drivers/triDriver.c of the RWRCCL. . . . . . . . . . . . . . . . . . . . . . . . 6213

18.47 conf/robots of the RWRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . 6213

18.48 puma/puma_comfxns.c of the RWRCCL. . . . . . . . . . . . . . . . . . . . 6215

18.49 jls_sim.c for the JLS process . . . . . . . . . . . . . . . . . . . . . . . . . . . 6233

18.50 rci.c for the planning-control process . . . . . . . . . . . . . . . . . . . . . 6239

18.51 rci.h for specifying the communication protocols . . . . . . . . . . . . . . 6245

18.52 Makefile for compiling the jls_sum.c and rci.c . . . . . . . . . . . . . . . . 6246

18.53 fifos.h of the RWRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6249

18.54 controlLevel.h of the RWRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . 6249

18.55 Commands received by the JLS task while /demo.rcclbox.560

is executing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6250

18.56 jls_sim_simple.c: an implementation of the JLS task for dry

run. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6252

18.57 puma/puma_comfxns.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6266

18.58 puma/sim_puma_comfxns.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6293

18.59 Conversion functions between joint angles and encoder counter

vaalues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6333

18.60 moperx_sim_clean.c for dry-run mode simulation . . . . . . . . . . . . . . 6342

18.61 parking.c for driving a PUMA 560 robot to the rcclpark position . . . 6358

18.62 moperx.c: EtherCAT-based implementation of the JLS task . . . . . . . . 6376

19.1 Command puse generation using a DDA. . . . . . . . . . . . . . . . . . . . 6457

19.2 Number of pulses generated by the DDA. . . . . . . . . . . . . . . . . . . 6459

19.3 axis.ngc of the 5axis example. . . . . . . . . . . . . . . . . . . . . . . . . . . 6531

20.1 Basic operations for PCL6045B . . . . . . . . . . . . . . . . . . . . . . . . . 6604

20.2 Interrupt handler for PCL6045B . . . . . . . . . . . . . . . . . . . . . . . . . 6609

20.3 Speed seting of PCL6045B . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6637

20.4 Header for HPCI-CPD534 – hcdtype.h . . . . . . . . . . . . . . . . . . . . . 6664

20.5 Header for HPCI-CPD534 – hicpd530.h . . . . . . . . . . . . . . . . . . . . 6666

20.6 Header for HPCI-CPD534 – cp531l1a.h . . . . . . . . . . . . . . . . . . . . 6668

191

LIST OF PROGRAMS

20.7 Modification of cp530l1a for gcc. . . . . . . . . . . . . . . . . . . . . . . . . 6671

20.8 DeviceGetBoardInfor() function taken from cif_api.c . . . . . . . . . . . . 6674

20.9 cif_ioctl.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6675

20.10 cp530.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6677

20.11 Files in MPC3035.zip . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6692

20.12 drv3035.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6693

20.13 drv3035.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6698

20.14 Interrupt handler implemented at drv3035.c . . . . . . . . . . . . . . . . . 6699

20.15 Initialization of the MPC3035 . . . . . . . . . . . . . . . . . . . . . . . . . . 6701

20.16 Buffer access test of PCL6045B . . . . . . . . . . . . . . . . . . . . . . . . . 6711

20.17 PCI8164_driver.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6713

20.18 pci8164_driver.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6714

20.19 Interrupt handler of PCI8164 with ioctl() and send_sig_info() . . . . . 6729

20.20 siginfo.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6732

20.21 Interrupt handler for PCI8164 . . . . . . . . . . . . . . . . . . . . . . . . . . 6733

20.22 mmap.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6739

20.23 mmap_test.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6744

20.24 find_3035() function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6747

20.25 MPC3035_T_curve_position_move() function . . . . . . . . . . . . . . . . . 6760

20.26 MPC3035_S_curve_position_move() . . . . . . . . . . . . . . . . . . . . . . 6767

20.27 Default parameters of the Panasonic servo driver MSDDT2210. . . . . 6791

20.28 Parameters of the Panasonic servo driver MSDDT2210 for

the test run. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6808

20.29 Implementation of a programmable one-shot in Verilog. . . . . . . . . . 6852

20.30 Implementation of a quadrature encoder in Verilog. . . . . . . . . . . . . 6856

20.31 Implementation of a command pulse generator with a 4-

bit BRM and a quadrature encoder in Verilog. . . . . . . . . . . . . . . . 6860

20.32 Verilog code for a loopback test. . . . . . . . . . . . . . . . . . . . . . . . . 6876

20.33 Loopback test program for the interpolation board . . . . . . . . . . . . . 6897

20.34 Simple general purpose IO (simple_gpio.v). . . . . . . . . . . . . . . . . . 6917

20.35 Wishbone 8-bit output port (simple_output_8.v). . . . . . . . . . . . . . . 6920

20.36 Wishbone 8-bit input port (simple_input_8.v). . . . . . . . . . . . . . . . . 6921

20.37 Wishbone master model (wb_master_model.v). . . . . . . . . . . . . . . . 6923

20.38 Test bench for testing Wishbone IO ports. . . . . . . . . . . . . . . . . . . 6926

192

LIST OF PROGRAMS

20.39 Wishbone interconnection module for two slaves (wb_intercon.vhd). . 6929

20.40 Wishbone interconnection module for two slaves (wb_intercon.v). . . . 6932

20.41 Wishbone interconnection module for 4 slaves (wb_intercon_4.v). . . . 6933

20.42 Wishbone interconnection module for 4 IO slaves (wb_intercon_io_4.v).6936

20.43 Testbench code for testing the interconnection module and

four IO modules (wb_io_tst_bench_top.v). . . . . . . . . . . . . . . . . . . 6938

20.44 quad_decoder.vhd. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6942

20.45 quad_decoder.v. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6956

20.46 VHDL implmentation of the digital noise filter. . . . . . . . . . . . . . . . 6999

20.47 Verilog implmentation of the digital noise filter. . . . . . . . . . . . . . . 6999

20.48 Verilog implmentation of the DDA (DDA16.v). . . . . . . . . . . . . . . . 7001

20.49 Verilog implmentation of the DDA (dda16_ext_sync.v). . . . . . . . . . . 7003

20.50 Testbench of the dda16_ext_sync.v(tb_dda16_ext_sync.v). . . . . . . . . 7006

20.51 Verilog implmentation of the DDA (dda16_ext_sync_short.v). . . . . . . 7008

20.52 Testbench of the dda16_ext_sync_short.v(tb_dda16_ext_sync_short.v). 7012

20.53 DDA with an internal modulo M counter. . . . . . . . . . . . . . . . . . . 7020

20.54 DDA with keep counting the number of remaining pulses. . . . . . . . . 7026

20.55 Definition of the chg and how structures. . . . . . . . . . . . . . . . . . . 7034

20.56 Bit definition of the external I/O register. . . . . . . . . . . . . . . . . . . 7038

20.57 Conversion between an encoder count and a 64-bit integer. . . . . . . . 7044

20.58 Software structure for the interpolation board. . . . . . . . . . . . . . . . 7046

20.59 Toque mode programming example (akd_torque.c). . . . . . . . . . . . . 7068

20.60 Velocity mode programming example (akd_velocity.c). . . . . . . . . . . 7072

20.61 Cyclic sync position mode programming example (akd_position.c). . . 7077

20.62 Yaskawa servo drive programming example (main_yas.c). . . . . . . . . 7083

20.63 DC-mode programming example (dc_users_yas.c). . . . . . . . . . . . . . 7091

D.1 Configuration of RTAI-patched kernel 2.6.32-122. . . . . . . . . . . . . . 10523

D.2 Configuration of RTAI 3.8.1. . . . . . . . . . . . . . . . . . . . . . . . . . . . 10604

D.3 Script for compling adeos-patched Linux kernel 2.6.32.20

and Xenomai 2.5.5.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10606

D.4 Configuration of adeos-patched Linux kernel 2.6.32.20. . . . . . . . . . . 10607

D.5 Kernel configuration of the preempt-rt kernel. . . . . . . . . . . . . . . . . 10689

D.6 Kernel configuration differences between plane and preempt-

rt kernels. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10781

193

LIST OF PROGRAMS

D.7 Configuration example of Linux kernel 3.10.32 for RTAI 4.1. . . . . . . 10787

D.8 Custom dynamic memory allocators in RCCL v1.0. . . . . . . . . . . . . . 11035

D.9 “Old” dlmalloc released in 1991 (old-malloc.c). . . . . . . . . . . . . . . . 11039

D.10 Dynamic memory allocators designed for RCCL v1.0. . . . . . . . . . . . 11060

E.1 Source code directory structure of the tinybc BASIC interpreter. . . . . 16793

E.2 Source code of the tinybc BASIC interpreter. . . . . . . . . . . . . . . . . 16793

F.1 Grammar of the VAL II language written for ANTLR. . . . . . . . . . . . 17233

F.2 Source code of a VAL-II syntax checking utility designed

with Flex/Bison . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17247

F.3 Source code directory structure of the RCCL v1.0. . . . . . . . . . . . . . 19741

F.4 Source code of the RCCL v1.0. . . . . . . . . . . . . . . . . . . . . . . . . . 19744

F.5 Makfeile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22319

F.6 allprint.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22320

F.7 arcl.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22321

F.8 decompil.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22321

F.9 lexio.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22332

F.10 main.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22335

F.11 monitor.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22343

F.12 parse.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22354

F.13 points.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22357

F.14 robot.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22360

F.15 run.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22374

F.16 symtab.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22403

F.17 val2.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22406

F.18 Makefile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22407

F.19 vallex.l . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22408

F.20 val2.y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22412

F.21 Source code of the VAL language implemented with the

RCCL v1.0 and the BIFES interpreter . . . . . . . . . . . . . . . . . . . . . 22417

F.22 Makfeile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22501

F.23 bifers.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22502

F.24 bifers.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22503

F.25 monitor.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22512

F.26 tokenize.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22513

194

LIST OF PROGRAMS

F.27 tokenize.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22515

F.28 compile.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22521

F.29 decompile.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22591

F.30 execute.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22617

F.31 robot.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22650

F.32 robot.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22650

F.33 symtab.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22651

F.34 symtab.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22653

H.1 Inventor model of the PUMA 560 robot (puma560.iv). . . . . . . . . . . 25753

H.2 VRML model of the PUMA 560 robot (puma560.wrl). . . . . . . . . . . 25765

H.3 VRML model of the PUMA 560 robot’s (link 0). . . . . . . . . . . . . . . 25766

H.4 VRML model of the PUMA 560 robot’s link 1. . . . . . . . . . . . . . . . 25769

H.5 VRML model of the PUMA 560 robot’s link 2. . . . . . . . . . . . . . . . 25770

H.6 VRML model of the PUMA 560 robot’s link 3. . . . . . . . . . . . . . . . 25773

H.7 VRML model of the PUMA 560 robot’s link 4. . . . . . . . . . . . . . . . 25774

H.8 VRML model of the PUMA 560 robot’s link 5. . . . . . . . . . . . . . . . 25776

H.9 VRML model of the PUMA 560 robot’s link 6. . . . . . . . . . . . . . . . 25778

H.10 Source code directory structure of the PUMA 560 simulator. . . . . . . 25780

H.11 Source code of the PUMA 560 simulator. . . . . . . . . . . . . . . . . . . . 25780

I.1 main.py of the pyRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27804

I.2 rccl.py of the pyRCCL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27804

I.3 puma560.pro of the PUMA 560 graphic simulator. . . . . . . . . . . . . . 27887

I.4 puma560.pro.user of the PUMA 560 graphic simulator. . . . . . . . . . . 27887

I.5 mainwindow.ui of the PUMA 560 graphic simulator. . . . . . . . . . . . . 27893

I.6 mainwindow.h of the PUMA 560 graphic simulator. . . . . . . . . . . . . 27913

I.7 project.h of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . . . 27914

I.8 server.h of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . . . 27917

I.9 shm_com.h of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . 27917

I.10 main.cpp of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . . 27918

I.11 mainwindow.cpp of the PUMA 560 graphic simulator. . . . . . . . . . . . 27922

I.12 project.cpp of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . 27932

I.13 server.cpp of the PUMA 560 graphic simulator. . . . . . . . . . . . . . . . 27935

I.14 vrml/coord_0.wrl of the PUMA 560 graphic simulator. . . . . . . . . . . 27938

I.15 vrml/coord_6.wrl of the PUMA 560 graphic simulator. . . . . . . . . . . 27943

195

LIST OF PROGRAMS

I.16 vrml/PUMA-560_0.wrl of the PUMA 560 graphic simulator. . . . . . . . 27947

I.17 vrml/PUMA-560_1.wrl of the PUMA 560 graphic simulator. . . . . . . . 27951

I.18 vrml/PUMA-560_2.wrl of the PUMA 560 graphic simulator. . . . . . . . 27953

I.19 vrml/PUMA-560_3.wrl of the PUMA 560 graphic simulator. . . . . . . . 27957

I.20 vrml/PUMA-560_4.wrl of the PUMA 560 graphic simulator. . . . . . . . 27958

I.21 vrml/PUMA-560_5.wrl of the PUMA 560 graphic simulator. . . . . . . . 27961

I.22 vrml/PUMA-560_6.wrl of the PUMA 560 graphic simulator. . . . . . . . 27963

P.1 UCF file for the FPGA+FT2232H experiment. . . . . . . . . . . . . . . . . 57718

P.2 Verilog code for the FPGA+FT2232H experiment. . . . . . . . . . . . . . 57720

P.3 New helloworld.c for the bootloader. . . . . . . . . . . . . . . . . . . . . . 57766

P.4 alteramaxiipci.c: kernel-space device driver. . . . . . . . . . . . . . . . . . 57895

P.5 alteramaxiipci_test.c: user-space test program . . . . . . . . . . . . . . . . 57905

P.6 pci_to_wb IP core implemented in VHDL. . . . . . . . . . . . . . . . . . . 57927

P.7 maxii_rtt_test.c for measuring round-trip time. . . . . . . . . . . . . . . . 57945

P.8 pci_to_wb IP core modified for handling interrupts. . . . . . . . . . . . . 57947

P.9 Source code directory structure of the Linux device driver. . . . . . . . 57971

P.10 Linux device driver of the Hilscher PCI CC-Link slave boards. . . . . . 57972

P.11 Source code directory structure of the UIO driver. . . . . . . . . . . . . . 58293

P.12 Linux UIO driver of the Hilscher PCI CC-Link slave boards. . . . . . . . 58293

P.13 Source code directory structure of the RTAI/RTDM driver. . . . . . . . . 58329

P.14 RTAI/RTDM driver of the Hilscher PCI CC-Link slave boards. . . . . . . 58330

Q.1 Source code of the PMSM3-4 software module, pmsm3_4.c . . . . . . . 59208

Q.2 Source code of the PMSM_FOC_ENCODER_MC56F84789_TWR_DEMO 59257

196