• No results found

A Computer Exercise in the Course

N/A
N/A
Protected

Academic year: 2021

Share "A Computer Exercise in the Course"

Copied!
18
0
0

Loading.... (view fulltext now)

Full text

(1)

Robot Kinematics {

A Computer Exercise in the Course

\Algebraic Methods for Systems Theory"

Krister Forsman

Department of Electrical Engineering Linkoping University, S-581 83 Linkoping, Sweden

email: krister@isy.liu.se

1994-01-27

REGLERTEKNIK

AUTOMATIC CONTROL

LINKÖPING

Technical reports from the Automatic Control group in Linkoping are available as UNIX-compressed Postscript les by anonymous ftp at the address130.236.24.1 (joakim.isy.liu.se).

1

(2)

Preface and Reading Instructions

It is the purpose of this exercise to provide an application example where the methods dis- cussed in 4] can be used. The particular example chosen here relates to the inverse kinematics of a simple industrial robot. All computations can be made in Maple 2], and Maple code is supplied. There is a similar example in chapter 6 of 3].

An advice when working through this material: the basic idea is that every page ends with a question and begins with the answer of the previous question. All questions are answered in the handout, but make sure you understand the problem before you look at the solution.

Sometimes a line is used as a \virtual page-break", i.e. as separator between question and answer.

IMPORTANT:When the text starts you are supposed to have started Maple and loaded thelinalgand grobner package:

|\^/| MAPLE V

._|\| |/|_. Copyright (c) 1981-1990 by the University of Waterloo.

\ MAPLE / All rights reserved. MAPLE is a registered trademark of

<____ ____> Waterloo Maple Software.

| Type ? for help.

> with(linalg):

> with(grobner):

The following Maple commands are used, some of them without further comments. Use the Maple on-line help facilities to get more info (e.g. ?gbasis)

discrim, evalf, evalm, expand, factor finduni, gbasis, indets, leadmon, map op, solve, sort, subs, vector

A very short introduction to Maple is 5].

In the anonymous ftp are there is a Maple worksheet le called (something like)

Src/maple/robot-kinematics/computer-exercise.ms

which contains the Maple session in this document. On the same directory is a lerobotplot

which contains a simple procedure for plotting the robotarm. It has syntax: robotplot( q1, q2, a, b )

wherea, bare optional parameters with default value 1.

(3)

-

x 6

u

















...

a



q

1 u













 b

q

2 P

Figure 1: Two link manipulator.

1 Setup

Consider a two link manipulator performing a planar motion, i.e. the positionP of the end- eector, orhand, is uniquely determined by two coordinates (xy). See gure 1. The lengths of the links are aand b.

Here we will not study the dynamics of the robot problem, only the geometry. Straight- forward calculations yield the forward kinematics, i.e. the cartesian coordinates (xy) as a function of the anglesq1q2:

> p1 := vector( a * cos(q1) , a * sin(q1)])

p1 :=  a cos(q1), a sin(q1) ]

> P := evalm( p1 + b*vector( cos(q1+q2), sin(q1+q2) ]) )

P :=  a cos(q1) + b cos(q1 + q2), a sin(q1) + b sin(q1 + q2) ]

Thus

x = acos(q1) +bcos(q1+q2)

y = asin(q1) +bsin(q1+q2) (1) Since this course deals with polynomial systems we wish to rewrite the system (1) in polynomial form. >How can we do that?

3

(4)

First expand the trigonometric functions of the sumq1+q2:

> Pe := map(expand, P)

Pe :=  a cos(q1) + b cos(q1) cos(q2) - b sin(q1) sin(q2), a sin(q1) + b sin(q1) cos(q2) + b cos(q1) sin(q2) ]

Then introduce separate variables for the sine and cosine of each angle:

s

i = sin(qi) ci = cos(qi) (2)

i.e.

> A := { cos(q1) = c1, sin(q1) = s1, cos(q2) = c2, sin(q2) = s2 }:

> P := subs( A, op(Pe) )

P :=  a c1 + b c1 c2 - b s1 s2, a s1 + b s1 c2 + b c1 s2 ]

This solution is not yet satisfactory, since we have thrown away the information thats1c1 are related in a certain way:::

Here this can be amended by adding that

s 2

1+c21;1 = 0 s22+c22;1 = 0 All in all we should study an ideal generated by the set

F

1 = fx;ac1;bc1c2+bs1s2 y;as1;bs1c2;bc1s2

s 2

1+c21;1 s22+c22;1g (3)

