Introduction to Robotics - uni-hamburg.de

Universitat Hamburg

MIN-FakultatDepartment Informatik

- Introduction to Robotics

Introduction to Robotics

Jianwei [email protected]

Universitat HamburgFakultat fur Mathematik, Informatik und NaturwissenschaftenDepartment Informatik

Technische Aspekte Multimodaler Systeme

26. April 2013

J. Zhang 80

Universitat Hamburg


Inverse kinematics for manipulators Introduction to Robotics

Outline

General InformationIntroductionCoordinates of a manipulatorKinematic equationsInverse kinematics for manipulators

Analytical solvability of manipulatorExample 1: a planar 3 DOF manipulatorThe algebraical solution using the example of PUMA 560The solution for RPY anglesGeometrical solution for PUMA 560A Framework for robots under UNIX: RCCL

J. Zhang 81

Universitat Hamburg



Inverse kinematics for manipulators

Set of problems:

I The control of robot manipulators take place in the majority ofcases in joint space,

I The information about objects are mostly given in the cartesianspace.

To achieve a one specific tool frame T related to the world, thejoint values θ(t) = (θ1(t), θ2(t), ..., θn(t))T should be calculated intwo steps:1. Calculation of T6 = Z−1BGE−1;2. Calculation of θ1, θ2, ..., θn over T6.⇒: In this case the inverse kinematics is more important then theforward kinematics.

J. Zhang 82

Universitat Hamburg



The solution using the example of PUMA 560 - I

T6 = T ′T ′′ =

nx ox ax pxny oy ay pynz oz az pz0 0 0 1

where

J. Zhang 83

Universitat Hamburg



The solution using the example of PUMA 560 - II

nx = C1[C23(C4C5C6 − S4S6)− S23S5C6]− S1(S4C5C6 + C4S6) (2)

