Adaptation of the Inverse Kinematics of the DLR Light Weight Robot for

(1)

2008:097

M A S T E R ' S T H E S I S

Adaptation of the Inverse Kinematics of the DLR Light Weight Robot for

Its Use as Haptic Device

Zhongyang Wu

Luleå University of Technology Master Thesis, Continuation Courses

Space Science and Technology Department of Space Science, Kiruna

(2)

Julius–Maximilians–Universit¨at W¨urzburg

Lehrstuhl f¨ur Informatik VII Institut f¨ur Robotik und Telematik

Lule˚a Tekniska Universitet

Institutet f¨or rymdfysik Space Campus Kiruna

Adaptation of the Inverse Kinematics of the DLR Light

Weight Robot for Its Use as Haptic Device

Master Thesis presented by

Zhongyang Wu

Completed at

German Aerospace Center

Institute of Robotics and Mechatronics Supervisor:

Thomas Hulin (German Aerospace Center)

Dr. Martin Bohm (Lule˚a Tekniska Universitet)

Prof. Dr. K. Schilling

(Julius–Maximilians–Universit¨at W¨urzburg) Delivery date:

4th, November, 2007

(3)

Declaration

I confirm that the work presented in this thesis is, except of the instructions of my tutors, completely on my own achievement and that any resources used in it are properly denoted in the bibliography.

Munich. 4th, November, 2007

(Zhongyang Wu)

(4)

!

"

! " " # $

% " # # " & '

$ ( )*+%, " - & . " " " #

%

/ " ""

