# CS计算机代考程序代写 AI arm chain 681 – Introduction to Computer Graphics

681 – Introduction to Computer Graphics

Rick Parent – CIS682

Inverse Kinematics

Set goal configuration of end effector

calculate interior joint angles

Analytic approach – when linkage is simple enough, directly calculate joint angles in configuration that satifies goal

Numeric approach – complex linkages

At each time slice, determine joint movements that take you in direction of goal position (and orientation)

Rick Parent – CIS682

Forward Kinematics – review

Pose – linkage is a specific configuration

Pose Vector – vector of joint angles for linkage

Degrees of Freedom (DoF) – of joint or of whole figure

Articulated linkage – hierarchy of joint-link pairs

Types of joints: revolute, prismatic

Tree structure – arcs & nodes

Recursive traversal – concatenate arc matrices

Push current matrix leaving node downward

Pop current matrix traversing back up to node

Rick Parent – CIS682

Goal

End Effector

q1

q2

q3

L1

L2

L3

Inverse Kinematics

Rick Parent – CIS682

Underconstrained – if fewer constraints than DoFs

Many solutions

Overconstrained – too many constraints

No solution

Dextrous workspace – volume end effector can reach in any orientation

Reachable workspace – volume the end effector can reach

Inverse Kinematics

Rick Parent – CIS682

Given arm configuration (L1, L2, …)

Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, y1,y2, y3]

Analytically compute goal configuration (q1,q2)

Interpolate pose vector from initial to goal

Inverse Kinematics – Analytic

Rick Parent – CIS682

Analytic Inverse Kinematics

(X,Y)

L1

L2

q1

q2

Goal

Rick Parent – CIS682

Analytic Inverse Kinematics

(X,Y)

Goal

Multiple solutions

Rick Parent – CIS682

Analytic Inverse Kinematics

(X,Y)

L1

L2

q1

qT

180- q2

Rick Parent – CIS682

Analytic Inverse Kinematics

(X,Y)

L1

L2

q1

qT

180- q2

X

Y

Rick Parent – CIS682

Law of Cosines

A

B

C

a

Rick Parent – CIS682

Analytic Inverse Kinematics

(X,Y)

L1

L2

q1

qT

180- q2

X

Y

Rick Parent – CIS682

Analytic Inverse Kinematics

Rick Parent – CIS682

Iterative Inverse Kinematics

When linkage is too complex for analytic methods

At each time step, determine changes to joint angles that take the end effector toward goal position and orientation

Need to recompute at each time step

Rick Parent – CIS682

End Effector

q2

a2

d2=EF-J2

a2 x d2

– Compute instantaneous effect of each joint

– Linear approximation to curvilinear motion

– Find linear combination to take end effector towards goal position

Inverse Jacobian Method

Rick Parent – CIS682

Inverse Jacobian Method

Instantaneous linear change in end effector for ith joint

= (EF – Ji) x ai

Rick Parent – CIS682

Inverse Jacobian Method

What is the change in orientation of end effector induced by joint i that has axis of rotation a i

and position Ji?

Angular velocity

Rick Parent – CIS682

Solution only valid for an

instantaneous step

Angular affect is really

curved, not straight line

Once a step is taken, need

to recompute solution

Inverse Jacobian Method

Rick Parent – CIS682

Inverse Jacobian Method

– Mathematics

Set up equations

yi: state variable

xi : system parameter

fi : relate system parameters to state variable

Rick Parent – CIS682

Inverse Jacobian Method

– Mathematics

Matrix Form

Rick Parent – CIS682

Inverse Jacobian Method

– Mathematics

Use chain rule to differentiate equations to relate changes in system parameters to changes in state variables

Rick Parent – CIS682

Matrix Form

Inverse Jacobian Method

– Mathematics

Rick Parent – CIS682

Inverse Jacobian Method

Change in position (and orientation) of end effector

Change in joint angles

Linear approximation that relates change in joint angle to change in end effector position (and orientation)

Rick Parent – CIS682

Inverse Jacobian Method

Rick Parent – CIS682

Inverse Jacobian Method

Rick Parent – CIS682

Inverse Jacobian Method

= (S – J1) x a1

= w1

Rick Parent – CIS682

The Matrices

Rick Parent – CIS682

The Matrices

V – desired linear and angular velocities

J – Jacobian

Matrix of partials

q – change to joint angles (unknowns)

3×1, 6×1

3xN, 6xN

N DoFs

N x 1

Rick Parent – CIS682

Pseudo Inverse of the Jacobian

Rick Parent – CIS682

Solving using the Pseudo Inverse

LU decomposition

Rick Parent – CIS682

Adding a Control Term

But it can be used to bias

The solution vector

A solution of this form

…doesn’t affect the desired configuration

When put into this formula

Like this

After some manipulation, you can show that it…

Rick Parent – CIS682

Form of the Control Term

Bias to desired angles

(not the same as hard joint limits)

Where the deviation is large, you bump up the solution vector in such a way that you don’t disturb the desired effect

Desired angles and corresponding gains are input

‘z’ is H differentiated

Rick Parent – CIS682

Some Algebraic Manipulation

Include this in equation

Isolate vector of unknown

Rearrange to isolate the inverse

Rick Parent – CIS682

Solving the Equations

LU decomp.

Rick Parent – CIS682

Use to bias to desired mid-angle

Does not enforce joint angles

Does not address “human-like” or “natural” motion

Control Term

Only kinematic control – no forces involved

Rick Parent – CIS682

Jacobian transpose

Alternate Jacobian – use goal position

HAL – human arm linkage

Other ways to numerically IK

CCD

Damped Least Squares

Rick Parent – CIS682

Jacobian Transpose

Use projection of effect vector onto desired movement

Rick Parent – CIS682

Jacobian Transpose

S

Rick Parent – CIS682

Jacobian Transpose

…

Rick Parent – CIS682

Alternate Jacobian

…

G

Use the goal postion instead of the end-effector!!??

!?

Rick Parent – CIS682

Damped Least Squares

G

substitution

Solve

Rick Parent – CIS682

Hueristic Human-Like Linkage (HAL)

3 DoF

3 DoF

1 DoF

G

Decompose into simpler subproblems

Fix wrist position – use as Goal

7 DoF linkage

RA (q1,q2 ,q3) , RB ( q4), RC(q5,q6 ,q7)

Set hand position and rotation based on relative position of Goal to shoulder

Rick Parent – CIS682

Hueristic Human-Like Linkage (HAL)

Set q4 based on distance between shoulder and wrist

s

e

w

Assume axis of elbow is perpendicular to plane defined by s, e, w use law of cosines

L1

L2

L

Rick Parent – CIS682

Hueristic Human-Like Linkage (HAL)

Determine elbow position based on heuristics

s

w

e

For example:

project forearm straight from hand orientation

Clamp to inside of limits

if arm intersects torso or a shoulder angle exceeds joint limit (or exceeds comfort zone) –

Elbow lies on circle defined by w, s & q4

q4

Rick Parent – CIS682

Hueristic Human-Like Linkage (HAL)

From e and w and hand orientation, determine RB

s

w

e

From e and s, determine RA

q4

Rick Parent – CIS682

Traverse linkage from distal joint inwards

Optimally set one joint at a time

Update end effector with each joint change

Cyclic-Coordinate Descent

Use weighted average of position and orientation.

At each joint, minimize difference between end effector and goal

Easy if only trying to match position; heuristic if orientation too

Rick Parent – CIS682

Cyclic-Coordinate Descent

.

Rick Parent – CIS682

Cyclic-Coordinate Descent

Rotational joint:

.

Rick Parent – CIS682

Cyclic-Coordinate Descent

Rotational joint:

.

Rick Parent – CIS682

Cyclic-Coordinate Descent

Rotational joint:

.

Rick Parent – CIS682

Cyclic-Coordinate Descent

Rotational joint:

.

Rick Parent – CIS682

Cyclic-Coordinate Descent

Translational joint:

..

Rick Parent – CIS682

IK w/ constraints

Chris Welman, “Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001.

Basic idea:

Constraints are geometric, e.g., point-to-point, point-to-plan, specific orientation, etc.

Assume starting out in satisfied configuration

Forces are applied to system

Detect, and cancel out, force components that would violate constraints.

Rick Parent – CIS682

IK w/ constraints

Point-on-a-plane constraint

Fa

Fc

Ft

Given: geometric constraints & applied forces

Determine: what constraints will be violated & what (minimal) forces are needed to counteract the components of the applied forces responsible for the violations.