> F1 :=  x - P1], y - P2], s1^2 + c1^2- 1, s2^2 + c2^2 - 1 ]

F1 := x - a c1 - b c1 c2 + b s1 s2, y - a s1 - b s1 c2 - b c1 s2,

2 2 2 2

s1 + c1 - 1, s2 + c2 - 1]

Clearly the forward kinematics problem does not provide many problems interesting from a commutative algebraic point of view, but the converse problem is certainly very interesting.

(5)

2 Inverse Kinematics

Theinverse kinematicsproblem consists in determining what values of the angles q1q2 that correspond to a certain position of the hand. Thus (xy) are given and the angles wanted.

Some references to inverse kinematics are 1, 6, 7]. Let me rst remark that for problems as simple as this using Grobner bases is overkill there are easier solutions to the inverse kinematics problem for the robot above.

In other words we wish to determine the inverse of the mapping from (q1 q2) to (xy).

Let us rst consider a simpler version of the problem by setting a =b = 1 in the set F1 of equation (3). We denote the set obtained after this substitution by F2.

> F2 := subs( {a=1, b=1}, F1 )

2 2

F2 := x - c1 - c2 c1 + s1 s2, y - s1 - c2 s1 - c1 s2, s1 + c1 - 1,

2 2

s2 + c2 - 1]

(Indeed,subsis the Maple implementation of ring-homomorphisms!) Now consider ideal I1 generated by the setF2 in the ring

R

1 = Qc1c2s1s2xy] (4)

Use elimination theory (Grobner bases) to determine what the relation between q1y and x is!

>What do you think dimI1 is, considering the background of the problem?

5

(6)

A plex gb will do the elimination for us according to theorem 2.8. We should use a term-ordering that ranks yx lowest and eliminates c2s2, so

c

2

>s

2

>c

1

>s

1

>y>x (5)

will do.

> vars1 :=  c2, s2, c1, s1, y, x ]

vars1 := c2, s2, c1, s1, y, x]

> g1 := gbasis( F2, vars1, plex )

2 2 2 2

g1 := 2 c2 - y - x + 2, s1 x - c1 y + s2, s1 + c1 - 1,

2 2 2 3

4 c1 s1 y - 2 c1 y - 4 s1 x - x y + 2 x s1 y - x + 4 x,

2 2

- y + 2 s1 y - x + 2 c1 x,

4 3 2 2 2 2 2 2 2 4 2

y - 4 s1 y + 2 x y + 4 s1 y - 4 s1 y x + 4 s1 x + x - 4 x ]

> map( indets, g1 )

{x, y, c2}, {x, y, c1, s1, s2}, {c1, s1}, {x, y, c1, s1}, {x, y, c1, s1}, {x, y, s1}]

One would expect I1 to be two-dimensional from the geometry of the problem: there should only be nitely many values ofq1q2, given xy. We see ong1 that

I

1

\Qxy] = 0 (6)

so by theorem 1.15 dimI1 2.

One way of checking if I1 has dimension two is to extend it to a ring of fractions where we let two of the variables be parameters and then check if I1e has dimension zero.

>What condition must a set of variables satisfy if we are to consider them as parameters?

>What particular variables should we choose as parameters in this example?

>How do you check in Maple ifI1e is zero-dimensional?

(7)

The denition of parameters is that they do not themselves satisfy any equation: they arealgebraically independent. In particular they are transcendental over the ground eld.

In this example, according to formula (6), xy are algebraically independent, so we could choose xy as parameters.

Formally, letting variables be parameters is the same thing as extending the ideal consid- ered to a suitable ring of fractions. In this case we can consider the extensionI2 of I1 to the ring

R

2 = Q(xy)c1c2s1s2] (7) i.e.I2 is the ideal generated by F2 in the ring R2.

>But what happens if we do that?

>Will I2 give us the same information asI1?

The answer is provided by theorems 1.13 and 1.14. We have already said that I1 \

Qxy] = 0, so one of the conditions of theorem 1.13 is satised. >But isI1 prime?

We don't answer that questions completely here, we are content with the simple necessary condition for primality provided by exercise 2.8: If you type

factor(g1)

you will see that no element of g1 was factored, so all elements ofg1 are irreducible overQ. At least we have an indication thatI1 is prime:::

Let's go ahead consideringxy as parameters!

Of course, we may ask if a gb of an ideal with parameters is also a gb for every special- ization of the parameters. This question, which is not trivial, is discussed in 3], page 270 etc. Here we don't enter into that discussion either.