$ ( % ( 0 " #

- & . % ( # 1 %

# # 2 " # %

" 3 " $

, # ( %

4 "" " # $ # (0

, ( ( # (

5

2 " # $ "

" #

6# " " (

)*+ ! 2 7%8 9%86 ( :"" 6%8!: ; %8< '%8= 6%8

; % >??> & '@ 1 $ ( ,,, $

$A, = , '6 >??> % *-*? *-*B , , "

' ( 6 , '6 / &% % 4 6 ; >??>

#

!

& ' 7 6

, "' ( ;

5( """ "

& C>>DE/

(5)

Abstract

Haptic devices play an important role in haptic applications. Light Weight Robot III is the latest version of DLR light weight robot. It has seven Degree of Freedom (DoF) with only 14 kg self weight. Seven torque sensors mounted on its joints and one 6 DoF force-torque sensor on the end-effector enable it to precisely sense the external forces. Existing inverse kinematics library is design for a general model of Light Weight Robot III, which is slightly different from the one used as haptic device. This thesis is intended to adapt and optimize this inverse kinematics library for the haptic applications of Light Weight Robot III. It describes the adaptation of the inverse kinematics library to the hardware, which conquered the problems caused by the different configuration of the robot used as haptic device. It realized the null space movement of the robot, which enables the position changing of the intermediate joints due to applied force on it. At last it gives a solution for detecting and avoiding the singular configurations of the robot, which is especial important due to unpredictable trajectory of the end-effector. The simulation and experiment results of improved kinematics library are also shown in this thesis.

(6)

Acknowledgements

First, I would like to thank my supervisor Mr. Thomas Hulin for guiding me on this thesis. I find it hard to imagine that anyone could be a more sincere and better supervisor then what he has been. I have never seen anyone who can spend the whole afternoon with me on finding bugs in my code other than him.

I also wish to express my gratitude to Professor Klaus Schilling and Professor Gerd Hirzinger for giving me the opportunity to write my thesis at Institute of Robotics and Telematics at W¨urzburg University and the Institute of Robotics and Mechatronics at the German Aerospace Center. Especially, to Professor Schilling, who helped me a lot during last years. And I’m grateful to Dr. Mar- tin Bohm for being my supervisor in Lule˚a Tekniska Universitet.

I also want to show my appreciation to Mr. Sven Molin, who gave me a chance to pursue my master degree in Europe. I’m thankful for being together with all my classmates in Space Master program, I will never forget the time we spent in W¨urzburg and Kiruna, I’m proud of being a member of this crazy and talented group.

Special thanks to Dr. Nao Hao and Dr. Lei Ma, who gave a hand when I mostly need a help.

Of course, I would like to express my extreme appreciation to my parents, who gave the biggest support for my thesis to made me never feel working along.

Please forgive me to write a Chinese word ’Xie Xie’ to thank my parents.

(7)

List of Figures

1.1 Tactile haptic devices . . . . 2

1.2 Kinesthetic haptic devices . . . . 3

2.1 Light Weight Robot with connected joystick handle . . . . 5

2.2 Controller architecture of LBR III . . . . 7

2.3 Structure of the joint controller . . . . 8

2.4 One dimensional mass-spring-damper model . . . . 8

2.5 Impedance control . . . . 9

2.6 Admittance control . . . . 10

3.1 D-H parameters of LBR-Papas . . . . 13

3.2 D-H parameters of LBR-Haptic . . . . 13

3.3 Different postures of two robots with identical commanded joint angles . . . . 14

3.4 Joint angles compensation method . . . . 15

3.5 Conversion matrix method . . . . 16

3.6 Simulation model . . . . 17

3.7 Displacements of LBR’s end-effector in Simulation 1 . . . . 17

3.8 Rotations of the LBR’s end-effector in Simulation 1 . . . . 18

3.9 Joint angles of LBR in Simulation 1 . . . . 18

3.13 Experiment model . . . . 20

3.14 Displacements of LBR’s end-effector in Test 1 . . . . 21

3.15 Rotations of the LBR’s end-effector in Test 1 . . . . 21

3.16 Joint angles of LBR in Test 1 . . . . 22

4.1 Examples of collisions between robot arm and obstacles . . . . . 24

4.2 An example for rotation of rigid bodies . . . . 26

4.3 Example of a force applied on a single joint . . . . 27

(10)

4.16 Trajectory of the elbow during Test 3 . . . . 37

5.1 Singular configurations of LBR III . . . . 44

5.2 Singular configurations of LBR III . . . . 47

A.1 An example of D-H notation . . . . 51

C.1 Hard ware layout of Cartesian controller . . . . 55

C.2 Control block for haptic rendering . . . . 56

C.3 Control block on real time computer . . . . 57

(11)

List of Symbols

(. . .)h Notations for LBR3 robot as haptic device

(. . .)p Notations for LBR3-Papas robot(Inverse kinematics library)

i(. . .) Vectors or matrices in framei

ai The distance from ˆZi to ˆZi+1 measured along ˆXi

ci Abbreviation for cosθi

di The distance from ˆXi−1 to ˆXi measured along ˆZi

si Abbreviation for sinθi

ks Factor for singularity push back force.

v Linear velocity in Cartesian space

vf,4 Virtual linear velocity caused by the force on the elbow

q A set of joint variables

C_p^h Conversion matrix from LBR3-Haptic to LBR3-Papas

F Cartesian force-torque vector

H Transformation matrix of tool center point

In Identity matrix of sizen

J Jacobian matrix

Jt Translational part of Jacobian

Jr Rotational part of Jacobian

j

iR Rotation matrix changing reference frame from framei to frame j Ra Rotation matrix for arrow object

j

iT Transformation matrix changing reference frame from framei to frame j

Ns Dangerous region of the singularity

Xˆi X-axis of framei

Yˆi Y-axis of framei

Zˆi Z-axis of framei

αi The angle from ˆZi to ˆZi+1 measured about ˆXi

θi The angle from ˆXi−1to ˆXi measured along ˆZi

ω Angular velocity vector in Cartesian space ν Six dimensional velocity vector in Cartesian space

χ Cartesian position vector

(12)

Chapter 1 Introduction

Haptic is from the Greek word αφ´η (Haphe), means pertaining to the sense of touch. Haptic technology refers to technology which interfaces the user via the sense of touch by applying forces, vibrations and/or motions to the user [25]. Since more and more powerful computers came out, haptic technology had a great development during last five decades after its first appearance at the Argonne National Lab in 1950s [14]. Nowadays, haptic technologies are applied in different areas, such as virtual reality, tele-operation, medicine, game industry, education and so on. A haptic device or called haptic interface plays a important role in the haptic application, which enables human machine communication by capturing and responding to user movement. This thesis is intended to address the work on improving the performance of the DLR third generation Light Weight Robot (LBR III) as a haptic device.

1.1 Definition of Haptic Device

A haptic interface is a mechanical device that mediate communication between the user and the computer [10]. It is usually an input-output device. It can be manipulated, touched or felt by the human operator and track his or her move- ments; meanwhile it provides feedbacks due to his or her interaction with the environment to simulate tactile stimuli of the environment. The environment can be virtual reality, or real situation sensed by a tele-operating slave robot.

Haptic interfaces can be divided into two categories due to the type of force feedback. They are Tactile haptic devices and Kinesthetic haptic devices.

Tactile haptic device is a human-machine interface producing touch feedback which can be felt by the skin. It allows the user to feel tactile stimuli including pressure, puncture, the texture of surfaces, thermal properties, softness wetness, frictions and vibrations [15]. These feelings are mainly generated by mechanic actuators like piezoelectric actuators, shape mem- ory alloys, pneumatic actuators, surface acoustic actuators, ultrasound pressure actuators and electric or magnetic fields. There are a lot of tactile haptic devices have already been developed. For example, STRESS from McGill Haptic Laboratory [24], MATRIX from MIT Media Labo- ratory [21], FEELEX from VR Lab in University of Tsukuba [17] and

(13)

VITAL from CEA-LIST [9]

Kinesthetic haptic device produces directional forces and torques as feedback, and can simulate solid boundaries, the weight of virtual objects, inertia and mechanical compliance of an object. Different from the tactile haptic devices, kinesthetic devices are usually used for simulating the dynamics properties of a virtual object. Various of kinesthetic devices have been developed, for example, PHANToM manufactured by SensAble Technologies [5], OMEGA developed by Force Dimension [3], VIRTUOSE developed by HAPTION [4] and Lhifam developed by CEIT [1]. DLR Light Weight Robot can also be used as a kinesthetic haptic device.

(a) STRESS (b) MATRIX

(c) FEELEX (d) VITAL

Figure 1.1: Tactile haptic devices

(14)

(a) PHNToM (b) OMEGA

(c) VIRTUOSE (d) Lhifam

Figure 1.2: Kinesthetic haptic devices

1.2 Motivation

LBR III is a newly developed light weight robot by DLR Institute of Robotics and Mechatronics. It is a multi-functional robot arm, haptic rendering is an important branch in its application fields. With only 14kg self weight, it is relatively safer for human operators in human-machine interactions. And its low inertia reduces the feeling of the dynamics of the robot. Although only weighted only 14 kg, the maximum is maximum payload of LBR III is as high as 14kg, which grantees that the robot arm can simulate strong impact of the virtual object. LBR III is seven joints robot, which means it has 7 DoF. The redundant DoF makes it possible to optimizes the joints configuration of the robot in the application. Based on this redundant DoF, Null Space movement can also be implemented avoid the collision between intermediate joints of the robot and obstacles. There are seven torque sensors and a 6 DoF force-torque sensor equipped on LBR III in order to precisely capture the moving behavior of the human operator. On the other side, there exist a newly designed inverse kinematics library, which is designed for mapping the posture of the end-effector into joint space based on standard LBR III. But it is not optimized haptic applications. Another problem is that the hardware configuration of one used as haptic device is not as same as the standard model of LBR III. The goal of this thesis is to adapt the existing inverse kinematics library to LBR III as haptic device, and to optimize the inverse kinematic library for haptic applications by enabling the Null Space movement due to external forces, detecting and avoiding the singularity configurations of the robot.

(15)

1.3 Thesis Outlines

Chapter 2 is a brief introduction to the LBR III including the specification of the hardware, Cartesian control strategy and joint control strategy. At last it explains the principle of the impedance control and admittance control.

Chapter 3 introduces joint angles compensation method and conversion matrix method for solving the problem that the initial configuration of the robot used as haptic device is different from the standard LBR III. And it describes the solution to enable the tool frame online changing for the inverse kinematics library. At last, it shows the simulation and experiment results of these methods.

Chapter 4 describes a general way for calculating the Jacobian matrix based on D-H parameters instead of analytic solutions. It also gives a solution for detecting the effective external forces on the forth joint, and realizes the null space movement due to the external forces. The simulation and experiment result is show at the last of this chapter.

Chapter 5 introduces the strategy to detect and avoid the singular configuration of LBR III, including using the determinants of the sub-matrices of the Jacobian to determine the singularity and introducing the push back force on the end-effector and indication arrows in the graphic environment to avoid the singular configurations.

Chapter 6 concludes all the works of this thesis and shows the open points for the future work.

(16)

Chapter 2 DLR Light Weight Robot

The DLR Light Weight Robot, called Leichtbauroboter (LBR) in German, is a light-weight, flexible, revolute joint robot. This chapter will introduce the third generation DLR Light Weight Robot.

2.1 Specifications

LBR III is especially designed to work in the sensitive area of human interaction by being equipped overall sensory devices. The size, power and manipulation capabilities of the robot are similar to that of a human arm. With its seven serially linked joints, the robot possesses a redundant DoF that enabling the null space movement which is valuable for avoiding collision and optimizing its configuration. The standard robot flange interface makes it possible to connect any gripper or tool to the robot.

Figure 2.1: Light Weight Robot with connected joystick handle

(17)

With only 14 kg total mass, it is able to handle payloads up to 14 kg over the whole dynamic range. This high payload-to-weight ratio is achieved by integrating very light gears, powerful motors and weight optimized brakes. The electronics, including the power converters, are integrated inside the robot arm.

The integrated sensors are most progressive - each of the Light Weight Robot’s joints has pair of sensors for both motor position and joint position and joint torque. Thus, the robot can be operated in the ways of position, velocity and torque control. An additional 6-DoF force-torque sensor can be mounted on the wrist of the robot, which can measure very precisely external forces, for example, forces applied by a human operator.

Total Weight 14 kg

Max. Payload 14 kg

Max. Joint Speed 120°/s Sensor angular resolution 20⁰⁰

Maximum Reach 936mm

Nr. of Axes 7 (R - P - R - P - R - P - P)

Motors DLR-Robodrive

Gears Harmonic Drive

Sensors (each Joint) 2 Position, 1 Torque Sensor

Brakes Electromagnetic Safety Brake

Power Supply 48V DC

Control Position-, Torque,- Impedance Control Control Cycles Current 40 kHz; Joint 3 kHz; Cartesian

1 kHz

Electronics Integrated Electronics, internal Ca- bling, Communications by optical SERCOS-Bus

Table 2.1: Key features of LBR III

2.2 Controller Architecture

There are two levels in the controller architecture of LBR III, one is in the joint level and the other is in Cartesian level, as shown in figure 2.2.

The first stage in the controller architecture is a state feed back controller including gravity and friction compensation for seven joints. The control loop updates 3 times every millisecond, the state vector concerns the motor posi- tions and the joint torques, as well as their derivatives. Under the conditions for the passivity of this structure derived in [6], the global asymptotic stability can be proven [7].

Figure 2.3 shows the structure of the joint controller. By appropriately setting the parameters of the feedback gains, the joint level controller can apply position, torque or impedance control. For the impedance control, the gains of the controller are computed every Cartesian cycle (1ms), based on the desired joint

(18)

Impedance control

Torque control Position

control

Impedance control

Force control

motor position joint torque link position

Force-torque sensor stiff robot

dynamics

adaptive gain adjustment 4-th order joint state feedback: q^m, dq^m, τ, dτ

k➔0 k➔max

inverse kinematics

serial bus 1kHz

cartesian level Joint level

3kHz joint7

N joint1

Figure 2.2: Controller architecture of LBR III [16]

stiffness and damping, as well as actual inertia matrix.

The second stage is in Cartesian level. Based on the joint level controller, three different strategies have been realized on the robot, which are admittance control, impedance control and Cartesian stiffness control. Cartesian stiffness control is normally used to achieve the compliant manipulation skills of the human arm, the other two are usually applied for the haptic devices.

(19)

Figure 2.3: Structure of the joint controller [18]

2.3 Control methods in Cartesian Space

The end-effector of the robot requires a suitable compliant behavior in Cartesian space when it interacts with the environment. To achieve this goal it requires to select an adequate control methods to establish a mass-spring-damper relation between the Cartesian force-torque vector F and the Cartesian displacement

∆χ:

F = M ∆ ¨χ + Dk∆ ˙χ + Kk∆χ, (2.1) where M, Dk and Kk are all positive definite matrices representing virtual inertia, damping factor and stiffness of the system [8]. Impedance control and admittance control are two widely used Cartesian control methods for haptic devices. A brief introduction to these two control methods will be presented in the following.

M

F Kk

D_k χ

Figure 2.4: One dimensional mass-spring-damper model

(20)

2.3.1 Impedance control

In this methods, it uses directly (2.1) to generate the feedback Cartesian force- torqueF.

F = D^k∆ ˙χ + Kk∆χ, (2.2)

where, the actual Cartesian position χ is computed using forward kinematics K{q},

χ =K{q}. (2.3)

For generating the feedback from virtual world, the desired joint torques τd

can be transformed from the Cartesian force-torque vector by the transposed Jacobian J^T(q),

τd=J^T(q)F, (2.4)

the motor torque controllerT^R calculates the motor torque commandsτmfrom the desired joint torques:

τm=T^R{τ^d}. (2.5)

Figure 2.5 shows how the impedance control works in the haptic application.

When a human operator manipulate the end-effector of the robot, the robot will react a force due to virtual environment. In the simulated free space, the user feel free to move the robot, because the motors do not generate feedback force from virtual world. But when contact with a simulated stiff surface, for example, a virtual wall, a small displacement will cause very high motor reac- tion torque, which is tends to be instable. Another thing have to be taken into account is that the stiffness matrix can be distorted when the end-effector moves in the vicinity of singularities, but the movement is still stable and smooth.

M

Motors

Sensors

+ +

Virtual world

Telepresence or Impedance

Controller

Human Operator

Robot Inertia

F

χ

Figure 2.5: Impedance control

2.3.2 Admittance Control

This method is base on the joint position controller P^R, it can generate the motor torques command:

τm=P^R{q} (2.6)

the desired joint angle can be calculated by the inverse kinematicsK⁻¹:

q =K⁻¹{χ^d} (2.7)

(21)

where χd is the desired Cartesian position generated from the Cartesian force- torque vector:

χd(s) = χ0(s)− ∆F(s)

Kk+Dks (2.8)

The principal of the controller is shown in figure 2.6. In contrast to the impedance control, it need a high control gain to simulate a free space motion for admittance control. It means a light touch can make the robot accelerate very quickly, because of the very low or even zero stiffness. So there is a potential stability problem for the admittance controlled device in simulated free space. But it has a good feature in simulating the contact to a virtual wall, e.g. for simulating a infinitely stiff wall, the gain of the controller is zero. An other potential problem arises in the vicinity of the singularities, where the controller can lead the robot to fast unstable movement.

Sensors

M

Motors

+ +

Human Operator

Virtual world

Telepresence or Adimittance

Controller F

χ

Inertia Virtual

Figure 2.6: Admittance control

Because admittance control has better performance when the robot contact to the stiff virtual object which is the common situation for the haptic rendering.

And as shown in figure 2.5, in impedance control method, the force is applied directly on the robot, whose inertia is configuration and manipulation depen- dent, so the human operator will feel it takes different effort to move the robot in different directions. And the admittance control method does not has this effect. So admittance control method is chosen for the Cartesian controller of the robot.

(22)

Chapter 3 Integration of Inverse Kinematics Library

Newly designed inverse kinematics library is based on LBR-Papas, which is the standard model of LBR III robots in the institute. The robot used as haptic device is called LBR-Haptic which is a modified version of the standard one. In order to use the library, two problems have to be solved. The first is that the library does not support changing tool frame online, which is inconvenient for haptic application, in the case that various kinds of tools may be attached to the robot during an experiment. Therefore, a way to change the tool frame online is required. The second problem is originated by the different configurations of the two robots. More detailed, the last two joints of two robots are mounted in different ways, so a solution to eliminate the differences is also needed for adapting the inverse kinematics library to the modified robot.

3.1 Adaption of Tool Frame

In this thesis, tool frame represents the transformation matrix from tool coordinate to frame 7. As mentioned above, being able to change the tool frame online will be a useful add on for the new inverse kinematics. In order to do so, following method can be applied.

In the inverse kinematics library, the tool frame is set to be an identity matrix of dimension 4. So the pose of the tool on LBR-Papas Hp is identical to homogeneous transformation⁰₇Tp

0Hp= ⁰₇Tp. (3.1)

In fact, the tool frame⁷_tThdiffers from tool to tool. The posture of the tool on the robot can be expressed as

0Hh= ⁰₇Th7

tT. (3.2)

According to (3.1) and (3.2) holds

0Hh−1 0Hp= (⁰₇Th7

tT )^{−1 0}₇Tp, (3.3)

(23)

Equation (3.3) is the function for adapting the online tool frame changing to the library. Assume ⁰₇Tp= ⁰₇Th, the adapting function becomes:

0Hh−1 0Hp= ⁷_tT⁻¹. (3.4) By post-multiply⁷_tT⁻¹ to the input desired posture, it generates the new input desired posture for the inverse kinematics

0Hp= ⁰Hh7

tT⁻¹. (3.5)

3.2 Adaption of D-H Differences

The new inverse kinematics library is designed for LBR-Papas. Figure 3.1 shows the configuration of LBR-Papas. The configuration of LBR Haptic is different from the above, which is shown in figure 3.2.

(24)

x0 y0

z0 x1 y1

z1 x2 y2

z2 x3

y3 z3

x4 y4

z4 x5 y5

z5 x6

y6 z6

x7 y7 z7

Figure 3.1: D-H parameters of LBR-Papas

x0 y0

z0 x1 y1

z1 x2 y2

z2 x3

y3 z3

x4 y4

z4 x5 y5

z5

x6 y6

z6 x7 y7

z7

Figure 3.2: D-H parameters of LBR-Haptic

(25)

The viewer is helpful for having a better understanding of the differences between these two robots. When arbitrary joint angles q = (−161^◦,−36^◦, 148^◦, 104^◦,−61^◦,−20^◦,−3^◦) are set to these two robots, the differences between them can be seen from figure 3.3.

(a) (b)

Figure 3.3: Different postures of two robots with identical commanded joint angles: (a) LBR-Papas, (b) LBR-Haptic

There are two methods to conquer this problem, which are joint angles compensation (JAC) and conversion matrix (CM).

3.2.1 Joint Angles Compensation

As shown in figure 3.1 and figure 3.2, the differences between LBR-Papas and LBR-Haptic are the initial joint angles for Joint 6 and Joint 7. The idea of this method is to compensate the differences of the initial conditions to make both robots have the same postures when same joint angles are commanded.

qp,i= qh,i+qcomp,i, (3.6)

In this way the new inverse kinematics can work for LBR-Haptic. According to the D-H parameters of the robots the exact set of compensation angles is q_comp= [0^◦,0^◦, 0^◦, 0^◦, 0^◦,−90^◦,−180^◦]. Figure 3.4 shows schematically how this method can be implemented. Because the method of joint angles compensation need to change the joint limits of joint 6 and joint 7 from [−130^◦, 130^◦] and [−170^◦, 170^◦] to [−40^◦, 220^◦] and [10^◦, 350^◦], and at first, the inverse kinematics library can not handle angles larger than 180^◦. There is a need to find another way for the adaptation of the inverse kinematics to the haptic robot.

(26)

LBR−Papas Inverse Kinematics Library

LBR−Haptic

qp qdp qdh

Hp qh

- +

[0^◦, 0^◦, 0^◦, 0^◦, 0^◦,−90^◦,−180^◦]

Figure 3.4: Joint angles compensation method

3.2.2 Conversion Matrix

Conversion matrix method is another way to adapting the library to the robots, the main idea behind this method is that if the same set of joint angles is commanded to both robots, the postures of the robots will be different, and the difference from LBR-Haptic to LBR-Papas can be described by a conversion matrixCp^h.

If the joint angles for both robots are set to q(q1, q2, q3,q4, q5, q6, q7), the transformation changes the reference frame from the last joint to base for LBR-Papas is

0

7T = ⁰₁T¹₂T · · · ⁶7T. (3.7) Equation (3.7) also holds for LBR-Haptic. So that, it is possible to find the relation between these two robots with conversion matrix, which is

0

7Tp= ⁰₇ThC_p^h. (3.8)

By pre-multiply⁰₇T_p⁻¹ on both sides of (3.8),

C_p^h= ⁰₇T_h⁻¹⁰₇Tp, (3.9) the right side can be extended to

Cp^h= ⁵₇T_h⁻¹(⁰₅T_h⁻¹⁰5Tp)⁵₇Tp. (3.10) Because the D-H parameters for the first five joints of two robots are identical,

0

5Th= ⁰₅Tp, follows

Cp^h= ⁵₇Tp⁻¹ 5

7Tp. (3.11)

It can be calculated,

7

5T_h⁻¹=







c6c7 −c⁶s7 −s⁶ 0

s7 c7 0 0

s6c7 −s⁶s7 c6 0

0 0 0 1







, (3.12)

(27)

7 5Tp=







c6c7 s7 s6c7 0

−c⁶s7 c7 −s⁶s7 0

−s⁶ 0 c6 0

0 0 0 1







. (3.13)

By combining equations (3.11), (3.12) and (3.13), the conversion matrix can be calculated,

C_p^h=







−s²7 −s⁷c7 c7 0

−s⁷c7 −c²7 −s⁷ 0

c7 −s⁷ 0 0

0 0 0 1







. (3.14)

LBR−Papas Inverse Kinematics Library

LBR−Haptic

qdp qdh

qh qp

Hp

Hh ×

C^h_p

Figure 3.5: Conversion matrix method Figure 3.5 is the schematic of the conversion matrix method.

3.3 Simulation

Previous sections presented two methods in adapting the inverse kinematics library to LBR-Haptic. This section will show simulation results for both method, and select the better one due to the simulation performance.

In order to have a clear sense of the result, the end-effector will be simulated to perform two elementary motions which are the translation along the x-axis with respect to the base coordinate system; and the rotation around z-axis with respect to the base coordinate system.

For the simulation, an arbitrary initial position is chosen which is with the joint angles q = (−72^◦,−31^◦, 133^◦, 70^◦,−54^◦, 63^◦, 20^◦), the respective TCP for this initial position is





0.9999 −0.0073 0.0145 0.3936 0.0163 0.4619 −0.8868 −0.4025

−0.0003 0.8869 0.4619 0.6293



.

(28)

Trajectory Generator

Inverse Kinematics

Library

Forward Kinematics

Viewer

Trajectory Analysor

Figure 3.6: Simulation model

Figure 3.6 shows the schematic of the this simulation model. The trajectory generator can produce displacement ∆x, ∆y, ∆z along x, y, z axis and rotations

∆γ, ∆β, ∆α around x, y, z axis relative to initial orientation of the end-effector.

Simulation 1 : Translation along x-axis

Conditions Moving the end-effector along x-axis with the displacement ∆x = A sin(ωt), where A = 0.1m and ω = 0.5rad/s.

0 5 10 15 20 25

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

time [s]

Displacement [m]

∆x

∆y

∆z

(a) Conversion matrix (CM)

0 5 10 15 20 25

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

time [s]

Displacement [m]

∆x

∆y

∆z

(b) Joint angles compensation (JAC)

Figure 3.7: Displacements of LBR’s end-effector in Simulation 1 In figure 3.7, it shows that Jonit Angles Compensation (JAC) method works perfectly in displacement side, the position of the end-effector keeps still in y- axis and z-axis, and in x-axis it follows a Sines curve with respect to time, which just follows the desired trajectory. For the Conversion Matrix (CM) methods, the trajectory is not as smooth as the other method, because of the joint limits as shown in figure 3.9.

(29)

0 5 10 15 20 25

−20

−15

−10

−5 0 5 10 15 20

time [s]

Rotation [ °]

∆γ

∆β

∆α

0 5 10 15 20 25

−20

−15

−10

−5 0 5 10 15 20

time [s]

Rotation [ °]

∆γ

∆β

∆α

Figure 3.8: Rotations of the LBR’s end-effector in Simulation 1

Figure 3.8(b) shows that the JAC method is also perfect for the orientation property of the end-effector. The orientation did not change while it translating along x-axis. In figure 3.8(a), there is a peak at the beginning of the simulation and also a perturbation occured near the end, which means this method is not stable all the time.

0 5 10 15 20 25

−150

−100

−50 0 50 100 150

time [s]

Joint Angle [ °]

q₁ q₄ q5

q3

q₆ q₂

q₇

0 5 10 15 20 25

−150

−100

−50 0 50 100 150

time [s]

Joint Angle [ °]

q₃

q6 q₄

q₇

q5 q2

q1

Figure 3.9: Joint angles of LBR in Simulation 1

How the joint angles behave is also important for its use as a haptic device.

The smoother it is, the less inertia of the robot can be felt, because the smoother motion means less acceleration on the joints. In figure 3.9(a), the joint angles were changing dramatically at the beginning, which means the robot was making reconfiguration of its joints. At around the 2nd second, Joint 7 went to its joint limit 170^◦, that is the reason for the jump shown in figure 3.7(a). At the time around the 11th second, the reconfiguration happens again, that can explain the perturbation shown in figure 3.8(a). After that Joint 5 went to its joint limit 170^◦again, which stops the inverse kinematics, which made the end-effector not following the desired trajectory. In figure 3.9(b), the curves for the joint angles are quite smooth and harmonic, which is desired.

(30)

Simulation 2 : Rotation around z-axis

Conditions Rotating the end-effector around z-axis with the angle ∆α = A sin(ωt), where A =−20^◦andω = 0.5rad/s.

0 5 10 15 20 25

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

time [s]

Displacement [m]

∆x

∆y

∆z

0 5 10 15 20 25

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

time [s]

Displacement [m]

∆x

∆y

∆z

Figure 3.10: Displacements of LBR’s end-effector in Simulation 2 Figure 3.10 shows that both methods can guarantee the position of the end- effector does not change while the orientation of it is changing

0 5 10 15 20 25

−20

−15

−10

−5 0 5 10 15 20

time [s]

Rotation [ °]

∆γ

∆β

∆α

0 5 10 15 20 25

−20

−15

−10

−5 0 5 10 15 20

time [s]

Rotation [ °]

∆γ

∆β

∆α

Figure 3.11: Rotations of the LBR’s end-effector in Simulation 2 Both methods worked good when looking at the orientation of the end- effector shown in figure 3.11, it just follows what is commanded except a peak occurs in the first figure.

(31)

0 5 10 15 20 25

−200

−150

−100

−50 0 50 100 150 200

time [s]

Joint Angle [ °] q

3 q5

q₄

q2

q₆ q1

q₇

0 5 10 15 20 25

−150

−100

−50 0 50 100 150

time [s]

Joint Angle [ °]

q₃ q4

q6 q₇ q₂ q5 q1

Figure 3.12: Joint angles of LBR in Simulation 2

The CM method works well in this simulation if only concerning the trajectory of the end-effector as shown in figure 3.10 and figure 3.11, but when look at how the joint angles changes, it is not so. As shown in figure 3.12(a), there is a reconfiguration of the robot at the beginning, and afterwards Joint 7 when to its limit.

From the simulation results, it can be concluded that the Joint angles compen- sations method is stable and accurate whereas the Conversion matrix method is not, so the following work will be based on the Joint angles compensation method.

3.4 Testing on Hardware

After being verified by the simulation, the next step is to implement and test the Joint angles compensation method on the hardware. In order to see different between simulation and the hardware test, the test conditions are same as in the simulation.

Trajectory Generator

Inverse Kinematics

Library

LBR III

Trajectory Analysor

Figure 3.13: Experiment model

Figure 3.13 shows the schematic of the experiment model, the only difference from the simulation model is that the robot hardware takes the place of forward kinematics block in the model.

(32)

Test 1 : Translation along x-axis

Conditions Moving the end-effector along x-axis with the displacement ∆x = A sin(ωt),where A = 0.1m and ω = 0.5rad/s.

0 5 10 15 20 25

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

time [s]

Displacement [m]

∆x

∆y

∆z

Figure 3.14: Displacements of LBR’s end-effector in Test 1

0 5 10 15 20 25

−20

−15

−10

−5 0 5 10 15 20

time [s]

Rotation [ °]

∆γ

∆β

∆α

Figure 3.15: Rotations of the LBR’s end-effector in Test 1

Adaptation of the Inverse Kinematics of the DLR Light Weight Robot for

M A S T E R ' S T H E S I S