Rick Parent – CIS682

Constraints

.

To maintain constraints, need:

Notation:

Rick Parent – CIS682

Constraints

.

Generalized force

Constraint Jacobian

IK Jacobian

Rick Parent – CIS682

example constraint

Usually sparse

.

Geometric constraint on point that is function of pose

Rick Parent – CIS682

Computing the constraint force

To counteract ga’s affect on constraints:

.

Applied force

Yet to be determined constraint force

g should lie in the nullspace of JcK

Rick Parent – CIS682

Computing the constraint force

.

Solve linear system to find Lagrange multiplier vector.

The system is usually underconstrained

Restrict gc to move the system in a direction it may not go

Rick Parent – CIS682

Solving for Lagrange Multipliers

Shortest distance from A to B passing through a point P

Constrain P to lie on g

g

A

B

Points on ellipse are set of points for which sum of distances to foci is equal to some constant

Direction of gradients are equal

gradient

Rick Parent – CIS682

Solving for Lagrange Multipliers

Use truncated SVD with backsubstitution on

diagonal

Range basis

Nullspace basis.

Rick Parent – CIS682

Solving for Lagrange Multipliers

Rick Parent – CIS682

Feedback term

Spring that penalizes deviation from constraints.

Rick Parent – CIS682

Implementation

Handles on skeletons

Point handle

orientation handle

Center-of-mass handle

Each handle must know how to compute the Jacobian.

Each handle must know how to its value from q

Rick Parent – CIS682

Constraints on handles

Constraining a point handle to a location

Constraining a point handle to a plane

Constraining a point handle to a line

Constraining an orientation handle to an orientation

.

Rick Parent – CIS682

Dataflow approach

..

Constraint function block

Knows how to compute

Its function in term of x

Knows its Jacobian wrt x

Rick Parent – CIS682

Example network

..

c1

Jc

C

h1

h2

h3

h4

q

c2

x

2

+

y

2

y

1

=

f

1

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

y

2

=

f

2

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

y

3

=

f

3

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

y

4

=

f

4

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

y

5

=

f

5

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

y

6

=

f

6

(

x

1

,

x

2

,

x

3

,

x

4

,

x

5

,

x

6

)

)

(

)

)

((

S

G

a

J

G

n

n

–

×

´

–

Y

=

F

(

X

)

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ë

é

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ë

é

=

ú

ú

ú

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ê

ê

ê

ë

é

n

z

y

x

z

n

y

y

y

n

x

x

x

z

y

x

z

y

x

a

a

a

p

p

p

p

p

p

p

v

v

v

q

q

q

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

w

w

w

&

K

&

&

K

K

K

K

K

K

K

K

K

K

K

K

K

K

2

1

1

1

1

1

2

1

2

1

6

6

5

5

4

4

3

3

2

2

1

1

dx

x

f

dx

x

f

dx

x

f

dx

x

f

dx

x

f

dx

x

f

dy

i

i

i

i

i

i

i

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

+

+

+

+

+

=

q

q

q

q

&

&

&

&

=

=

=

=

+

–

–

V

J

J

J

J

J

V

J

J

J

J

J

V

J

J

V

T

T

T

T

T

T

1

1

)

(

)

(

V

J

T

=

D

q

J

+

=

(

J

T

J

)

–

1

J

T

=

J

T

(

J

J

T

)

–

1

0

0

)

(

)

(

)

(

)

(

=

=

–

=

–

=

–

=

=

–

=

+

+

+

V

z

V

z

J

J

V

z

J

J

JJ

V

z

I

J

J

J

V

J

V

z

I

J

J

q

q

&

&

H

I

J

J

J

J

V

q

q

Ñ

–

–

=

+

)

(

&

H

=

a

i

i

=

1

n

å

(

q

i

–

q

ci

)

y

z

=

Ñ

q

H

=

dH

d

q

=

y

a

i

i

=

1

n

å

(

q

i

–

q

ci

)

y

–

1

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ë

é

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ê

ë

é

=

ú

ú

ú

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ê

ê

ê

ë

é

n

z

y

x

z

n

y

y

y

n

x

x

x

z

y

x

z

y

x

a

a

a

p

p

p

p

p

p

p

v

v

v

q

q

q

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

w

w

w

&

K

&

&

K

K

K

K

K

K

K

K

K

K

K

K

K

K

2

1

1

1

1

1

2

1

2

1

H

I

J

J

V

J

q

q

Ñ

–

+

=

+

+

)

(

&

H

H

J

V

JJ

J

H

H

J

V

JJ

J

H

H

J

V

J

H

H

J

J

V

J

H

I

J

J

V

J

T

T

T

T

q

q

q

q

q

q

q

q

q

q

q

q

q

q

Ñ

–

Ñ

+

=

Ñ

–

Ñ

+

=

Ñ

–

Ñ

+

=

Ñ

–

Ñ

+

=

Ñ

–

+

=

–

–

+

+

+

+

+

)]

