Differential Kinematics Robert Platt Northeastern University - - PowerPoint PPT Presentation
Differential Kinematics Robert Platt Northeastern University 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 (
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
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
q1
x
y
q2
l1
l2
Forward kinematics of the two- link manipulator
1
q x y
2
q
1
l
2
l
1
Chain rule: If the Jacobian is square and full rank, then we can invert it:
1
J
joint ctlr joint position sensor
The Jacobian relates joint velocities with end effector twist:
End effector twist Jacobian Joint angle velocities
It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures
kinematics in general.
End effector twist:
velocity and angular velocity:
and angular velocity have different units Linear velocity Angular velocity
End effector twist:
velocity and angular velocity:
and angular velocity have different units Linear velocity Angular velocity What is angular velocity? Angular velocity is a vector that: – points in the direction of the axis of rotation – has magnitude equal to the velocity of rotation
Angular velocity is a vector that: – points in the direction of the axis of rotation – has magnitude equal to the velocity of rotation
Symbol for angular velocity: Relation between angular velocity and linear velocity: We will often write it this way:
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
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.
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
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
J v=[J v1 ⋯ J vn]
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
J ω=[J ω1 ⋯ J ωn]
1
i bz
i
Where
i
J=[ J v1 ⋯ J vn J ω1 ⋯ J ωn]
0 T1=(
cq1 −sq1 l1cq1 sq1 cq 1 l1sq1 1 1 )
1 T2=(
cq2 −sq 2 l2cq2 sq2 cq2 l2sq2 1 1 )
From before: J ω=[
0^
z0
0^
z1
0^
z2]=[ 1 1 1]
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 2 T3=(
cq3 −sq 3 l3cq3 sq3 cq3 l3 sq3 1 1 )
J v 1=
0 ^
z0¿(
0o3− 0o0)=[
1] ×([ l1c1+l2c12+l3c123 l1s1+l2s12+l3s123
−[ 0]) =[ −l1s1−l2s12−l3 s123 l1c1+l2c12+l3c123
x y
1q
z
2q
3q
x
1
y
1
y
2
x
2
x
3
y
3
1l
2l
3l
J v 2=
0 ^
z1¿(
0 o3− 0 o1)=[
1] ×([ l1c1+l2c12+l3c123 l1s1+l2s12+l3 s123
−[ l1c1 l1s1 0 ]) =[ −l2s12−l3s123 l2c12+l3c123
J v 3=
0 ^
z2¿(
0 o3− 0 o2)=[
1] ×([ l1c1+l2c12+l3c123 l1s1+l2s12+l3 s123
−[ l1c1+l2c12 l1s1+l2s12
=[ −l3 s123 l3c123 0 ] J v=[ −l1s1−l2s12−l3 s123 −l2 s12−l3 s123 −l3 s123 l1c1+l2c12+l3c123 l2c12+l3 c123 l3c123 0 ]
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
J=[ J v J ω] =
−l1s1−l2s12−l3 s123 −l2s12−l3 s123 −l3 s123 l1c1+l2c12+l3c123 l2c12+l3 c123 l3c123 1 1 1 ]
Calculate the end effector Jacobian with respect to the base frame
1
q
2
q
1
1
2
2
1
l
2
l
0 T1=(
−c1 −s1 −s1 c1 1 l1 1)
1 T2=(
c2 −s2 l2 c2 s2 c2 l2s2 1 1 )
2 T3=(
c3 −s3 l3 c3 s3 c3 l3s3 1 1 )
The kinematics of this arm are described by the following:
q1
q2
q3
z1 x1 y1
l1
l2 l3
3
1
b b b v
1 3 1
2
b b b v
2 3 2
3
b b b v
b p0=(
0)
b p1=(
l1)
b p2=(
−l2c1c2 −l2 s1c2 l2 s2+l1)
b p3=(
−c1(l2c2+l3 c23) −s1(l2c2+l3c23) l2s2+l3 s23+l1 )
b z0=(
1)
b z1=(
−s1 c1 0 )
b z2=(
−s1 c1 0 )
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
J ω1=( 1)
J ω2=( −s1 c1 0 ) J ω3=( −s1 c1 0 )
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
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:
k J=( k Rb k Rb) b 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:
0 R3=(
J=
−l1s1−l2 s12−l3 s123 −l2s12−l3 s123 −l3s123 l1c1+l2 c12+l3c123 l2c12+l3c123 l3c123 1 1 1 ]
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:
0 R3=(
3 J=
c123 s123 −s123 c123 1 c123 s123 −s123 c123 1)[ −l1 s1−l2 s12−l3s123 −l2 s12−l3s123 −l3 s123 l1c1+l2c12+l3 c123 l2c12+l3c123 l3 c123 1 1 1 ]
1
q
2
q
3
q
1
1
2
2
1
l
2
l
3
l
Given (in base frame) Given: and Calculate: