Robotic Mechanics and Modeling/Kinematics

Introduction to Kinematics for Robotic Mechanics and Modeling (Portion from Link)

edit
 
Inverse kinematics of SCARA robot done with MeKin2D.

Kinematics is the science and math of describing the configuration, position, velocity, and acceleration of an object or a machine. We ignore the dynamics or changes in velocity/acceleration due to inertia of the machine. We also ignore internal or external forces that may be acting on the machine.

Robot kinematics applies geometry to the study of the movement of multi-degree of freedom kinematic chains that form the structure of robotic systems.[1][2] The emphasis on geometry means that the links of the robot are modeled as rigid bodies and its joints are assumed to provide pure rotation or translation.

Robot kinematics studies the relationship between the dimensions and connectivity of kinematic chains and the position, velocity and acceleration of each of the links in the robotic system, in order to plan and control movement and to compute actuator forces and torques.

Motion along a Line (1-D)

edit

Position in 1-D

edit

In 1-D, we can imagine the position of a particle along a line given as  , and this position is not subject to rotation but can only move along the line (translate) to a new position  .

Velocity in 1-D

edit

In 1-D, we can describe the change in position   (a translation) over time   as a velocity   (a vector with speed and direction) without accounting for rotation and angular velocity:

 

A familiar expression for describing position motion from   to   as a function of time with constant speed   is:

 

Acceleration in 1-D

edit

In 1-D, we can describe the change in velocity   (a translation) over time   as an acceleration   (a vector with speed and direction) without accounting for rotation, angular velocity, or acceleration:

 

Motion from   to   as a function of time with constant speed   and constant acceleration   is:

 

Example for 1-D Motion in IPython (JupyterLab)

edit

An example of how to setup a symbolic expression   in IPython.

Code
%reset -f
from sympy import symbols
x_f,x_i,v_c,a_c,t = symbols('x_f x_i v_c a_c t')
x_f=x_i+v_c*t+1/2*a_c*t*t
x_f

If  ,  , and  , what is   after 1 second?

Code
x_f=x_f.subs(x_i,0)
x_f=x_f.subs(v_c,1)
x_f=x_f.subs(a_c,1)
x_f=x_f.subs(t,1)
x_f

The answer is 1.5  .

Additional Examples of 1-D Kinematics

edit

Additional Examples of 1-D Kinematics

Kinematics of a particle trajectory in a non-rotating frame of reference (Copied and edited from Kinematics[3])

edit
 
Kinematic quantities of a classical particle: mass m, position r, velocity v, acceleration a.

Representation of Vectors and Paths

edit

Particle kinematics is the study of the trajectory of particles. The position of a particle is defined as the coordinate vector from the origin of a coordinate frame to the particle. In the most general case, a three-dimensional coordinate system is used to define the position of a particle. However, if the particle is constrained to move within a plane, a two-dimensional coordinate system is sufficient. All observations in physics are incomplete without being described with respect to a reference frame.

The position vector of a particle is a vector drawn from the origin of the reference frame to the particle. It expresses both the distance of the point from the origin and its direction from the origin. In three dimensions, the position of point P can be expressed as

 

where  ,  , and   are the Cartesian coordinates and  ,   and   are the unit vectors along the  ,  , and   coordinate axes, respectively. The magnitude of the position vector   gives the distance between the point   and the origin.

 

The direction cosines of the position vector provide a quantitative measure of direction. It is important to note that the position vector of a particle isn't unique. The position vector of a given particle is different relative to different frames of reference.

The trajectory of a particle is a vector function of time,  , which defines the curve traced by the moving particle, given by

 

where the coordinates xP, yP, and zP are each functions of time.

An Example for Representing a Position Vector in IPython

edit

An example of how to define a "vector"   as an array in IPython.

Code
%reset -f
import numpy as np
P=np.array([1,2,3])

An example of how to calculate the magnitude of vector  .

Code
MagnP=np.linalg.norm(P) #one approach
MagnP=np.dot(P,P)**0.5 #another approach

An example of how to plot the vector  .

Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

P=np.array([1,2,3])
# We don't know where P starts. We can place it at the origin
# and think of P as having U,V,W.
Porigin=np.array([0,0,0,P[0],P[1],P[2]])
print(Porigin)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(Porigin[0],Porigin[1],Porigin[2],Porigin[3],
          Porigin[4],Porigin[5],pivot='tail',normalize=False)
ax.set_xlim([-3, 3])
ax.set_ylim([-3, 3])
ax.set_zlim([-3, 3])

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.view_init(20,-45)
plt.savefig('vector123in3D.png', bbox_inches='tight')
 
3D representation of Vector  .

An Example of Representing a 2D Path in IPython

edit

Let's represent a trajectory with vector path  , where   and  . Then, we can use the following code to represent this path of this trajectory.[4]

Code
# See https://matplotlib.org/3.1.1/tutorials/introductory/
# pyplot.html#sphx-glr-tutorials-introductory-pyplot-py
# for instructions on 2D plotting
%reset -f
import numpy as np
import matplotlib.pyplot as plt

def x_p(t):
    return t

def y_p(t):
    return np.exp(-t) * np.cos(2*np.pi*t)

t1=np.arange(0,5,0.1)
t2=np.arange(0,5,0.02)

plt.figure()
plt.plot(x_p(t1),y_p(t1),'bo',x_p(t2),y_p(t2),'k')
plt.title('2D Path of $P(t)$',size=20)
plt.xlabel('$x_p(t)$')
plt.ylabel('$y_p(t)$')

# You can save a png of the produced plot by pressing 
# shift + right click.

plt.savefig('Path_xp_yp.png', dpi=300, bbox_inches='tight')
 
2D Path of   for   and  .

Additional Examples for Vectors

edit

Additional Examples for Vectors

 
The distance travelled is always greater than or equal to the displacement.

Velocity and speed

edit

The velocity of a particle is a vector quantity that describes the magnitude as well as direction of motion of the particle. More mathematically, the rate of change of the position vector of a point, with respect to time is the velocity of the point. Consider the ratio formed by dividing the difference of two positions of a particle by the time interval. This ratio is called the average velocity over that time interval and is defined as Velocity=displacement/time taken

 

where ΔP is the change in the position vector over the time interval Δt.

In the limit as the time interval Δt becomes smaller and smaller, the average velocity becomes the time derivative of the position vector,

 

Thus, velocity is the time rate of change of position of a point, and the dot denotes the derivative of those functions x, y, and z with respect to time. Furthermore, the velocity is tangent to the trajectory of the particle at every position the particle occupies along its path. Note that in a non-rotating frame of reference, the derivatives of the coordinate directions are not considered as their directions and magnitudes are constants.

The speed of an object is the magnitude |V| of its velocity. It is a scalar quantity:

 

where s is the arc-length measured along the trajectory of the particle. This arc-length traveled by a particle over time is a non-decreasing quantity. Hence, ds/dt is non-negative, which implies that speed is also non-negative.

Acceleration

edit

The velocity vector can change in magnitude and in direction or both at once. Hence, the acceleration accounts for both the rate of change of the magnitude of the velocity vector and the rate of change of direction of that vector. The same reasoning used with respect to the position of a particle to define velocity, can be applied to the velocity to define acceleration. The acceleration of a particle is the vector defined by the rate of change of the velocity vector. The average acceleration of a particle over a time interval is defined as the ratio.

 

where ΔV is the difference in the velocity vector and Δt is the time interval.

The acceleration of the particle is the limit of the average acceleration as the time interval approaches zero, which is the time derivative,

 

or

 

Thus, acceleration is the first derivative of the velocity vector and the second derivative of the position vector of that particle. Note that in a non-rotating frame of reference, the derivatives of the coordinate directions are not considered as their directions and magnitudes are constants.

