Robotic Mechanics and Modeling/Kinematics

From Wikiversity
Jump to navigation Jump to search

Introduction to Kinematics for Robotic Mechanics and Modeling (Portion from Link)[edit | edit source]

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 | edit source]

Position in 1-D[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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

Representation of Vectors and Paths[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

Additional Examples for Vectors

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

Velocity and speed[edit | edit source]

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 | edit source]

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 | edit source]

Additional Examples for Velocity and Acceleration

Relative position vector[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

Basic rotations[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

Additional Examples for Rotations

Properties[edit | edit source]

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 | edit source]

  • 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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

Additional Examples for Rotations and Coordinates

Properties[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

Additional Examples of Homogeneous Transforms

Finite Displacement Twist[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 source]

The Planar Three-Link Manipulator[edit | edit source]

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:

Graphical Example of Planar, Three-link Manipulator using Compound Transformations[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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]

Denavit-Hartenberg Notation (Copied from Link [12] and Link[13])[edit | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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 | edit source]

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.

Note that this is the product of two screw displacements, The matrices associated with these operations are:

This gives:

where R is the 3×3 submatrix describing rotation and T is the 3×1 submatrix describing translation.

In some books, the order of transformation for a pair of consecutive rotation and translation (such as and ) is replaced. However, because matrix multiplication order for such pair does not matter, the result is the same. For example: .

Use of Denavit and Hartenberg matrices[edit | edit source]

The Denavit and Hartenberg notation gives a standard methodology to write the kinematic equations of a manipulator. This is especially useful for serial manipulators where a matrix is used to represent the pose (position and orientation) of one body with respect to another.

The position of body with respect to may be represented by a position matrix indicated with the symbol or

This matrix is also used to transform a point from frame to

Where the upper left submatrix of represents the relative orientation of the two bodies, and the upper right represents their relative position or more specifically the body position in frame n − 1 represented with element of frame n.

The position of body with respect to body can be obtained as the product of the matrices representing the pose of with respect of and that of with respect of

An important property of Denavit and Hartenberg matrices is that the inverse is

where is both the transpose and the inverse of the orthogonal matrix , i.e. .

Example of Planar, Three-link Manipulator with D-H Parameters[edit | edit source]

Planar, three-link manipulator with described D-H Parameters.

D-H Parameters for planar manipulator with three links

Link 1: , , , and

Link 2: , , , and

Link 3: , , , and

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

#All the joints set to angles of 45 degrees.
#Link 1
d1=0; theta1=45*np.pi/180; a1=1; alpha1=0
#Link 2
d2=0; theta2=0*np.pi/180; a2=1; alpha2=0
#Link 3
d3=0; theta3=45*np.pi/180; a3=1; alpha3=0

Link2_1_T=np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
                [np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
                [0, np.sin(alpha1), np.cos(alpha1), d1],
                [0, 0, 0, 1]])
Link3_2_T=np.array([[np.cos(theta2), -np.sin(theta2)*np.cos(alpha2), np.sin(theta2)*np.sin(alpha2), a2*np.cos(theta2)],
                [np.sin(theta2), np.cos(theta2)*np.cos(alpha2), -np.cos(theta2)*np.sin(alpha2), a2*np.sin(theta2)],
                [0, np.sin(alpha2), np.cos(alpha2), d2],
                [0, 0, 0, 1]])
Link4_3_T=np.array([[np.cos(theta3), -np.sin(theta3)*np.cos(alpha3), np.sin(theta3)*np.sin(alpha3), a3*np.cos(theta3)],
                [np.sin(theta3), np.cos(theta3)*np.cos(alpha3), -np.cos(theta3)*np.sin(alpha3), a3*np.sin(theta3)],
                [0, np.sin(alpha3), np.cos(alpha3), d3],
                [0, 0, 0, 1]])

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

Joint1Link0=End_Coord
Joint2Link1=np.dot(Link2_1_T,End_Coord)
Joint3Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord))
Joint4Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord)))

fig=plt.figure()
ax=fig.add_subplot(111)
plt.plot([Joint1Link0[0][0], Joint2Link1[0][0]],
         [Joint1Link0[1][0], Joint2Link1[1][0]],'bo-')
plt.plot([Joint2Link1[0][0], Joint3Link2[0][0]],
         [Joint2Link1[1][0], Joint3Link2[1][0]],'ro-')
