CS计算机代考程序代写 arm chain CS 4610/5335

CS 4610/5335
Differential and Inverse Kinematics
Robert Platt Northeastern University
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A

Inverse Kinematics (IK) Problem
Given a desired position and orientation (pose) of the end effector, what arm joint angles achieve that pose?

IK Problem:
Given a desired position and orientation of the end effector …
Calculate the corresponding joint angles
q3
Two approaches to IK
Suppose we want to move the end effector here…
q2
q1
Approach #1: Analytic IK – solve for joint angles directly using algebra
Approach #2: Numerical IK – solve for joint angles using an iterative numerical technique like gradient descent
– this approach can also be used as an online controller

Analytic Inverse Kinematics

Motivating Example Suppose you want to move the end
Goal: move the end effector onto this line
effector above a specified point, Analytic IK:
xg
l
q
x xg

Inverse kinematics
There is no general analytical inverse kinematics solution
All analytical inverse kinematics solutions are specific to a robot or class of robots.
based on geometric intuition about the robot I’ll give one example – there are many variations.

Inverse kinematics

q3
q5
Inverse kinematics
q2
q4
q6
Spherical wrist: the axes of the last three joints intersect in a point.
q1
Consider this 6-joint robot
Puma 560 Robot

Inverse kinematics
q3
q5
Problem:
Given: desired transform,
q2 q1
q4
q6
Find: Note:
qq q q qq 1234 n
The desired transform (pose) encodes six degrees of freedom (this info can be represented by six numbers)
Since we only have six joints at our disposal, there is no manifold of redundant solutions.
For this manipulator, the problem can be decomposed into a position component (the first three joints) and an orientation component (the last three joints)
The first three joints tell you what the position of the spherical wrist

Example: Inverse kinematics q3 q2
q4
q6
Solution:
First, back out the position of the spherical wrist:
q1
Since it’s a spherical wrist, the last three joints can be thought of as rotating
about a point.
A constant transform exists that goes from the last wrist joint out to the end
swT eff
effector (sometimes this is called the “tool” transform):
Back out the position of the wrist:
1 sw eff eff
b b sw TT T
q5

Example: Inverse kinematics q3
Next, solve for the first three joints
q4
q6
•
q5 q2
First, solve for q1. (look down from above)
q atan2x ,y  1gg
or
q atan2x ,y  1gg
q1
Goal position in horizontal plane
q1

Example: Inverse kinematics
Finally, the last three joints completely specify the orientation of the end effector.
Note that the last three joints look just like ZYZ Euler angles
Determination of the joint angles is easy – just calculate the ZYZ Euler angles corresponding to the desired orientation.
q3
q5
q2 q1
q4
q6

Example: Inverse kinematics
Next, solve for q2 and q3 .
(look at the manipulator orthogonal to the plane of the
q3
q5
q2 first two links) q1
q4
q6
Given and , calculate joint angles that cause eff to reach

zyz
( )( )( ) 0 01−sinθ0cosθ0 01
ZYZ Euler Angles
cosφ −sinφ 0 cosθ 0 sinθ cosψ −sinψ 0 R (φ,θ,ψ)=sinφ cosφ 0 0 1 0 sinψ cosψ 0
cφcθcψ−sφsψ −cφcθsψ−sφcψ cφsθ Rzyz(φ,θ,ψ)= s c c +c s −s c s +c c s s
(φθψφψ φθψφψφθ) −sθ cψ sθ sψ cθ
θ=±a tan2 1−r ,r (√ 332 33)
atan2r ,r k 23 13
atan2r ,r 32 31

Inverse kinematics for a humanoid arm
You can do similar types of things for a humanoid (7-DOF) arm.
Since this is a redundant arm, there are a manifold of solutions…
Spherical wrist
Spherical shoulder
General strategy:
Solve for elbow angle
elbow
Solve for a set of shoulder angles that places the wrist in the right position (note that you have to choose an elbow orbit angle)
Solve for the wrist angles

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
But what about the first derivative?
This would tell us the velocity of the end effector as a function of joint angle velocities.
f (q) q

Consider a one-link arm
As the arm rotates, the end effector sweeps out an arc
Let’s assume that we are only interested in the x coordinate…
Forward kinematics: Differentialkinematics:
l
q
Motivating Example
x l cos(q)
dx lsin(q)
x
dq
x  l sin(q)q
q 1 x l sin(q)