The magnitude of the acceleration of an object is the magnitude |A| of its acceleration vector. It is a scalar quantity:

 

Additional Examples for Velocity and Acceleration

edit

Additional Examples for Velocity and Acceleration

Relative position vector

edit

A relative position vector is a vector that defines the position of one point relative to another. It is the difference in position of the two points. The position of one point A relative to another point B is simply the difference between their positions

 

which is the difference between the components of their position vectors.

If point A has position components  

If point B has position components  

then the position of point A relative to point B is the difference between their components:  

Relative velocity

edit
 
Relative velocities between two particles in classical mechanics.

The velocity of one point relative to another is simply the difference between their velocities

 

which is the difference between the components of their velocities.

If point A has velocity components  

and point B has velocity components  

then the velocity of point A relative to point B is the difference between their components:  

Alternatively, this same result could be obtained by computing the time derivative of the relative position vector RB/A.

In the case where the velocity is close to the speed of light c (generally within 95%), another scheme of relative velocity called rapidity, that depends on the ratio of V to c, is used in special relativity.

Relative acceleration

edit

The acceleration of one point C relative to another point B is simply the difference between their accelerations.

 

which is the difference between the components of their accelerations.

If point C has acceleration components  

and point B has acceleration components  

then the acceleration of point C relative to point B is the difference between their components:  

Alternatively, this same result could be obtained by computing the second time derivative of the relative position vector PB/A.

Rotation Matrix (Copied and edited from Rotation Matrix[5])

edit

One of the key tools used to help in the representation of robotic pose, configuration, and orientation is the rotation matrix. In linear algebra, a rotation matrix is a matrix that is used to perform a rotation in Euclidean space. For example, using the convention below, the matrix

 

rotates points in the xy-plane counterclockwise through an angle θ with respect to the x axis about the origin of a two-dimensional Cartesian coordinate system. To perform the rotation on a plane point with standard coordinates v = (x,y), it should be written as column vector, and multiplied by the matrix R:

 

The examples in this article apply to active rotations of vectors counterclockwise in a right-handed coordinate system (y counterclockwise from x) by pre-multiplication (R on the left). If any one of these is changed (such as rotating axes instead of vectors, a passive transformation), then the inverse of the example matrix should be used, which coincides with its transpose.

Since matrix multiplication has no effect on the zero vector (the coordinates of the origin), rotation matrices describe rotations about the origin. Rotation matrices provide an algebraic description of such rotations, and are used extensively for computations in geometry, physics, and computer graphics. In some literature, the term rotation is generalized to include improper rotations, characterized by orthogonal matrices with determinant −1 (instead of +1). These combine proper rotations with reflections (which invert orientation). In other cases, where reflections are not being considered, the label proper may be dropped. The latter convention is followed in this article.

Rotation matrices are square matrices, with real entries. More specifically, they can be characterized as orthogonal matrices with determinant 1; that is, a square matrix R is a rotation matrix if and only if RT = R−1 and det R = 1. The set of all orthogonal matrices of size n with determinant +1 forms a group known as the special orthogonal group SO(n), one example of which is the rotation group SO(3). The set of all orthogonal matrices of size n with determinant +1 or −1 forms the (general) orthogonal group O(n).

Rotations in Two Dimensions

edit
 
A counterclockwise rotation of a vector through angle θ. The vector is initially aligned with the x-axis.

In two dimensions, the standard rotation matrix has the following form,

 .

This rotates column vectors by means of the following matrix multiplication,

 .

For example, if the vector   is rotated by an angle  , its new coordinates are  

and if the vector   is rotated by an angle  , its new coordinates are  

More generally, the new coordinates (x′, y′) of the point (x, y) after rotation are

 .

The direction of vector rotation is counterclockwise if θ is positive (e.g. 90°), and clockwise if θ is negative (e.g. −90°). Thus the clockwise rotation matrix is found as

 .

The two-dimensional case is the only non-trivial (i.e. not one-dimensional) case where the rotation matrices group is commutative, so that it does not matter in which order multiple rotations are performed. An alternative convention uses rotating axes,[6] and the above matrices also represent a rotation of the axes clockwise through an angle θ.

An Example of Rotating a Vector in Two Dimensions with IPython

edit

We will create a 2D rotation matrix to provide a rotation of   to the vector   at the origin.

Code
# https://matplotlib.org/gallery/images_contours_and_fields/quiver_simple_demo.html#sphx-glr-gallery-images-contours-and-fields-quiver-simple-demo-py
%reset -f
import matplotlib.pyplot as plt
import numpy as np

theta=np.pi/4
R=np.array([[np.cos(theta),-np.sin(theta)],[np.sin(theta),np.cos(theta)]])
print("Rotation Matrix R:")
print(R)

Vector=np.array([[1,0]])
Vector=Vector.T
print("Column Vector V:")
print(Vector)

RotatedVector=np.dot(R,Vector)
print("Rotated Vector:")
print(RotatedVector)

fig=plt.figure()
ax=fig.add_subplot(111)
ax.quiver(0,0,Vector[0],Vector[1],scale=4)
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.quiver(0,0,RotatedVector[0],RotatedVector[1],scale=4,color='r')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
plt.savefig('2D_rotation_45_deg_vector10.png', dpi=300, bbox_inches='tight')
 
Counterclockwise rotation of vector   by   about the origin in two dimensions.


Rotations in Three Dimensions

edit

Basic rotations

edit

A basic rotation (also called elemental rotation) is a rotation about one of the axes of a coordinate system. The following three basic rotation matrices rotate vectors by an angle θ about the x-, y-, or z-axis, in three dimensions, using the right-hand rule—which codifies their alternating signs. (The same matrices can also represent a clockwise rotation of the axes.[nb 1])

 

For column vectors, each of these basic vector rotations appears counterclockwise when the axis about which they occur points toward the observer, the coordinate system is right-handed, and the angle θ is positive. Rz, for instance, would rotate toward the y-axis a vector aligned with the x-axis, as can easily be checked by operating with Rz on the vector (1,0,0):

 

This is similar to the rotation produced by the above-mentioned two-dimensional rotation matrix. See below for alternative conventions which may apparently or actually invert the sense of the rotation produced by these matrices.

General rotations

edit

Other rotation matrices can be obtained from these three using matrix multiplication. For example, the product

 

represents a fixed-body rotation whose yaw, pitch, and roll angles are α, β and γ, respectively. More formally, it is an intrinsic rotation whose Tait–Bryan angles/Euler angles are α, β, γ, about axes z, y, x, respectively. Similarly, the product

 

represents an extrinsic spaced-fixed rotation whose rotation angles are α, β, γ, about axes x, y, z.

These matrices produce the desired effect only if they are used to premultiply column vectors, and (since in general matrix multiplication is not commutative) only if they are applied in the specified order (see Ambiguities for more details).

An Example of a Single Rotation of a Vector in Three Dimensions

edit

Create a 3D rotation matrix to provide a rotation of   about the Z-axis to the vector   at the origin.

Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np


theta=np.pi/4
Rz=np.array([[np.cos(theta),-np.sin(theta),0],
            [np.sin(theta),np.cos(theta),0], 
            [0,0,1]])
print("Rotation Matrix Rz:")
print(Rz)

Vector=np.array([[1,0,0]])
Vector=Vector.T
print("Column Vector V:")
print(Vector)

Rv=np.dot(Rz,Vector)
print("Rotated Vector:")
print(Rv)

MagnVector=np.dot(Vector.T,Vector)**0.5
MagnRv=np.dot(Rv.T,Rv)**0.5
print("Magnitude of the original vector:",MagnVector[0][0])
print("Magnitude of the rotated vector:",MagnRv[0][0])

VectorOrigin=np.array([0,0,0,Vector[0],Vector[1],Vector[2]])
RvOrigin=np.array([0,0,0,Rv[0],Rv[1],Rv[2]])

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(VectorOrigin[0],VectorOrigin[1],VectorOrigin[2],
          VectorOrigin[3],VectorOrigin[4],VectorOrigin[5],
          pivot='tail',normalize=False)
ax.quiver(RvOrigin[0],RvOrigin[1],RvOrigin[2],
          RvOrigin[3],RvOrigin[4],RvOrigin[5],
          pivot='tail',color='red',normalize=False)
ax.set_xlim([0, 1])
ax.set_ylim([0, 1])
ax.set_zlim([0, 1])

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.view_init(30,-115)
plt.savefig('RotatedVector100in3D.png', bbox_inches='tight')
 
A vector   rotated by   about the Z-axis.

An Example of Successive XYZ Space-fixed Rotations of a Vector in Three Dimensions

edit

Successive XYZ space-fixed rotations of vector   at the origin of   about X, then about Y, and then about Z.

Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np


alpha=90*np.pi/180 #Space-fixed rotation (x)
beta=90*np.pi/180 #Space-fixed rotation (y)
gamma=90*np.pi/180 #Spac-fixed rotation (z)

Rx=np.array([[1,0,0],
             [0,np.cos(alpha),-np.sin(alpha)],
             [0,np.sin(alpha),np.cos(alpha)]])
Ry=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
Rz=np.array([[np.cos(gamma),-np.sin(gamma),0],
             [np.sin(gamma),np.cos(gamma),0], 
             [0,0,1]])

V=np.array([[1,0,0]])
V=V.T
print("Column Vector V:")
print(V)

RxV=np.dot(Rx,V)
RyxV=np.dot(Ry,RxV)
RzyxV=np.dot(Rz,RyxV)


VOrigin=np.array([0,0,0,V[0],V[1],V[2]])
RxVOrigin=np.array([0,0,0,RxV[0],RxV[1],RxV[2]])
RyxVOrigin=np.array([0,0,0,RyxV[0],RyxV[1],RyxV[2]])
RzyxVOrigin=np.array([0,0,0,RzyxV[0],RzyxV[1],RzyxV[2]])

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(VOrigin[0],VOrigin[1],VOrigin[2],
          VOrigin[3],VOrigin[4],VOrigin[5],
          pivot='tail',normalize=False)
ax.quiver(RxVOrigin[0],RxVOrigin[1],RxVOrigin[2],
          RxVOrigin[3],RxVOrigin[4],RxVOrigin[5],
          pivot='tail',color='red',normalize=False)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('Original Vector and RxV(Red) Overalap')
ax.view_init(30,-115)
plt.savefig('XYZSpaceFixedRotations100X.png',bbox_inches='tight',dpi=300)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(RxVOrigin[0],RxVOrigin[1],RxVOrigin[2],
          RxVOrigin[3],RxVOrigin[4],RxVOrigin[5],
          pivot='tail',normalize=False)
ax.quiver(RyxVOrigin[0],RyxVOrigin[1],RyxVOrigin[2],
          RyxVOrigin[3],RyxVOrigin[4],RyxVOrigin[5],
          pivot='tail',color='red',normalize=False)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('RxV(Blue) + RyxV(Red)')
ax.view_init(30,-130)
plt.savefig('XYZSpaceFixedRotations100XY.png',bbox_inches='tight',dpi=300)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(RyxVOrigin[0],RyxVOrigin[1],RyxVOrigin[2],
          RyxVOrigin[3],RyxVOrigin[4],RyxVOrigin[5],
          pivot='tail',normalize=False)
ax.quiver(RzyxVOrigin[0],RzyxVOrigin[1],RzyxVOrigin[2],
          RzyxVOrigin[3],RzyxVOrigin[4],RzyxVOrigin[5],
          pivot='tail',color='red',normalize=False)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('RyxV(Blue) and RzyxV(Red) Overlap')
ax.view_init(30,-115)
plt.savefig('XYZSpaceFixedRotations100Final.png',bbox_inches='tight',dpi=300)
 
Original vector   rotated   about the X-axis has the same orientation.


 
Original vector   rotated about X and then Y by   each time.
 
Penultimate and resulting vector with space-fixed XYZ successive rotations.


An Example of Successive ZYX Body-fixed, Euler Rotations of a Vector in Three Dimensions

edit

Successive ZYX space-fixed rotations of vector   at the origin of   about Z, then about Y', and then about X''. The final axes are given by x,y,z.

Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np


alpha=90*np.pi/180 #Yaw for body-fixed rotation (z)
beta=90*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=90*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

V=np.array([[1,0,0]])
V=V.T
print("Column Vector V:")
print(V)

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)
EulerRzyxV=np.dot(EulerRzyx,V)

VOrigin=np.array([0,0,0,V[0],V[1],V[2]])
EulerRzyxVOrigin=np.array([0,0,0,EulerRzyxV[0],EulerRzyxV[1],EulerRzyxV[2]])

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(VOrigin[0],VOrigin[1],VOrigin[2],
          VOrigin[3],VOrigin[4],VOrigin[5],
          pivot='tail',normalize=False)
ax.quiver(EulerRzyxVOrigin[0],EulerRzyxVOrigin[1],EulerRzyxVOrigin[2],
          EulerRzyxVOrigin[3],EulerRzyxVOrigin[4],EulerRzyxVOrigin[5],
          pivot='tail',color='red',normalize=False)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])

ax.set_xlabel('X,-z',size=14)
ax.set_ylabel('Y,-y', size=14)
ax.set_zlabel('Z,-x', size=14)
ax.set_title('Original Vector (Blue) + Euler ZYX')
ax.view_init(30,-115)
plt.savefig('ZYXBodyFixedRotations100.png',dpi=300,bbox_inches='tight')
print(VOrigin[0])
 
Original vector   gone through body-fixed, Euler ZYX rotations of   about each axis.


Additional Examples for Rotations

edit

Additional Examples for Rotations

Properties

edit

For any n-dimensional rotation matrix R acting on n,

  •   (The rotation is an orthogonal matrix)

It follows that:

  •  

A rotation is termed proper if det R = 1, and improper (or a roto-reflection) if det R = –1.

Mathematical Examples

edit
  • The 2 × 2 rotation matrix
 
corresponds to a 90° planar rotation clockwise about the origin.
 
is its inverse, but since its determinant is −1, this is not a proper rotation matrix; it is a reflection across the line 11y = 2x.
  • The 3 × 3 rotation matrix
 
corresponds to a −30° rotation around the x-axis in three-dimensional space.
  • The 3 × 3 rotation matrix
 
corresponds to a rotation of approximately −74° around the axis (−1/3,2/3,2/3) in three-dimensional space.
 
is a rotation matrix, as is the matrix of any even permutation, and rotates through 120° about the axis x = y = z.
  • The 3 × 3 matrix
 
has determinant +1, but is not orthogonal (its transpose is not its inverse), so it is not a rotation matrix.
  • The 4 × 3 matrix
 
is not square, and so cannot be a rotation matrix; yet MTM yields a 3 × 3 identity matrix (the columns are orthonormal).
  • The 4 × 4 matrix
 
describes an isoclinic rotation in four dimensions, a rotation through equal angles (180°) through two orthogonal planes.
  • The 5 × 5 rotation matrix
 
rotates vectors in the plane of the first two coordinate axes 90°, rotates vectors in the plane of the next two axes 180°, and leaves the last coordinate axis unmoved.

Geometry

edit