plt.plot([Joint3Link2[0][0], Joint4Link3[0][0]],
         [Joint3Link2[1][0], Joint4Link3[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('PlanarThreeLinkManipulatorDH.png',dpi=300,bbox_inches='tight')

Animated Example of the Top-Down Perspective of a Three-link Manipulator with D-H Parameters[edit | edit source]

Animation of a three-link, serial manipulator described with D-H notation.

D-H Parameters for serial manipulator with three links

Link 1: , , , and

Link 2: , , , and

Link 3: , , , and

We vary from 0 to 45 degrees over 10 frames while the other joint angles remain constant with and .

Code
%reset -f
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import os

#Make sure you create the ImageDirectory (mkdir) before running
ImageDirectory="Session11_Linux_Animation"
ImageName="Linux_test"
VideoName="DH_2D_animation_200302"
FrameTime=50 #100ths of a second

for ImageNum in range(0,10,1):
    #Link 1
    d1=0.1; theta1=ImageNum*5*np.pi/180; a1=1; alpha1=0
    #Link 2
    d2=0.1; theta2=0*np.pi/180; a2=1; alpha2=0
    #Link 3
    d3=0.1; theta3=45*np.pi/180; a3=1; alpha3=0
    
    Link2_1_T=np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
                [np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
                [0, np.sin(alpha1), np.cos(alpha1), d1],
                [0, 0, 0, 1]])
    Link3_2_T=np.array([[np.cos(theta2), -np.sin(theta2)*np.cos(alpha2), np.sin(theta2)*np.sin(alpha2), a2*np.cos(theta2)],
                [np.sin(theta2), np.cos(theta2)*np.cos(alpha2), -np.cos(theta2)*np.sin(alpha2), a2*np.sin(theta2)],
                [0, np.sin(alpha2), np.cos(alpha2), d2],
                [0, 0, 0, 1]])
    Link4_3_T=np.array([[np.cos(theta3), -np.sin(theta3)*np.cos(alpha3), np.sin(theta3)*np.sin(alpha3), a3*np.cos(theta3)],
                [np.sin(theta3), np.cos(theta3)*np.cos(alpha3), -np.cos(theta3)*np.sin(alpha3), a3*np.sin(theta3)],
                [0, np.sin(alpha3), np.cos(alpha3), d3],
                [0, 0, 0, 1]])
    
    End_Coord=np.array([[0,0,0,1]])
    End_Coord=End_Coord.T
    
    ## Find the missing joints on the previous link
    End_Coord_for_Joint1Link1=np.transpose(np.array([[-a1,0,0,1]]))
    End_Coord_for_Joint2Link2=np.transpose(np.array([[-a2,0,0,1]]))
    End_Coord_for_Joint3Link3=np.transpose(np.array([[-a3,0,0,1]]))
    
    
    Joint1Link0=End_Coord
    Joint2Link1=np.dot(Link2_1_T,End_Coord)
    Joint3Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord))
    Joint4Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord)))
    
    #New Joint Calculations
    Joint1Link1=np.dot(Link2_1_T,End_Coord_for_Joint1Link1)
    Joint2Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord_for_Joint2Link2))
    Joint3Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord_for_Joint3Link3)))
    
    fig=plt.figure()
    ax=fig.add_subplot(111)
    plt.plot([Joint1Link1[0][0], Joint2Link1[0][0]],
         [Joint1Link1[1][0], Joint2Link1[1][0]],'bo-')
    plt.plot([Joint2Link2[0][0], Joint3Link2[0][0]],
         [Joint2Link2[1][0], Joint3Link2[1][0]],'ro-')
    plt.plot([Joint3Link3[0][0], Joint4Link3[0][0]],
         [Joint3Link3[1][0], Joint4Link3[1][0]],'go-')
    ax.set_xlim([-0.5, 3])
    ax.set_xbound([-0.5,3])
    ax.set_ylim([-0.5, 3])
    ax.set_ybound([-0.5, 3])
    #plt.axis('equal')
    plt.gca().set_aspect("equal", adjustable="box")
    ax.set_xlabel('X',size=20)
    ax.set_ylabel('Y',size=20)
    ax.set_autoscale_on(False)
    ax.set_title('Theta_1='+str(ImageNum*5)+' deg; Theta_3=45 deg', size=12)
    #Jae and Jess, thank you for helping improve the look of the plots.
    

    #There are automatic ways to make a directory, but go ahead and
    #make an appropriate directory instead of using some of these tricks:
    # https://stackoverflow.com/questions/11373610/save-matplotlib-file-to-a-directory
    plt.savefig(ImageDirectory+"/"+ImageName+str(ImageNum),dpi=300, bbox_inches='tight')

# If running windows, be sure to install ImageMagick before running this code.
if os.name=='nt':
    os.system("magick -delay "+str(FrameTime)+" "+ImageDirectory+"/"+ImageName+"*.png"+" "+ImageDirectory+"/"+VideoName+".gif")

