Post on 10-Jan-2016
description
Robot Vision SS 2005 Matthias Rüther 1
ROBOT VISION Lesson 9: Robot Kinematics
Matthias Rüther
Robot Vision SS 2005 Matthias Rüther 2
Contents
Introduction– Overview
– Basic Terms
– Typical Configurations
Geometry– Homogeneous Transformations
– Representing Orientation
Direct Kinematics– Open Chain
– Denavit-Hartenberg
– Typical Configurations
Inverse Kinematics– Problem
– Numerical Algorithms
Robot Vision SS 2005 Matthias Rüther 3
Overview
An industrial robot consists of
– A manipulator that consists of a sequence of rigid bodies (links) connected by means of articulation (joints). It is characterized by an arm (for mobility), a wrist (for dexterity) and an end effector (to perform a task).
– Actuators that set the manipulator in motion by actuating the joints (electric, hydraulic or pneumatic)
– Sensors that measure the manipulators status (proprioceptive sensors) or the status of the environment (exteroceptive sensors).
– A control system that controls and supervises manipulator motion.
Robot Vision SS 2005 Matthias Rüther 4
Overview
An industrial robot has three fundamental capacities
– Material handling: objects are transported from one location to another, their physical characteristics are not altered (e.g. pallettizing, warehouse loading/unloading, sorting, packaging)
– Material Manipulation: physical characteristics of objects are changed or their physical identity is lost (e.g. welding, painting, gluing, cutting, drilling, screwing, assembly, …)
– Measurement: object properties are measured by sensors on the end effector, so they are able to explore a 3D environment (e.g. quality control by image processing, tactile sensors, …)
Robot Vision SS 2005 Matthias Rüther 5
Basic Terms
Kinematic chain: sequence of links and joints connecting the end effector to the base. Can be open (single chain) or closed (loop within the chain).
Joints: – Revolute: rotational joint– Prismatic: translational joint
Degree of mobility: each joint typ. provides an additional DOM (e.g. 6DOM necessary for 3D motion)
Accuracy: „How close does the robot get to a desired point?“, distance between desired point and actual point. (off line)
Repeatability: „How close does the robot get to a position it has reached before?“ (on line)
Robot Vision SS 2005 Matthias Rüther 6
Repeatability and Accuracy
),,(aaaa
zyxP
),,(tttt
zyxP
X
Z
YWhere: Pt is the target position,
Pa the average achieved position,
R is the repeatability, and A is the accuracy.
Repeatability: the measurement of how closely a robot returns to the same position n number of times.
Accuracy: the measurement of how closely the robot moves to a given target coordinate.
Robot Vision SS 2005 Matthias Rüther 7
Many factors can effect the accuracy of a robot such as:
– environmental factors (eg temperature, humidity and electrical noise);
– kinematic parameters (eg robot link lengths etc);
– dynamic parameters (eg structural compliance, friction);
– measurement factors (eg resolution and non-linearity of encoders and resolvers);
– computational factors (eg digital round off, steady-state control errors); and
– application factors (eg installation errors and errors in defining work-piece coordinate frames).
Robot Vision SS 2005 Matthias Rüther 8
Why?
Each of these can effect the robot kinematics– depending on the robot model design and manufacturing process the kinematics can
be the most significant source of error.
– if the internal kinematic model of the robot does not match the actual model we can not accurately predict where the end-effector is.
Make them match by either– tighter manufacturing tolerances; or
– by measuring actual kinematics and incorporating this information into the kinematic model.
Robot Vision SS 2005 Matthias Rüther 9
Basic Terms
Joint space: end effector position and orientation given in joint coordinates.
Operational space: end effector position and orientation given in world coordinates (e.g. cartesian coordinate system)
Workspace:– Reachable WS: set of positions the EE can reach
– Dexterous WS: set of positions the EE can reach with a given orientation
Redundancy: more DOM than needed are available
Robot Vision SS 2005 Matthias Rüther 10
Revolute Joint1 DOF ( Variable - )
Prismatic Joint1 DOF (linear) (Variables - d)
Basic Joints
Robot Vision SS 2005 Matthias Rüther 11
The PUMA 560 has SIX revolute jointsA revolute joint has ONE degree of freedom ( 1 DOF) that is
defined by its angle
1
23
4
There are two more joints on the end effector (the gripper)
An Example - The PUMA 560
Robot Vision SS 2005 Matthias Rüther 12
Manipulator Structures
Robot Vision SS 2005 Matthias Rüther 13
Manipulator Structures
Robot Vision SS 2005 Matthias Rüther 14
Forward and Inverse Kinematics
xw
Joint 1
Joint 2
Joint 3
Link 1z1
World (Base) Coordinate Frame
Tool Coordinate Frame
Joint 1
Joint 2
Joint 3
Link 1z1
World (Base) Coordinate Frame
Tool Coordinate Frame
1
2
zw
yt
zt
Link Space
n variables (1 … n)
Tool Space
6 variables (x,y,z,x,y,z)
Forward K
Inverse K
Robot Vision SS 2005 Matthias Rüther 15
Co-ordinate Frames
right handed coordinate frames
used as reference frames
x
z
y
xw
World (Base) Coordinate Frame
Link frame
zw
x 2
z 2
Tool Coordinate Frame
yt
zt
Robot Vision SS 2005 Matthias Rüther 16
F o r w a r d K i n e m a t i c s
F o r w a r d K i n e m a t i c s
Robot Vision SS 2005 Matthias Rüther 17
The Situation:You have a robotic arm that
starts out aligned with the xo-axis.You tell the first link to move by 1 and the second link to move by 2.
The Quest:What is the position of the
end of the robotic arm? Solution:1. Geometric Approach
This might be the easiest solution for the simple situation. However, notice that the angles are measured relative to the direction of the previous link. (The first link is the exception. The angle is measured relative to it’s initial position.) For robots with more links and whose arm extends into 3 dimensions the geometry gets much more tedious.
2. Algebraic Approach Involves coordinate transformations.
Robot Vision SS 2005 Matthias Rüther 18
X2
X3Y2
Y3
1
2
3
1
2 3
Example Problem: You are have a three link arm that starts out aligned in the x-
axis. Each link has lengths l1, l2, l3, respectively. You tell the first one to move by 1 , and so on as the diagram suggests. Find the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame.
H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 )
i.e. Rotating by 1 will put you in the X1Y1 frame. Translate in the along the X1 axis by l1. Rotating by 2 will put you in the X2Y2 frame. and so on until you are in the X3Y3 frame.
The position of the yellow dot relative to the X3Y3 frame is(l1, 0). Multiplying H by that position vector will give you the coordinates of the yellow point relative the the X0Y0 frame.
X1
Y1
X0
Y0
Robot Vision SS 2005 Matthias Rüther 19
Slight variation on the last solution:Make the yellow dot the origin of a new coordinate X4Y4 frame
X2
X3Y2
Y3
1
2
3
1
2 3
X1
Y1
X0
Y0
X4
Y4
H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 ) * Tx3(l3)
This takes you from the X0Y0 frame to the X4Y4 frame.
The position of the yellow dot relative to the X4Y4 frame is (0,0).
1
0
0
0
H
1
Z
Y
X
Notice that multiplying by the (0,0,0,1) vector will equal the last column of the H matrix.
Robot Vision SS 2005 Matthias Rüther 20
More on Forward Kinematics…
Denavit - Hartenberg Parameters
Robot Vision SS 2005 Matthias Rüther 21
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i Y i
X i a i
d i
i
IDEA: Each joint is assigned a coordinate frame. Using the Denavit-Hartenberg notation, you need 4 parameters to describe how a frame (i) relates to a previous frame ( i -1 ).
THE PARAMETERS/VARIABLES: , a , d,
Denavit-Hartenberg Notation
Robot Vision SS 2005 Matthias Rüther 22
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i Y i
X i a i d i
i
You can align the two axis just using the 4 parameters
1) a(i-1)
Technical Definition: a(i-1) is the length of the perpendicular between the joint axes. The joint axes is the axes around which revolution takes place which are the Z(i-1) and Z(i) axes. These two axes can be viewed as lines in space. The common perpendicular is the shortest line between the two axis-lines and is perpendicular to both axis-lines.
The Parameters
Robot Vision SS 2005 Matthias Rüther 23
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i Y i
X i a i d i
i
a(i-1) cont...
Visual Approach - “A way to visualize the link parameter a(i-1) is to imagine an expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the joint axis i the radius of the cylinder is equal to a(i-1).” (Manipulator
Kinematics)
It’s Usually on the Diagram Approach - If the diagram already specifies the various coordinate frames, then the common perpendicular is usually the X(i-1) axis. So a(i-1) is just the displacement along the X(i-1) to move from the (i-1) frame to the i frame.
If the link is prismatic, then a(i-1) is a variable, not a parameter.
Robot Vision SS 2005 Matthias Rüther 24
2) (i-1)
Technical Definition: Amount of rotation around the common perpendicular so that the joint axes are parallel.
i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is pointing in the same direction as the Zi axis. Positive rotation follows the right hand rule.
3) d(i-1)
Technical Definition: The displacement along the Zi axis needed to align the a(i-1) common perpendicular to the ai common perpendicular.
In other words, displacement along the Zi to align the X(i-1) and Xi axes.
4) i
Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi axis.
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i Y i
X i a i d i
i
Robot Vision SS 2005 Matthias Rüther 25
1000
cosαcosαsinαcosθsinαsinθ
sinαsinαcosαcosθcosαsinθ
0sinθcosθ
i1)(i1)(i1)(ii1)(ii
i1)(i1)(i1)(ii1)(ii
1)(iii
d
d
a
Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a transformation matrix from one coordinate frame to the next. Using a series of D-H Matrix multiplications and the D-H Parameter table, the final result is a transformation matrix from some frame to your initial frame.
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i Y i
X i a i d i
i
The Denavit-Hartenberg Matrix
Robot Vision SS 2005 Matthias Rüther 26
i (i-1) a(i-1) di
i
0 0 0 0 0
1 0 a0 0 1
2 -90 a1 d2 2
Z0
X0
Y0
Z1
X2
Y1
Z2
X1
Y2
d2
a0 a1
Denavit-Hartenberg Link Parameter Table
Notice that the table has two uses:
1) To describe the robot with its variables and parameters.
2) To describe some state of the robot by having a numerical values for the variables.
3 Revolute Joints
Robot Vision SS 2005 Matthias Rüther 27
Z0
X0
Y0
Z1
X2
Y1
Z2
X1
Y2
d2
a0 a1
i (i-1) a(i-1) di
i
0 0 0 0 0
1 0 a0 0 1
2 -90 a1 d2 2
1
V
V
V
TV2
2
2
000
Z
Y
X
ZYX
T)T)(T)((T 12
010
Note: T is the D-H matrix with (i-1) = 0 and i = 1.
Robot Vision SS 2005 Matthias Rüther 28
1000
0100
00cosθsinθ
00sinθcosθ
T 00
00
0
i (i-1) a(i-1) di i
0 0 0 0 0
1 0 a0 0 1
2 -90 a1 d2 2
This is just a rotation around the Z0 axis
1000
0000
00cosθsinθ
a0sinθcosθ
T 11
011
01
1000
00cosθsinθ
d100
a0sinθcosθ
T22
2
122
12
This is a translation by a0 followed by a rotation around the Z1 axis
This is a translation by a1 and then d2 followed by a rotation around the X2 and Z2 axis
T)T)(T)((T 12
010
Robot Vision SS 2005 Matthias Rüther 29
Review of Direct Kinematics
What we have:– Geometric layout of robot (constant under robot motion)– Joint positions qnx1 (variable under robot motion)
What we want:– End Effector (EE) position and orientation wrt base coordinate frame (variable
under robot motion)
A coordinate transformation T from EE coordinates to base coordinates, where T is a function of q:
– E.g. position of EE coordinate origin is:
10
)()()( T
baseEE
qpqRqT
1
0
0
0
)(qTpEE
Robot Vision SS 2005 Matthias Rüther 30
Review of Direct Kinematics
How we get there:– Place local coordinate frame at each joint i, i=1..n:
• Determine common normal (CN) between joint i and i-1 (i.e. line connecting both joint axes in the shortest possible way)
• Place coordinate origin oi-1 at intersection of CN and joint axis i-1– Attention Sciavicco/Siciliano: oi-1 at intersection of CN and joint axis i!
• Set z-axis in direction of joint axis• Set x-axis in direction of CN
– Model robot arm using Denavit-Hartenberg (DH) parameters:• a: shortest distance between consecutive joint axes (length of common normal
CN). : angle between consecutive joint axes around CN.• d: distance between consecutive coordinate origins along i-th joint axis (distance
between i-th coordinate origin and intersection of CN with i-th joint axis.• y or : angle between consecutive CN‘s around joint axis.
• Depending on joint type, either d or y are variables.
i (i-1) a(i-1) di
i
0 0 0 0 0
1 0 a0 0 1
2 -90 a1 d2 2
Robot Vision SS 2005 Matthias Rüther 31
Review of Direct Kinematics
– From DH-table, calculate T recursively:
i (i-1) a(i-1) di
i
0 0 0 0 0
1 0 a0 0 1
2 -90 a1 d2 2
1000
0100
00cosθsinθ
00sinθcosθ
T 00
00
0
1000
0000
00cosθsinθ
a0sinθcosθ
T 11
011
01
1000
00cosθsinθ
d100
a0sinθcosθ
T22
2
122
12
T)T)(T)((T 12
010
1000
cosαcosαsinαcosθsinαsinθ
sinαsinαcosαcosθcosαsinθ
0sinθcosθ
i1)(i1)(i1)(ii1)(ii
i1)(i1)(i1)(ii1)(ii
1)(iii
d
d
a
T1ii
Robot Vision SS 2005 Matthias Rüther 32
Inverse Kinematics
(q1 … qn) (x,y,z,x,y,z)
K-1
From Position to Angles
Robot Vision SS 2005 Matthias Rüther 33
Inverse Kinematics
What we have:– Geometric layout of robot (constant under robot motion)– End Effector (EE) position and orientation wrt base coordinate frame (variable
under robot motion)
What we want:– Joint coordinates qnx1 (variable under robot motion). A mapping f from EE position and orientation to joint coordinates:
Problematic, because:– EE position may be out of reachable workspace (no solution)– EE position and orientation may be out of dexterous workspace (no solution)– EE position may be reachable in several ways (multiple solutions, redundancy)– EE position may be reachable in infinitely many ways (infinite number of solutions,
redundancy)– f is highly nonlinear, no closed form solution for complicated canfigurations.
zyx
n
zyxf
q
q
,,,,,...1
Robot Vision SS 2005 Matthias Rüther 34
Inverse Kinematics
How we get there:
– Calculate closed-form solution, if possible• fast
• accurate
• unique for every arm configuration (no general concept)
– Use Iterative Method based on differential kinematics • slower
• gives approximate solution
• converges to a single solution (even if multiple solutions exist)
• general algorithm, applicable to all arm configurations
Robot Vision SS 2005 Matthias Rüther 35
Multiple Solutions
Because we are dealing with transcendental functions there is often more then one solution.
eg: cos-1(0.5) = 60º and 300º
Robot Vision SS 2005 Matthias Rüther 36
Insoluble
Robot Vision SS 2005 Matthias Rüther 37
Inverse Kinematics
Solution easier if the three joint axes intersect at one point => a closed form or analytical solution possible (Pieper’s solution).
Otherwise use a numerical (iterative) solution where:• speed is poor; and •we may not be able to calculate all solutions.
x5
y5
x6
z6
x3
y3
x4
z4
Robot Vision SS 2005 Matthias Rüther 38
1
X
Y
S
Revolute and Prismatic Joints Combined
(x , y)
Finding :
)x
yarctan(θ
More Specifically:
)x
y(2arctanθ arctan2() specifies that it’s in
the first quadrant
Finding S:
)y(xS 22
A Simple Example
Robot Vision SS 2005 Matthias Rüther 39
2
1
(x , y)
l2
l1
Given: l1, l2 , x , y
Find: 1, 2
Redundancy:
A unique solution to this problem does not exist. Sometimes no solution is possible.
(x , y)l2
l1
l2
l1
Inverse Kinematics of a Two Link Manipulator
Robot Vision SS 2005 Matthias Rüther 40
l1
l22
1
(x , y)Using the Law of Cosines:
21
22
21
22
21
22
21
22
212
22
122
222
2arccosθ
2)cos(θ
)cos(θ)θ180cos(
)θ180cos(2)(
cos2
ll
llyx
ll
llyx
llllyx
Cabbac
2
2
22
2
Using the Law of Cosines:
x
y2arctanα
αθθ
yx
)sin(θ
yx
)θsin(180θsin
sinsin
11
22
2
22
2
2
1
l
c
C
b
B
x
y2arctan
yx
)sin(θarcsinθ
22
221
l
Redundant since 2 could be in the first or fourth quadrant.
Redundancy caused since 2 has two possible values
The Geometric Solution
Robot Vision SS 2005 Matthias Rüther 41
21
22
21
22
2
2212
22
1
211211212
22
1
211212
212
22
12
1211212
212
22
12
1
2222
2
yxarccosθ
c2
)(sins)(cc2
)(sins2)(sins)(cc2)(cc
yx)2((1)
ll
ll
llll
llll
llllllll
l1
l22
1
(x , y)
21
21211
21211
1221
11
θθθ(3)
sinsy(2)
ccx(1)
)θcos(θc
cosθc
ll
ll
Only Unknown
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(
:
abbaba
bababa
Note
The Algebraic Solution
Robot Vision SS 2005 Matthias Rüther 42
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(
:
abbaba
bababa
Note
)c(s)s(c
cscss
sinsy
)()c(c
ccc
ccx
2211221
12221211
21211
2212211
21221211
21211
lll
lll
ll
slsll
sslll
ll
We know what 2 is from the previous slide. We need to solve for 1 . Now we have two equations and two unknowns (sin 1 and cos 1 )
2222221
1
2212
22
1122221
221122221
221
221
2211
yx
x)c(ys
)c2(sx)c(
1
)c(s)s()c(
)(xy
)c(
)(xc
slll
llllslll
lllll
sls
ll
sls Substituting for c1 and simplifying many times
Notice this is the law of cosines and can be replaced by x2+ y2
22
222211
yx
x)c(yarcsinθ
slll
Robot Vision SS 2005 Matthias Rüther 43
A General Solution
A general solution to the IK problem is obtained using differential kinematics.
There exists a linear mapping between EE velocity and Joint velocity.
Joint positions can be computed by integrating velocities over time.
Robot Vision SS 2005 Matthias Rüther 44
Differential Kinematics
What we have:– Geometric layout of robot (constant under robot motion)– Joint velocities
What we want:– Linear velocity
– Angular velocity
A mapping J from joint velocities to EE velocities:
3 1
Tdx dy dz
pdt dt dt
11 ...
T
nn
dqdqq
dt dt
3 1
T
yx zdd d
dt dt dt
( )
( )P
O
p J q q
J q q
( )p
v J q q
6
P
O n
JJ
J
Robot Vision SS 2005 Matthias Rüther 45
The Jacobian
Jacobian matrix J(q) gives linear and angular velocity (p‘, ) as a function of joint velocity (q‘)
Depends on the location in joint space, hence J(q)
Can be computed via closed formula.
nO
P
O
P
T
J
JJ
qqJp
v
qqJ
qqJp
qpqRqT
6
)(
)(
)(
10
)()()(
Robot Vision SS 2005 Matthias Rüther 46
Derivative of a Rotation Matrix
Consider rotation matrix R(t), what is its derivative?
( )
0
( ) , 0
( ) ( ) ( )
T
T T
T T
R R t
RR I
RR RR
S t RR S S
R t S t R t
Interpretation of S: S is the cross-matrix of angular velocities
0
( ) 0 ,
0
z y
z x x y z
y x
S t
Robot Vision SS 2005 Matthias Rüther 47
Example: rotation about z-axis
We want to compute the time-derivative of Rz(t)
cos sin 0
( ( )) sin cos 0
0 0 1
sin cos 0 cos sin 0
( ) cos sin 0 sin cos 0
0 0 0 0 0 1
0 0
0 0 ( ( )), 0 0
0 0 0
z
T
T
R t
S RR
S t
Robot Vision SS 2005 Matthias Rüther 48
Velocity composition rule
Consider the coordinate transformation of a point P from coordinate frame 1 to frame 0:
0 0 0 11 1p o R p
point wrt 0 point wrt 1origin of 1 wrt 0 rotation of 1 wrt 0
0 0 0 1 0 11 1 1
0 0 0 1 0 0 11 1 1 1
0 1 01 1
( )
:
0 0 0 1 0 01 1 1 1
0 0 0 01 1 1
p o R p R p
p o R p S R p
set R p r
p o R p r
p o r
If p1 is fixed:
Robot Vision SS 2005 Matthias Rüther 49
Link Velocity
Consider link i connecting joints i and i+1 with their coordinate frames
Let pi and pi+1 be the coordinate frame origins wrt base coordinate frame
Let rii,i+1 be the coordinate frame origin of i+1 wrt coordinate
frame i, expressed in coordinate frame i1 , 1
1 , 1 , 1 , 1 , 1
1 , 1 , 1
ii i i i i
i ii i i i i i i i i i i i i i i
ii i i i i i i i
p p R r
p p R r R r p v r
R
Lin. velocity of i+1 Lin. velocity of i Lin. velocity of i+1 wrt i
Angular velocity of i
… angular velocity (without derivation)
Angular velocity of i+1
Angular velocity of i
Angular velocity of i+1 wrt i
Robot Vision SS 2005 Matthias Rüther 50
Link Velocity
For prismatic joint:
, 1
, 1 1 1
1 1 1 , 1
1
0i i
i i i i
i i i i i i i
i i
v d z
p p d z r
For revolute joint:
, 1 1 1
, 1 , 1 , 1
1 1 , 1
1 1 1
i i i i
i i i i i i
i i i i i
i i i i
z
v r
p p r
z
Robot Vision SS 2005 Matthias Rüther 51
Computation of Jacobian
Let the Jacobian J be partitioned into the following 3x1 vectors:
Prismatic joint:
Revolute joint:
1
1
...P Pn
O On
J J
J
J J
i Pi
i Oi
q J
q J
… contribution of joint i to EE linear velocity
… contribution of joint i to EE angular velocity
0 0i Oi Oi
i Pi i i Pi i
q J J
q J d z J z
1, , ( ) ( )
i Oi i i Oi i
i Pi i i i n i i i Pi i i
q J z J z
q J r z p p J z p p
Robot Vision SS 2005 Matthias Rüther 52
Computation of Jacobian
1
1
...
0
( )
P Pn
O On
i
Pi
Oi i i
i
J J
J
J J
z
J
J z p p
z
… if prismatic
… if revolute
Robot Vision SS 2005 Matthias Rüther 53
Example: three link planar arm
0 0 1 1 2 2
0 1 2
1 1 1 1 2 12
0 1 1 1 2 1 1 2 12
1 1 2 12 3 123
1 1 2 12 3 123
0 1 2
1 1 2 1
( ) ( ) ( )( )
0
0 ; ; ;
0 0 0
0
0
0
1
z p p z p p z p pJ q
z z z
a c a c a c
p p a s p a s a s
a c a c a c
p a s a s a s
z z z
a s a s
J
2 3 123 2 12 3 123 3 123
1 1 2 12 3 123 2 12 3 123 3 123
0 0 0
0 0 0
0 0 0
1 1 1
a s a s a s a s
a c a c a c a c a c a c
si…j, ci…j denote sin(qi+…+qj), cos(qi+…+qj)
Robot Vision SS 2005 Matthias Rüther 54
Iterative Inverse Kinematics
1. Specify target transformation T you want to reach.
2. Make an initial guess for joint coords at time tk: q(tk).
3. Compute T(tk) using forward kinematics.
4. Compute differential motion required to move from T(tk) to T:
5. Compute pseudo-inverse of Jacobian J-1 at the position of q(tk).
6. Add Joint motion to initial guess, make result new initial guess.
7. Repeat 2-6 until differential motion is satisfactorily small.
3 1 3 1 3 1 3 1 3 1 3 1
6 1
( )( ) ; ; ( ) ;
0.5k k k
k k k
k t t tk kt t t
p p tv t t R n s a R t n s a
n n s s a a
11( ) ( ) ( )k k kq t q t J v t t
Robot Vision SS 2005 Matthias Rüther 55
Example: Robotics Toolbox
http://www.cat.csiro.au/cmst/staff/pic/robot/