In Euclidean geometry, a rotation is an example of an isometry, a transformation that moves points without changing the distances between them. Rotations are distinguished from other isometries by two additional properties: they leave (at least) one point fixed, and they leave "handedness" unchanged. By contrast, a translation moves every point, a reflection exchanges left- and right-handed ordering, and a glide reflection does both.

A rotation that does not leave handedness unchanged is an improper rotation or a rotoinversion.

If a fixed point is taken as the origin of a Cartesian coordinate system, then every point can be given coordinates as a displacement from the origin. Thus one may work with the vector space of displacements instead of the points themselves. Now suppose (p1,…,pn) are the coordinates of the vector p from the origin O to point P. Choose an orthonormal basis for our coordinates; then the squared distance to P, by Pythagoras, is

 

which can be computed using the matrix multiplication

 

A geometric rotation transforms lines to lines, and preserves ratios of distances between points. From these properties it can be shown that a rotation is a linear transformation of the vectors, and thus can be written in matrix form, Qp. The fact that a rotation preserves, not just ratios, but distances themselves, is stated as

 

or

 

Because this equation holds for all vectors, p, one concludes that every rotation matrix, Q, satisfies the orthogonality condition,

 

Rotations preserve handedness because they cannot change the ordering of the axes, which implies the special matrix condition,

 

Equally important, it can be shown that any matrix satisfying these two conditions acts as a rotation.

Multiplication

edit

The inverse of a rotation matrix is its transpose, which is also a rotation matrix:

 

The product of two rotation matrices is a rotation matrix:

 

For n > 2, multiplication of n × n rotation matrices is generally not commutative.

 

Noting that any identity matrix is a rotation matrix, and that matrix multiplication is associative, we may summarize all these properties by saying that the n × n rotation matrices form a group, which for n > 2 is non-abelian, called a special orthogonal group, and denoted by SO(n), SO(n,R), SOn, or SOn(R), the group of n × n rotation matrices is isomorphic to the group of rotations in an n-dimensional space. This means that multiplication of rotation matrices corresponds to composition of rotations, applied in left-to-right order of their corresponding matrices.

Robotic Position and Orientation (Copied and edited from Wikibooks[7])

edit

In general, a rigid body in three-dimensional space has six degrees of freedom: three rotational and three translational.

 
The position and orientation of a rigid body can be described by attaching a frame to it.

A conventional way to describe the position and orientation of a rigid body is to attach a frame to it. After defining a reference coordinate system, the position and orientation of the rigid body are fully described by the position of the frame's origin and the orientation of its axes, relative to the reference frame.

Rotation Matrix

edit

A rotation matrix describes the relative orientation of two such frames. The columns of this 3 × 3 matrix consist of the unit vectors along the axes of one frame, relative to the other, reference frame. Thus, the relative orientation of a frame   with respect to a reference frame   is given by the rotation matrix  :

 

Rotation matrices can be interpreted in two ways:

  1. As the representation of the rotation of the first frame into the second (active interpretation).
  2. As the representation of the mutual orientation between two coordinate systems (passive interpretation).

The coordinates, relative to the reference frame  , of a point  , of which the coordinates are known with respect to a frame   with the same origin, can then be calculated as follows:  .

Representation of Reference Frame Using 3x3 Matrix of Direct Cosines (Dot Products)