ny = S1[C23(C4C5C6 − S4S6 − S23S5S6] + C1(S4C5C6 + C4S6) (3)

nz = −S23[C4C5C6 − S4S6]− C23S5C6 (4)

ox = ... (5)

oy = ... (6)

oz = ... (7)

ax = ... (8)

ay = ... (9)

az = ... (10)

J. Zhang 84

Universitat Hamburg



The solution using the example of PUMA 560 - III

px = C1[d6(C23C4S5 + S23C5) + S23d4 + a3C23 + a2C2]− S1(d6S4S5 + d2)(11)

py = S1[d6(C23C4S5 + S23C5) + S23d4 + s3C23 + a2C2] + C1(d6S4S5 + d2)(12)

pz = d6(C23C5 − S23C4S5) + C23d4 − a3S23 − a2S2 (13)

J. Zhang 85

Universitat Hamburg



Remark

I Non-linear equations

I Existence of solutions: Workspace: the volume of spacethat is recheable for the tool of manipulator.I ”dextrous workspace”I ”reachable workspace”

I Many joint positions that produce the similar TCPposition using the example of PUMA 560:I Ambiguity of solutions for θ1, θ2, θ3 related to given p.I For each solution of θ4, θ5, θ6 exist the alternative solution:

J. Zhang 86

Universitat Hamburg



Remark

θ′4 = θ4 + 180◦

θ′5 = −θ5θ′6 = θ6 + 180◦

I Different solution strategy: closed solutions vs.numerical solutions

J. Zhang 87

Universitat Hamburg



Different methods for solution finding

Closed form:I algebraic solution

+ : accurate solution over the equations,− : solution is not geometrical representative.

I geometrical solution

+ : case-by-case analysis of the possible robot configurations,− : robot specific.

Numerical form:I iterative methods

+ : the methods are transferable,− : computationally intensive, for several exceptions the

convergence can not be guaranteed.

J. Zhang 88

Universitat Hamburg



Methods for solution finding

”The inverse kinematics for all systems with 6 DOF (translationalor rotational joints) in a simple serial chain is always numericalsolvable.”

J. Zhang 89

Universitat Hamburg


Inverse kinematics for manipulators - Analytical solvability of manipulator Introduction to Robotics

Analytical solvability of manipulator

The closed solution exists if a specific conditions (sufficientconditions) for the arm geometry are given:

I If 3 sequent axis intersect in a given point,

I Or: 3 sequent axis are parallel to each other.

It’s important to design a manipulator in the way that the closedsolution exists.Almost all manipulators are conceptualized in this way.An example for PUMA 560:The axis 4, 5 and 6 intersect in one point.

J. Zhang 90

Universitat Hamburg


Inverse kinematics for manipulators - Example 1: a planar 3 DOF manipulator Introduction to Robotics

Example 1: a planar 3 DOF manipulator

J. Zhang 91

Universitat Hamburg



Example 1: a planar 3 DOF manipulator

Joint αi−1 ai−1 di θi1 0 0 0 θ12 0 l1 0 θ23 0 l2 0 θ3

0T3 =

C123 −S123 0 l1C1 + l2C12

S123 C123 0 l1S1 + l2S120 0 1 00 0 0 1

J. Zhang 92

Universitat Hamburg



The algebraical solution for the example 1 - I

Specification for the TCP: (x , y , φ). For this such kind of vektors applies:

0T3 =

Cφ −Sφ 0 xSφ Cφ 0 y0 0 1 00 0 0 1

Resultant four equations can be derived:

Cφ = C123 (14)

Sφ = S123 (15)

x = l1C1 + l2C12 (16)

y = l1S1 + l2S12 (17)

J. Zhang 93

Universitat Hamburg



The algebraical solution for the example 1 - II

(Derivative)The function atan2 is define as:

θ = atan2(y , x) =

0 for x = 0, y = 0

π/2 for x = 0, y > 0

3 ∗ π/2 for x = 0, y < 0

atan(y , x) for +x and +y

2π − atan(y , x) for +x und -y

π − atan(y , x) for -x und +y

π + atan(y , x) for -x und -y

The solution:θ2 = atan2(S2,C2)

J. Zhang 94

Universitat Hamburg



The algebraical solution for the example 1 - II

where S2 = ±√

1− C 22 , and C2 =

x2+y2−l21−l222l1l2

.

θ1 = atan2(y , x)− atan2(k2, k1)

where k1 = l1 + l2C2 and k2 = l2S2.

θ3 can be calculated as follow:

θ1 + θ2 + θ3 = atan2(Sφ,Cφ) = φ

J. Zhang 95

Universitat Hamburg



The geometrical solution for the example 1 - I

Calculate θ2 over “law of cosines”:

x2 + y2 = l21 + l22 − 2l1l2 cos(180 + θ2)

The solution:

θ2 = ±cos−1 x2 + y2 − l21 − l22

2l1l2

θ1 = β ± ψ

where:

β = atan2(y , x), cosψ =x2 + y2 − l21 − l22

2l1√

x2 + y2

For θ1, θ2, θ3 applies:θ1 + θ2 + θ3 = φ

J. Zhang 96

Universitat Hamburg



Algebraical solution with the help of polynomial conversion

The following substitutions are used for the polynomial conversion oftranscendental equations:

u = tanθ

2

cos θ =1− u2

1 + u2

sin θ =2u

1 + u2

J. Zhang 97

Universitat Hamburg



Algebraical solution with the help of polynomial conversion

Example:The following transcendental equation is given:

a cos θ + b sin θ = c

After the polynomial conversion:

a(1− u2) + 2bu = c(1 + u2)

The solution for u:

u =b ±√b2 − a2 − c2

a + c

Then:

θ = 2tan−1(b ±√b2 − a2 − c2

a + c)

J. Zhang 98

Universitat Hamburg


Inverse kinematics for manipulators - The algebraical solution using the example of PUMA 560 Introduction to Robotics

The algebraical solution using the example of PUMA 560 -I

Calculation of θ1, θ2, θ3:The first three joint angle θ1, θ2, θ3 affect the position of TCP(px , py , pz)T (in case d6 = 0).

px = C1[S23d4 + a3C23 + a2C2]− S1d2 (18)

py = S1[S23d4 + a3C23 + a2C2] + C1d2 (19)

pz = C23d4 − a3S23 − a2S2 (20)

The outcome of this is:

θ1 = tan−1(∓py

√p2x + p2y − d2

2 − pxd2

∓px√p2x + p2y − d2

2 + pyd2)

J. Zhang 99

Universitat Hamburg



The algebraical solution using the example of PUMA 560 -II

θ3 = tan−1(∓A3

√A23 + B2

3 − D23 + B3D3

∓B3

√A23 + B2

3 − D23 + A3D3

)

where

A3 = 2a2a3

B3 = 2a2d4

D3 = p2x + p2y + p2z − a22 − a23 − d22 − d2

4

J. Zhang 100

Universitat Hamburg



The algebraical solution using the example of PUMA 560 -II

and

θ2 = tan−1(∓B2

√p2x + p2y − d2

2 + A2pz

∓A2

√p2x + p2y − d2

2 + B2pz)

where

A2 = d4C3 − a3S3

B2 = −a3C3 − d4S3 − a2

J. Zhang 101

Universitat Hamburg


Inverse kinematics for manipulators - The solution for RPY angles Introduction to Robotics

The solution for RPY angles

(RPY: Roll, Pitch, Yaw)

T = Rz,φRy ,θRx,ψ

The solution for following equation is sought:

R−1z,φT = Ry ,θRx,ψ

f11(n) f11(o) f11(a) 0f12(n) f12(o) f12(a) 0f13(n) f13(o) f13(a) 0

0 0 0 1

=

Cθ SθSψ SθCψ 00 Cψ −Sψ 0−Sθ CθSψ CθCψ 0

0 0 0 1

where

J. Zhang 102

Universitat Hamburg




f11 = Cφx + Sφy

f12 = −Sψx + Cφy

f13 = z

The equation for f12(n) leads to:

−Sφnx + Cφny = 0

⇒:φ = atan2(ny , nx)

andφ = φ+ 180◦

The solution with the elements 1,3 and 1,1 are as appropriate:

−Sθ = nzJ. Zhang 103

Universitat Hamburg




andCθ = Cφnx + Sφny

⇒:θ = atan2(−nz ,Cφnx + Sφay )

The solution with the elements 2,3 and 2,2 are as appropriate:

−Sψ = −Sφax + Cφay

Cψ = −Sφox + Cφoy

⇒:ψ = atan2(Sφax − Cφay ,−Sφox + Cφoy )

J. Zhang 104

Universitat Hamburg


Inverse kinematics for manipulators - Geometrical solution for PUMA 560 Introduction to Robotics

Geometrical solution for PUMA 560 - I

Definition of different arm configurations:

For shoulder:RIGHT-arm, LEFT-arm

For elbow:ABOVE-arm, BELOW-arm

For wrist:WRIST-DOWN, WRIST-UP

Adapted from this following variable can be defined:

ARM =

{+1 RIGHT-arm

−1 LEFT-arm

J. Zhang 105

Universitat Hamburg



Geometrical solution for PUMA 560 - I

ELBOW =

{+1 ABOVE-arm

−1 BELOW-arm

WRIST =

{+1 WRIST-DOWN

−1 WRIST-UP

The complete solution for the inverse kinematics can be achievedover analysis of such arm configurations.

J. Zhang 106

Universitat Hamburg



Technical difficulties during the development of controlsoftware

I Until now the development of control software was maked onlyfor specified robot.

I Each robot had special software, that used exactly skills of thismodel range.

I ⇒ Consequently, the extensibility and portability were reallydifficult ant took a lot of time.

J. Zhang 107

Universitat Hamburg



Technical difficulties during the development of controlsoftware

The idea was to create a universal cotrol software, that achievefollowing aims:

I Possibility to control of low-level hardware properties;

I Maximum portability to different platforms;

I Maximum flexibility to fast programming of an applications.

J. Zhang 108

Universitat Hamburg


Inverse kinematics for manipulators - A Framework for robots under UNIX: RCCL Introduction to Robotics

A Framework for robots under UNIX: RCCL

RCCL: Robot Control C Library

J. Zhang 109

Universitat Hamburg



The system architecture

J. Zhang 110

Universitat Hamburg



Possibility to control of multiple robots

J. Zhang 111

Universitat Hamburg



Motion description with position equations

J. Zhang 112

Universitat Hamburg



Motion description with position equations

J. Zhang 113

Universitat Hamburg



An example for program to control of a robot

#include <rccl.h>

#include "manex .560.h"

main()

{

TRSF_PTR p, t; /*#1*/

POS_PTR pos; /*#2*/

MANIP *mnp; /*#3*/

JNTS rcclpark; /*#4*/

char *robotName; /*#5*/

rcclSetOptions (RCCL_ERROR_EXIT ); /*#6*/

robotName = getDefaultRobot (); /*#7*/

if (! getRobotPosition (rcclpark.v, "rcclpark", robotName ))

{ printf (’’position ’rcclpark ’ not defined for robot\n’’);

exit (-1);

}

/*#8*/

t = allocTransXyz ("T", UNDEF , -300.0, 0.0, 75.0);

p = allocTransRot ("P", UNDEF , P_X , P_Y , P_Z , xunit , 180.0);

pos = makePosition ("pos", T6, EQ, p, t, NULL); /*#9*/

J. Zhang 114

Universitat Hamburg




mnp = rcclCreate (robotName , 0); /* #10 */

rcclStart ();

movej (mnp , &rcclpark ); /* #11 */

setMod (mnp , ’c’); /* #12 */

move (mnp , pos); /* #13 */

stop (mnp , 1000.0);

movej (mnp , &rcclpark ); /* #14 */

stop (mnp , 1000.0);

waitForCompleted (mnp); /* #15 */

rcclRelease (YES); /* #16 */

}

J. Zhang 115

Universitat Hamburg




J. Zhang 116

Documents

Introduction to Robotics - uni-hamburg.de