if os.name=='posix':
    os.system("convert -delay "+str(FrameTime)+" "+ImageDirectory+"/"+ImageName+"*.png"+" "+ImageDirectory+"/"+VideoName+".gif")

3D Example of a Three-link Manipulator with D-H Parameters[edit | edit source]

3D representation of a pose of a three-link, serial manipulator.

D-H Parameters for serial manipulator with three links

Link 1: , , , and

Link 2: , , , and

Link 3: , , , and

We use D-H notation, but we have to perform "extra" calculations to determine the locations of Joint 1 on Link 1 (J1L1), Joint 2 on Link 2 (J2L2), and Joint 3 on Link 3 (J3L3). One way to envision their locations (Joint on Link ) relative to the Joint on link is to remember them as being offset by . Thus, we can find the location of Joint 1 on Link 1 in the global reference frame with the following D-H transformation:

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

#All the joints set to angles of 45 degrees.
#Link 1
d1=0.1; theta1=45*np.pi/180; a1=1; alpha1=0
#Link 2
d2=0.1; theta2=45*np.pi/180; a2=1; alpha2=0
#Link 3
d3=0.1; theta3=45*np.pi/180; a3=1; alpha3=0

Link2_1_T=np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
                [np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
                [0, np.sin(alpha1), np.cos(alpha1), d1],
                [0, 0, 0, 1]])
Link3_2_T=np.array([[np.cos(theta2), -np.sin(theta2)*np.cos(alpha2), np.sin(theta2)*np.sin(alpha2), a2*np.cos(theta2)],
                [np.sin(theta2), np.cos(theta2)*np.cos(alpha2), -np.cos(theta2)*np.sin(alpha2), a2*np.sin(theta2)],
                [0, np.sin(alpha2), np.cos(alpha2), d2],
                [0, 0, 0, 1]])
Link4_3_T=np.array([[np.cos(theta3), -np.sin(theta3)*np.cos(alpha3), np.sin(theta3)*np.sin(alpha3), a3*np.cos(theta3)],
                [np.sin(theta3), np.cos(theta3)*np.cos(alpha3), -np.cos(theta3)*np.sin(alpha3), a3*np.sin(theta3)],
                [0, np.sin(alpha3), np.cos(alpha3), d3],
                [0, 0, 0, 1]])

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

## Find the missing joints on the previous link
End_Coord_for_Joint1Link1=np.transpose(np.array([[-a1,0,0,1]]))
End_Coord_for_Joint2Link2=np.transpose(np.array([[-a2,0,0,1]]))
End_Coord_for_Joint3Link3=np.transpose(np.array([[-a3,0,0,1]]))


Joint1Link0=End_Coord
print("Joint1Link0:\n",Joint1Link0)
Joint2Link1=np.dot(Link2_1_T,End_Coord)
Joint3Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord))
Joint4Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord)))

#New Joint Calculations
Joint1Link1=np.dot(Link2_1_T,End_Coord_for_Joint1Link1)
print("Joint1Link1:\n",Joint1Link1)
Joint2Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord_for_Joint2Link2))
Joint3Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord_for_Joint3Link3)))


fig=plt.figure()
ax=fig.add_subplot(111,projection='3d')

ax.plot([Joint1Link0[0][0]], [Joint1Link0[1][0]],[Joint1Link0[2][0]],'c*')
ax.plot([Joint1Link1[0][0], Joint2Link1[0][0]],
         [Joint1Link1[1][0], Joint2Link1[1][0]],
        [Joint1Link1[2][0], Joint2Link1[2][0]],'bo-')
ax.text(Joint1Link0[0][0]+0.1, Joint1Link0[1][0], Joint1Link0[2][0],
        'J1L0',None,size=12)
    
ax.plot([Joint2Link2[0][0], Joint3Link2[0][0]],
         [Joint2Link2[1][0], Joint3Link2[1][0]],
        [Joint2Link2[2][0], Joint3Link2[2][0]],'ro-')
ax.text(Joint1Link1[0][0]+0.1, Joint1Link1[1][0], Joint1Link1[2][0],
        'J1L1',None,size=12)
ax.text(Joint2Link1[0][0]+0.1, Joint2Link1[1][0], Joint2Link1[2][0],
        'J2L1',None,size=12)
    
ax.plot([Joint3Link3[0][0], Joint4Link3[0][0]],
         [Joint3Link3[1][0], Joint4Link3[1][0]],
        [Joint3Link3[2][0], Joint4Link3[2][0]],'go-')
plt.plot([Joint4Link3[0][0]],
         [Joint4Link3[1][0]],
         [Joint4Link3[2][0]],'c*')
ax.text(Joint2Link2[0][0]+0.1, Joint2Link2[1][0], Joint2Link2[2][0],
        'J2L2',None,size=12)
ax.text(Joint3Link2[0][0]+0.1, Joint3Link2[1][0], Joint3Link2[2][0],
        'J3L2',None,size=12)
ax.text(Joint3Link3[0][0]+0.1, Joint3Link3[1][0], Joint3Link3[2][0],
        'J3L3',None,size=12)
ax.text(Joint4Link3[0][0]+0.1, Joint4Link3[1][0], Joint4Link3[2][0],
        'J4L3',None,size=12)
    
ax.set_xlabel('X',size=20)
ax.set_ylabel('Y',size=20)
ax.set_zlabel('Z',size=20)
ax.set_xlim([-0.25,3])
ax.set_ylim([-0.25,3])
    
ax.view_init(30,-115)
ax.set_title('Theta_1,Theta_2,Theta_3=45 deg', size=12)

plt.savefig('Stationary_DH_200302.png',dpi=300, bbox_inches='tight')

3D Example of an Animated, Three-link Manipulator with D-H Parameters[edit | edit source]

3D animation of a three-link, serial manipulator using D-H parameters.

D-H Parameters for a serial manipulator with three links

Link 1: , , , and

Link 2: , , , and

Link 3: , , , and

We vary , , and from 0 to 45 degrees over 10 frames.

Code
%reset -f
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
import os
ImageDirectory="Session11_3DAnimation"
ImageName="test"
VideoName="DH_3D_animation_200302"
FrameTime=75 #100ths of a second

for ImageNum in range(0,10,1):
    #All the joints set to angles varying from 0-45 degrees.
    #Link 1
    d1=0.1; theta1=ImageNum*5*np.pi/180; a1=1; alpha1=0
    #Link 2
    d2=0.1; theta2=ImageNum*5*np.pi/180; a2=1; alpha2=0
    #Link 3
    d3=0.1; theta3=ImageNum*5*np.pi/180; a3=1; alpha3=0
    
    Link2_1_T=np.array([[np.cos(theta1), -np.sin(theta1)*np.cos(alpha1), np.sin(theta1)*np.sin(alpha1), a1*np.cos(theta1)],
                [np.sin(theta1), np.cos(theta1)*np.cos(alpha1), -np.cos(theta1)*np.sin(alpha1), a1*np.sin(theta1)],
                [0, np.sin(alpha1), np.cos(alpha1), d1],
                [0, 0, 0, 1]])
    Link3_2_T=np.array([[np.cos(theta2), -np.sin(theta2)*np.cos(alpha2), np.sin(theta2)*np.sin(alpha2), a2*np.cos(theta2)],
                [np.sin(theta2), np.cos(theta2)*np.cos(alpha2), -np.cos(theta2)*np.sin(alpha2), a2*np.sin(theta2)],
                [0, np.sin(alpha2), np.cos(alpha2), d2],
                [0, 0, 0, 1]])
    Link4_3_T=np.array([[np.cos(theta3), -np.sin(theta3)*np.cos(alpha3), np.sin(theta3)*np.sin(alpha3), a3*np.cos(theta3)],
                [np.sin(theta3), np.cos(theta3)*np.cos(alpha3), -np.cos(theta3)*np.sin(alpha3), a3*np.sin(theta3)],
                [0, np.sin(alpha3), np.cos(alpha3), d3],
                [0, 0, 0, 1]])
    
    End_Coord=np.array([[0,0,0,1]])
    End_Coord=End_Coord.T
    
    ## Find the missing joints on the previous link
    End_Coord_for_Joint1Link1=np.transpose(np.array([[-a1,0,0,1]]))
    End_Coord_for_Joint2Link2=np.transpose(np.array([[-a2,0,0,1]]))
    End_Coord_for_Joint3Link3=np.transpose(np.array([[-a3,0,0,1]]))
    
    
    Joint1Link0=End_Coord
    # print("Joint1Link0:\n",Joint1Link0)
    Joint2Link1=np.dot(Link2_1_T,End_Coord)
    Joint3Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord))
    Joint4Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord)))
    
    #New Joint Calculations
    Joint1Link1=np.dot(Link2_1_T,End_Coord_for_Joint1Link1)
    #print("Joint1Link1:\n",Joint1Link1)
    Joint2Link2=np.dot(Link2_1_T,np.dot(Link3_2_T,End_Coord_for_Joint2Link2))
    Joint3Link3=np.dot(Link2_1_T,np.dot(Link3_2_T,np.dot(Link4_3_T,End_Coord_for_Joint3Link3)))
    
    
    fig=plt.figure()
    ax=fig.add_subplot(111,projection='3d')
    
    ax.plot([Joint1Link0[0][0]], [Joint1Link0[1][0]],[Joint1Link0[2][0]],'c*')
    ax.plot([Joint1Link1[0][0], Joint2Link1[0][0]],
         [Joint1Link1[1][0], Joint2Link1[1][0]],
        [Joint1Link1[2][0], Joint2Link1[2][0]],'bo-')
    ax.text(Joint1Link0[0][0]+0.1, Joint1Link0[1][0], Joint1Link0[2][0],
        'J1L0',None,size=12)
    
    ax.plot([Joint2Link2[0][0], Joint3Link2[0][0]],
         [Joint2Link2[1][0], Joint3Link2[1][0]],
        [Joint2Link2[2][0], Joint3Link2[2][0]],'ro-')
    ax.text(Joint1Link1[0][0]+0.1, Joint1Link1[1][0], Joint1Link1[2][0],
        'J1L1',None,size=12)
    ax.text(Joint2Link1[0][0]+0.1, Joint2Link1[1][0], Joint2Link1[2][0],
        'J2L1',None,size=12)
    
    ax.plot([Joint3Link3[0][0], Joint4Link3[0][0]],
         [Joint3Link3[1][0], Joint4Link3[1][0]],
        [Joint3Link3[2][0], Joint4Link3[2][0]],'go-')
    plt.plot([Joint4Link3[0][0]],
         [Joint4Link3[1][0]],
         [Joint4Link3[2][0]],'c*')
    ax.text(Joint2Link2[0][0]+0.1, Joint2Link2[1][0], Joint2Link2[2][0],
        'J2L2',None,size=12)
    ax.text(Joint3Link2[0][0]+0.1, Joint3Link2[1][0], Joint3Link2[2][0],
        'J3L2',None,size=12)
    ax.text(Joint3Link3[0][0]+0.1, Joint3Link3[1][0], Joint3Link3[2][0],
        'J3L3',None,size=12)
    ax.text(Joint4Link3[0][0]+0.1, Joint4Link3[1][0], Joint4Link3[2][0],
        'J4L3',None,size=12)
    
    ax.set_xlabel('X',size=20)
    ax.set_ylabel('Y',size=20)
    ax.set_zlabel('Z',size=20)
    ax.set_xlim([-0.25,3])
    ax.set_ylim([-0.25,3])
    
    ax.view_init(30,-115)
    ax.set_title('Theta_1,Theta_2,Theta_3='+str(ImageNum*5)+' deg', size=12)

    plt.savefig(ImageDirectory+"/"+ImageName+str(ImageNum),dpi=300, bbox_inches='tight')