(

)

[(

)

(

)

(

)

(

)

(

1

1

&

&

&

&

&

)

,

,

,

,

,

(

6

5

4

3

2

1

1

x

x

x

x

x

x

f

y

i

=

V

J

I

J

J

T

T

1

2

)

(

–

–

=

D

l

q

q

&

J

V

=

b

b

q

b

q

q

q

)

(

)

(

)

(

1

T

T

T

JJ

H

J

V

H

J

H

J

V

JJ

=

Ñ

+

Ñ

–

=

Ñ

+

=

–

&

q

&

J

V

=

AB

C

B

A

2

)

cos(

2

2

2

–

+

=

a

2

2

)

cos(

Y

X

X

T

+

=

q

2

2

1

2

2

2

2

2

1

1

2

)

cos(

Y

X

L

L

Y

X

L

T

+

–

+

+

=

–

q

q

T

Y

X

L

L

Y

X

L

q

q

+

+

–

+

+

=

–

)

2

(

cos

2

2

1

2

2

2

2

2

1

1

1

(

)

)

2

(

cos

180

2

1

2

2

2

2

2

1

1

2

L

L

Y

X

L

L

+

–

+

–

=

–

q

(

)

2

1

2

2

2

2

2

1

2

2

)

180

cos(

L

L

Y

X

L

L

+

–

+

=

–

q

÷

÷

ø

ö

ç

ç

è

æ

+

=

–

2

2

1

cos

Y

X

X

T

q

i

i

a

w

=

&

[

]

[

]

ú

ú

ú

ú

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ê

ê

ê

ê

ë

é

=

=

=

n

z

z

z

n

y

y

y

n

x

x

x

T

n

T

z

y

x

z

y

x

a

a

a

p

p

p

p

p

p

J

v

v

v

V

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

¶q

¶

q

q

q

q

q

w

w

w

…

…

…

…

…

…

…

…

2

1

2

1

2

1

3

2

1

&

&

&

&

&

q

&

J

V

=

6

6

5

5

4

4

3

3

2

2

1

1

dx

x

f

dx

x

f

dx

x

f

dx

x

f

dx

x

f

dx

x

f

y

i

i

i

i

i

i

i

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

¶

+

+

+

+

+

=

)

(

)

)

((

S

G

a

J

S

n

n

–

×

´

–

)

(

)

)

((

S

G

a

J

S

i

i

–

×

´

–

V

f

I

JJ

T

=

–

)

(

2

l

dX

X

F

dY

¶

¶

=

)

(

)

)

((

1

1

S

G

a

J

G

–

×

´

–

)

(

)

(

)

(

q

E

q

E

q

E

o

p

+

=

)

(

)

)

((

2

2

S

G

a

J

S

–

×

´

–

Alt

J

=

ú

ú

ú

û

ù

ê

ê

ê

ë

é

–

–

–

ú

ú

ú

ú

û

ù

ê

ê

ê

ê

ë

é

´

–

´

–

´

–

´

–

´

–

=

z

y

x

T

z

y

x

n

n

x

x

S

G

S

G

S

G

a

J

S

a

J

S

a

J

S

a

J

S

a

J

S

)

(

)

(

)

(

…

)

)

((

…

…

…

)

)

((

)

)

((

)

)

((

)

)

((

1

1

1

1

2

2

1

1

q

D

=

=

V

J

T

)

(

)

)

((

1

1

S

G

a

J

S

–

×

´

–

V

I

JJ

J

T

T

1

2

)

(

–

–

=

D

l

q

dX

X

F

dY

¶

¶

=

)

(

)

)