edit
Code
%reset -f
import numpy as np

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)
b_a_R=EulerRzyx
print('b_a_R or rotation matrix R for {b} respect to ref {a}')
print(np.round(b_a_R,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

b_a_R2=np.array([[np.vdot(x_b,x_a), np.vdot(y_b,x_a),np.vdot(z_b,x_a)],
                [np.vdot(x_b,y_a),np.vdot(y_b,y_a),np.vdot(z_b,y_a)],
                [np.vdot(x_b,z_a),np.vdot(y_b,z_a),np.vdot(z_b,z_a)]])

print('b_a_R or rotation matrix R for {b} respect to ref {a} using other definition')
print(np.round(b_a_R2,2))

print('Are the rotation matrices the same?')
print(b_a_R==b_a_R2)

Graphical Representation of Rotated Reference Frame about Origin

edit
 
Reference reference frame A at origin
 
Rotated frame B at origin. Euler, body-fixed rotations (ZYX) each of 45 degrees.
Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)
b_a_R=EulerRzyx

print('b_a_R or rotation matrix R for {b} respect to ref {a}')
print(np.round(b_a_R,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('ReferenceFrame_a_at_origin.png',dpi=300,bbox_inches='tight')

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(x_b[0][0],x_b[1][0],x_b[2][0],'$x_b$',None,size=20)
ax.text(y_b[0][0],y_b[1][0],y_b[2][0],'$y_b$',None,size=20)
ax.text(z_b[0][0],z_b[1][0],z_b[2][0],'$z_b$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Rotated_frame_b_at_origin.png',dpi=300,bbox_inches='tight')

Graphical Representation of Point in Rotated Frame about Origin

edit

If we have   in reference frame  . What is   in frame   after Euler sequence ZYX by   about each axis?

 
Point   in reference frame  .
 
Point   in rotated frame  .
Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)

b_a_R=EulerRzyx
a_p=np.array([[0.5,0.5,0.5]])
a_p=a_p.T
a_b_R=b_a_R.T

#or

a_b_R=np.linalg.inv(b_a_R) #Simpler to use transpose, but...

b_p=np.dot(a_b_R,a_p)
print('The resulting b_p is')
print(np.round(b_p,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_a p$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Ref_a_with_point.png',dpi=300,bbox_inches='tight')

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(x_b[0][0],x_b[1][0],x_b[2][0],'$x_b$',None,size=20)
ax.text(y_b[0][0],y_b[1][0],y_b[2][0],'$y_b$',None,size=20)
ax.text(z_b[0][0],z_b[1][0],z_b[2][0],'$z_b$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_b p$',None,size=20)
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
ax.set_zlim([-1, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-1,1,num=5))
plt.yticks(np.linspace(-1,1,num=5))
ax.set_zticks(np.linspace(-1,1,num=5))
ax.view_init(30,-115)
plt.savefig('Rotated_Frame_b_with_point_at_origin.png',dpi=300,bbox_inches='tight')

Additional Examples for Rotations and Coordinates

edit

Additional Examples for Rotations and Coordinates

Properties

edit

Some of the properties of the rotation matrix that may be of practical value, are:

  1. The column vectors of   are normal to each other.
  2. The length of the column vectors of   equals 1.
  3. A rotation matrix is a non-minimal description of a rigid body's orientation. That is, it uses nine numbers to represent an orientation instead of just three. (The two above properties correspond to six relations between the nine matrix elements. Hence, only three of them are independent.) Non-minimal representations often have some numerical advantages, though, as they do not exhibit coordinate singularities.
  4. Since   is orthonormal,  .

Elementary Rotations about Frame Axes

edit
 
Rotation by an angle   about the z-axis.

The expressions for elementary rotations about frame axes can easily be derived. From the figure on the right, it can be seen that the rotation of a frame by an angle   about the z-axis, is described by:

 

Similarly, it can be shown that the rotation of a frame by an angle   about the x-axis, is given by:

 

Derived in exactly the same manner, the rotation of a frame by an angle   about the y-axis, is described by:

 

Compound Rotations

edit

Compound rotations are found by multiplication of the different elementary rotation matrices.

The matrix corresponding to a set of rotations about moving axes can be found by postmultiplying the rotation matrices, thus multiplying them in the same order in which the rotations take place. The rotation matrix formed by a rotation by an angle   about the z-axis followed by a rotation by an angle   about the moved y-axis, is then given by:

 

The composition of rotations about fixed axes, on the other hand, is found by premultiplying the different elementary rotation matrices.

Inverse Rotations

edit

The inverse of a single rotation about a frame axis is a rotation by the negative of the rotation angle about the same axis:

 

The inverse of a compound rotation follows from the inverse of the matrix product:

 

Euler Angles

edit

Contrary to the rotation matrix, Euler angles are a minimal representation (a set of just three numbers, that is) of relative orientation. This set of three angles describes a sequence of rotations about the axes of a moving reference frame. There are, however, many (12, to be exact) sets that describe the same orientation: different combinations of axes (e.g. ZXZ, ZYZ, and so on) lead to different Euler angles. Euler angles are often used for the description of the orientation of the wrist-like end-effectors of many serial manipulator robots.

Note: Identical axes should not be in consecutive places (e.g. ZZX). Also, the range of the Euler angles should be limited in order to avoid different angles for the same orientation. E.g.: for the case of ZYZ Euler angles, the first rotation about the z-axis should be within  . The second rotation, about the moved y-axis, has a range of  . The last rotation, about the moved z-axis, has a range of  .

Forward Mapping

edit

Forward mapping, or finding the orientation of the end-effector with respect to the base frame, follows from the composition of rotations about moving axes. For a rotation by an angle   about the z-axis, followed by a rotation by an angle   about the moved x-axis, and a final rotation by an angle   about the moved z-axis, the resulting rotation matrix is:

 

After writing out:

 

Note: Notice the shorthand notation:   stands for  ,   stands for  , and so on.

Inverse Mapping

edit

In order to drive the end-effector, the inverse problem must be solved: given a certain orientation matrix, which are the Euler angles that accomplish this orientation?

For the above case, the Euler angles  ,   and   are found by inspection of the rotation matrix:

 
 
 

Example of Forward Mapping and Inverse Mapping by Inspection

edit
Code
%reset -f
import numpy as np
import sympy as sp

alpha, beta, gamma = sp.symbols('alpha beta gamma')

EulerRz_alpha=np.array([[sp.cos(alpha),-sp.sin(alpha),0],
             [sp.sin(alpha),sp.cos(alpha),0], 
             [0,0,1]])
EulerRx_beta=np.array([[1,0,0],
             [0,sp.cos(beta),-sp.sin(beta)],
             [0,sp.sin(beta),sp.cos(beta)]])
EulerRz_gamma=np.array([[sp.cos(gamma),-sp.sin(gamma),0],
             [sp.sin(gamma),sp.cos(gamma),0], 
             [0,0,1]])

Rzxz=np.dot(np.dot(EulerRz_alpha,EulerRx_beta),
                           EulerRz_gamma)
print("Rzxz:")
print(Rzxz)

alpha_inspected=sp.atan(Rzxz[0,2]/-(Rzxz[1,2]))
alpha_inspected=sp.simplify(alpha_inspected)
print("alpha:")
print(alpha_inspected)

beta_inspected=sp.atan((-Rzxz[1][2]*sp.cos(alpha)+
                       Rzxz[0][2]*sp.sin(alpha))/Rzxz[2][2])
beta_inspected=sp.simplify(beta_inspected)
print("beta:")
print(beta_inspected)

gamma_inspected=sp.atan(Rzxz[2][0]/Rzxz[2][1])
gamma_inspected=sp.simplify(gamma_inspected)
print("gamma:")
print(gamma_inspected)

Coordinate Singularities

edit

In the above example, a coordinate singularity exists for  . The above equations are badly numerically conditioned for small values of  : the first and last equaton become undefined. This corresponds with an alignment of the first and last axes of the end-effector. The occurrence of a coordinate singularity involves the loss of a degree of freedom: in the case of the above example, small rotations about the y-axis require impossibly large rotations about the x- and z-axes.

No minimal representation of orientation can globally describe all orientations without coordinate singularities occurring.

Roll-Pitch-Yaw Angles

edit

The orientation of a rigid body can equally well be described by three consecutive rotations about fixed axes. This leads to a notation with Roll-Pitch-Yaw (RPY) angles.

Forward Mapping
edit

The forward mapping of RPY angles to a rotation matrix similar to that of Euler angles. Since the frame now rotates about fixed axes instead of moving axes, the order in which the different rotation matrices are multiplied is inversed:

 

After writing out:

 
Inverse Mapping
edit

The inverse relationships are found from inspection of the rotation matrix above:

 
 
 

Note: The above equations are badly numerically conditioned for values of   near   and  .

Unit Quaternions

edit

Unit quaternions (quaternions of which the absolute value equals 1) are another representation of orientation. They can be seen as a compromise between the advantages and disadvantages of rotation matrices and Euler angle sets.

Homogeneous Transform

edit

The notations above describe only relative orientation. The coordinates of a point   with   relative to a frame  , rotated and translated with respect to a reference frame  , are given by:

 

This can be compacted into the form of a homogeneous transformation matrix or pose (matrix). It is defined as follows:

 

This matrix represents the position and orientation of a frame   whose origin, relative to a reference frame  , is described by  , and whose orientation, relative to the same reference frame  , is described by the rotation matrix  .

  is, thus, the representation of a frame in three-dimensional space. If the coordinates of a point   are known with respect to a frame  , then its coordinates, relative to   are found by:

 

This is the same as writing:

 

Note that the above vectors are extended with a fourth coordinate equal to one: they're made homogeneous.

As was the case with rotation matrices, homogeneous transformation matrices can be interpreted in an active ("displacement"), and a passive ("pose") manner. It is also a non-minimal representation of a pose, that does not suffer from coordinate singularities.

Graphical Representation of Point in Rotated and Translated Frame (Not Using T)

edit
 
Point   in rotated and translated frame  .
Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

a_p=np.array([[1,1,1]])
a_p=a_p.T

a_p_ab=np.array([[0.8, 0.8, 0.8]])
a_p_ab=a_p_ab.T

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)

b_a_R=EulerRzyx
a_b_R=b_a_R.T


b_p=np.dot(a_b_R,a_p-a_p_ab)

print('The resulting b_p is')
print(np.round(b_p,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.text(a_p[0][0]+0.1,a_p[1][0],a_p[2][0],'$_a p$',None,size=20)

#fig=plt.figure()
#ax=fig.add_subplot(111,projection='3d')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(a_p_ab[0][0]+x_b[0][0],a_p_ab[1][0]+x_b[1][0],
        a_p_ab[2][0]+x_b[2][0],'$x_b$',None,size=20)
ax.text(a_p_ab[0][0]+y_b[0][0],a_p_ab[1][0]+y_b[1][0],
        a_p_ab[2][0]+y_b[2][0],'$y_b$',None,size=20)
ax.text(a_p_ab[0][0]+z_b[0][0],a_p_ab[1][0]+z_b[1][0],
        a_p_ab[2][0]+z_b[2][0],'$z_b$',None,size=20)
ax.scatter(a_p[0],a_p[1],a_p[2],s=60,marker='x')
ax.set_xlim([-0.5, 1.5])
ax.set_ylim([-0.5, 1.5])
ax.set_zlim([-0.5, 1.5])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(-0.5,1.5,num=5))
plt.yticks(np.linspace(-0.5,1.5,num=5))
ax.set_zticks(np.linspace(-0.5,1.5,num=5))
ax.view_init(30,-115)
plt.savefig('a_P_Frameb_translation_rotation.png',dpi=300,bbox_inches='tight')

Representation of Point in Rotated and Translated Frame (Using T)

edit
Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

a_p=np.array([[1,1,1]])
a_p=a_p.T

a_p_ab=np.array([[0.8, 0.8, 0.8]])
a_p_ab=a_p_ab.T

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)

b_a_R=EulerRzyx

print("b_a_R (Rotation of frame b with respect to frame a):")
print(np.round(b_a_R,2))
print("a_p_ab (Translation of frame b from frame a in frame a):")
print(np.round(a_p_ab,2))

b_a_T=np.append(b_a_R,np.zeros([1,3]),axis=0)
b_a_T=np.append(b_a_T,np.append(a_p_ab,[[1]],axis=0),axis=1)

print("Homogeneous Transform b_a_T:")
print(np.round(b_a_T,2))


a_b_T=np.linalg.inv(b_a_T)
# or
a_b_T=np.append(b_a_R.T,np.zeros([1,3]),axis=0)
a_b_T=np.append(a_b_T,np.append(-np.dot(b_a_R.T,a_p_ab),
                                [[1]],axis=0),axis=1)

print("Inverse of b_a_T or a_b_T:")
print(np.round(a_b_T,2))

a_p_1=np.append(a_p,[[1]],axis=0)
b_p_1=np.dot(a_b_T,a_p_1)
b_p=np.delete(b_p_1,3,axis=0)

print("Resulting b_p")
print(np.round(b_p,2))

Compound Poses

edit

If the pose of a frame   is known, relative to  , whose pose is known with respect to a third frame  , the resulting pose   is found as follows:

 

Graphical Representation of Compound Poses

edit

If we know the transformation   from frame   relative to reference frame   and the transformation   from frame   to  , we can describe the transformation from   from frame   to reference frame  .

 

 
Plotted frames for compound poses going from reference frame   to   to  .
Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np

a_p_ab=np.array([[0.8, 0.8, 0.8]])
a_p_ab=a_p_ab.T

alpha=45*np.pi/180 #Yaw for body-fixed rotation (z)
beta=45*np.pi/180 #Pitch for body-fixed rotation (y)
gamma=45*np.pi/180 #Roll for body-fixed rotation (x)

EulerRz=np.array([[np.cos(alpha),-np.sin(alpha),0],
             [np.sin(alpha),np.cos(alpha),0], 
             [0,0,1]])
EulerRy=np.array([[np.cos(beta),0,np.sin(beta)],
             [0,1,0],
             [-np.sin(beta),0,np.cos(beta)]])
EulerRx=np.array([[1,0,0],
             [0,np.cos(gamma),-np.sin(gamma)],
             [0,np.sin(gamma),np.cos(gamma)]])

EulerRzyx=np.dot(np.dot(EulerRz,EulerRy),EulerRx)

b_a_R=EulerRzyx

b_a_T=np.append(b_a_R,np.zeros([1,3]),axis=0)
b_a_T=np.append(b_a_T,np.append(a_p_ab,[[1]],axis=0),axis=1)

print("Homogeneous Transform b_a_T:")
print(np.round(b_a_T,2))

c_b_T=b_a_T;

print("Homogeneous Transform c_b_T:")
print(np.round(c_b_T,2))

c_a_T=np.dot(b_a_T,c_b_T)

print("Homogeneous Transform c_a_T:")
print(np.round(c_a_T,2))

x_a=np.array([[1,0,0]])
x_a=x_a.T
y_a=np.array([[0,1,0]])
y_a=y_a.T
z_a=np.array([[0,0,1]])
z_a=z_a.T

#Extract rotation matrix from homogeneous transform
b_a_R=np.delete(b_a_T,3,axis=0);
b_a_R=np.delete(b_a_R,3,axis=1);

#Extract translation from homogenous transform
a_p_ab=np.array([[b_a_T[0][3]],[b_a_T[1][3]],[b_a_T[2][3]]])

#Calculate coordinate axes for {b} in {a}
x_b=np.dot(b_a_R,x_a)
y_b=np.dot(b_a_R,y_a)
z_b=np.dot(b_a_R,z_a)

#Extract rotation matrix from homogeneous transform
c_a_R=np.delete(c_a_T,3,axis=0);
c_a_R=np.delete(c_a_R,3,axis=1);

#Extract translation from homogeneous transform
a_p_ac=np.array([[c_a_T[0][3]],[c_a_T[1][3]],[c_a_T[2][3]]])

#Calculate coordinate axes for {c} in {a}
x_c=np.dot(c_a_R,x_a)
y_c=np.dot(c_a_R,y_a)
z_c=np.dot(c_a_R,z_a)

fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.quiver(0,0,0,x_a[0],x_a[1],x_a[2],pivot='tail',normalize=False,color='blue')
ax.quiver(0,0,0,y_a[0],y_a[1],y_a[2],pivot='tail',normalize=False,color='red')
ax.quiver(0,0,0,z_a[0],z_a[1],z_a[2],pivot='tail',normalize=False,color='green')
ax.text(x_a[0][0],x_a[1][0],x_a[2][0],'$x_a$',None,size=20)
ax.text(y_a[0][0],y_a[1][0],y_a[2][0],'$y_a$',None,size=20)
ax.text(z_a[0][0],z_a[1][0],z_a[2][0],'$z_a$',None,size=20)

#fig=plt.figure()
#ax=fig.add_subplot(111,projection='3d')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          x_b[0],x_b[1],x_b[2],pivot='tail',normalize=False,color='blue')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          y_b[0],y_b[1],y_b[2],pivot='tail',normalize=False,color='red')
ax.quiver(a_p_ab[0],a_p_ab[1],a_p_ab[2],
          z_b[0],z_b[1],z_b[2],pivot='tail',normalize=False,color='green')
ax.text(a_p_ab[0][0]+x_b[0][0],a_p_ab[1][0]+x_b[1][0],
        a_p_ab[2][0]+x_b[2][0],'$x_b$',None,size=20)
ax.text(a_p_ab[0][0]+y_b[0][0],a_p_ab[1][0]+y_b[1][0],
        a_p_ab[2][0]+y_b[2][0],'$y_b$',None,size=20)
ax.text(a_p_ab[0][0]+z_b[0][0],a_p_ab[1][0]+z_b[1][0],
        a_p_ab[2][0]+z_b[2][0],'$z_b$',None,size=20)

#fig=plt.figure()
#ax=fig.add_subplot(111,projection='3d')
ax.quiver(a_p_ac[0],a_p_ac[1],a_p_ac[2],
          x_c[0],x_c[1],x_c[2],pivot='tail',normalize=False,color='blue')
ax.quiver(a_p_ac[0],a_p_ac[1],a_p_ac[2],
          y_c[0],y_c[1],y_c[2],pivot='tail',normalize=False,color='red')
ax.quiver(a_p_ac[0],a_p_ac[1],a_p_ac[2],
          z_c[0],z_c[1],z_c[2],pivot='tail',normalize=False,color='green')
ax.text(a_p_ac[0][0]+x_c[0][0],a_p_ac[1][0]+x_c[1][0],
        a_p_ac[2][0]+x_c[2][0],'$x_c$',None,size=20)
ax.text(a_p_ac[0][0]+y_c[0][0],a_p_ac[1][0]+y_c[1][0],
        a_p_ac[2][0]+y_c[2][0],'$y_c$',None,size=20)
ax.text(a_p_ac[0][0]+z_c[0][0],a_p_ac[1][0]+z_c[1][0],
        a_p_ac[2][0]+z_c[2][0],'$z_c$',None,size=20)

ax.set_xlim([0, 2])
ax.set_ylim([0, 2])
ax.set_zlim([0, 2])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.xticks(np.linspace(0,2,num=5))
plt.yticks(np.linspace(0,2,num=5))
ax.set_zticks(np.linspace(0,2,num=5))
ax.view_init(30,-115)
plt.savefig('CompoundPoses_c_b_a.png',dpi=300,bbox_inches='tight')

Additional Examples of Homogeneous Transforms

edit

Additional Examples of Homogeneous Transforms

Finite Displacement Twist

edit

A pose matrix is a non-minimal way of describing a pose. A frequently used minimal alternative is the finite displacement twist:

 

Here,  ,   and   are a valid set (any) of Euler angles, while  ,   and   are the coordinates of a reference point on the rigid body.

Forward Kinematics (Copied and Edited from Link[8] and Link[9])

edit
 
An articulated six DOF robotic arm uses forward kinematics to position the gripper.
 
The forward kinematics equations define the trajectory of the end-effector of a PUMA robot reaching for parts.

Forward kinematics refers to the use of the kinematic equations of a robot to compute the position of the end effector from specified values for the joint parameters.[10]

The kinematics equations of the robot are used in robotics, computer games, and animation. The reverse process that computes the joint parameters that achieve a specified position of the end effector is known as inverse kinematics.

 
Forward vs Backwards Kinematics

Forward Position Kinematics

edit

The forward position kinematics problem is: given the different joint angles, what is the position of the end-effector? With the previous sections in mind, the answer is: construct the different transformation matrices and combine them in the right way, the result being  , where   is the base frame of the robot manipulator.

Solution

edit

Suppose the mutual orientation matrices between adjacent links are known. (As the fixed parameters of each link are known, and the joint angles are a given to the problem, these can be calculated. One possible way to do this would be to make use of the Denavit-Hartenberg convention.) The transformation that relates the last and first frames in a serial manipulator arm, and thus, the solution to the forward kinematics problem, is then represented by the compound homogeneous transformation matrix. The axes are moving, thus, the compound homogeneous transformation matrix is found by multiplying the individual transformation matrices:

 

Examples

edit
edit
 
A planar three link manipulator. Each  -axis lies along the  th link. Each  -axis lies perpendicular to the corresponding  -axis in such a way that a positive   corresponds with a rotation from   to  .

The equations below use 3 × 3 pose matrices (homogeneous transformations), as this is just a 2-dimensional case (cf. the figure on the right).

The pose of the first link, relative to the reference frame, is given by (recall the elementary rotation about the z-axis from the previous section):

 

The pose of the second link, relative to the first link, is given by:

 

This corresponds to a rotation by an angle   and a translation by a distance  , where   is the length of the first link.

The pose of the third link, relative to the second link, is given by:

 

The pose of the end effector, relative to the third link, is given by:

 

The solution to the forward kinematics problem is then:

 

Hence:

 

The resulting kinematic equations are:

 
edit
 
Planar, three-link manipulator with all three angles set to  .
Code
%reset -f
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt

theta1, theta2, theta3=sp.symbols('theta1 theta2 theta3')
l1, l2, l3 = sp.symbols('l1 l2 l3')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0],
             [sp.sin(theta1), sp.cos(theta1), 0],
             [0, 0, 1]])
f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), l1],
            [sp.sin(theta2), sp.cos(theta2),0],
            [0, 0, 1]])
f3_f2_T=np.array([[sp.cos(theta3), -sp.sin(theta3), l2],
            [sp.sin(theta3), sp.cos(theta3),0],
            [0, 0, 1]])
f4_f3_T=np.array([[sp.cos(0), -sp.sin(0), l3],
            [sp.sin(0), sp.cos(0),0],
            [0, 0, 1]])
ee_bs_T=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,f4_f3_T)))
ee_bs_T=sp.simplify(ee_bs_T) # Unfortunately, we do not get the simple
# expression on the wiki.
#print("ee_bs_T")
#display(ee_bs_T)

#For substituting in the following: 30 degree increments
#print("ee_bs_T after substitutions")
replacements=[(theta1,30*np.pi/180),(theta2,30*np.pi/180), 
              (theta3,30*np.pi/180), (l1,1), (l2,1), (l3,1)]

#display(ee_bs_T.subs(replacements))

End_Coord=np.array([[0,0,1]])
End_Coord=End_Coord.T

#print("f1_f0_T")
f1_f0_T=sp.simplify(f1_f0_T)
#display(f1_f0_T.subs(replacements))
f1_f0_T=np.array(f1_f0_T.subs(replacements)).astype(np.float64)
f1_f0_T=np.reshape(f1_f0_T,(3,3))
print("f1_f0_T dot End_Coord")
display(np.dot(f1_f0_T,End_Coord))
Joint0=np.dot(f1_f0_T,End_Coord)

