Computational Kinematic Approach
Transcript of Computational Kinematic Approach
14
Constrained Kinematics Typically, mechanical systems are made by connecting bodies (links) through sequence of joints. Each
joint allows specific degrees of freedom and restrict the other degrees of freedom.
For example, a hinge (revolute) joint allows rotation and restricts the remaining degrees of freedom (5 in
the 3-D space and 2 in a plane).
In the following we will explore the constraints associated with some common joints.
15
Examples of Planar Constraints
Revolute (Hinge) Joint
A revolute joint ensures relative rotation between the two joined bodies, i.e.
๐๐๐๐๐๐ = ๐๐๐๐๐๐
This formulation will yield two equations that correspond to the x and y
components.
However,
๐๐๐๐ โ ๐๐๐๐
Prismatic Joint
On the other hand, prismatic joint allows relative translation between two connected bodies. Relative
rotation is however restricted.
These relations can be represented as follows:
1. The two bodies always rotate together
๐๐๐๐ โ ๐๐๐๐ = ๐๐
2. The dot product of these two vectors is always zero:
โข Line of action (axis) of the prismatic joint
โข A vector is one of the two bodies that is normal to this line of action:
โ๐๐ โ ๐๐๐๐๐๐๐๐ = 0
In matrix notation, the above equation becomes:
โ๐๐๐๐๐๐๐๐๐๐๐๐ = 0 While easier formulation exist especially in the case when one of the two bodies is completely fixed, the
above formulation is general and works when both bodies are moving.
We will expand on these formulations later.
Class Exercise: Derive the constraint equations for a wheel rolling on a flat ground
ฮธi
Body i
Body j
P
ฮธi
Body j
Body i
Pj
Pi
hi
RijP
16
Computational Kinematic Approach
โข Classical kinematics can be used to accurately formulate equations describing the kinematics of
variables of a machine in terms of its input motion.
โข Formulating these equations required a prior knowledge of the geometry and sequence of joints
within the machine.
โข However, in any computer-aided software, the user creates a machine on the spot. In this case,
classical approach cannot be used. Instead, a computational approach can be general enough to solve
the kinematics of any machine.
โข Kinematic analysis can be viewed as solving a set of algebraic equations that describes the joint
connectivity of a certain machine.
โข Another approach is to combine the equations and solve them.
โข The resulting system of the coupled equations can be solved using numerical techniques.
โข This is the technique used by most computer simulation software packages.
17
Absolute Coordinates
X
Y XiYi
ฮธi
Pi
Ri
uiP
riP Oi
โข The position of a body can be described using Ri and ฮธi (Absolute Cartesian Coordinates)
๐๐๐๐๐๐ = ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐
Ri global position vector of the reference point Ai transformation matrix from the body to global coordinate systems
๐ ๐ ๐๐ = ๏ฟฝ๐ ๐ ๐๐๐ฅ๐ฅ๐ ๐ ๐๐๐ฆ๐ฆ
๏ฟฝ
u๏ฟฝiP = ๏ฟฝ๏ฟฝฬ ๏ฟฝ๐ฅ๐๐๐๐๐ฆ๐ฆ๏ฟฝ๐๐๐๐
๏ฟฝ
๐ด๐ด๐๐ = ๏ฟฝcos๐๐๐๐ โ sin๐๐๐๐sin๐๐๐๐ cos ๐๐๐๐
๏ฟฝ
18
Extension to Multibody System A multi-body system consisting of nb unconstrained rigid bodies has 3nb independent generalized coordinates that are described using the vector q
๐๐ = ๏ฟฝ๐ ๐ 1๐ฅ๐ฅ ๐ ๐ 1๐ฆ๐ฆ ๐๐1 ๐ ๐ 2๐ฅ๐ฅ ๐ ๐ 2๐ฆ๐ฆ ๐๐2 โฏ โฏ ๐ ๐ ๐๐๐๐๐ฅ๐ฅ ๐ ๐ ๐๐๐๐๐ฆ๐ฆ ๐๐๐๐๐๐๏ฟฝ๐๐ ๐๐ = [๐ ๐ 1 ๐๐1 ๐ ๐ 2 ๐๐2 โฏ โฏ โฏ โฏ โฏ ๐ ๐ ๐๐๐๐ ๐๐๐๐๐๐]๐๐
๐๐ = [๐๐1 ๐๐2 โฏ โฏ โฏ โฏ โฏ โฏ ๐๐๐๐๐๐]๐๐ where,
๐๐๐๐ = ๏ฟฝ๐ ๐ ๐๐
๐๐๐๐๏ฟฝ
19
Kinematic Constraints The relation between two bodies i and j can be related by the following equation
๐๐๐๐๐๐ โ ๐๐๐๐๐๐ = ๐๐(๐ก๐ก)
Or,
๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ โ ๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ = ๐๐(๐ก๐ก)
f(t) is a known function of time
20
Ground Constraints
A body that has zero degrees of freedom (ground or fixed link)
๐๐๐๐ = ๐๐
It is convenient to have the coordinates of frame i matching those of the global frame or,
๏ฟฝ๐ ๐ ๐๐๐ฅ๐ฅ๐ ๐ ๐๐๐ฅ๐ฅ๐๐๐๐๏ฟฝ = 0
21
Revolute Joint Constraint
X
Y
Xi
ฮธi
Ri
uiP
Oi
P
Yi
XjYj
Rj
Oj
ujP
riP,
rjP
Revolute joint ensures relative rotation between the two joined bodies, i.e.
๐๐๐๐๐๐ = ๐๐๐๐๐๐
๐๐๐๐ โ ๐๐๐๐
Therefore,
๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ = ๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ
or,
๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ โ ๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ = 0
We end with one matrix equation (two scalar equations).
๏ฟฝ๐ ๐ ๐๐๐ฅ๐ฅ๐ ๐ ๐๐๐ฆ๐ฆ
๏ฟฝ + ๏ฟฝcos๐๐๐๐ โ sin๐๐๐๐sin๐๐๐๐ cos ๐๐๐๐
๏ฟฝ ๏ฟฝ๏ฟฝฬ ๏ฟฝ๐ฅ๐๐๐๐๐ฆ๐ฆ๏ฟฝ๐๐๐๐
๏ฟฝ โ ๏ฟฝ๐ ๐ ๐๐๐ฅ๐ฅ๐ ๐ ๐๐๐ฆ๐ฆ
๏ฟฝ โ ๏ฟฝcos ๐๐๐๐ โ sin๐๐๐๐sin๐๐๐๐ cos ๐๐๐๐
๏ฟฝ ๏ฟฝ๏ฟฝฬ ๏ฟฝ๐ฅ๐๐๐๐๐ฆ๐ฆ๏ฟฝ๐๐๐๐
๏ฟฝ = 0
22
Prismatic Joint
ฮธi
Body j
Body i
Pj
Pi
hi Qirij P
Prismatic joint allows relative translation between the connected bodies. Relative rotation is however
restricted as two bodies have to rotate together, which means,
๐๐๐๐ โ ๐๐๐๐ = ๐๐
On the other hand, one of the two bodies is free to slide in and out of the other one along a specified line
of action. Geometrically, this can be represented using two vectors:
โข The line of action connecting two points on j and i respectively as in the figure above and
โข A vector in Body i that is normal to this line of action
Regardless of the orientations of these two bodies, these two vectors will always remain normal to each
other. This relation can be represented using dot product of these two vectors:
โ๐๐๐๐๐๐๐๐๐๐๐๐ = 0
where,
โ๐๐ = ๐ด๐ด๐๐๏ฟฝ ๐ข๐ข๏ฟฝ๐๐๐๐ โ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ
๐๐๐๐๐๐๐๐ = ๐๐๐๐๐๐ โ ๐๐๐๐๐๐ = ๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ โ ๏ฟฝ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐๏ฟฝ
These equations can be combined as,
๏ฟฝ๐๐๐๐ โ ๐๐๐๐ โ ๐๐โ๐๐๐๐๐๐๐๐๐๐๐๐
๏ฟฝ = ๏ฟฝ00๏ฟฝ
23
Example
Derive the algebraic kinematic constraint equations of this two-link manipulator.
Assumptions:
โข The two links are uniform rods.
โข Each link has its frame at its geometric center, representing the center of mass. This is typical when
modeling dynamic systems.
โข For convenience, the frame of link 1 coincides with the global frame.
X1
Y1
X3
ฮธ2O
Y3
X2
Y2
ฮธ3
A
Solution:
โข Since frame 1 is fixed and coincides with the global frame
๏ฟฝ๐ ๐ 1๐ฅ๐ฅ๐ ๐ 1๐ฆ๐ฆ๐๐1
๏ฟฝ = 0
โข Joint O connects links 1 and 2. It is easy to see that frame 2 traces a circle whose center is O.
Therefore, this hinge can be described using this equation:
(๐ ๐ 2 + ๐ด๐ด2 ๐ข๐ข๏ฟฝ2๐๐) โ ๐ ๐ 1 = 0
๏ฟฝ๐ ๐ 2๐ฅ๐ฅ๐ ๐ 2๐ฆ๐ฆ
๏ฟฝ + ๏ฟฝcos๐๐2 โ sin๐๐2sin๐๐2 cos๐๐2
๏ฟฝ ๏ฟฝโ๐๐2
20๏ฟฝ โ ๏ฟฝ00๏ฟฝ = 0
Remember that R1=0, which means this equation becomes,
๏ฟฝ๐ ๐ 2๐ฅ๐ฅ๐ ๐ 2๐ฆ๐ฆ
๏ฟฝ + ๏ฟฝcos ๐๐2 โ sin๐๐2sin๐๐2 cos ๐๐2
๏ฟฝ ๏ฟฝโ๐๐2
20๏ฟฝ = 0
24
โข The same process can be repeated for joint A by moving:
1. frame 2 a distance of ๏ฟฝ๐๐2
2๏ฟฝ (forward) and
2. frame 3 a distance of ๏ฟฝโ๐๐3
2๏ฟฝ (backward) to have both frames meet at joint A.
(๐ ๐ 3 + ๐ด๐ด3 ๐ข๐ข๏ฟฝ3๐ด๐ด) โ (๐ ๐ 2 + ๐ด๐ด2 ๐ข๐ข๏ฟฝ2๐ด๐ด) = 0
๏ฟฝ๐ ๐ 3๐ฅ๐ฅ๐ ๐ 3๐ฆ๐ฆ
๏ฟฝ + ๏ฟฝcos๐๐3 โ sin๐๐3sin๐๐3 cos๐๐3
๏ฟฝ ๏ฟฝโ๐๐3
20๏ฟฝ โ ๏ฟฝ
๐ ๐ 2๐ฅ๐ฅ๐ ๐ 2๐ฆ๐ฆ
๏ฟฝ โ ๏ฟฝcos๐๐2 โ sin๐๐2sin๐๐2 cos๐๐2
๏ฟฝ ๏ฟฝ๐๐2
20๏ฟฝ = 0
โข These kinematic constraint equations can be written in a vector form as:
๐ถ๐ถ(๐๐1, ๐๐2, ๐๐3) =
โฉโชโชโชโชโจ
โชโชโชโชโง ๐ ๐ 1๐ฅ๐ฅ
๐ ๐ 1๐ฆ๐ฆ๐๐1
๐ ๐ 2๐ฅ๐ฅ โ๐๐2 cos ๐๐2
2
๐ ๐ 2๐ฆ๐ฆ โ๐๐2 sin๐๐2
2
๐ ๐ 3๐ฅ๐ฅ โ๐๐3 cos๐๐3
2โ ๐ ๐ 2๐ฅ๐ฅ โ
๐๐2 cos ๐๐2
2
๐ ๐ 3๐ฆ๐ฆ โ๐๐3 sin๐๐3
2โ ๐ ๐ 2๐ฆ๐ฆ โ
๐๐2 sin๐๐2
2 โญโชโชโชโชโฌ
โชโชโชโชโซ
=
โฉโชโจ
โชโง
0000000โญโชโฌ
โชโซ
where C=[C1,C2,โฆ.,C7]T is the vector of algebraic constraints that describe this system.
โข Since the system has nine absolute coordinates.
โข Therefore, the number of degrees of freedom is equal to two
Homework โข Derive the kinematic constraint equations for the machines in Figures P3.6 and P3.24.
25
Computational Method in Kinematics
โข The presented approach is based on exploiting the idea of kinematic constraints numerically. โข This way we avoid deriving specific solution for every mechanical system. Introduction A multi-body system consisting of nb unconstrained rigid bodies has 3nb independent generalized coordinates that are described using the vector q
๐๐ = ๏ฟฝ๐ ๐ 1๐ฅ๐ฅ ๐ ๐ 1๐ฆ๐ฆ ๐๐1 ๐ ๐ 2๐ฅ๐ฅ ๐ ๐ 2๐ฆ๐ฆ ๐๐2 โฏ โฏ ๐ ๐ ๐๐๐๐๐ฅ๐ฅ ๐ ๐ ๐๐๐๐๐ฆ๐ฆ ๐๐๐๐๐๐๏ฟฝ๐๐ ๐๐ = [๐ ๐ 1 ๐๐1 ๐ ๐ 2 ๐๐2 โฏ โฏ โฏ โฏ โฏ ๐ ๐ ๐๐๐๐ ๐๐๐๐๐๐]๐๐
๐๐ = [๐๐1 ๐๐2 โฏ โฏ โฏ โฏ โฏ โฏ ๐๐๐๐๐๐]๐๐ where,
๐๐๐๐ = ๏ฟฝ๐ ๐ ๐๐
๐๐๐๐๏ฟฝ
The vector of algebraic kinematic constraints that describe this system
C(q,t)=[C1(q,t),C2(q,t),โฆ.,Cnc(q,t)]T
nc is the total number of constraints
26
Categories of Mechanical Systems
Mechanical systems can be divided into:
1. Dynamically-Driven: The number of the constraint equations is less than the number of the system
coordinates (nc < n).
Dynamics forces are needed to perform the analysis.
Example: A ball falling toward ground.
โข No driving input
โข No kinematic constraint
โข The result is 0 equations that are less than the number of degrees of freedom of the system (3))
2. Kinematically-Driven: The number of the linearly independent constraint equations is equal to the
number of the system coordinates (nc = n).
Example: The two-link manipulator
โข Adding the number of driving inputs to the constraints equations (2 inputs to 7 constraints).
โข The result is 9 equations that are equal the number of degrees of freedom of the system (3x3))
This chapter focuses on the Kinematically-Driven systems only.
Dynamically-Driven system will be analyzed later.
27
Example on Formulating a Kinematically Driven Mechanical Systems:
X1
Y1
X3
ฮธ2O
Y3
X2
Y2
ฮธ3
A
Two additional inputs are needed: ฮธ2 and ฮธ3 for links 2 and 3 respectively.
If both links are driven using constant speeds: ฯ2 and ฯ3,
๐ถ๐ถ(๐๐1, ๐๐2, ๐๐3) =
โฉโชโชโชโชโชโชโจ
โชโชโชโชโชโชโง ๐ ๐ 1๐ฅ๐ฅ
๐ ๐ 1๐ฆ๐ฆ๐๐1
๐ ๐ 2๐ฅ๐ฅ โ๐๐2 cos ๐๐2
2
๐ ๐ 2๐ฆ๐ฆ โ๐๐2 sin๐๐2
2
๐ ๐ 2๐ฅ๐ฅ +๐๐2 cos ๐๐2
2โ ๐ ๐ 3๐ฅ๐ฅ +
๐๐3 cos๐๐3
2
๐ ๐ 2๐ฆ๐ฆ +๐๐2 sin๐๐2
2โ ๐ ๐ 3๐ฆ๐ฆ +
๐๐3 sin๐๐3
2๐ฝ๐ฝ๐๐ โ ๐ฝ๐ฝ๐๐๐๐ โ ๐๐๐๐๐๐๐ฝ๐ฝ๐๐ โ ๐ฝ๐ฝ๐๐๐๐ โ ๐๐๐๐๐๐๐๐ โญ
โชโชโชโชโชโชโฌ
โชโชโชโชโชโชโซ
=
โฉโชโชโจ
โชโชโง
0000000๐๐๐๐โญโชโชโฌ
โชโชโซ
28
Homework โข Derive the kinematic constraint equations including driving input constraints for the machines in
Figures P3.3 and P3.4.
29
Position Analysis
โข The kinematic constraints equations are of trigonometric natures and are generally nonlinear.
โข While a closed form solution can be obtained in many cases, it has to be performed at each case
separately.
โข A computer-driven (computational) approach requires solving these equations using a numerical
method.
โข A typical approach is to use Newton-Raphson Algorithm.
Sir Isaac Newton (1642-1727)
http://en.wikipedia.org/wiki/Isaac_Newton
Joseph Raphson (1648-1715)
http://en.wikipedia.org/wiki/Raphson
30
Newton-Raphson Algorithm
Newton-Raphsonโs algorithm is based on Taylorโs Theorem:
๐๐(๐ฅ๐ฅ + โ๐ฅ๐ฅ) = ๐๐(๐ฅ๐ฅ) +๐๐๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ
โ๐ฅ๐ฅ +12!๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ2
(โ๐ฅ๐ฅ)2 + โฏ+1
(๐๐ โ 1)!๐๐๐๐โ1๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ๐๐โ1
(โ๐ฅ๐ฅ)๐๐โ1
Brook Taylor (1685-1731)
http://en.wikipedia.org/wiki/Brook_Taylor
โข Taylorโs Theorem can be expanded to multivariable functions after expressing it in matrix notation
as follows:
1x m mx1
๐๐(๐ฅ๐ฅ + โ๐ฅ๐ฅ) = ๐๐(๐ฅ๐ฅ) + ๏ฟฝ๐๐๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ1
๐๐๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ2
โฆ๐๐๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ๐๐
๏ฟฝ๏ฟฝ
โ๐ฅ๐ฅ1โ๐ฅ๐ฅ2โฎ
โ๐ฅ๐ฅ๐๐
๏ฟฝ
+12!๏ฟฝ
โ๐ฅ๐ฅ1โ๐ฅ๐ฅ2โฎ
โ๐ฅ๐ฅ๐๐
๏ฟฝ
๐๐
โฃโขโขโขโขโขโขโก ๐๐
2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ12
๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ1๐๐๐ฅ๐ฅ2
โฆ๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ1๐๐๐ฅ๐ฅ๐๐
๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ2๐๐๐ฅ๐ฅ1
๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ22
โฆ๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ2๐๐๐ฅ๐ฅ๐๐
โฎ๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ๐๐๐๐๐ฅ๐ฅ1
โฎ๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ๐๐๐๐๐ฅ๐ฅ2
โฎ โฆ
โฎ ๐๐2๐๐(๐ฅ๐ฅ)๐๐๐ฅ๐ฅ๐๐2 โฆ
โฅโฅโฅโฅโฅโฅโค
๏ฟฝ
โ๐ฅ๐ฅ1โ๐ฅ๐ฅ2โฎ
โ๐ฅ๐ฅ๐๐
๏ฟฝ + โฏ
1xm Hessian Matrix: mxm mx1
31
โข If we have a set of functions, we can apply Taylorโs Theorem to them by expanding the above
equation,
๐ถ๐ถ(๐๐๐๐ + โ๐๐๐๐ , ๐ก๐ก) = ๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก) + ๐ถ๐ถ๐๐๐๐โ๐๐๐๐ + โฏ (โ๐๐๐๐โ๐๐๐๐ ๐๐๐๐๐๐๐๐๐๐ ๐ก๐ก๐๐๐๐๐ก๐ก๐ก๐ก) โ๐๐ = [โ๐๐1 โ๐๐2 โฏ โฏ โฏ โฏ โฏ โฏ โ๐๐๐๐]๐๐
Cqi is the Jacobian Matrix, which can be defined as,
๐ถ๐ถ๐๐๐๐ =
โฃโขโขโขโขโขโขโก๐๐๐ถ๐ถ1๐๐๐๐1
๐๐๐ถ๐ถ1๐๐๐๐2
๐๐๐ถ๐ถ2๐๐๐๐1
๐๐๐ถ๐ถ2๐๐๐๐2
โฏ
๐๐๐ถ๐ถ1๐๐๐๐๐๐๐๐๐ถ๐ถ2๐๐๐๐๐๐
โฎ โฑ โฎ๐๐๐ถ๐ถ๐๐๐๐๐๐1
๐๐๐ถ๐ถ๐๐๐๐๐๐2
โฏ๐๐๐ถ๐ถ๐๐๐๐๐๐๐๐โฆ
โฅโฅโฅโฅโฅโฅโค
Note: ๐ถ๐ถ๐๐๐๐ is a square matrix for a kinematically-driven system since n=nc
Carl Gustav Jacob Jacobi (1804-1851)
https://en.wikipedia.org/wiki/Carl_Gustav_Jacob_Jacobi
32
If the constraint equations are linearly independent1, C is a nonsingular matrix.
Assuming ๐๐๐๐ + โ๐๐๐๐ to be the exact solution, then
๐ถ๐ถ(๐๐๐๐ + โ๐๐๐๐ , ๐ก๐ก) = 0
Therefore,
๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก) + ๐ถ๐ถ๐๐๐๐โ๐๐๐๐ + โฏ = 0
If the assumed solution is close enough to the correct solution, the second order and higher terms can
be neglected, which means:
๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก) + ๐ถ๐ถ๐๐๐๐โ๐๐๐๐ โ 0
๐ถ๐ถ๐๐๐๐โ๐๐๐๐ = โ๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก)
โ๐๐๐๐ = โ๐ถ๐ถ๐๐๐๐โ1๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก)
Remember that the constraint Jacobian matrix ๐ถ๐ถ๐๐๐๐ is assumed to be nonsingular, i.e., an inverse of
๐ถ๐ถ๐๐๐๐ exists.
The equation can be then solved for the vector of the Newton Differences, โ๐๐๐๐
The vector is updated as,
๐๐๐๐+1 = ๐๐๐๐ + โ๐๐๐๐
where, i is the iteration number. The updated vector can be used to reconstruct the equation above.
The process continues until:
|โ๐๐๐๐| < ๐๐1 or,
|๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก)| < ๐๐2
The above termination criteria means that search stops because either: 1. The changes of the vector have become extremely small or, 2. The Kinematic Constraints matrix approaches zero Note: ฮต1 is a vector while ฮต2 is a one value.
1 Linearly independent constraints is a set of unique constraints where no constraint can be expressed in terms of the other ones.
33
Example:
X1
Y1
X3
ฮธ2O
Y3
X2
Y2
ฮธ3
A
Two additional inputs are needed: ฮธ2 and ฮธ3 for links 2 and 3 respectively. If both links are driven using
constant speeds: ฯ2 and ฯ3
๐ถ๐ถ(๐๐1, ๐๐2, ๐๐3) =
โฉโชโชโชโชโชโชโจ
โชโชโชโชโชโชโง ๐ ๐ 1๐ฅ๐ฅ
๐ ๐ 1๐ฆ๐ฆ๐๐1
๐ ๐ 2๐ฅ๐ฅ โ๐๐2 cos ๐๐2
2
๐ ๐ 2๐ฆ๐ฆ โ๐๐2 sin๐๐2
2
๐ ๐ 2๐ฅ๐ฅ +๐๐2 cos๐๐2
2โ ๐ ๐ 3๐ฅ๐ฅ +
๐๐3 cos ๐๐3
2
๐ ๐ 2๐ฆ๐ฆ +๐๐2 sin๐๐2
2โ ๐ ๐ 3๐ฆ๐ฆ +
๐๐3 sin๐๐3
2๐๐2 โ ๐๐2๐๐ โ ๐๐2๐ก๐ก๐๐3 โ ๐๐3๐๐ โ ๐๐3๐ก๐ก โญ
โชโชโชโชโชโชโฌ
โชโชโชโชโชโชโซ
=
โฉโชโชโจ
โชโชโง
000000000โญโชโชโฌ
โชโชโซ
๐๐ = ๏ฟฝ๐ ๐ 1๐ฅ๐ฅ ๐ ๐ 1๐ฆ๐ฆ ๐๐1 ๐ ๐ 2๐ฅ๐ฅ ๐ ๐ 2๐ฆ๐ฆ ๐๐2 ๐ ๐ 3๐ฅ๐ฅ ๐ ๐ 3๐ฆ๐ฆ ๐๐3๏ฟฝ๐๐
34
The Jacobian matrix is,
๐ถ๐ถ๐๐๐๐ =
โฃโขโขโขโขโขโขโก๐๐๐ถ๐ถ1๐๐๐๐1
๐๐๐ถ๐ถ1๐๐๐๐2
๐๐๐ถ๐ถ2๐๐๐๐1
๐๐๐ถ๐ถ2๐๐๐๐2
โฏ
๐๐๐ถ๐ถ1๐๐๐๐๐๐๐๐๐ถ๐ถ2๐๐๐๐๐๐
โฎ โฑ โฎ๐๐๐ถ๐ถ๐๐๐๐๐๐1
๐๐๐ถ๐ถ๐๐๐๐๐๐2
โฏ๐๐๐ถ๐ถ๐๐๐๐๐๐๐๐โฆ
โฅโฅโฅโฅโฅโฅโค
In this case ๐ถ๐ถ๐๐๐๐ can be obtained by partial differentiation of the constraint matrix as follows:
๐ถ๐ถ๐๐ =
โฃโขโขโขโขโขโขโขโก1 0 00 1 00 0 1
0 0 0 0 0 0 0 0 0
0 0 00 0 0
0 0 00 0 00 0 00 0 0
1 0 ๐๐2 sin๐๐2 2โ 0 1 โ ๐๐2 cos๐๐2 2โ 1 0 โ ๐๐2 sin๐๐2 2โ
0 0 0 0 0 0
โ1 0 โ ๐๐3 sin๐๐3 2โ0 0 00 0 00 0 0
0 1 ๐๐2 cos๐๐2 2โ 0 0 1 0 0 0
0 โ1 ๐๐3 cos ๐๐3 2โ 0 0 0 0 0 1 โฆ
โฅโฅโฅโฅโฅโฅโฅโค
35
Velocity Analysis Differentiating the vector constraint equation
๐๐๐ถ๐ถ(๐๐, ๐ก๐ก)๐๐๐ก๐ก
= 0
๐๐๐ถ๐ถ๐๐๐๐
๐๐๐๐๐๐๐ก๐ก
+๐๐๐ถ๐ถ๐๐๐ก๐ก
= 0
Or,
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐ก๐ก = 0
Cq is the constraint Jacobian Matrix
Ct is the partial differentiation of C with respect to time:
๐ถ๐ถ๐ก๐ก = ๏ฟฝ๐๐๐ถ๐ถ1๐๐๐ก๐ก
๐๐๐ถ๐ถ2๐๐๐ก๐ก
โฏ ๐๐๐ถ๐ถ๐๐๐๐๐ก๐ก
๏ฟฝ๐๐
Rearranging,
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ = โ๐ถ๐ถ๐ก๐ก
Or,
๏ฟฝฬ๏ฟฝ๐ = โ๐ถ๐ถ๐๐โ1๐ถ๐ถ๐ก๐ก
You may want to notice the similarity between:
โข Position formulation (โ๐๐๐๐ = โ๐ถ๐ถ๐๐๐๐โ1๐ถ๐ถ(๐๐๐๐ , ๐ก๐ก)) and
โข Velocity formulation (๏ฟฝฬ๏ฟฝ๐ = โ๐ถ๐ถ๐๐โ1๐ถ๐ถ๐ก๐ก)
Why? What is the difference?
36
Acceleration Analysis
Differentiating the velocity equation ๐๐๐๐๐ก๐ก๏ฟฝ๐ถ๐ถ๐๐(๐๐, ๐ก๐ก)๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐ก๐ก(๐๐, ๐ก๐ก)๏ฟฝ = 0
๐๐๐๐๐ก๐ก๏ฟฝ๐ถ๐ถ๐๐(๐๐, ๐ก๐ก)๏ฟฝฬ๏ฟฝ๐๏ฟฝ +
๐๐๐๐๐ก๐ก๏ฟฝ๐ถ๐ถ๐ก๐ก(๐๐, ๐ก๐ก)๏ฟฝ = 0
๏ฟฝ๏ฟฝ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐๏ฟฝ + ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ + ๏ฟฝ๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐ก๐ก๐ก๐ก๏ฟฝ = 0
Rearranging,
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ + ๏ฟฝ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐ + ๐ถ๐ถ๐๐๐ก๐ก๏ฟฝ ๏ฟฝฬ๏ฟฝ๐ + ๏ฟฝ๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐ก๐ก๐ก๐ก๏ฟฝ = 0
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ + ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐ + 2๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐ + ๐ถ๐ถ๐ก๐ก๐ก๐ก = 0
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ = โ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐ โ 2๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐ โ ๐ถ๐ถ๐ก๐ก๐ก๐ก = ๐๐๐๐
Or,
๏ฟฝฬ๏ฟฝ๐ = ๐ถ๐ถ๐๐โ1๐๐๐๐
37
Homework
โข Solve Problem 3.19
Problem 3.19
๐๐๐๐๐๐ = ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๐ข๐ข๏ฟฝ๐๐๐๐
๏ฟฝ๐ฃ๐ฃ๐ก๐ก0 ๏ฟฝ = ๐ ๐ ๐๐ + ๐ด๐ด๐๐ ๏ฟฝ031.2๏ฟฝ
๐ ๐ ๐๐ = ๏ฟฝ๐ฃ๐ฃ๐ก๐ก0 ๏ฟฝ โ ๐ด๐ด๐๐ ๏ฟฝ031.2๏ฟฝ
where,
๐ด๐ด๐๐ = ๏ฟฝcos๐๐๐๐ โ sin๐๐๐๐sin๐๐๐๐ cos ๐๐๐๐
๏ฟฝ
Hint:
โข Start by identifying the generalized coordinates
โข Develop the kinematic constraints equations
โข Once you have both velocity and acceleration equations will be easy to derive
38
Example Derive the velocity and acceleration equations of this two-link manipulator
X1
Y1
X3
ฮธ2O
Y3
X2Y2
ฮธ3
A
In this case, the generalized coordinates are,
๐๐ = ๏ฟฝ๐ ๐ 1๐ฅ๐ฅ ๐ ๐ 1๐ฆ๐ฆ ๐๐1 ๐ ๐ 2๐ฅ๐ฅ ๐ ๐ 2๐ฆ๐ฆ ๐๐2 ๐ ๐ 3๐ฅ๐ฅ ๐ ๐ 3๐ฆ๐ฆ ๐๐3๏ฟฝ๐๐ The kinematic constraint equations are,
๐ถ๐ถ(๐๐1, ๐๐2, ๐๐3) =
โฉโชโชโชโชโชโจ
โชโชโชโชโชโง ๐ ๐ 1๐ฅ๐ฅ
๐ ๐ 1๐ฆ๐ฆ๐๐1
๐ ๐ 2๐ฅ๐ฅ โ๐๐2 cos๐๐2
2
๐ ๐ 2๐ฆ๐ฆ โ๐๐2 sin๐๐2
2
๐ ๐ 2๐ฅ๐ฅ + ๐๐2 cos๐๐2
2โ ๐ ๐ 3๐ฅ๐ฅ + ๐๐3 cos๐๐3
2
๐ ๐ 2๐ฆ๐ฆ + ๐๐2 sin๐๐2
2โ ๐ ๐ 3๐ฆ๐ฆ + ๐๐3 sin๐๐3
2๐๐2 โ ๐๐2๐๐ โ ๐๐2๐ก๐ก๐๐3 โ ๐๐3๐๐ โ ๐๐3๐ก๐ก โญ
โชโชโชโชโชโฌ
โชโชโชโชโชโซ
=
โฉโชโชโจ
โชโชโง
000000000โญโชโชโฌ
โชโชโซ
39
Velocity Analysis:
Remember that the velocity equation is:
๏ฟฝฬ๏ฟฝ๐ = โ๐ถ๐ถ๐๐โ1๐ถ๐ถ๐ก๐ก
The Jacobian matrix becomes,
๐ถ๐ถ๐๐ =
โฃโขโขโขโขโขโขโขโก1 0 00 1 00 0 1
0 0 0 0 0 0 0 0 0
0 0 00 0 00 0 0
0 0 00 0 00 0 0
1 0 ๐๐2 sin๐๐2 2โ 0 1 โ ๐๐2 cos ๐๐2 2โ 1 0 โ ๐๐2 sin๐๐2 2โ
0 0 0 0 0 0
โ1 0 โ ๐๐3 sin๐๐3 2โ0 0 00 0 00 0 0
0 1 ๐๐2 cos๐๐2 2โ 0 0 1 0 0 0
0 โ1 ๐๐3 cos๐๐3 2โ 0 0 0 0 0 1 โฆ
โฅโฅโฅโฅโฅโฅโฅโค
In this case, Ct is simple since only the last two elements are explicit function of time.
๐ถ๐ถ๐ก๐ก = [0 0 0 0 0 0 0 โ๐๐2 โ๐๐3]๐๐
40
Acceleration Analysis
Remember that the acceleration equation is:
๏ฟฝฬ๏ฟฝ๐ = ๐ถ๐ถ๐๐โ1๐๐๐๐
The vector Qd is
๐๐๐๐ = โ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐ โ 2๐ถ๐ถ๐๐๐ก๐ก๏ฟฝฬ๏ฟฝ๐ โ ๐ถ๐ถ๐ก๐ก๐ก๐ก
Remember that Ct is,
๐ถ๐ถ๐ก๐ก = [0 0 0 0 0 0 0 โ๐๐2 โ๐๐3]๐๐
Ct matrix does not have any term that is in terms of time explicitly. Therefore,
๐ถ๐ถ๐๐๐ก๐ก = 0
๐ถ๐ถ๐ก๐ก๐ก๐ก = 0
Similarly, ๏ฟฝฬ๏ฟฝ๐ is not function of q, which means that,
๏ฟฝฬ๏ฟฝ๐๐๐ = 0
The vector Qd is reduced to,
๐๐๐๐ = โ๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐
41
Since,
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ =
โฃโขโขโขโขโขโขโขโก1 0 00 1 00 0 1
0 0 0 0 0 0 0 0 0
0 0 00 0 00 0 0
0 0 00 0 00 0 0
1 0 ๐๐2 sin๐๐2 2โ 0 1 โ ๐๐2 cos๐๐2 2โ 1 0 โ ๐๐2 sin๐๐2 2โ
0 0 0 0 0 0
โ1 0 โ ๐๐3 sin๐๐3 2โ0 0 00 0 00 0 0
0 1 ๐๐2 cos ๐๐2 2โ 0 0 1 0 0 0
0 โ1 ๐๐3 cos๐๐3 2โ 0 0 0 0 0 1 โฆ
โฅโฅโฅโฅโฅโฅโฅโค
โฉโชโชโชโชโจ
โชโชโชโชโง๏ฟฝฬ๏ฟฝ๐
1๐ฅ๐ฅ
๏ฟฝฬ๏ฟฝ๐ 1๐ฆ๐ฆ๐๐1ฬ๏ฟฝฬ๏ฟฝ๐ 2๐ฅ๐ฅ๏ฟฝฬ๏ฟฝ๐ 2๐ฆ๐ฆ๐๐2ฬ๏ฟฝฬ๏ฟฝ๐ 3๐ฅ๐ฅ๏ฟฝฬ๏ฟฝ๐ 3๐ฆ๐ฆ๐๐3ฬ โญ
โชโชโชโชโฌ
โชโชโชโชโซ
๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐ =
โฉโชโชโชโชโจ
โชโชโชโชโง ๏ฟฝฬ๏ฟฝ๐ 1
๐ฅ๐ฅ
๏ฟฝฬ๏ฟฝ๐ 1๐ฆ๐ฆ
๐๐1ฬ
๏ฟฝฬ๏ฟฝ๐ 2๐ฅ๐ฅ + ๐๐2ฬ ๐๐2 sin๐๐2 2โ
๏ฟฝฬ๏ฟฝ๐ 2๐ฆ๐ฆ โ ๐๐2ฬ ๐๐2 cos๐๐2 2โ
๏ฟฝฬ๏ฟฝ๐ 2๐ฅ๐ฅ โ ๐๐2ฬ ๐๐2 sin๐๐2 2 โ ๏ฟฝฬ๏ฟฝ๐ 3
๐ฅ๐ฅโ โ ๐๐3ฬ๐๐3 sin๐๐3 2โ๏ฟฝฬ๏ฟฝ๐ 2
๐ฆ๐ฆ + ๐๐2ฬ ๐๐2 cos๐๐2 2 โ ๏ฟฝฬ๏ฟฝ๐ 3๐ฆ๐ฆ๏ฟฝ + ๐๐3ฬ๐๐3 cos๐๐3 2โ
๐๐2ฬ
๐๐3ฬ โญโชโชโชโชโฌ
โชโชโชโชโซ
๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐
=
โฃโขโขโขโขโขโขโขโขโก0 0 00 0 00 0 0
0 0 00 0 00 0 0
0 0 00 0 00 0 0
0 0 00 0 00 0 0
0 0 ๐๐2ฬ ๐๐2 cos ๐๐2 2โ 0 0 ๐๐2ฬ ๐๐2 sin๐๐2 2โ 0 0 โ๐๐2ฬ ๐๐2 cos ๐๐2 2โ
0 0 0 0 0 0 0 0 โ๐๐3ฬ ๐๐3 cos๐๐3 2โ
0 0 00 0 00 0 0
0 0 โ๐๐2ฬ ๐๐2 sin๐๐2 2โ0 0 00 0 0
0 0 โ๐๐3ฬ ๐๐3 sin๐๐3 2โ 0 0 0 0 0 0 โฆ
โฅโฅโฅโฅโฅโฅโฅโฅโค
โฉโชโชโชโชโจ
โชโชโชโชโง๏ฟฝฬ๏ฟฝ๐
1๐ฅ๐ฅ
๏ฟฝฬ๏ฟฝ๐ 1๐ฆ๐ฆ
๐๐1ฬ
๏ฟฝฬ๏ฟฝ๐ 2๐ฅ๐ฅ
๏ฟฝฬ๏ฟฝ๐ 2๐ฆ๐ฆ
๐๐2ฬ
๏ฟฝฬ๏ฟฝ๐ 3๐ฅ๐ฅ
๏ฟฝฬ๏ฟฝ๐ 3๐ฆ๐ฆ
๐๐3ฬ โญโชโชโชโชโฌ
โชโชโชโชโซ
Therefore,
42
๏ฟฝ๐ถ๐ถ๐๐๏ฟฝฬ๏ฟฝ๐๏ฟฝ๐๐๏ฟฝฬ๏ฟฝ๐ =
โฉโชโชโชโชโชโชโจ
โชโชโชโชโชโชโง
000
๐๐2ฬ2๐๐2 cos ๐๐2
2
๐๐2ฬ2๐๐2 sin ๐๐2
2
โ๐๐2ฬ2
๐๐2 cos ๐๐2
2โ๐๐3ฬ2
๐๐3 cos ๐๐3
2
โ๐๐2ฬ2
๐๐2 sin ๐๐2
2โ๐๐3ฬ2
๐๐3 sin ๐๐3
200 โญ
โชโชโชโชโชโชโฌ
โชโชโชโชโชโชโซ
While we are not concerned with the closed form solutions, it may informative to have a look at the
result of multiplying these two matrices.
The results show the normal acceleration form of the constraint equations for joints O and A
respectively.
๏ฟฝฬ๏ฟฝ๐ = ๐ถ๐ถ๐๐โ1๐๐๐๐ = ๐ถ๐ถ๐๐โ1
โ
โโโโโโโโโโโโ
โ
โฉโชโชโชโชโชโชโจ
โชโชโชโชโชโชโง
000
๐๐2ฬ2๐๐2 cos ๐๐2
2
๐๐2ฬ2๐๐2 sin ๐๐2
2
โ๐๐2ฬ2
๐๐2 cos ๐๐2
2โ๐๐3ฬ2
๐๐3 cos ๐๐3
2
โ๐๐2ฬ2
๐๐2 sin ๐๐2
2โ๐๐3ฬ2
๐๐3 sin ๐๐3
200 โญ
โชโชโชโชโชโชโฌ
โชโชโชโชโชโชโซ
โ
โโโโโโโโโโโโ
Homework โข Based on the kinematic constraint equations that you already derived, develop the velocity and
acceleration equations in Figures P3.3 and P3.4.
43
Special Cases in Kinematic Analysis
Degenerate (Extreme) Configurations
โข Letโs start by discussing degenerate configurations, which are the ones when a mechanical system
loses on or more degrees of freedom.
โข Extreme position is associated with a reduction in the order of the geometry of the machines or
degeneracy (e.g., triangular to straight line or quadrilateral to triangular)
โข Extreme positions are associated with the loss of (zero) velocity. This should make sense as the
machine is changing direction
โข Here are some examples:
23 4
ฮธ2 ฮธ3
A
Bx4B
General Position
23
4
A BO
2 34
A BO
Extreme Positions
45
Singular (Lockup) Configurations Singularities
โข Singular (lockup) positions are associated with infinite joint velocities.
โข We are also interested in positions that are closed to singularity as they are associated with excessive
velocities that may damage the machine or results in jerky motion.
If cos ฮธ3 = 0 i.e. ฮธ3 = ฯ2๏ฟฝ or 3ฯ 2๏ฟฝ
Then, ๏ฟฝ๏ฟฝฬ๏ฟฝ๐ฅ4B
๏ฟฝฬ๏ฟฝ๐3 ๏ฟฝ = ๏ฟฝโโ๏ฟฝ
The figure below show the only time this case is feasible.
2 3
4ฮธ2=ฯ/2
A
Bฮธ3=3ฯ/2
234
ฮธ2=3ฯ/2
A
Bฮธ3=ฯ/2