Points, Vectors, Geometr, Algebra a A b v B Points and vectors in geometr and algebra The notion point in geometr is considered here as well known and as its definition is not mathematicall simple, I will not define it here. Let us emphasie that it should be considered as place in the space, which is independent on an numbers. The term vector in geometr we can define as ordered pair of points, which are called initial and terminal point of the vector. We denote vector as AB. We can use the phrases in geometr as point A is intersection of circle k with the center in point B and containing point C and line passing through points D and E. The problems in robotics are strictl speaking geometrical problems (at least in kinematics). We need to represent the geometr of robots in computer to be able to control robots. Unfortunatel the digital computer cannot handle geometr directl. Mathematical theor of linear spaces allows us to handle mathematical objects like ordered n-tuple of numbers. It is possible to show that structure preserving mapping (isomorphism) between geometrical and algebraic objects eists for Euclidean geometr and algebraic linear spaces of the finite dimension. To introduce such isomorphism, we need to introduce coordinate sstem. Geometrical point in space could be then made identical to the ordered triple of real numbers. This triple is usuall called vector, but this time being the algebraic term. The real numbers in the triple mean coordinates of geometrical point in the coordinate sstem. The radius vector in geometr could define the position of the point in the space. The initial point of the radius vector is in the origin of the coordinate sstem, the final point of the radius vector is the represented geometrical point. (Algebraic) linear space thus makes universal model of geometrical space and using the linear space we can represent all properties of geometrical space and vice versa. Considering the geometrical terms as primar can be justified b the historical fact, that geometr was cultivated b mathematicians ears without introducing coordinates. The eistence of isomorphism demonstrates that both descriptions of the realit are (mathematicall) equivalent and we can switch between them arbitraril. We can define between points and vectors in geometr several operators: two points define vector v = AB, point and vector define point A + v = B vectors could be added and subtracted b = a + v These operations hold without introducing coordinate sstem. The same operations could be introduced in algebra between ordered triples (n-tuples) representing points and vectors. The formulas remain the same, the meaning of the objects in formulas differs. Algebraic objects in formulas correctl represent geometrical objects if the coordinates of RBTICS: Vladimír Smutný Slide, Page
the geometrical objects are in the same coordinate sstem. Further holds that coordinate sstem can be chosen arbitraril. Form of the algebraic description will be for an choice of coordinate sstem same, the numbers in equations will depend on the choice of the coordinage sstem. Situation with several coordinate sstems will be described later. RBTICS: Vladimír Smutný Slide, Page
Technical intermeo C B u β α γ A v α The common approach for solving problems of determining parameters of triangle is to use e.g. Law of Cosine or Law of Sine. Such laws, sum of inner angles and so on, are difficult to appl in analtical geometr. The problem is that found solutions should be interpreted for different quadrants, new solutions shall be constructed or found solutions tested for satisfing input conditions. The reason is, that the orientation of angles cannot be determined from such computations and interpretation of results is difficult. I recommend to avoid such approaches as much as possible and use the following reliable tools. Tr to calculate coordinates of corners instead of length of sides or sie of angles. Use φ = atan(, ) for determining angle as much as possible. Avoid arccos as much as possible. When calculating angles and coordinates, strictl use oriented versions of them. The can be then sumed and subtracted without analsis of particular situations. If ou need analtical equation of line, use the form a + b + c =, which unlike = k + q works in all quadrants. See the calculation of the angle between ais and AB vector (see figure above). The oriented, four quadrant angle α could be calculated as α = atan(b A, B A ). The safest and easiest wa, how to calculate the oriented angle β between vectors v and u is the following: α = atan(b A, B A ), () γ = atan(c B, C B ), () β = γ α. () RBTICS: Vladimír Smutný Slide, Page
Technical intermeo II C u A w α δ v B β α γ Let us have given the coordinates of points A and C and lengths of vectors v and u. The safest wa how to determine the point B and associated angles, is not to fight via Law of Cosine with the angle δ but to calculate the intersection of circles with the centers in A and C and appropriate radii. This gives two solutions for point B. The angles could be then determined b the above equations. RBTICS: Vladimír Smutný Slide, Page
Bod in the coordinate sstem and its motion The rigid bod in a plane has degree of freedom. The same bod in D space has degree of freedom: Questions: How man DF has a rubber tape? Rigid bod in this course we alwas consider links and manipulated objects as rigid bodies. We can attach the coordinate sstem to the rigid bod and position of the individual points on the rigid bod is then unique and supposed to be known apriori, e.g. from CAD drawings of the bod. Actual bod position in time could be described b the position of the attached coordinate sstem in other, nonmoving, world coordinate sstem. Motion of the bod in time could be described as a actual position of the bod as a function of time. Relative position of two coordinates sstem can alwas be decomposed into the translation and rotation. Let the coordinate sstem of a base is. The coordinate sstem of a bod is b b b. The description of a coordinate sstem b b b in coordinate sstem of a base is: = o = o o o (n, t, b). Let us form a matri R = (n, t, b), n, t, b are unit and orthogonal vectors, then a matri R is ortonormal, that is R = R T. RBTICS: Vladimír Smutný Slide, Page
Description of Bod Position Point in D - described b three coordinates. Rigid bod in D - described b coordinates: coordinates of reference point t = orientation could be described e.g. b: coordinates of vectors attached to the bod (n, t, b), Euler angles (φ, θ, ψ), rotational matri R, ais angle, quaternions, rotation vector. Coordinates of reference point and rotation matri could be combined into transformation matri., Rotation matri see Eric W. Weisstein. Rotation Matri. From MathWorld A Wolfram Web Resource. http://mathworld.wolfram.com/rotationmatri.html For conversion among different descriptions see http://www.euclideanspace.com/maths/geometr /rotations/conversions/inde.htm Pa attention to the definitions used so ou do mi different definitions from different sources. RBTICS: Vladimír Smutný Slide, Page
Ais Angle, Quaternions, Rotation Vector S p r s P θ r P Euler s rotation theorem states than an rotation in D could be represented as a single rotation around a certain ais. We can describe this ais as s and angle of the rotation as θ. This pair (s, θ) could represent rotation and is called ais angle. Quaternions can be epressed as: q = (cos(θ/), sin(θ/)s T ) = (cos(θ/), sin(θ/)s, sin(θ/)s, sin(θ/)s ) Rotation vector uses the fact, that vector s is normalied and has onl DF, so one can epress rotation using three numbers v = (θs). Rodrigues rotation formula: / r = r cos θ + (s r ) sin θ + s(s r )( cos θ) R = I cos θ + [s] sin θ + ss T ( cos θ) Quaternions have various properties, among them: all four components are equal important, unlike Euler angles, q and q describe same rotation, it is relativel eas to interpolate the rotation in quaternion parametrisation. The task to rotate smoothl (e.g. manipulate in robotics or displa in computer graphics) could be easil achieved using quaternions. The method is called spherical linear interpolation (SLERP). We interpolate from q to q getting q and having interpolating parameter t, the angle α is the amount of rotation needed (angle between quaternions interpreted as four dimensional vectors is half of it, absolute value assures the shorter rotation): For details about quaternions see Eric W. Weisstein. Quaternion. From MathWorld A Wolfram Web Resource. http://mathworld.wolfram.com/quaternion.html Interpolation in ais-angle sstem is rotation around given ais, where angle changes linearl from to θ. The rotation vector is nonredundant and has good topolog, so it is heavil used e.g. in computer vision for rotation estimation. Good topolog means here that rotation vectors which are closed to each other (their difference is small) represent rotations which differ onl little. This hols also near ero rotation vector. Rodrigues rotation formula allow to calculate vector r or equivalentl point P when rotating around ais s b an angle θ. The modified Rodriques formula allows to calculate easil rotation matri from ais angle representation. The inverse transformation is: q = (q q )t q, () q = sin(( t)α) q + sin(tα) sin(α) sin(α) q, () cos(α/) = q q, () t <, >. () ( ) trace(r ) θ = arccos () r, r, s = r, r, () sin θ r, r, RBTICS: Vladimír Smutný Slide, Page
Definition of Euler angles precession nutation rotation Euler angles The matri R has nine coeficients, but its dimension is onl three, the bounding condition is unit sie and orthogonalit of vectors n, t, b: n T t = t T b = b T n = n = t = b = The matri R can be contructed easil using Euler angles. Rotate the coordinate sstem around the ais b the angle φ. We will get.. Rotate the coordinate sstem around the ais b the angle θ. We will get.. Rotate the coordinate sstem around the ais b the angle ψ. We will get b b b. R = R (φ)r (θ)r (ψ) R = R (φ) = R (θ) = R (ψ) = cos φ sin φ sin φ cos φ cos θ sin θ sin θ cos θ cos ψ sin ψ sin ψ cos ψ () () () [ cos φ cos ψ cos θ sin φ sin ψ cos θ cos ψ sin φ cos φ sin ψ sin φ sin θ cos ψ sin φ + cos φ cos θ sin ψ cos φ cos θ cos ψ sin φ sin ψ cos φ sin θ sin θ sin ψ cos ψ sin θ cos θ () Euler angles define uniquel the rotation, the same rotation can be reached b different triples of angles. ther definitions like roll pitch aw define similar angles with similar properties but with different equations. If the matri R is given, Euler angles can be calculated b the comparison of the matri coeficients r, r, r. ] RBTICS: Vladimír Smutný Slide, Page
Rotation Matri Resulting from Euler Angles Euler angles according definition used here (Asada, Slotine): cos ϕ cos ψ cos ϑ sin ϕ sin ψ cos ϑ cos ψ sin ϕ cos ϕ sin ψ sin ϑ sin ϕ cos ψ sin ϕ + cos ϑ cos ϕ sin ψ cos ϑ cos ϕ cos ψ sin ϕ sin ψ cos ϕ sin ϑ sin ϑ sin ψ cos ψ sin ϑ cos ϑ / Rotation matri based on Yaw, Pitch, Roll used in CRS robot, that is rotation around, then, then : cos α cos β cos α sin β sin γ cos γ sin α cos α cos γ sin β + sin α sin γ cos β sin α cos α cos γ + sin α sin β sin γ cos γ sin α sin β cos α sin γ sin β cos β sin γ cos β cos γ How to calculate Euler angles when rotation matri is known Let us have the known matri R ( ) and smbolic matri composed of three rotations defined b three angles. ur task is to find those angles. The smbolic matri has for three consecutive rotations around perpendicular aes special form, similar to the form on the above slide. So one has to solve the equation similar (not necessar indentical) to () with three uknowns φ, θ, ψ. First one has to find a pivot element in the smbolic matri which is function of just one variable (monom). This element (e.g. element in the third row and third column in the eample) is in the form either ± cos or ± sin. This can be directl compared to the corresponding element in the known matri and angle calculated. Note that generall there are two solutions in the each interval of length π. When first angle is calculated, other angles could be calculated using atan function from elements on the pivot s row and column. ne has to consider the case when the corresponding element in the constant matri R has value close to ±. This leads to degenerate case, where in each interval of the length π is onl one solution. The second problem is that the rest of the elements in the pivot s column and row are for this degenerate case, so one cannot calculate other unknown angles from other elements in pivot s row and column. When the angle calculated from monom is inserted to the submatri of the smbolic matri after removing pivot s column and row, the formulas could be simplified and one will find that the submatri is function either of sum or difference of the still unknown angles. When solving smbolicall one will get one dimensional space of solutions. When solving numericall, one can either fi one angle (e.g. to ) and calculate the other. In certain situations, one has to find other constraints for the solution. In robotics this situation indicates singular point and one can use additional information e.g where the robot arm was before or when it should move to preserve continuit of the trajector. Another aspect of the numerical solution is error caused b rounding in measurement or calculations. This can easil produce for eample the element of the known matri to be larger than, which shall be handled appropriatel. RBTICS: Vladimír Smutný Slide, Page
ther Sstems of Three Angles RBTICS: Vladimír Smutný Slide, Page
Comparison of Rotation Descriptions Sstem Smbol Equivalent Pars Conditions Rotation matri R orthonormal Vectors of aes n, t, b R unit vectors, orthogonal Euler angles φ, θ, ψ aw, pitch, roll,... Ais, angle s, θ unit vector Quaternion q ais, angle unit vector Rotation vector v ais, angle Sstem Advantages Disadvantages Used b R good for calculations redundant Matlab toolbo n, t, b human understandable redundant φ, θ, ψ nonredundant complicated topolog Mitsubishi Staubli, CRS s, θ human understandable redundant eas interpolation q eas interpolation redundant ABB v good topolog rotation estimation nonredundant RBTICS: Vladimír Smutný Slide, Page
Coordinate transformation p t b n P p u A t w v B We know the coordinates of a point P in the coordinate u sstem : P = p = v w and we are looking for itscoordinates in coordinate sstem : P = p =. Lower right inde refers usuall to the coordinate sstem, to which the element relates, upper right inde refers to the coordinate sstem in which the element is described. So origin of the coordinate sstem (point) has coordinates in coordinate sstem : = (,, ) T but in the coordinate sstem it has coordinates = t. Geometricall: Algebraicall: Rewritten: P = Inverse transform: + A + AB + BP. P = p = t + un + vt + wb. p = t + Rp. p = R T t + R T p. RBTICS: Vladimír Smutný Slide, Page
Homogeneous Coordinates Pe Ph Homogeneous coordinates Let us define homogenneous coordinates: Euclidean -metric Homogeneous - projective = = = /w /w /w does not eist (point at infinit) = w = w It is possible to show, that where A is a matri : Inverse matri: A = A = = A b, [ R t ] [ R T R T t ] RBTICS: Vladimír Smutný Slide, Page
Consecutive coordinate transformations P a b pa p b p b c c a a t a b b t c c c Consecutive coordinate transformations in euclidean coordinates and in homogenneous coordinates: P a = p a a = A a b A b cp c c = A a b A b cp c P a = p a a = t a + R a b p b b = t a + R a b (t b + R b cp c c) P = A A A A... A n n P n. () RBTICS: Vladimír Smutný Slide, Page