>How do you compute a gb forI2 in Maple?

7

(8)

If the term ordering is

c

2

>s

2

>c

1

>s

1 (8)

then we do as follows in Maple:

> vars2 :=  c2, s2, c1, s1 ]

vars2 := c2, s2, c1, s1]

> g2 := gbasis( F2, vars2, plex )

2 2 2 3 2 2

g2 := 2 c2 - y - x + 2, - y x - y + (2 x + 2 y ) s1 + 2 s2 x,

2 2

- y + 2 s1 y - x + 2 c1 x,

2 4 2 2 4 2 3 2 2 2

- 4 x + x + 2 x y + y + (- 4 y x - 4 y ) s1 + (4 x + 4 y ) s1 ]

> map( leadmon, g2, vars2, plex )

2 2 2

2, c2], 2 x, s2], 2 x, c1], 4 x + 4 y , s1 ]]

The last computation shows that I2 is zero-dimensional (every variable occurs alone in the leading term of an element of the gb) soI1 has dimension two, as expected!

Note that I2 is in generic position, and the minimal polynomial ofs1 is of degree two. So for each given position (xy) of the hand, there are only two pairs of angles (q1q2) achieving this position, as you would expect intuitively.

However, we should remember that we have cheated a little when drawing these conclu- sions. We have to check separately that we get real solutions ins1s2q1q2for a given (xy).

In fact, if we pick a pair (xy) outside the working range of the robot we will get angles that are not real. Prove this in Maple!

(9)

We get real solutions i s1 is real, since I2 is in g.p. The equation fors1 is

p = (4x2+ 4y2)s21;(4x2+ 4y2)ys1+y4;4x2+x4+ 2x2y2 = 0 (9) It is elementary that phas real zeroes i the discriminantof pis non-negative (the discrimi- nant ofax2+bx+cis b2;4ac).

> p := sort( g24], s1 ):

> d := discrim( p, s1 )

2 2 2 2 2

d := - 16 (y + x - 4) x (x + y )

Clearly

d>0 () 0<y2+x2 <4 (10) so we have proved that we get real solutions i (xy) is inside the disc of radius two. If you want to, you can think for a minute what happens in the singular cases (x = 0y = 0x2+y2= 4).

A word of warning regarding discriminants: even if these are dened for polynomials of higher degree, the condition for real zeroes is not as simple as that of the quadratic equation.

>What angles q1q2 can you choose to put the hand at (x= 1y= 0:5)?

9

(10)

The only problem when trying to solve numerically is to pick the right branch of the arcsine:

> # Pick one element for s1 and one for s2 from g2 and

> # specialize to the given x, y

> g2spec := subs( {x=1, y=1/2}, g22], g24]] )

2 39

g2spec := - 5/8 + 5/2 s1 + 2 s2, 5 s1 - 5/2 s1 - ----]

16

> # Solve for s1 (two solutions):

> S1 := solve( g2spec2] )]

1/2 1/2

S1 := 1/4 + 1/10 55 , 1/4 - 1/10 55 ]

> q1a := evalf( arcsin(S11]) ) q1b := evalf( arcsin(S12]) )

q1a := 1.4412 q1b := -.51395

> # Solve for S2 (one solution for each s1):

> S2 :=  solve( subs( s1=S11], g2spec1] ) ),

> solve( subs( s1=S12], g2spec1] ) ) ]

1/2 1/2

S2 := - 1/8 55 , 1/8 55 ]

> q2a := evalf( arcsin(S21]) ) q2b := evalf( arcsin(S22]) )

q2a := -1.1864 q2b := 1.1864

> # Only... arcsine picks the wrong value of q2.

> # Fix by adding/subtracting pi:

> q2a_pi := evalf(-q2a-Pi) q2b_pi := evalf(Pi-q2b)

q2a_pi := -1.9552 q2b_pi := 1.9552

> # Check that the result obtained is correct by plugging it into the

> # original expression for P:

> Pspec := subs( {a=1, b=1}, op(Pe) ):

(11)

Thus the answer is: either one of

q = (1:44 ;1:96) and q = (;0:511:96) (11) Let us now shift our attention back to the original problem where the lengths of the links were indeterminates. Try and solve the inverse kinematics problem using the equations F1!

11

(12)

One thing you shouldn't do is to consider the ideal generated byF1 in

R

3 = Qabc1c2s1s2xy] (12) because this will yield unnecessarily high computational complexity and large gb (even though they are not extreme in any way). Since ab are obviously parameters you could instead consider what is generated byF1 in the ring

