Robot Geometry

Taught by: Dr. Carl Crane
Taken: FS 2015
Text: Kinematic Analysis of Robot Manipulators
by: C. Crane and J. Duffy

Table of Contents

Coordinate Systems

Coordinate System A and Nomenclature
Coordinate System A and Nomenclature

Homogeneous Coordinates

Coordinate Transformation

Rotation Matrix and Example

Transformation Matrix

Transformation Matrix Definition
Reverse Transformation Matrix Definition

Rotation Around a General Vector Passing Through the Origin

Figure taken from the book
General Rotation Transformation Matrix

Formula Derivation

  • It is desired to find the rotation matrix from coordinate system A to coordinate system B after a rotation \theta about a generic vector \underline{m}
  • To begin consider a coordinate system C whose z-axis is along the vector \underline{m} (the location of the x and y vectors will be arbitrary)
  • The rotation matrix from C to A can then be defined as ^{A}_{C}R = \left[  \begin{array} a_{x} & b_{x} & m_{x} \\ a_{y} & b_{y} & m_{y} \\ a_{z} &  b_{z} & m_{z} \end{array} \right]
  • Now consider coordinate system D which is obtained by rotating coordinate system C about its z-axis by \theta degrees
  • The rotation matrix from D to C would then be defined as ^{C}_{D}R =  \left[ \begin{array} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) &  cos(\theta) & 0 \\ 0 & 0 & 1 \end{array} \right]
  • Taking note that coordinate system B will have the same relation to coordinate system D as coordinate system C has to coordinate system A, the relation ^{A}_{C}R = (^{B}_{D}R) exists
  • The desired rotation matrix can now be found by a series of matrix multiplications: ^{A}_{B}R = (^{A}_{C}R) * (^{C}_{D}R) * (^{D}_{B}R)
General Rotation Transformation Matrix Unsimplified
  • Unfortunately we do not have values for a_{x}, a_{y}, a_{z} or b_{x},  b_{y} and b_{z} because we chose the x and y axes for coordinate system C arbitrarily and so we will need to find a way to cancel them out of the resultant matrix
  • In order to simplify the terms in the rotation matrix ^{A}_{B}R we will use the geometric meanings of the rotation matrix with matrix ^{A}_{C}R
  • First each row in a rotation matrix represents a unit vector and so the dot product of one of the rows with itself equals one (using matrix ^{A}_{C}R)
    • a_{x}^{2} + b_{x}^{2} + m_{x}^{2} = 1
    • a_{x}^{2} + b_{x}^{2} = 1 - m_{x}^{2}
    • This rule should simplify all of the terms along the diagonal of rotation matrix ^{A}_{B}R to the form c(1-m_{i}^{2}) + m_{i}^{2}    or    c + m_{i}^{2}(1-c)
  • The second geometric meaning of the rotation matrix that is going to be utilized is that all of the rows in the matrix represent vectors that are orthogonal to one another and therefore their dot product will equal zero
    • a_{X}a_{y} + b_{x}b_{y} + m_{x}m_{y} = 0
    • a_{X}a_{y} + b_{x}b_{y} = -m_{x}m_{y}
  • The last geometric meaning of the rotation matrix that we are going to use is that the columns of the matrix represent vectors chosen using the right hand rule and therefore the cross product of the first two columns equals the third column
    • (a_{y}b_{z}-a_{z}b_{y})\hat{\imath} +  (a_{x}b_{z}-a_{z}b_{x})\hat{\jmath} + (a_{x}b_{y}-a_{y}b_{x})\hat{k} = m_{x}\hat{\imath} + m_{y}\hat{\jmath} + m_{z}\hat{k}
  • Using the second two rules the formula for the rest of rotation matrix ^{A}_{B}R can be simplified to the general form shown at the beginning of this section

Common Rotations

  • Rotation \alpha around the x-axis: ^{A}_{B}R = \left[ \begin{array} 1  & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha)  \end{array} \right]
  • Rotation \beta around the y-axis: ^{A}_{B}R = \left[ \begin{array}  cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta)  \end{array} \right]
  • Rotation \gamma around the z-axis: ^{A}_{B}R = \left[ \begin{array}  cos(\gamma) & -sin(\gamma) & 0 \\ sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 &  1 \end{array} \right]