Suppose you want to move the end effector
Goal: move the end effector onto this line
above a specified point, Analytic IK:
xg
Motivating Example
i0,q0 arbitrary 2. xi l cos(qi )
3. x xg  xi 
4.q 1 x  l sin(qi )
Numerical IK: 1.
l
xg
q
x
5.
6. igoto2.
qi1 qi q

Motivating Example
l
xg
q
q  qd
q
q
x
x
x
g
x
1
 l sin(q)
joint ctlr
l cos()
joint position sensor
This controller moves the link asymptotically toward the goal position.

Jacobian

Jacobian
⃗x=[l1cos(q1)+l2 cos(q1+q2)] l1sin(q1)+l2sin(q1+q2)
Forward kinematics of the two- link manipulator
Velocity Jacobian
l2 q2
l1
y
q1
x

Jacobian
Chain rule: x Jq
If the Jacobian is square and full rank, then we can
l2 q2
l1
invert it:
1 qJ x
y q1
x
J1

joint ctlr
joint position sensor

Numerical IK Solution
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q

Jacobian
The Jacobian relates joint velocities with end effector twist:
 Jq
End effector twist Jacobian
Joint angle velocities
First derivative of joint angles:
It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures
This makes differential kinematics a much easier sub-problem than kinematics in general.

Jacobian
The Jacobian relates joint velocities with end effector twist: What is twist?
 Jq
End effector twist Jacobian
Joint angle velocities
First derivative of joint angles:
It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures
This makes differential kinematics a much easier sub-problem than kinematics in general.

•
End effector twist:
Twist is a concatenation of linear velocity and angular velocity:
Linear velocity Angular velocity
What is Twist?

•
End effector twist:
Twist is a concatenation of linear velocity and angular velocity:
Linear velocity Angular velocity
What is Twist?
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

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
Symbol for angular velocity:
Relation between angular velocity and linear velocity:

Breakdown of the Jacobian:
v J q v
J q 
ξ=Jv q ̇ [Jω]
Jv x but q
Jacobian
Relation to the derivative:
r J  
q
That’s not an angular velocity

Jacobian in different reference frames
In the preceeding, the Jacobian has been expressed in the base frame
It can be expressed in other reference frames using rotation matrices
Velocity is transformed from one reference frame to another using:
3x 3y
q3
l3
l2
2x
1y
1x q2
2y
q1
0y
kkb 0 pR p
0x z
l1
b kkb
b pR p
Therefore, the velocity Jacobian can be
transformed using:
kkb
JR J vbv

Inverse Kinematics: An iterative approach

Jacobian
x Jq
If the Jacobian is square and full rank, then we
l2 q2
l1
can invert it:
1 qJ x
y q1
x
J1

joint ctlr
joint position sensor

Numerical IK Solution (method 1)
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q

Numerical IK Solution (method 1)
Input: x*
Output: q*
1. repeat until dx is small:
Idea:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q

Numerical IK Solution (method 2)
Input: x*
Output: q*
1. repeat until dx is small:
This also works
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * JT dx q = q + dq
return q* = q
This results in different behavior (see homework)

Non-square Jacobian matrix
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q
Can only take inverse of Jacobian if it is square

Non-square Jacobian matrix
Example of a non-square Jacobian matrix:
3x 3y
l3 2x
l2
J (q)=[−l1 s1−l2 s12−l3 s123 l1 c1+l2 c12+l3 c123
−l1 s1−l2 s12 l1 c1+l2 c12
−l1 s1 ] l1 c1
1y 2y
q3
1x q2
q ̇ 1 q
0y
q1
x ̇ =J q []() ̇
0 x z
l
Two equations of three y ̇ [2] variableseach…
1
0
q ̇ 3
This is an under-constrained system of equations.
multiple solutions
there are multiple joint angle velocities that realize the same EFF velocity.

Non-square Jacobian matrix
If the Jacobian is not a square matrix, then you can’t invert it.
3x 3y
what next?
l3 2x
l2
q3
1x q2
1y 2y
1
0x
0y
We have:
We are looking for a matrix
qJ#x 
 x Jq
q
l
1
such that:
  xJq
0z

Two cases:
Underconstrained manipulator (redundant) Overconstrained
Generalized inverse:
for the underconstrained manipulator: given that minimizes s.t.
, find a vector
Generalized inverse
for the overconstrained manipulator: given , find a vector s.t. is minimized

#TT1 J J JJ 
#T1T J J J J
J# J1
Underconstrained case (if there are more columns than rows (m