# If running windows, be sure to install ImageMagick before running this code.    
if os.name=='nt':
    os.system("magick -delay "+str(FrameTime)+" "+ImageDirectory+"/"+ImageName+"*.png"+" "+ImageDirectory+"/"+VideoName+".gif")

if os.name=='posix':
    os.system("convert -delay "+str(FrameTime)+" "+ImageDirectory+"/"+ImageName+"*.png"+" "+ImageDirectory+"/"+VideoName+".gif")

Addtional Examples for Denavit-Hartenberg Parameters[edit | edit source]

Additional Examples for Denavit-Hartenberg Parameters

Modified DH parameters[edit | edit source]

Some books such as Introduction to Robotics: Mechanics and Control (3rd Edition) [24] use modified DH parameters. The difference between the classic DH parameters and the modified DH parameters are the locations of the coordinates system attachment to the links and the order of the performed transformations.

Modified DH parameters

Compared with the classic DH parameters, the coordinates of frame is put on axis i − 1, not the axis i in classic DH convention. The coordinates of is put on the axis i, not the axis i + 1 in classic DH convention.

Another difference is that according to the modified convention, the transform matrix is given by the following order of operations:

Thus, the matrix of the modified DH parameters becomes

Note that some books (e.g.:[25]) use and to indicate the length and twist of link n − 1 rather than link n. As a consequence, is formed only with parameters using the same subscript.

In some books, the order of transformation for a pair of consecutive rotation and translation (such as and ) is replaced. However, because matrix multiplication order for such pair does not matter, the result is the same. For example: .

Surveys of DH conventions and its differences have been published.[26][27]

Velocity and Acceleration in Kinematics[edit | edit source]

Further matrices can be defined to represent velocity and acceleration of bodies.[22][23] The velocity of body with respect to body can be represented in frame by the matrix