Find Vector and Rotation Angle from the Rotation Matrix

  • For the rotation matrix about a general vector passing through the origin, using the notation ^{A}_{B}R = \left[ \begin{array} r_{11} & r_{12} &  r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}  \right]
  • Adding all of the elements in the diagonal gives r_{11} + r_{22} + r_{33}  = m_{x}^{2}v + c + m_{y}^{2}v + c + m_{z}^{2}v + c where vector m is a unit vector (m_{x}^{2} + m_{y}^{2} + m_{z}^{2} = 1)
    • r_{11} + r_{22} + r_{33} = (m_{x}^{2} + m_{y}^{2} + m_{z}^{2})v + 3c
    • r_{11} + r_{22} + r_{33} = v - 3c = (1 - cos(\theta)) + 3cos(\theta)
    • r_{11} + r_{22} + r_{33} = 1 + 2 cos(\theta)
  • To find the values of the vector m use the equation of the form r_{ji} -  r_{ij} = m_{i}m_{j}v + m_{k}s - (m_{j}m_{i}v - m_{k}s) for i \neq j \neq  k and i < j
    • This simplifies to r_{ji} - r_{ij} = 2m_{k}sin(\theta)
    • Example r_{21} - r_{12} = 2m_{z}sin(\theta)
    • Exception is r_{13} - r_{31} = 2m_{y}sin\theta
Physical to Kinematic Link
Standard Link Coordinate System

Joints

Revolute Joint, R

Revolute Joint

Prismatic Joint, P

Prismatic Joint

Cylindrical Joint, C

Cylindric Joint

Screw Joint, H

Screw Joint

Plane Joint, E

Plane Joint

Hook Joint, T

Hook Joint

Spherical Joint, S

Spherical Joint

Kinematic Chains

Standard Fixed Coordinate System

Kinematic Chain

Forward Analysis

General Link to Link Transformation Matrix

Reverse Analysis

Iterative Technique

Hypothetical Link Technique

Special Case 1: \underline{S}_{7} and \underline{S}_{1} are Parallel

  • As mentioned above, problems are encountered when \alpha_{71} is very close to either 0 or 180 degrees
  • The first step to solving special case one is to set S_{7} equal to zero (S_{7} = 0)
  • The vector loop equation from above therefore simplifies to the following expression
    • ^{F}\underline{P}_{6orig} + a_{71} (^{F}\underline{a}_{71}) + S_{1}  (^{F}\underline{S}_{1}) = \underline{0}
  • Now take the dot product of each term in the vector loop equation with vector ^{F}\underline{S}_{1}
    • ^{F}\underline{P}_{6orig} \cdot ^{F}\underline{S}_{1} + a_{71}  (^{F}\underline{a}_{71} \cdot ^{F}\underline{S}_{1}) + S_{1}  (^{F}\underline{S}_{1} \cdot ^{F}\underline{S}_{1}) = \underline{0}
    • The vectors ^{F}\underline{a}_{71} and ^{F}\underline{S}_{1} are perpendicular and so their dot product is zero and therefore the middle term goes away
    • The dot product of vector ^{F}\underline{S}_{1} with itself is one and so the vectors drop out of the last term in the expression
  • Simplifying the above expression for the length S_{1} gives the following expression
    • S_{1} = - ^{F}\underline{P}_{6orig} \cdot ^{F}\underline{S}_{1}
  • Now we rearrange the vector loop equation so that the a_{71} terms are all on one side
    • -(^{F}\underline{P}_{6orig} + S_{1} (^{F}\underline{S}_{1})) =  a_{71} (^{F}\underline{a}_{71})
  • From the expression the length can be found by recognizing that it represents the magnitude of the left hand side of the equation
    • a_{71} = \| (^{F}\underline{P}_{6orig} + S_{1}  (^{F}\underline{S}_{1})) \|
    • If a_{71} = 0 then problems arise and the process has to move to special case 2
  • Using the same expression above where the a_{71} terms are isolated, the unit vector ^{F}\underline{a}_{71} can be found by dividing the left hand side of the equation by length a_{71}
    • ^{F}\underline{a}_{71} = \frac{-(^{F}\underline{P}_{6orig} + S_{1}  (^{F}\underline{S}_{1}))}{a_{71}}
  • Now the same series of sin and cos equations as the normal case can be used to find the remaining angles \theta_{7} and \gamma_{1}

Special Case 2: \underline{S}_{7} and \underline{S}_{1} are Parallel and a_{71} = 0

  • If \underline{S}_{7} and \underline{S}_{1} are Parallel and a_{71} =  0 then we set S_{7} = 0 and \theta_{7} = 0
  • This makes ^{F}\underline{a}_{71} = ^{F}\underline{a}_{67}
  • The remaining value to find is the angle \gamma_{1} which is found using sin and cos as it was for the normal case and special case one
Hypothetical Link Flow Chart

Mobility

Planar Representation

Figure From Lecture
Example Planar Representation

Equivalent Spherical Mechanisms

Create Equivalent Spherical Mechanism

figure from lecture
spatial mechanism to spherical mechanism

Spherical Mechanism Groups

Find \mathbf{^{1}\underline{S}_{1}}

Find \mathbf{^{1}\underline{S}_{2}}

Find \mathbf{^{1}\underline{S}_{3}}

Single Subscript Notation

  • As can be imagined the result of the further vectors will become increasingly complicated and so a short hand notation has been developed to simplify the expressions
Single Subscript Notation
  • For single subscript notation three different terms are created (X, Y, Z) and will have different meanings based on their subscript and whether or not they have a bar
    • X \equiv ss_{\theta}
    • Y \equiv -(sc + csc_{\theta})
    • Z \equiv cc - ssc_{\theta}
  • For S_{j}, \theta_{j} the single subscript notation will involve \alpha_{ij} and \alpha_{jk} and the order of the \alpha angles depends on whether or not the X, Y, Z terms have overbars
    • For no bar \alpha_{ij} will be placed next to the \theta_{j} terms (the right side terms) and the angle \alpha_{jk} will fill in the remaining terms
      • Notice \alpha_{ij} is the first \alpha term encountered in the figure when traveling in the no bar direction
    • With bar \alpha_{jk} will be placed next to the \theta_{j} terms (the right side terms) and the angle \alpha_{ij} will fill in the remaining terms
      • Notice \alpha_{jk} is the first \alpha term encountered in the figure when traveling in the bar direction
  • The subscript in single subscript notation indicates which theta is included in its terms

Rewrite \mathbf{^{1}\underline{S}_{3}}

Find \mathbf{^{1}\underline{S}_{4}}

Multi-Subscript Notation

  • Just like before a new notation is going to be introduced in order to make keeping track of the multitude of terms easier
  • The general form for multiple notation is presented below
    • X_{ab} = X_{a} c_{b} - Y_{a} s_{b}
    • Y_{ab} = c_{b,b \rightarrow 1} (X_{a} s_{b} + Y_{a} c_{b}) - s_{b,b  \rightarrow 1}Z_{a}
    • Z_{ab} = s_{b,b \rightarrow 1} (X_{a} s_{b} + Y_{a} c_{b}) + c_{b,b  \rightarrow 1}Z_{a}
  • In the above expressions the subscript a represents all but the last of the subscripts for that variable and b represents the last subscript
    • Ex. X_{432} \rightarrow a = 43, b = 2
    • Ex. 2. Z_{2345} \rightarrow a = 234, b = 5
  • The subscript b, b \rightarrow 1 means that the subscript will be b and what ever the next number will be in the order of the subscripts whether that be ascending or descending
    • Ex. X_{432} \rightarrow (b,b \rightarrow 1) = 21
    • Ex. 2. Z_{2345} \rightarrow (b,b \rightarrow 1) = 56
  • For double subscript notation the order ij means the X, Y, Z terms will not have bars and the order ji means that they will have bars, where i < j

Rewrite \mathbf{^{1}\underline{S}_{4}}

Find \mathbf{^{1}\underline{S}_{5}}, \mathbf{^{1}\underline{S}_{6}} and \mathbf{^{1}\underline{S}_{7}}

Find \mathbf{^{1}\underline{a}_{12}}

Find \mathbf{^{1}\underline{a}_{23}}

Find \mathbf{^{1}\underline{a}_{34}}

Double Subscript Notation for \mathbf{\alpha} Terms

Rewrite \mathbf{^{1}\underline{a}_{34}}

Rewrite \mathbf{^{1}\underline{a}_{45}} Through \mathbf{^{1}\underline{a}_{67}

Spherical Mechanism Equation Toolkit

Example results

  • For a spherical triangle
    • \bar{X}_{2} = s_{31}s_{1}
    • \bar{Y}_{2} = s_{32}c_{1}
    • \bar{Z}_{2} = c_{31}

Solve \mathbf{Ac_{\theta} + Bs_{\theta} + D = 0}

Tan-Half Angle Method

  • This method begins using the tangent half angle trig identity
    • x_{1} = tan \left( \frac{\theta}{2} \right)
    • s_{\theta} = \frac{2x_{1}}{1+x_{1}^{2}}
    • c_{\theta} = \frac{1-x_{1}^{2}}{1+x_{1}^{2}}
  • Substituting these values into the original problem results in the following expression
    • A \left( \frac{1-x_{1}^{2}}{1+x_{1}^{2}} \right) + B \left(  \frac{2x_{1}}{1+x_{1}^{2}} \right) + D = 0
  • After multiplying through by \left( 1 + x_{1}^{2} \right) and regrouping by the x terms the following quadratic equation emerges
    • x_{1}^{2} (D-A) + x_{1} (2B) + (D+A) = 0
    • The quadratic formula can now be used to solve for x which can in turn be used to solve for \theta
  • Potential problem points for this solution process are as follows:
    • The term under the radical can be negative leading to non-real solutions
      • This is a symptom of not having a feasible link configuration and not necessarily a problem with the solution method itself
    • There could be a divide by zero error if D = A
      • This is an issue with the solution method and not the mechanism itself

Trig Solution Method

  • The second solution approach involves a few trig identities in its derivation
  • The first step in the process is to divide each term in the equation by the root of A^{2} + B^{2}
    • \frac{A}{\sqrt{A^{2}+B^{2}}} c_{\theta} +  \frac{B}{\sqrt{A^{2}+B^{2}}} s_{\theta} +  \frac{D}{\sqrt{A^{2}+B^{2}}} = 0
  • Now we note that the sum of the first two coefficients squared is equivalent to one due to the fact that it is adding together the components of the denominator
    • \frac{A^{2}}{A^{2}+B^{2}} + \frac{B^{2}}{A^{2}+B^{2}} = 1
  • The next step is to turn the above fractions into sine and cosine by keeping in mind the following trig identity
    • (s_{\gamma})^{2} + (c_{\gamma})^{2} = 1
  • We can therefore rewrite the fractions as follows
    • \frac{A^{2}}{A^{2}+B^{2}} = (c_{\gamma})^{2}
    • \frac{B^{2}}{A^{2}+B^{2}} = (s_{\gamma})^{2}
    • These definitions are used to determine the value of angle \gamma
  • Using these new terms the original equation can be rewritten as follows
    • c_{\gamma}c_{\theta} + s_{\gamma}s_{\theta} +  \frac{D}{\sqrt{A^{2}+B^{2}}} = 0
    • Note that the angle \gamma has not much of a physical meaning but is just a term used in the solution process
  • By moving the non-trig term to the other side of the equation the remaining trig terms form the difference between two angles identity
    • c_{\gamma}c_{\theta} + s_{\gamma}s_{\theta} = -  \frac{D}{\sqrt{A^{2}+B^{2}}} = cos(\theta - \gamma)
  • Now arccosine can be used to determine what the difference in the two angles are
    • \theta - \gamma = cos^{-1} \left( - \frac{D}{\sqrt{A^{2}+B^{2}}}  \right)
    • If the link lengths are unfeasible then this is the step that will encounter problems as the argument in cos^{-1} will not be between -1 and 1
  • This will provide an A case and a B case and the corresponding \theta values can be found by the following
    • \theta_{A} = (\theta - \gamma)_{A} + \gamma
    • \theta_{B} = (\theta - \gamma)_{B} + \gamma
  • This is considered the better solution approach as it only encounters problems for infeasible link lengths rather than also having a problem inherent to the solution approach

Spherical Pentagon Example

Spherical Pentagon Example

Group One Solution Process

Group Two Solution Process

Form System of Two Equations

Projection of Vector Loop Equation

Self-Scalar Product of Vector Loop Equation

Secondary Cosine Laws

Dual Numbers

Dual Angles

Solve \mathbf{x^{2}_{3} \left( a_{i}x^{2}_{1} + b_{i}x_{1} + d_{i}  \right)} \mathbf{+ x_{3} \left( e_{i}x^{2}_{1} + f_{i}x_{1} + g_{i}  \right)} \mathbf{+ \left( h_{i}x^{2}_{1} + i_{i}x_{1} + j_{i} \right) = 0,  \;\; i = 1,2}

Sylvester's Method

Bezout's Method