#print("\nf2_f1_T")
f2_f1_T=sp.simplify(f2_f1_T)
#display(f2_f1_T.subs(replacements))
f2_f1_T=np.array(f2_f1_T.subs(replacements)).astype(np.float64)
f2_f1_T=np.reshape(f2_f1_T,(3,3))
print("f2_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,End_Coord)))
Joint1=np.dot(f1_f0_T,np.dot(f2_f1_T,End_Coord))

#print("\nf3_f2_T")
f3_f2_T=sp.simplify(f3_f2_T)
#display(f3_f2_T.subs(replacements))
f3_f2_T=np.array(f3_f2_T.subs(replacements)).astype(np.float64)
f3_f2_T=np.reshape(f3_f2_T,(3,3))
print("f3_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,End_Coord))))
Joint2=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,End_Coord)))

#print("\nf4_f3_T")
f4_f3_T=sp.simplify(f4_f3_T)
#display(f4_f3_T.subs(replacements))
f4_f3_T=np.array(f4_f3_T.subs(replacements)).astype(np.float64)
f4_f3_T=np.reshape(f4_f3_T,(3,3))
print("f4_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord)))))
EndEffector=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord))))

fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot([Joint0[0][0], Joint1[0][0]],[Joint0[1][0],Joint1[1][0]],'bo-')
plt.plot([Joint1[0][0], Joint2[0][0]],[Joint1[1][0],Joint2[1][0]],'ro-')
plt.plot([Joint2[0][0], EndEffector[0][0]],
         [Joint2[1][0],EndEffector[1][0]],'go-')

ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
plt.savefig('2D_30_30_30_rotations_200212.png', dpi=300, bbox_inches='tight')

Two Configurations for the Same Resulting Location of the End Effector

edit
 
An example of two different solutions for the inverse kinematics problem leading to the same end-effector position and orientation.

We can demonstrate how to have the same position of an end effector with two different configurations of the planar serial manipulator.

Example for a Different Configuration for the Same Resulting Location of the End Effector

edit
Code
%reset -f
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt

theta1, theta2, theta3=sp.symbols('theta1 theta2 theta3')
l1, l2, l3 = sp.symbols('l1 l2 l3')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0],
             [sp.sin(theta1), sp.cos(theta1), 0],
             [0, 0, 1]])
f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), l1],
            [sp.sin(theta2), sp.cos(theta2),0],
            [0, 0, 1]])
f3_f2_T=np.array([[sp.cos(theta3), -sp.sin(theta3), l2],
            [sp.sin(theta3), sp.cos(theta3),0],
            [0, 0, 1]])
f4_f3_T=np.array([[sp.cos(0), -sp.sin(0), l3],
            [sp.sin(0), sp.cos(0),0],
            [0, 0, 1]])
ee_bs_T=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,f4_f3_T)))
ee_bs_T=sp.simplify(ee_bs_T) # Unfortunately, we do not get the simple
# expression on the wiki.
#print("ee_bs_T")
#display(ee_bs_T)

#For substituting in the following: 60,-30, 60 degree increments
#print("ee_bs_T after substitutions")
replacements=[(theta1,60*np.pi/180),(theta2,-30*np.pi/180), 
              (theta3,60*np.pi/180), (l1,1), (l2,1), (l3,1)]

#display(ee_bs_T.subs(replacements))

End_Coord=np.array([[0,0,1]])
End_Coord=End_Coord.T