R

4 = Q(abxy)c1c2s1s2] (13) Thus in Maple we get

> g3 := gbasis( F1, vars2, plex )

2 2 2 2

g3 := 2 a b c2 + a + b - x - y ,

2 2 3 2 2 2

2 b x a s2 + y b - x y - y - y a + (2 x a + 2 y a) s1,

2 2 2 2

2 x a c1 + 2 y a s1 - a - y - x + b ,

2 2 4 2 2 2 2 2 2 4 2 2 4

- 2 x a + b - 2 x b - 2 y b - 2 a b + x + 2 x y + y

2 2 4 2 2 3 3

+ 2 y a + a + (4 a y b - 4 a x y - 4 a y - 4 y a ) s1

2 2 2 2 2

+ (4 y a + 4 x a ) s1 ]

> map( indets, g3 )

{x, a, b, y, c2}, {x, a, b, y, s1, s2}, {x, a, b, y, c1, s1}, {x, a, b, y, s1}]

> map( leadmon, g3, vars2, plex )

2 2 2 2 2

2 a b, c2], 2 b x a, s2], 2 x a, c1], 4 y a + 4 x a , s1 ]]

If you are still not satised with the computational speed, >is there anything you could do to get the \answer" (s1 as an algebraic function of abxy) even faster?

(13)

Yes, you can use the bgk algorithm, i.e. finduni, to determine the minimal polynomial

p fors1.

> p3 := finduni( s1, F1, {op(vars2)} )

2 2 4 2 2 2 2 2 2 4 2 2 4

p3 := - 2 x a + b - 2 x b - 2 y b - 2 a b + x + 2 x y + y

2 2 4 2 2 3 3

+ 2 y a + a + (4 a y b - 4 a x y - 4 a y - 4 y a ) s1

2 2 2 2 2

+ (4 y a + 4 x a ) s1

> # Check that the answer is correct:

> p3-g34]

0

Let us end this part of the exercise with a question of less practical value:

Explain how you can think of the ideal hF1i (in some suitable ring) as a graph ideal.

13

(14)

If we disregard the equations for the trigonometric identities thenI1 is a graph ideal with

yx as tag-variables.

In my opinion a natural thing to do is to regard the quotient ring

Q

1 = R1=hs21+c21;1 s22+c22;1i (14) and the extension ofhx;P1y;P2i to Q1.

But in practice there is no dierence! For if you have an ideal

a = hf1:::fri (15)

in a ringR and want to compute with its extension to a quotient ring

R =b whereb = hh1:::hsi (16) the only thing you do is that you consider the ideal

a+b = hf1:::frh1:::hsi (17) inR!

In algebraic geometry the natural object to consider when you study a mapping

x7!(f1( x):::fr( x)) (18) is the k-algebra kf1:::fr] k x], which is isomorphic to a ring of quotient w.r.t. some graph ideal. Above we have studied, in some sense, the Q-algebra

QP1P2] Qc1c2s1s2] (19)

(15)

3 Constrained Motion

Let us now make the example more interesting by assuming that the robot is only supposed to work on a piece that forms a curve in the plane. Suppose e.g. that the only (xy)-values allowed for are those on the parabola

f(xy) y = x2+ 1g (20)

>How do you incorporate this in the model?

(Again we let a=b= 1 for simplicity.)

Answer: Add the polynomial y;x2;1 to the setF2:

> F3 := { op(F2), y-x^2-1 }

2 2 2 2

F3 := {s2 + c2 - 1, s1 + c1 - 1, x - c1 - c1 c2 + s1 s2, y - x - 1, y - s1 - s1 c2 - c1 s2}2

>What, do you think, is the dimension ofI3 generated byF3 inR1 (R1 given in equa- tion (4))?

Check your answer.

15

(16)

The restriction to the parabola should decrease the dimension by 1, so that I3has dimen- sion 1. That this is also the case can be seen if we treat I3 in a similar way as I1, but we should only have one parameter, i.e. we extendI3 to

R

5 = Q(x)c1c2s1s2y] (21)

> vars4 :=  c2, s2, y, c1, s1 ]

vars4 := c2, s2, y, c1, s1]

> g4 := gbasis( F3, vars4, plex )

4 2

g4 := 2 c2 - x - 3 x + 1,

4 2 6 2 4

- 4 x - 4 x - x - 1 + (6 x + 2 x + 2) s1 + 2 s2 x,