((

2

2

S

G

a

J

G

–

×

´

–

V

J

I

J

J

T

T

=

D

–

q

l

)

(

2

V

I

JJ

f

T

1

2

)

(

–

–

=

l

q

b

b

b

q

q

&

&

&

=

=

=

=

=

–

–

+

T

T

T

T

T

J

V

JJ

V

JJ

V

JJ

J

V

J

)

(

)

(

)

(

1

1

dX

X

F

dY

¶

¶

=

q

D

=

f

J

T

dX

X

F

dY

¶

¶

=

t

t

L

L

L

L

L

s

w

L

e

w

L

s

e

L

q

q

q

–

=

–

+

=

–

=

–

=

–

=

180

2

)

cos(

4

2

1

2

2

2

2

1

2

1

2

)

(

)

(

c

d

p

P

P

q

E

–

=

2

3

1

)

1

)

((

)

(

–

×

=

å

=

jc

j

jd

o

u

u

q

E

ic

axis

ic

P

R

P

i

)

(

)

(

f

f

=

¢

id

ic

p

P

P

g

×

¢

=

)

(

)

(

f

f

)

(

)

(

3

1

f

f

jc

j

jd

o

u

u

g

¢

×

=

å

=

)

(

)

(

)

(

f

f

f

o

o

p

p

g

w

g

w

g

+

=

p

f

+

c

j

k

k

i

j

i

c

q

P

P

c

q

c

J

¶

¶

¶

¶

=

¶

¶

=

)

(

)

(

)

(

f

f

f

o

o

p

p

g

w

g

w

g

+

=

p

f

–

c

)]

(

)

(

[

3

1

3

jc

j

jd

o

ic

id

p

i

u

u

w

P

P

w

axis

k

å

=

´

+

´

×

=

)

(

)

(

3

1

2

jc

j

jd

o

ic

id

p

u

u

w

P

P

w

k

å

=

×

+

×

=

1

=

o

w

)

1

(

r

a

+

=

p

w

W

k

/

=

a

)

,

max(

)

,

min(

ic

id

ic

id

P

P

P

P

=

r

)

sin(

)

cos(

))

cos(

1

(

)

(

3

2

1

f

f

f

f

k

k

k

g

+

+

–

=

)

(

)

(

)

)(

(

3

1

1

i

jc

j

i

jd

o

i

ic

i

id

p

axis

u

axis

u

w

axis

P

axis

P

w

k

×

×

+

×

×

=

å

=

0

)

cos(

)

sin(

)

(

3

2

1

=

+

–

f

f

k

k

k

)

)

(

(

tan

1

2

3

1

k

k

k

–

=

–

f

i

ic

id

axis

P

P

×

–

=

)

(

l

T

c

c

a

kCJ

g

g

K

q

–

+

=

)

(

&

f

i

i

i

w

q

q

+

=

Kg

J

C

c

=

&

j

i

c

q

c

J

¶

¶

=

0

)

(

=

¶

¶

=

q

q

C

q

C

&

&

b

A

=

l

a

c

c

c

Kg

J

Kg

J

–

=

0

)

(

=

q

C

0

))

(

(

2

=

–

q

P

R

[

]

]

)

(

,

)

(

,

)

[(

))

(

(

)),

(

(

)),

(

(

)

(

2

2

2

3

2

1

z

z

y

y

x

x

z

y

x

P

R

P

R

P

R

q

P

c

q

P

c

q

P

c

q

C

–

–

–

=

=

c

a

g

g

g

+

=

a

c

T

c

c

Kg

J

KJ

J

–

=

l

x

0

=

Kg

J

c

T

UDV

A

=

Kg

F

J

K

q

T

Þ

=

&

T

c

c

KJ

J

]

,…,

[

1

m

l

l

l

=

c

c

J

g

l

=

q

x

¶

¶

)

(

x

f

q

x

x

f

q

f

¶

¶

¶

¶

=

¶

¶

q

f

¶

¶

f

b

A

UDV

T

=

=

l

l

i

i

i

T

T

T

T

T

T

V

w

b

U

b

U

VD

b

U

D

V

b

U

DV

b

UDV

å

×

=

=

=

=

=

–

–

l

l

l

l

l

1

1

i

i

i

T

T

T

T

T

T

V

w

b

U

b

U

VD

b

U

D

V

b

U

DV

b

UDV

å

×

=

=

=

=

=

–

–

l

l

l

l

l

1

1