where is the angular velocity of body with respect to body and all the components are expressed in frame ; is the velocity of one point of body with respect to body (the pole). The pole is the point of passing through the origin of frame .

The acceleration matrix can be defined as the sum of the time derivative of the velocity plus the velocity squared

The velocity and the acceleration in frame of a point of body can be evaluated as

It is also possible to prove that

Velocity and acceleration matrices add up according to the following rules

in other words the absolute velocity is the sum of the parent velocity plus the relative velocity; for the acceleration the Coriolis' term is also present.

The components of velocity and acceleration matrices are expressed in an arbitrary frame and transform from one frame to another by the following rule

Jacobian Matrix (Portions Copied from Link, Link, and Link)[edit | edit source]

The time derivative of the kinematics equations yields the Jacobian of the robot, which relates the joint rates to the linear and angular velocity of the end-effector. The principle of virtual work shows that the Jacobian also provides a relationship between joint torques and the resultant force and torque applied by the end-effector. Singular configurations of the robot are identified by studying its Jacobian.

Velocity kinematics[edit | edit source]

The robot Jacobian results in a set of linear equations that relate the joint rates to the six-vector formed from the angular and linear velocity of the end-effector, known as a twist. Specifying the joint rates yields the end-effector twist directly.

Twists[edit | edit source]

A popular, and minimal way to represent the time derivative of a pose is a twist. A twist consists of two 3 × 1 vectors: and , where is the angular velocity of the moving rigid body (manipulator link), and is the linear velocity of the point on the moving rigid body that instantaneously coincides with the origin of the reference frame.

Propagation of Velocity[edit | edit source]

Rotational velocities that are written with respect to the same frame can simply be added. Hence:

The linear velocity of the origin of a link is that of the origin of the preceding link plus a rotational component:

Note: In the above equations the resulting velocities are expressed relative to the frame of the current link. is the vector pointing from the origin of frame to that of frame .

Example: The Two-Link Planar Manipulator[edit | edit source]

The two-link planar manipulator.

The link transformations that come with this manipulator can be easily derived graphically from looking at the figure on the right:

Here, and are the lengths link 1 and 2, respectively.

Note: Notice again the use of and as shorthand notations for and , respectively.

Hence, from the above equations:

and:

Also:

and:

With respect to the base frame, the end-effector velocity becomes:

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

theta1, theta2 = sp.symbols('theta1 theta2')
l1, l2 = sp.symbols('l1 l2')
theta1dot, theta2dot = sp.symbols('theta1dot theta2dot')

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 1, 0]])
f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 1, 0]])