#print("f1_f0_T")
f1_f0_T=sp.simplify(f1_f0_T)
#display(f1_f0_T.subs(replacements))
f1_f0_T=np.array(f1_f0_T.subs(replacements)).astype(np.float64)
f1_f0_T=np.reshape(f1_f0_T,(3,3))
print("f1_f0_T dot End_Coord")
display(np.dot(f1_f0_T,End_Coord))
Joint0=np.dot(f1_f0_T,End_Coord)

#print("\nf2_f1_T")
f2_f1_T=sp.simplify(f2_f1_T)
#display(f2_f1_T.subs(replacements))
f2_f1_T=np.array(f2_f1_T.subs(replacements)).astype(np.float64)
f2_f1_T=np.reshape(f2_f1_T,(3,3))
print("f2_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,End_Coord)))
Joint1=np.dot(f1_f0_T,np.dot(f2_f1_T,End_Coord))

#print("\nf3_f2_T")
f3_f2_T=sp.simplify(f3_f2_T)
#display(f3_f2_T.subs(replacements))
f3_f2_T=np.array(f3_f2_T.subs(replacements)).astype(np.float64)
f3_f2_T=np.reshape(f3_f2_T,(3,3))
print("f3_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,End_Coord))))
Joint2=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,End_Coord)))

#print("\nf4_f3_T")
f4_f3_T=sp.simplify(f4_f3_T)
#display(f4_f3_T.subs(replacements))
f4_f3_T=np.array(f4_f3_T.subs(replacements)).astype(np.float64)
f4_f3_T=np.reshape(f4_f3_T,(3,3))
print("f4_f0_T dot End_Coord")
display(np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord)))))
EndEffector=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,np.dot(f4_f3_T,End_Coord))))

fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot([Joint0[0][0], Joint1[0][0]],[Joint0[1][0],Joint1[1][0]],'bo-')
plt.plot([Joint1[0][0], Joint2[0][0]],[Joint1[1][0],Joint2[1][0]],'ro-')
plt.plot([Joint2[0][0], EndEffector[0][0]],
         [Joint2[1][0],EndEffector[1][0]],'go-')

ax.set_xlim([0, 1])
plt.axis('equal')
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
plt.savefig('2D 60 neg30 60 rotations 200212.png', dpi=300, bbox_inches='tight')
 
Planar, three-link manipulator with  ,  , and  .


Kinematic Equations for a Serial Manipulating Robotic Arm

edit

The kinematic equations for the series chain of a robot are obtained using a rigid transformation [Z] to characterize the relative movement allowed at each joint and separate rigid transformation [X] to define the dimensions of each link. The result is a sequence of rigid transformations alternating joint and link transformations from the base of the chain to its end link, which is equated to the specified position for the end link,

 

where [T] is the transformation locating the end-link. These equations are called the kinematics equations of the serial chain.[11]

edit

In 1955, Jacques Denavit and Richard Hartenberg introduced a convention for the definition of the joint matrices [Z] and link matrices [X] to standardize the coordinate frame for spatial linkages.[14][15] This convention positions the joint frame so that it consists of a screw displacement along the Z-axis

 

and it positions the link frame so it consists of a screw displacement along the X-axis,

 

Using this notation, each transformation-link goes along a serial chain robot, and can be described by the coordinate transformation,

 

where θi, di, αi,i+1 and ai,i+1 are known as the Denavit-Hartenberg parameters.

Kinematics equations revisited

edit

The kinematics equations of a serial chain of n links, with joint parameters θi are given by[16]

 

where   is the transformation matrix from the frame of link   to link  . In robotics, these are conventionally described by Denavit–Hartenberg parameters.[17]

Denavit-Hartenberg matrix

edit

The matrices associated with these operations are:

 

Similarly,

 

The use of the Denavit-Hartenberg convention yields the link transformation matrix, [ii-1T] as

 

known as the Denavit-Hartenberg matrix.

Denavit-Hartenberg Notation Re-visited

edit

In mechanical engineering, the Denavit–Hartenberg parameters (also called DH parameters) are the four parameters associated with a particular convention for attaching reference frames to the links of a spatial kinematic chain, or robot manipulator.

Jacques Denavit and Richard Hartenberg introduced this convention in 1955 in order to standardize the coordinate frames for spatial linkages.[18][19]

Richard Paul demonstrated its value for the kinematic analysis of robotic systems in 1981.[20] While many conventions for attaching reference frames have been developed, the Denavit–Hartenberg convention remains a popular approach.

Denavit–Hartenberg Convention and Transformations

edit

A commonly used convention for selecting frames of reference in robotics applications is the Denavit and Hartenberg (D–H) convention which was introduced by Jacques Denavit and Richard S. Hartenberg. In this convention, coordinate frames are attached to the joints between two links such that one transformation is associated with the joint, [Z], and the second is associated with the link [X]. The coordinate transformations along a serial robot consisting of n links form the kinematics equations of the robot,

 

where [T] is the transformation locating the end-link.

In order to determine the coordinate transformations [Z] and [X], the joints connecting the links are modeled as either hinged or sliding joints, each of which have a unique line S in space that forms the joint axis and define the relative movement of the two links. A typical serial robot is characterized by a sequence of six lines Si, i = 1,...,6, one for each joint in the robot. For each sequence of lines Si and Si+1, there is a common normal line Ai,i+1. The system of six joint axes Si and five common normal lines Ai,i+1 form the kinematic skeleton of the typical six degree of freedom serial robot. Denavit and Hartenberg introduced the convention that Z coordinate axes are assigned to the joint axes Si and X coordinate axes are assigned to the common normals Ai,i+1.

This convention allows the definition of the movement of links around a common joint axis Si by the screw displacement,

 

where θi is the rotation around and di is the slide along the Z axis—either of the parameters can be constants depending on the structure of the robot. Under this convention the dimensions of each link in the serial chain are defined by the screw displacement around the common normal Ai,i+1 from the joint Si to Si+1, which is given by

 

where αi,i+1 and ri,i+1 define the physical dimensions of the link in terms of the angle measured around and distance measured along the X axis.

In summary, the reference frames are laid out as follows:

  1. the  -axis is in the direction of the joint axis
  2. the  -axis is parallel to the common normal:   (or away from zn-1)
    If there is no unique common normal (parallel   axes), then   (below) is a free parameter. The direction of   is from   to  , as shown in the video below.
  3. the  -axis follows from the  - and  -axis by choosing it to be a right-handed coordinate system.

Four parameters

edit
 
The four parameters of classic DH convention are shown in red text, which are  . With those four parameters, we can translate the coordinates from   to  .

The following four transformation parameters are known as D–H parameters:.[21]

  •  : offset along previous   to the common normal
  •  : angle about previous  , from old   to new  
  •  : length of the common normal (do not confuse with  ). Assuming a revolute joint, this is the radius about previous  .
  •  : angle about common normal, from old   axis to new   axis

A visualization of D–H parameterization is available: YouTube

There is some choice in frame layout as to whether the previous   axis or the next   points along the common normal. The latter system allows branching chains more efficiently, as multiple frames can all point away from their common ancestor, but in the alternative layout the ancestor can only point toward one successor. Thus the commonly used notation places each down-chain   axis collinear with the common normal, yielding the transformation calculations shown below.

We can note constraints on the relationships between the axes:

  • the  -axis is perpendicular to both the   and   axes
  • the  -axis intersects both   and   axes
  • the origin of joint   is at the intersection of   and  
  •   completes a right-handed reference frame based on   and  

Denavit–Hartenberg matrix

edit

It is common to separate a screw displacement into the product of a pure translation along a line and a pure rotation about the line,[22][23] so that

 

and

 

Using this notation, each link can be described by a coordinate transformation from the concurrent coordinate system to the previous coordinate system.