Lecture 4: Inverse and Velocity Kinematics
Lecture 4: Inverse and Kinematics
Forward Kinematics Recap
We have already seen how to solve forward kinematics problems.
The D-H convention help us to assign frames to joints and derive transformation matrices.
The transformation matrices can be used to obtain the end effector pose when the joint parameters are given.
Example: Furuta Pendulum
It consists of a DC motor with its shaft vertical. Attached to the shaft is a horizontal rod. At the end of the rod, a second rod is attached to a horizontal pivot. The second rod rotates freely in a vertical plane.
The problem is to swing the second rod up to its inverted position and stabilize it there.
3
This example is optional. These may be skipped while teaching, but can be uploaded. This example is not in the book.
3
Furuta Pendulum Kinematics
4
Transform θi di ai i
0 -> 1 θ1 0 0 -90
1 -> 2 θ2 l1 l2 0
z0
x0
z1
x1
z2
x2
Furuta Pendulum Kinematics
5
Transform θi di ai i
0 -> 1 θ1 0 0 -90
1 -> 2 θ2 l1 l2 0
Inverse Kinematics
Almost always the real goals of a robotic manipulator are to position and orient the end effector.
E.g., position the basket of a cherry picker so the human in the basket can fix a power line.
E.g., position the dental drill so it can clean out a cavity and form the hole to be filled.
E.g., position a robot riveter so it puts the rivet in the right hole.
6
6
Why is IK hard?
The examples show that positioning and orienting the end effector can be difficult even for humans.
Two reasons for the difficulty.
Tight constraints on the locations of the links. Constraints make it difficult for a human to perform the task, compelling the use of a robot.
Example: Laparoscopic surgery. The tool is inserted through a hole in the abdomen and is constrained by the edges of the hole.
There are often multiple solutions, some of which are much better than others (i.e. a non-linear problem).
7
Why is IK hard?
Suppose you know the position and orientation of the end effector.
Solving the IK problem is as follows:
The desired position and orientation (pose) of the end effector is specified, in general by a six component vector [x y z ϑ φ ψ]T.
The actual position and orientation of the end effector is determined by an n component vector [q1 q2 … qn]T (joint parameters)
Even ignoring constraints on joint parameters, solving six equations in n unknowns is challenging.
8
Unconstrained Inverse Kinematics
Unconstrained means we don’t limit joint angles or the joint displacement between certain ranges.
A precise mathematical formulation of the unconstrained inverse kinematics problem is as follows
Let the desired end-effector pose be
Rd specifies the desired orientation and od specifies the desired position of the end effector with respect to the world frame or frame 0.
And H can be written as,
Where
9
H can be represented as a function of all the joint parameters (q). The A matrices were previously derived from D-H parameters.
9
Unconstrained Inverse Kinematics
The unconstrained inverse kinematics problem is then to solve the equation
for
The expression above is a convenient shorthand for the following twelve equations in n unknowns.
10
YOU NEED TO ADD A POINT THAT THESE 12 UNKNOWNS ARE NOT INDEPENDENT. REMEMBER THAT THEY ARE CONSTRAINED, IN TERMS OF THE FACT THAT THE MATRIX IS ORTHOGONAL?
10
The Number of Equations
We used six equations to reduce the nine variables (elements of a rotation matrix) needed to specify orientation from nine to three.
You must solve 12 nonlinear equations in n unknowns to determine the joint variables that are needed to position and orient the end effector.
Even if the equations were linear, there would be three possibilities
No solution—especially when n < 12 in both linear and non-linear case.
Many solutions—an infinite number in the linear case.
A unique solution—easy to determine in the linear case.
11
I FEEL THAT MENTIONING 12 UNKNOWNS IS CONFUSING. REMEMBER THAT THERE REALLY 6 FREE VARIABLES. THER OTHER 6 IN THESE 12 ARE DEPENDENT.
11
Kinematic Decoupling
The inverse kinematics problem is greatly simplified for manipulators having 6 joints with the last 3 joint axes coincident. This condition has to be satisfied before applying a simplification method called decoupling.
Kinematic decoupling means that we can consider the Inverse Position and Inverse Orientation problems independent of each other.
Many manipulators satisfy these conditions Stanford Manipulator is one among many others.
12
Spherical wrist with 3 coinciding axes.
CAN WE PROVIDE SOME INTUITION ON HOW THIS DECOUPLING MAY OCCUR AND WHY IT IS USEFUL?
12
Kinematic Decoupling
A spherical wrist has the origin of all three axes coincident at a point known as the wrist center.
Thus, a 3 axis chain with a spherical wrist satisfies the decoupling conditions.
The inverse kinematics problem then decouples into
the inverse position kinematics for the wrist center,
and the inverse orientation kinematics for the wrist.
13
Figure shows a manipulator which satisfies the decoupling condition.
Wrist center is here.
HARD TO SEE THREE FRAMES SHARING THE ORIGIN IN THIS FIGURE?
The wrist center is where z3, z4, and z5 meet. I have modified the figure.
NOTE: The z3 may be defined in another place with a constant offset too. Kinematic decoupling still applies.
13
Spherical Wrist
Consider a spherical wrist as shown in the figure.
The wrist has 3 rotational degrees of freedom.
The end effector is displaced by an amount d6 as is shown in the diagram.
The y-axes are not needed in the diagram because they are uniquely determined by the x and z axes
14
Transform
3 -> 4 d3 0 -90
4 -> 5 0 0 90
5 -> 6 d6 0 0
DH Parameters for the wrist
z6
x6
d3
NOTE: The axis z3 is defined with an offset of d3. This is just another way of defining the axis. You can also define z3 at the origin of z4 and z5. See pg 88 in Spong.
WHAT ARE YOU TRYING TO CONVEY IN TERMS OF INTUITION? NOT CLEAR FROM THE BULLETS
This example also gives a good intuition about Kinematic decoupling. Please see the sentence in red in the next slide.
DO YOU HAVE ANY NICE 3D FIGURE OF THIS WRIST JOINT? OTHERWISE, IT IS HARD TO SEE HOW d_3 AND d_6 ARE BEING USED?
14
Solving Inverse Position
For simplicity, suppose the end effector is attached to the wrist and d6=0.
The orientation of the spherical wrist is completely controllable at the wrist. This means that you can use the arm (excluding the wrist) to place the wrist center at the desired location without considering the wrist orientation at all. (See figure in slide 13)
If d60, then you have to remove the effect of the end effector displacement first, and then the arm alone locates the revised wrist center as before
Solve for the q values.
15
The last point gives us the IK equations to solve for q1, q2, q3, q4, q5, and q6.
The desired position of the end effector is obtained by manipulating q1, q2 and q3.
The desired orientation of the end effector is obtained by manipulating the spherical wrist’s parameters, which are q4, q5 and q6.
15
Correcting for d6 0
We denote the location of the wrist center wrt the world frame by O0c .
The end effector is a distance d6 along
the z6 axis. The last column of Rd gives the coordinates of the z6 axis in the world frame or frame 0.
Mathematically, computes this.
Therefore, to position the end effector, we need
16
WHAT DOES THE SYMBOL. O^0_c MEANS OR IMPLIES?
It is the position of wrist center with respect to the world frame.
WHAT ARE YOU TRYING TO CONVEY HERE?
When there is an offset d6 between the spherical wrist and the end effector, how do we decouple Inverse Position and Inverse Orientation. That is shown through this example.
o_{d} is formally defined in slide 9.
16
Correcting for d6 0
Solving for the needed wrist center position gives
We can write this more explicitly as
The homogeneous transformation that positions the wrist center is then
17
The final point: This again gives us equations to solve for q1, q2, and q3 (similar to slide 15) because these
Joint parameters are responsible for positioning the wrist center o_{c}.
17
Summary
R03 is a function of only q1, q2, and q3.
We now have the equations that must be solved for q1, q2, and q3.
Once we have done so, we know that .
We can determine the needed R36 from
R36 can be solved for q4, q5, q6
Remember that the inverse of a rotation matrix is its transpose.
18
Geometric Approach for Inverse Position
Let us consider a manipulator which has 3 joints as shown in figure.
This is called an elbow manipulator.
We need to calculate θ1, θ2, and θ3 given the coordinates of wrist center xc, yc, and zc.
Replace the prismatic joint in the Stanford arm by a rotary joint.
Again, ϑ1=Atan2 (xc,yc).
Joint 1 is the only one that can position the arm in the x0, y0 plane.
The remaining two joints form a planar manipulator.
19
xc, yc, and zc.
Represent the coordinates of the center of the wrist.
THIS IS CONFUSING. YOU ARE TALKING ABOUT A MANIPULATOR WITH PRISMATIC JOINTS IN THE FIRST BULLET. BUT YOU ARE REFERRING TO THREE ROTATIONAL JOINTS IN THE FIGURE AND SUBSEQUENT BULLETS
19
Elbow Manipulator
Figure shows the side view of the links 2 and 3.
The long dotted line has length
The angle with the horizontal is
Using the Law of Cosines,
and,
So
20
Example Summary
Note that there is an obvious case where the calculation breaks down—when xc=yc=0.
There are infinitely many solutions for θ1 in this case. This is a singularity.
There are always two solutions for the case on the previous slide. The solution given is for the elbow up and elbow down configuration.
It is also obvious that there is no solution if the desired point is outside the reach of the arm.
21
Offset Shoulder Manipulator
We add an offset d between link 1 and link 2 in elbow manipulator.
You need to choose ϑ1 so that the remainder of the arm is on a line through the point (xc ,yc).
This offset helps avoid the singularity. There are 2 solutions for θ1 in this case.
22
Left arm configuration
Offset Shoulder Manipulators
There can be both left and right shoulder manipulator.
Unlike humans, these can have two solutions for the elbow joint. The human version is always elbow down. Some robots can have an elbow up configuration.
23
Adding Constraints
Note that all of our calculations have assumed no constraints.
In nature, joints are generally constrained.
Eg: Human elbow joints are constrained to always make an acute angle.
It is generally cheaper to make robot joints that cannot rotate a full 360 because it is expensive to supply power across a joint that can rotate through a full circle.
Inverse Orientation
The rotation matrix for the spherical wrist is
This is identical to the rotation matrix for the Euler Angles (compare Eqns. (3.15) & (2.26)).
Just identify ϑ4 as φ, ϑ5 as ϑ, and ϑ6 as ψ.
Note the shorthand, ci=cos(ϑi) and sk=sin(ϑk).
25
Reading
Spong et. Al chapter 3 covers Forward kinematics in detail and solving the Inverse Kinematics with manipulators with spherical wrists.
Go through the examples and derivations in sections 3.1 to 3.3.
26
Inverse Kinematics: Recent applications
A very important area of IK research in robotics today is in applications to assist humans, especially the aged, ill, and/or those with disabilities.
Must operate in unstructured and unpredictable environment.
Calculations should be in real-time.
The existence of many solutions to the inverse kinematics often means there is a useful solution to a constrained problem.
Define a performance measure to determine precisely how good an inverse kinematic solution is, and then solve for an optimal configuration.
27
Inverse Kinematics: General Numeric Solvers
Algorithmic schemes for solving nonlinear optimization problems exist.
Gradient based methods
Newton’s method and its improvements
Convex optimization methods (if we are lucky) such as ADMM (alternating direction method of multipliers)
Random search
Neural networks have been proposed and used.
Numerical Methods for IK
The examples we have seen so far are analytical solutions which aim to obtain a precise answer.
They may not be always possible:
Large solution space (redundancy)
Empty solution space (unreachable goal)
If we consider the forward kinematics problem as x = f(q). f is nonlinear due to sin’s and cos’s in the rotations.
Find linear approximation to f -1
Numerical solutions necessary
Fast
Reasonably accurate
Yet Robust
x is the vector of the pose of the end effector.
q is the vector of joint parameters.
29
The Jacobian
Consider the forward kinematics problem for a 6 DOF manipulator to be represented as: x = f(q) = (f1(q), f2(q),…., f6(q))
The Jacobian gives the linear approximation of f.
The partial derivatives in Jf must be evaluated at some q^ to yield the local linear approximation of f.
THE LAST BULLET WIH q^ IS UNCLEAR? FIX THAT.
30
Jacobian
I don’t know what the notation K here stands for.
The UNC slides are entirely pictures. I couldn’t crop out the images alone.
31
Jacobian
Collectively, J tells us how x changes locally in the world space with respect to all the parameters qi.
Meaning of K is unclear.
32
Computing the Jacobian
To compute the Jacobian, we must compute the derivatives of the forward kinematics equation.
We already know how to represent the Forward Kinematics function f in the form of homogeneous transformation matrices.
We will look at how to differentiate a matrix.
Matrix Derivatives
Let us consider a generic matrix A(t) whose entries depend on time.
Let X = [A(t)]x be a relationship where X is the transformed point and x is the initial point. Then,
Where [Ȧ A-1] is called the tangent operator.
IS THERE ANY BACKGROUND REFERENCE ON “Matrix Derivatives”? IF YOU SHOULD POINT IT HERE AND PUT IT AS BACKGROUND MATERIAL.
34
Rotation Matrix Derivatives
What is the tangent operator for rotation matrices?
Ω is the angular velocity matrix of R(t).
We will have a look at the properties of skew symmetric matrices.
Skew Symmetric Matrices properties
Rotation in three dimensions is complicated.
Skew symmetric matrices simplify the computations involved in 3D rotational velocities like differentiation of rotation matrices.
Defn: A matrix S is skew symmetric iff
ST + S =0
If we let sij denote the ijth component of S, then the definition implies that
sij + sji = 0 for all i,j=1,2,3
Doing the math shows that sii = 0, i=1,2,3 and that sij = -sji for all ij, i,j=1,2,3.
36
IS THERE A REFERENCE TO THIS MATH ON Skew-Symmetric Matrices? PLEASE INCLUDE THAT.
36
Skew Symmetric Matrices properties
Thus, the most general skew symmetric matrix in three dimensions has the form
We can associate a vector with this as follows, let a=[ax ay az]T and choose ax=s1, ay=s2, and az=s3.
This establishes the notation
37
Skew Symmetric Matrices properties
Properties of skew symmetric matrices:
The operator S is linear, i.e., for any vectors in and any scalars α and .
For any vectors belonging to ,
38
Skew Symmetric Matrices properties
For ,
For any n x n skew symmetric matrix S and any vector
For any direct calculation shows that
39
(similarity transformation)
EXPLAIN WHAT ARE THESE PROPERTIES REALLY USED FOR?
39
Application and Reason for our Interest in Skew Symmetric Matrices
The derivative of a rotation matrix:
Assume that R(ϑ) is a function of a single variable ϑ. There is no loss of generality in this because we can always write an arbitrary rotation matrix as a product of single variable rotations.
We know that R(ϑ)R(ϑ)T=I. (3)
40
These slides on the derivation that derivative of R * Transpose of R is skew symmetric are optional.
40
Continued
Using the chain rule to differentiate both sides of R(ϑ)R(ϑ)T=I wrt ϑ gives
Define the matrix S (at this point we do not know what the properties of S are so this is an arbitrary choice) to be:
Then the transpose of S is
41
This seems complicated but it is very simple.
41
Result
We just proved that S + ST =0.
Thus, by the definition of skew symmetric,
is skew symmetric
Furthermore
This is very useful. It is much easier for a computer to multiply by a matrix than to differentiate.
42
YOU HAVE 5-6 SLIDES ON THE PROPERTIES OF SKEW-SYMMETRIC MATRICES? HOW ARE THEY USED OR PUT TOGETHER?
42
Summary of Jacobians
This is the basis for Velocity Kinematics.
43
Velocity Kinematics
The objective of Velocity Kinematics is to obtain the velocities of all the points of a mechanical system.
Differentiate their positions with respect to time.
For a kinematic chain we differentiate all the joint variables (ϑi and dj) for all of the joints.
The concepts of skew symmetric matrices and Jacobians form the basis for Velocity Kinematics
44
IS THIS STUFF USED LATER AS PART OF DYNAMICS? OTHERWISE, LETS REMOVE IT.
44
Angular Velocity about a Fixed Axis
Given a rigid body in three dimensions.
Given also a fixed vector .
Suppose the body rotates about the axis with angular velocity
Then the linear velocity of any point on the body is given by .
Where is a vector from the origin (a point on ) to the point of interest.
45