2 2 4 2

y - x - 1, 2 c1 x - 3 x - x - 1 + (2 x + 2) s1,

2 4 6 8 4 2 6

2 x + 11 x + 6 x + x + 1 + (- 16 x - 16 x - 4 x - 4) s1

2 4 2

+ (12 x + 4 x + 4) s1 ]

> map( leadmon, g4, vars4, plex )

2 4 2

2, c2], 2 x, s2], 1, y], 2 x, c1], 12 x + 4 x + 4, s1 ]]

Again, the extension is zero-dimensional and in g.p. and there are two solutions for every choice ofx.

Finally, we may consider a robot moving on a rail running along the x-axis, so that we have one more degree of freedom again.

Find equations describing the situation above and analyze the dimension of the new ideal.

(17)

Replace x by x+z everywhere and you're done. The eect on the ideal should be (and is) that the dimension increases by one, because you increased the dimension of the ring by one and you didn't add any new equations.

> F4 := expand( subs( x=x+z, F3 ) )

2 2 2 2 2 2

F4 := {s2 + c2 - 1, s1 + c1 - 1, y - x - 2 x z - z - 1, x + z - c1 - c1 c2 + s1 s2, y - s1 - s1 c2 - c1 s2}

> g5 := gbasis( F4, vars4, plex ):

> map( leadmon, g5, vars4, plex )

2, c2], 2 x + 2 z, s2], 1, y], 2 x + 2 z, c1],

2 2 4 3 2 2 3

12 x + 24 x z + 12 z + 4 x + 16 x z + 24 x z + 4 + 16 x z

4 2

+ 4 z , s1 ]

]

Above, g5 is the gb for the idealI4 generated by F4 in the ring

R

6 = Q(xz)c1c2s1s2y] (22) SinceI4is zero-dimensional (it is also in gp) the original ideal has dimension 2, as we guessed.

The end.

17

(18)

References

1] J.F. Canny. The Complexity of Robot Motion Planning. MIT Press, 1988.

2] B. Char, K.O. Geddes, G.H. Gonnet, B.L. Leong, M.B. Monagan, and S.M. Watt. Maple V Language Reference Manual. Springer, 1991.

3] D. Cox, J. Little, and D. O'Shea. Ideals, Varieties, and Algorithms: An Introduction to Computational Algebraic Geometry and Commutative Algebra. Undergraduate Texts in Mathematics. Springer, 1992.

4] K. Forsman. A short Maple primer. Technical Report LiTH-ISY-I-1340, Dept. of Electrical Engineering, Linkoping University, S-581 83 Linkoping, Sweden, March 1992. Included in the Maple share library. Also available by anonymous ftp 130.236.24.1.

5] K. Forsman. Elementary aspects of constructive commutative algebra. second version.

Technical Report LiTH-ISY-I-1395, Dept. of Electrical Engineering, Linkoping University, S-581 83 Linkoping, Sweden, December 1993. Available by anonymous ftp130.236.24.1.

6] M. Raghavan. Manipulator kinematics. In J. Baillieul et al., editor, Robotics, volume 41 of Proceedings of symposia in applied mathematics, pages 21{47. American Mathematical Society, Providence, Rhode Island, 1990. Lecture notes from a short course held in Louisville, Kentucky, January 1990.

7] M.W. Spong and M. Vidyasagar. Robot Dynamics and Control. Wiley, 1989.

References

Related documents

Using the Bayesian approach to the estimation problem, the probability density function of the position in the map conditioned on the measurements gathered, is updated re-

Department of Management and Engineering Link¨oping University, SE-581 83, Link¨oping, Sweden. Link¨oping,

Keywords: Grobner bases, elimination, commutative algebra, localization, linear algebra, remainders, characteristic sets, zero-dimensional ideals.. 1 Introduction

In this picture MD i denotes a modeling domain, ID j denotes an implementation domain and PDS denotes polynomial dy- namical systems (over finite fields).... In figure 2 we receive

From a control theory users point of view, tempo- ral algebra offers a straight forward way of translat- ing informal verbal specification into a formal alge- braic specification

Our team represents the student association FIA Robotics, the Division for Artificial Intelligence and Integrated Computer Systems (AIICS) at the Department of Computer Science

Division of Communication Systems Department of Electrical Engineering (ISY) Link¨ oping University, SE-581 83 Link¨ oping,

Computer Vision Laboratory Department of Electrical Engineering. Link¨ oping University SE-581 83 Link¨