Differential Kinematics Up to this point, we have only considered - - PowerPoint PPT Presentation
Differential Kinematics Up to this point, we have only considered the relationship of the joint angles to the Cartesian location of the end effector: = f ( q ) x f ( q ) But what about the first derivative? q This
Up to this point, we have only considered the relationship of the joint angles to the Cartesian location of the end effector:
But what about the first derivative?
effector as a function of joint angle velocities.
q x
dq dx
Consider a one-link arm
sweeps out an arc
interested in the coordinate…
Forward kinematics:
Differential kinematics:
Suppose you want to move the end effector above a specified point,
−
g g 1
g
q x
g
Goal: move the end effector onto this line Answer #1: Answer #2:
i
i i
i g
i i
+1
0 =
) sin( 1 q l
g
d
q x
g
This controller moves the link asymptotically toward the goal position.
Velocity Jacobian
2 1 2 1 1 2 1 2 1 1
1
q x y
2
q
1
l
2
l
2 1 2 2 1 2 1 1 2 1 2 2 1 2 1 1
( )
Forward kinematics of the two- link manipulator
1
q x y
2
q
1
l
2
l
( )
2 1 2 2 1 2 1 1 2 1 2 2 1 2 1 1
1 −
1 −
J
+
) (q FK
d
x x q x
δ
q
δ +
d
q q
joint ctlr joint position sensor
Chain rule: If the Jacobian is square and full rank, then we can invert it:
The Jacobian relates joint velocities with end effector twist:
End effector twist Jacobian Joint angle velocities
n
1
It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures
kinematics in general.
End effector twist:
velocity and angular velocity:
angular velocity have different units
this quantity as a 6-vector, it is NOT
Linear velocity Angular velocity
a a b b =
a a b b
Just differentiate all elements of the rotation matrix w.r.t. time.
b T a b a b b
T a b a b b
This is the matrix representation
b b b
This FO differential equation encodes how the particle rotates FYI: this expression can be solved using an exponential:
( )
2
b b b t S b
b
ω
( )
( )
( )
( )
2
b b b t S b
b
ω
2 2
b b
x y x z y z
If you interpret the skew symmetric matrix like this: Then this is another way of writing the cross product:
T
Def’n of skew symmetry
− − − =
Skew symmetric matrices always look like this
T a b a b
b
Skew symmetry of :
T a b a b T a b a b
T a b a b T a b a b
T b b
b b b
You probably already know this formula
b b b
Twist concatenates linear and angular velocity:
Linear velocity Angular velocity
ω
φθψ ω
v
Breakdown of the Jacobian:
ω
Relation to the derivative: but That’s not an angular velocity
1
q
2
q
1
1
eff 1 −
1
l
2
l
3
q
eff 1 −
eff
eff
3
l
Approach:
time
end effector due to the motion of that joint
are frozen, and calculate the motion at the end effector caused by i.
i 1 −
i 1 −
i
i
i
l
eff i i i b eff b
, 1 1 1 − − −
Vector from reference frame i-1 to the end effector Orientation of the link
th
−
1
q
2
q
1
1
eff 1 −
1
l
2
l
3
q
eff 1 −
eff
eff
3
l
How does the end effector translate as the link moves?
th
eff i i i b eff i i i b eff b
, 1 1 1 , 1 1 1 − − − − − −
eff i b eff i i i b T i b i b eff b
, 1 , 1 1 1 1 1 − − − − − −
T i b i b i b
1 1 1 − − − =
eff i b eff i b i b eff b
, 1 , 1 1 − − −
i i b eff i b i b eff b
, 1 , 1 1 − − −
effector caused by motion at the i-1 link:
eff i i i b eff b
, 1 1 1 − − −
i 1 −
i 1 −
i
i
i
l
Velocity at end effector due to rotation at joint i-1 Velocity at end effector due to change in length of link i-1
i i b eff i b i b eff b
, 1 , 1 1 − − −
caused by motion at the i-1 link:
i 1 −
i 1 −
i
i
i
l
eff i b i b v
i
, 1 1 − − ×
Rotational DOF
1 −
i b v
i
Rotation about
x
i 1 −
y
i 1 −
y
i
i
i
l
i
q
z
i 1 −
z
i
z
i 1 −
x
i 1 −
y
i 1 −
y
i
i
i
l
z
i 1 −
z
i
Extension/contraction along
z
i 1 −
Prismatic DOF
Vector from i-1 to the end effector
z
i 1 −
z
i 1 −
1 1 − −
i b eff b i b v
i
i 1 −
i 1 −
i
i
i
l
eff i i i i b eff b
1 1 − −
1
q
2
q
1
1
eff 1 −
1
l
2
l
3
q
eff 1 −
eff
eff
3
l
How does the end effector rotate as the link moves?
th
How does rotate as this rotates?
eff bR
eff i i i i b eff b
1 1 − −
eff i i i i b eff b
1 1 − −
eff i i i i i i i b eff b eff b
1 , 1 1 1 − − − −
eff i i b T i b i i i i b eff b eff b
1 1 1 , 1 1 1 − − − − − −
eff i i b i i i i b eff b eff b
1 1 , 1 1 1 − − − − −
eff b i i b eff b eff b
, 1 −
i i b eff b , 1 −
Perhaps this was kind of obvious… Angular velocity at end effector Angular velocity caused by rotation of joint i-1
Rotational DOF
Rotation about
x
i 1 −
y
i 1 −
y
i
i
i
l
i
q
z
i 1 −
z
i
z
i 1 −
x
i 1 −
y
i 1 −
y
i
i
i
l
z
i 1 −
z
i
Extension/contraction along
z
i 1 −
Prismatic DOF
z
i 1 −
z
i 1 −
i i bz
i
, 1 −
ω
i
[ ]
n
v v v
J J J
1
=
x y
1
q
z
2
q
x
1
y
1
y
eff 1 −
1
l
2
l
3
q
x
eff 1 −
x
eff
y
eff
3
l
1 1 − −
i b eff b i b v
i
Where
1 −
i b v
i
[ ]
n
J J J
ω ω ω
1
=
1 −
i bz
i
ω
Where
i
=
n n
J J J J J
v v
ω ω
1 1
− = 1 1
1 1 1 1 1 1
1 1 1 q q q q q q
s l c s c l s c T − = 1 1
2 2 2 2 2 2
2 2 2 1 q q q q q q
s l c s c l s c T
From before:
[ ] = =
1 1 1 ˆ ˆ ˆ
2 1
z z z Jω
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
− = 1 1
3 3 3 3 3 3
3 3 3 2 q q q q q q
s l c s c l s c T
+ + − − − = − + + + + × = − × = 1 ) ( ˆ
123 3 12 2 1 1 123 3 12 2 1 1 123 3 12 2 1 1 123 3 12 2 1 1 3
1
c l c l c l s l s l s l s l s l s l c l c l c l
Jv
x y
1q
z
2q
3q
x
1
y
1
y
2
x
2
x
3
y
3
1l
2l
3l
+ − − = − + + + + × = − × = 1 ) ( ˆ
123 3 12 2 123 3 12 2 1 1 1 1 123 3 12 2 1 1 123 3 12 2 1 1 1 3 1
2
c l c l s l s l s l c l s l s l s l c l c l c l
Jv − = + + − + + + + × = − × = 1 ) ( ˆ
123 3 123 3 12 2 1 1 12 2 1 1 123 3 12 2 1 1 123 3 12 2 1 1 2 3 2
3
c l s l s l s l c l c l s l s l s l c l c l c l
Jv + + + − − − − − − =
123 3 123 3 12 2 123 3 12 2 1 1 123 3 123 3 12 2 123 3 12 2 1 1
c l c l c l c l c l c l s l s l s l s l s l s l Jv
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
+ + + − − − − − − = = 1 1 1
123 3 123 3 12 2 123 3 12 2 1 1 123 3 123 3 12 2 123 3 12 2 1 1
c l c l c l c l c l c l s l s l s l s l s l s l J J J
v ω
− − − = 1 1
1 1 1 1 1 1
l c s s c T
− = 1 1
2 2 2 2 2 2 2 2 2 1
s l c s c l s c T
− = 1 1
3 3 3 3 3 3 3 3 3 2
s l c s c l s c T
The kinematics of this arm are described by the following:
1
q
2
q
3
q
1
1
1
2
2
2
3
3
3
1
l
2
l
3
l
3
1
b b b v
1 3 1
2
b b b v
2 3 2
3
b b b v
=
p
b
=
1 1
l p
b
+ − − =
1 2 2 2 1 2 2 1 2 2
l s l c s l c c l p
b
( ) ( )
+ + + − + − =
1 23 3 2 2 23 3 2 2 1 23 3 2 2 1 3
l s l s l c l c l s c l c l c p
b
=
1 z
b
− =
1 1 1
c s z
b
− =
1 1 2
c s z
b
1
q
2
q
3
q
x y z
1
z
1
x
1
y
2
z
2
x
2
y
3
z
3
y
3
x
1
l
2
l
3
l
( ) ( ) ( ) ( )
23 3 2 2 1 23 3 2 2 1 1 23 3 2 2 23 3 2 2 1 23 3 2 2 1
1
=
1
1
ω
J
− =
1 1
2
c s Jω
− =
1 1
3
c s Jω
( ) ( ) ( ) ( )
23 3 2 2 23 3 2 2 1 23 3 2 2 1 23 3 2 2 23 3 2 2 1 23 3 2 2 1 1 1
2
23 3 23 1 3 23 1 3 23 3 23 3 1 23 3 1 1 1
3
1
q
2
q
3
q
x y z
1
z
1
x
1
y
2
z
2
x
2
y
3
z
3
y
3
x
1
l
2
l
3
l
( ) ( ) ( ) ( )
1 1 1 1 23 3 23 3 2 2 23 1 3 23 3 2 2 1 23 3 2 2 1 23 1 3 23 3 2 2 1 23 3 2 2 1
1
q
2
q
3
q
x y z
1
z
1
x
1
y
2
z
2
x
2
y
3
z
3
y
3
x
1
l
2
l
3
l
In the preceeding, the Jacobian has been expressed in the base frame
frames using rotation matrices
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
b b k k =
Velocity is transformed from one reference frame to another using:
b b k k
Therefore, the velocity Jacobian can be transformed using:
v b b k v k
First, let’s express angular velocity in a different reference frame:
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
b b b
Def’n of angular velocity
b b b k b b k
k T b k b b k k
k b b k k
b b k k
Angular velocity can also be rotated by a rotation matrix Therefore, the angular velocity Jacobian can be transformed using:
ω ω
b b k k
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
Therefore, the full Jacobian is rotated:
J R R J
b b k b k k
=
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
Express the Jacobian for the three-link arm in the reference frame of the end effector:
123 123 123 123 3
+ + + − − − − − − = 1 1 1
123 3 123 3 12 2 123 3 12 2 1 1 123 3 123 3 12 2 123 3 12 2 1 1
c l c l c l c l c l c l s l s l s l s l s l s l J
x y
1
q
z
2
q
3
q
x
1
y
1
y
2
x
2
x
3
y
3
1
l
2
l
3
l
Express the Jacobian for the three-link arm in the reference frame of the end effector:
123 123 123 123 3
+ + + − − − − − − − − = 1 1 1 1 1
123 3 123 3 12 2 123 3 12 2 1 1 123 3 123 3 12 2 123 3 12 2 1 1 123 123 123 123 123 123 123 123 3
c l c l c l c l c l c l s l s l s l s l s l s l c s s c c s s c J