Robotic Mechanics and Modeling/Kinematics

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

edit
 
Inverse kinematics of SCARA robot done with MeKin2D.

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

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

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

Motion along a Line (1-D)

edit

Position in 1-D

edit

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

Velocity in 1-D

edit

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

 

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

 

Acceleration in 1-D

edit

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

 

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

 

Example for 1-D Motion in IPython (JupyterLab)

edit

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

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

If  ,  , and  , what is   after 1 second?

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

The answer is 1.5  .

Additional Examples of 1-D Kinematics

edit

Additional Examples of 1-D Kinematics

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

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

Representation of Vectors and Paths

edit

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

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

 

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

 

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

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

 

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

An Example for Representing a Position Vector in IPython

edit

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

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

An example of how to calculate the magnitude of vector  .

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

An example of how to plot the vector  .

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

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

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

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

An Example of Representing a 2D Path in IPython

edit

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

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

def x_p(t):
    return t

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

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

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

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

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

Additional Examples for Vectors

edit

Additional Examples for Vectors

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

Velocity and speed

edit

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

 

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

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

 

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

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

 

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

Acceleration

edit

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

 

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

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

 

or

 

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

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

 

Additional Examples for Velocity and Acceleration

edit

Additional Examples for Velocity and Acceleration

Relative position vector

edit

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

 

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

If point A has position components  

If point B has position components  

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

Relative velocity

edit
 
Relative velocities between two particles in classical mechanics.

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

 

which is the difference between the components of their velocities.

If point A has velocity components  

and point B has velocity components  

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

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

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

Relative acceleration

edit

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

 

which is the difference between the components of their accelerations.

If point C has acceleration components  

and point B has acceleration components  

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

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

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

edit

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

 

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

 

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

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

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

Rotations in Two Dimensions

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

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

 .

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

 .

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

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

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

 .

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

 .

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

An Example of Rotating a Vector in Two Dimensions with IPython

edit

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

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

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

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

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

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


Rotations in Three Dimensions

edit

Basic rotations

edit

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

 

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

 

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

General rotations

edit

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

 

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

 

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

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

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

edit

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

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


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

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

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

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

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

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

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

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

edit

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

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


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

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

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

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


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

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

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

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

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

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

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


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


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

edit

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

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


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

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

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

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

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

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

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


Additional Examples for Rotations

edit

Additional Examples for Rotations

Properties

edit

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

  •   (The rotation is an orthogonal matrix)

It follows that:

  •  

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

Mathematical Examples

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

Geometry

edit

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

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

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

 

which can be computed using the matrix multiplication

 

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

 

or

 

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

 

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

 

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

Multiplication

edit

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

 

The product of two rotation matrices is a rotation matrix:

 

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

 

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

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

edit

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

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

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

Rotation Matrix

edit

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

 

Rotation matrices can be interpreted in two ways:

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

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

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

edit
Code
%reset -f
import numpy as np

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

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

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

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

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

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

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

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

Graphical Representation of Rotated Reference Frame about Origin

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

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

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

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

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

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

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

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

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

Graphical Representation of Point in Rotated Frame about Origin

edit

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

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

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

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

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

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

#or

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

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

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

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

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

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

Additional Examples for Rotations and Coordinates

edit

Additional Examples for Rotations and Coordinates

Properties

edit

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

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

Elementary Rotations about Frame Axes

edit
 
Rotation by an angle   about the z-axis.

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

 

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

 

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

 

Compound Rotations

edit

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

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

 

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

Inverse Rotations

edit

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

 

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

 

Euler Angles

edit

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

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

Forward Mapping

edit

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

 

After writing out:

 

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

Inverse Mapping

edit

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

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

 
 
 

Example of Forward Mapping and Inverse Mapping by Inspection

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

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

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

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

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

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

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

Coordinate Singularities

edit

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

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

Roll-Pitch-Yaw Angles

edit

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

Forward Mapping
edit

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

 

After writing out:

 
Inverse Mapping
edit

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

 
 
 

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

Unit Quaternions

edit

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

Homogeneous Transform

edit

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

 

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

 

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

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

 

This is the same as writing:

 

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

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

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

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

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

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

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

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

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

b_a_R=EulerRzyx
a_b_R=b_a_R.T


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

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

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

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

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

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

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

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

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

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

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

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

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

b_a_R=EulerRzyx

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

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

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


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

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

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

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

Compound Poses

edit

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

 

Graphical Representation of Compound Poses

edit

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

 

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

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

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

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

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

b_a_R=EulerRzyx

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

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

c_b_T=b_a_T;

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

c_a_T=np.dot(b_a_T,c_b_T)

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

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

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

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

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

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

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

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

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

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

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

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

Additional Examples of Homogeneous Transforms

edit

Additional Examples of Homogeneous Transforms

Finite Displacement Twist

edit

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

 

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

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

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

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

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

 
Forward vs Backwards Kinematics

Forward Position Kinematics

edit

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

Solution

edit

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

 

Examples

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

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

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

 

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

 

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

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

 

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

 

The solution to the forward kinematics problem is then:

 

Hence:

 

The resulting kinematic equations are:

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

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

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

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

#display(ee_bs_T.subs(replacements))

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

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

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

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

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

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

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

Two Configurations for the Same Resulting Location of the End Effector

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

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

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

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

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

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

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

#display(ee_bs_T.subs(replacements))

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

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

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

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

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

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

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


Kinematic Equations for a Serial Manipulating Robotic Arm

edit

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

 

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

edit

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

 

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

 

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

 

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

Kinematics equations revisited

edit

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

 

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

Denavit-Hartenberg matrix

edit

The matrices associated with these operations are:

 

Similarly,

 

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

 

known as the Denavit-Hartenberg matrix.

Denavit-Hartenberg Notation Re-visited

edit

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

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

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

Denavit–Hartenberg Convention and Transformations

edit

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

 

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

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

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

 

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

 

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

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

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

Four parameters

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

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

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

A visualization of D–H parameterization is available: YouTube

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

We can note constraints on the relationships between the axes:

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

Denavit–Hartenberg matrix

edit

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

 

and

 

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

 

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

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.  .

edit
 
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')
edit
 
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")
edit
 
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')
edit
 
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

Additional Examples for Denavit-Hartenberg Parameters

Modified DH parameters

edit

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

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

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

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

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

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  .

edit
 
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

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.

 

edit

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

Additional Examples for Jacobian Matrices for Robotics

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

edit


References

edit
  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
  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.