Robotic Mechanics and Modeling/Kinematics
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)
editPosition in 1-D
editIn 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
editIn 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
editIn 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)
editAn example of how to setup a symbolic expression in IPython.
%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?
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
editKinematics of a particle trajectory in a non-rotating frame of reference (Copied and edited from Kinematics[3])
editRepresentation of Vectors and Paths
editParticle 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
editAn example of how to define a "vector" as an array in IPython.
%reset -f
import numpy as np
P=np.array([1,2,3])
An example of how to calculate the magnitude of vector .
MagnP=np.linalg.norm(P) #one approach
MagnP=np.dot(P,P)**0.5 #another approach
An example of how to plot the vector .
%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')
An Example of Representing a 2D Path in IPython
editLet's represent a trajectory with vector path , where and . Then, we can use the following code to represent this path of this trajectory.[4]
# 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')
Additional Examples for Vectors
editAdditional Examples for Vectors
Velocity and speed
editThe 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
editThe 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
editAdditional Examples for Velocity and Acceleration
Relative position vector
editA 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
editThe 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
editThe 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])
editOne 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
editIn 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
editWe will create a 2D rotation matrix to provide a rotation of to the vector at the origin.
# 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')
Rotations in Three Dimensions
editBasic rotations
editA 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
editOther 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
editCreate a 3D rotation matrix to provide a rotation of about the Z-axis to the vector at the origin.
%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')
An Example of Successive XYZ Space-fixed Rotations of a Vector in Three Dimensions
editSuccessive XYZ space-fixed rotations of vector at the origin of about X, then about Y, and then about Z.
%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)
An Example of Successive ZYX Body-fixed, Euler Rotations of a Vector in Three Dimensions
editSuccessive 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.
%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])
Additional Examples for Rotations
editAdditional Examples for Rotations
Properties
editFor 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.
- The transpose of the 2 × 2 matrix
- 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.
- The 3 × 3 permutation matrix
- 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
editIn 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
editThe 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.
In general, a rigid body in three-dimensional space has six degrees of freedom: three rotational and three translational.
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
editA 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:
- As the representation of the rotation of the first frame into the second (active interpretation).
- 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%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%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
editIf we have in reference frame . What is in frame after Euler sequence ZYX by about each axis?
%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
editAdditional Examples for Rotations and Coordinates
Properties
editSome of the properties of the rotation matrix that may be of practical value, are:
- The column vectors of are normal to each other.
- The length of the column vectors of equals 1.
- 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.
- Since is orthonormal, .
Elementary Rotations about Frame Axes
editThe 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
editCompound 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
editThe 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
editContrary 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
editForward 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
editIn 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%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
editIn 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
editThe 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
editThe 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
editThe 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
editUnit 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
editThe 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%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%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
editIf 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
editIf 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 .
%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
editAdditional Examples of Homogeneous Transforms
Finite Displacement Twist
editA 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 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 Position Kinematics
editThe 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
editSuppose 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
editThe Planar Three-Link Manipulator
editThe equations below use 3 × 3 pose matrices (homogeneous transformations), as this is just a 2-dimensional case (cf. the figure on the right).
The pose of the first link, relative to the reference frame, is given by (recall the elementary rotation about the z-axis from the previous section):
The pose of the second link, relative to the first link, is given by:
This corresponds to a rotation by an angle and a translation by a distance , where is the length of the first link.
The pose of the third link, relative to the second link, is given by:
The pose of the end effector, relative to the third link, is given by:
The solution to the forward kinematics problem is then:
Hence:
The resulting kinematic equations are:
Graphical Example of Planar, Three-link Manipulator using Compound Transformations
edit%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
editWe 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%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')
Kinematic Equations for a Serial Manipulating Robotic Arm
editThe 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]
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
editThe 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
editThe 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
editIn 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
editA 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:
- the -axis is in the direction of the joint axis
- 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. - the -axis follows from the - and -axis by choosing it to be a right-handed coordinate system.
Four parameters
editThe 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
editIt 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.