f3_f2_T=np.array([[1, 0, 0, l2],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

#Extract rotation matrices from homogeneous transforms
f1_f0_R=np.delete(f1_f0_T,3,axis=0)
f1_f0_R=np.delete(f1_f0_R,3,axis=1)

f2_f1_R=np.delete(f2_f1_T,3,axis=0)
f2_f1_R=np.delete(f2_f1_R,3,axis=1)

f3_f2_R=np.delete(f3_f2_T,3,axis=0)
f3_f2_R=np.delete(f3_f2_R,3,axis=1)

f1_P_2=np.transpose(np.array([[f2_f1_T[0][3],f2_f1_T[1][3],f2_f1_T[2][3]]]))
f2_P_3=np.transpose(np.array([[f3_f2_T[0][3],f3_f2_T[1][3],f3_f2_T[2][3]]]))


f1_omega_1=np.array([[0,0,theta1dot]])
f1_omega_1=f1_omega_1.T

f0_f1_R=f1_f0_R.T
f1_f2_R=f2_f1_R.T
f2_f3_R=f3_f2_R.T

#Calculate f2_omega_2
f2_omega_2=np.dot(f1_f2_R,f1_omega_1)+np.transpose(np.array([[0,0,theta2dot]]))
print("f2_omega_2:\n", f2_omega_2)

#Calculate f3_omega_3
f3_omega_3=np.dot(f2_f3_R,f2_omega_2)+np.transpose(np.array([[0,0,0]]))
print("f3_omega_3:\n", f3_omega_3)

#Calculate f1_v_1
f1_v_1=np.transpose(np.array([[0,0,0]]))

#Calculate f2_v_2
#Note: Taking the cross product of two column vectors requires taking the transpose of
#the cross of the transposed column vectors
f2_v_2=np.dot(f1_f2_R,(f1_v_1+np.transpose(np.cross(f1_omega_1.T,f1_P_2.T))))
print("f2_v_2:\n",f2_v_2)

#Calculate f3_v_3
f3_v_3=np.dot(f2_f3_R,(f2_v_2+np.transpose(np.cross(f2_omega_2.T,f2_P_3.T))))
f3_v_3=sp.simplify(f3_v_3)
print("f3_v_3:\n",f3_v_3)

#Calculate f0_v_3
f0_v_3=np.dot(f1_f0_R,np.dot(f2_f1_R,np.dot(f3_f2_R,f3_v_3)))
f0_v_3=sp.simplify(f0_v_3)
print("f0_v_3:\n",f0_v_3)

Jacobian[edit | edit source]

Suppose kinematic (end-effector) equations. Each is a function of degrees of freedom:

Note that in the case of a non-redundant manipulator, .

The time derivate of the above equations is found as follows:

This can be written in vector form:

The matrix is called the Jacobian, the matrix of which the elements are the partial derivatives of the kinematics equations. The relationship between the end-effector velocity and the (known) joint velocities is thus fully described by the Jacobian. The end-effector velocity is a linear function of the joint velocities.

Example: The Two-Link Planar Manipulator[edit | edit source]

This example presents a slightly other way of solving the above problem. The position kinematics equations of this manipulator can be easily derived using the technique presented in the Serial Manipulator Position Kinematics section, or graphically from looking at the figure above:

Here, the Jacobian is:

Hence, the end-effector velocity is:

Note that the end-effector's angular velocity is, of course, the sum of the joint velocities. Another possibility that yields exactly the same result would be to explicitly include the equation for angular velocity and then calculate the Jacobian accordingly. This result is, as could already be expected, the same as the one in the above example, where the link transformation matrices were used to achieve the result.

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

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

f1_f0_T=np.array([[sp.cos(theta1), -sp.sin(theta1), 0, 0],
             [sp.sin(theta1), sp.cos(theta1), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]])

f2_f1_T=np.array([[sp.cos(theta2), -sp.sin(theta2), 0, l1],
            [sp.sin(theta2), sp.cos(theta2),0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

f3_f2_T=np.array([[1, 0, 0, l2],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])

End_Coord=np.transpose(np.array([[0,0,0,1]]))
X=np.dot(f1_f0_T,np.dot(f2_f1_T,np.dot(f3_f2_T,End_Coord)))
X=sp.simplify(X)
print("X:\n",X)

J=np.array([[sp.diff(X[0],theta1),sp.diff(X[0],theta2)],
           [sp.diff(X[1],theta1),sp.diff(X[1],theta2)]])
print("J:\n",J)

theta1dot, theta2dot = sp.symbols('theta1dot theta2dot')
thetadot=np.transpose(np.array([[theta1dot, theta2dot]]))

f0_v_3=np.dot(J,thetadot)
print("f0_v_3:\n",f0_v_3)

Additional Examples for Jacobian Matrices in Robotics[edit | edit source]

Additional Examples for Jacobian Matrices for Robotics

Quaternions for Rotation (forthcoming, but see this link[28])[edit | edit source]


References[edit | edit source]

  1. Paul, Richard (1981). Robot manipulators: mathematics, programming, and control : the computer control of robot manipulators. MIT Press, Cambridge, Massachusetts. ISBN 978-0-262-16082-7. https://books.google.com/books?id=UzZ3LAYqvRkC&printsec=frontcover&source=gbs_ge_summary_r&cad=0#v=onepage&q&f=false. 
  2. J. M. McCarthy, 1990, Introduction to Theoretical Kinematics, MIT Press, Cambridge, Massachusetts.
  3. Kinematics of a particle trajectory in a non-rotating frame of reference, "Kinematics". Wikipedia. 2020-01-14. https://en.wikipedia.org/w/index.php?title=Kinematics&oldid=935691489. 
  4. "Pyplot tutorial — Matplotlib 3.1.2 documentation". matplotlib.org. Retrieved 2020-01-30.
  5. "Rotation matrix". Wikipedia. 2020-01-16. https://en.wikipedia.org/w/index.php?title=Rotation_matrix&oldid=936013512. 
  6. Swokowski, Earl (1979). Calculus with Analytic Geometry (Second ed.). Boston: Prindle, Weber, and Schmidt. ISBN 0-87150-268-2. https://archive.org/details/studentsupplemen01bron. 
  7. "Robotics Kinematics and Dynamics/Description of Position and Orientation - Wikibooks, open books for an open world". en.wikibooks.org. Retrieved 2020-02-04.
  8. "Forward kinematics". Wikipedia. 2019-05-30. https://en.wikipedia.org/w/index.php?title=Forward_kinematics&oldid=899438331. 
  9. "Robotics Kinematics and Dynamics/Serial Manipulator Position Kinematics - Wikibooks, open books for an open world". en.wikibooks.org. Retrieved 2020-02-10.
  10. Paul, Richard (1981). Robot manipulators: mathematics, programming, and control : the computer control of robot manipulators. MIT Press, Cambridge, Massachusetts. ISBN 978-0-262-16082-7. https://books.google.com/books?id=UzZ3LAYqvRkC&printsec=frontcover&source=gbs_ge_summary_r&cad=0#v=onepage&q&f=false. 
  11. J. M. McCarthy, 1990, Introduction to Theoretical Kinematics, MIT Press, Cambridge, Massachusetts.
  12. "Forward kinematics". Wikipedia. 2020-01-24. https://en.wikipedia.org/w/index.php?title=Forward_kinematics&oldid=937417017. 
  13. "Denavit–Hartenberg parameters". Wikipedia. 2019-12-07. https://en.wikipedia.org/w/index.php?title=Denavit%E2%80%93Hartenberg_parameters&oldid=929693604. 
  14. J. Denavit and R.S. Hartenberg, 1955, "A kinematic notation for lower-pair mechanisms based on matrices." Trans ASME J. Appl. Mech, 23:215–221.
  15. Hartenberg, R. S., and J. Denavit. Kinematic Synthesis of Linkages. New York: McGraw-Hill, 1964 on-line through KMODDL
  16. Jennifer Kay. "Introduction to Homogeneous Transformations & Robot Kinematics" (PDF). Retrieved 2010-09-11.
  17. Learn About Robots. "Robot Forward Kinematics". Retrieved 2007-02-01.
  18. Denavit, Jacques; Hartenberg, Richard Scheunemann (1955). "A kinematic notation for lower-pair mechanisms based on matrices". Trans ASME J. Appl. Mech 23: 215–221. 
  19. Hartenberg, Richard Scheunemann; Denavit, Jacques (1965). Kinematic synthesis of linkages. McGraw-Hill series in mechanical engineering. New York: McGraw-Hill. pp. 435. http://ebooks.library.cornell.edu/k/kmoddl/toc_hartenberg1.html. 
  20. Paul, Richard (1981). Robot manipulators: mathematics, programming, and control : the computer control of robot manipulators. Cambridge, MA: MIT Press. ISBN 978-0-262-16082-7. https://books.google.com/books?id=UzZ3LAYqvRkC&printsec=frontcover. 
  21. Spong, Mark W.; Vidyasagar, M. (1989). Robot Dynamics and Control. New York: John Wiley & Sons. ISBN 9780471503521. 
  22. 22.0 22.1 Legnani, Giovanni; Casolo, Federico; Righettini, Paolo; Zappa, Bruno (1996). "A homogeneous matrix approach to 3D kinematics and dynamics — I. Theory". Mechanism and Machine Theory 31 (5): 573–587. doi:10.1016/0094-114X(95)00100-D. 
  23. 23.0 23.1 Legnani, Giovanni; Casolo, Federico; Righettini, Paolo; Zappa, Bruno (1996). "A homogeneous matrix approach to 3D kinematics and dynamics—II. Applications to chains of rigid bodies and serial manipulators". Mechanism and Machine Theory 31 (5): 589–605. doi:10.1016/0094-114X(95)00101-4. 
  24. John J. Craig, Introduction to Robotics: Mechanics and Control (3rd Edition) ISBN 978-0201543612
  25. Khalil, Wisama; Dombre, Etienne (2002). Modeling, identification and control of robots. New York: Taylor Francis. ISBN 1-56032-983-1. https://books.google.com/books?id=Nx4X95PNyAkC. 
  26. Lipkin, Harvey (2005). "A Note on Denavit–Hartenberg Notation in Robotics". Volume 7: 29th Mechanisms and Robotics Conference, Parts a and B. Vol. 2005. pp. 921–926. doi:10.1115/DETC2005-85460. ISBN 0-7918-4744-6.
  27. Waldron, Kenneth; Schmiedeler, James (2008). "Kinematics". Springer Handbook of Robotics. pp. 9–33. doi:10.1007/978-3-540-30301-5_2. ISBN 978-3-540-23957-4. 
  28. "Quaternions and spatial rotation". Wikipedia. 2019-11-27. https://en.wikipedia.org/w/index.php?title=Quaternions_and_spatial_rotation&oldid=928266335. 

Footnotes[edit | edit source]

  1. Note that if instead of rotating vectors, it is the reference frame that is being rotated, the signs on the sin θ terms will be reversed. If reference frame A is rotated anti-clockwise about the origin through an angle θ to create reference frame B, then Rx (with the signs flipped) will transform a vector described in reference frame A coordinates to reference frame B coordinates.