1

Kinematics, Kinematics Chains

CS 685

Jana Kosecka

1

Previously

• Representation of rigid body motion• Two different interpretations

- as transformations between different coord. frames - as operators acting on a rigid body

• Representation in terms of homogeneous coordinates• Composition of rigid body motions• Inverse of rigid body motion

2

2

Rigid Body Motion – Homogeneous Coordinates

3-D coordinates are related by:

Homogeneous coordinates:

Homogeneous coordinates are related by:

3

Properties of Rigid Body Motions

Rigid body motion composition

Rigid body motion inverse

Rigid body motion acting on vectors

Vectors are only affected by rotation – 4th homogeneous coordinate is zero

4

3

3D Rotation (axis & angle)

with

or

<latexit sha1_base64="4OhIB9iTHuOIDyfhlZhRy6kkqQw=">AAACE3icbZA9SwNBEIb3/IzxK2ppsxgEFQl3ImojiDaWUUwi5M4wt5kki3sf7M4J4ch/sPGv2FgoYmtj57/xklzh1wsLL8/MMDuvHytpyLY/rYnJqemZ2cJccX5hcWm5tLJaN1GiBdZEpCJ97YNBJUOskSSF17FGCHyFDf/2bFhv3KE2MgqvqB+jF0A3lB0pgDLUKu1cbrlRgF3YdamHBNv8mONN6vaAxpyP+aBYbJXKdsUeif81Tm7KLFe1Vfpw25FIAgxJKDCm6dgxeSlokkLhoOgmBmMQt9DFZmZDCNB46eimAd/MSJt3Ip29kPiIfp9IITCmH/hZZwDUM79rQ/hfrZlQ58hLZRgnhKEYL+okilPEhwHxttQoSPUzA0LL7K9c9ECDoCzGYQjO75P/mvpexTmo2Bf75ZPTPI4CW2cbbIs57JCdsHNWZTUm2D17ZM/sxXqwnqxX623cOmHlM2vsh6z3L8ZlnNY=</latexit>

R(!, ✓) = e!✓

5

Rigid Body Transform

{A}

{B}

XA

XB

tAB

XA = XB + tAB

The points from frame A to frame B are transformed by the inverse of(see example next slide)

XA = RABXB + tAB

T = (RAB , tAB)

tAB

T = (RAB , tAB)

Translation only is the origin of the frame B expressed in the Frame A

Composite transformation:

Transformation:

XA =�

RAB tAB

0 1

⇥XB

Homogeneous coordinates

6

4

Kinematic Chains

• Robot manipulator; multiple rigid bodies linked together• Kinematics – study of position, orientation, velocity, acceleration

regardless of the forces• Simple examples of kinematic model of robot manipulator• Components – links, connected by joints, important frames

{B} – base frame{T} – tool frame{S} – station frame{G} – goal/object frame{W} – wrist frame

{B} {S}

{W}

{T}{O}

7

Various joints• In general rigid bodies can be connected by various articulated joints

8

5

Kinematic Chains in 2D

work space frame

Base frame

�1, �2

�1�2

• Given determine what is • Given determine what is• We can control want to understand how it affectsposition of the tool frame

• How does the position of the tool frame change as the manipulator articulates

• Actuators change the joint angles

�1, �2

�1, �2

XY

!

"#

$

%&

X,YX.,Y.

9

Forward kinematics 2D arm

• Find position of the end effector as a function of the joint angles

f(�1, �2) =�

XY

⇥

10

6

Example

<latexit sha1_base64="VPstW+OrcKCiLZOua0y2zNRSDfE=">AAACZ3icbVFNaxsxENVu+pFuP+I2JRR6mSY0FEqNtoe0l4JpLz2mECcBa1m08qwtotUu0myJWZxf0V/WW+699F9UthNI4g4I3rw3Mxo9FY3Rnji/jOKNe/cfPNx8lDx+8vTZVu/5i2Nft07hUNWmdqeF9Gi0xSFpMnjaOJRVYfCkOPu20E9+ovO6tkc0azCr5MTqUitJgcp7F0d5x9M5fAFhsKQRiAIn2naVJKfP55AoEDRFknkK+/AB/M2UgxDJLUatyXwJ9iFNBNrx9dxEOD2ZUpb39nifLwPWQXoF9ga74v2vy8HsMO/9FuNatRVaUkZ6P0p5Q1knHWllMMxtPTZSnckJjgK0skKfdUuf5vA2MGMoaxeOJViyNzs6WXk/q4pQGfac+rvagvyfNmqp/Jx12jYtoVWri8rWANWwMB3G2qEiMwtAKqfDrqCm0klF4WuSYEJ698nr4PhjPz3o8x/Bja9sFZvsNdtl71jKPrEB+84O2ZAp9idKou3oZfQ33op34ler0ji66tlmtyJ+8w+Yd7R2</latexit>

T01 =

2

4c✓1 �s✓1 0s✓1 c✓1 00 0 1

3

5

<latexit sha1_base64="TuU/s+DyIn0mxkjOKB5mfIztwlA=">AAACaXicbVFRaxNBEN47ba1nramiiL4MDS1CMdzlQfsiBH3xsULTFrLHsbeZS5bs7R27c2I4Av6I/jLf+gd88U+4SSo0rQML33zfzOzst3mtlaM4vg7CBw+3th/tPI6e7D7de9bZf37uqsZKHMpKV/YyFw61MjgkRRova4uizDVe5LMvS/3iO1qnKnNG8xrTUkyMKpQU5Kms8/Msa5P+Aj4B11jQCHiOE2XaUpBVPxYQSeA0RRJZH47gPbjbqc4S4Dza4Dbq46Ucr8ARJBFHM/43OeJWTaaUZp1u3ItXAfdBcgO6gwN+fHU9mJ9mnV98XMmmRENSC+dGSVxT2gpLSmr0cxuHtZAzMcGRh0aU6NJ25dQCDj0zhqKy/hiCFXu7oxWlc/My95V+z6m7qy3J/2mjhoqTtFWmbgiNXF9UNBqogqXtMFYWJem5B0Ja5XcFORVWSPKfE3kTkrtPvg/O+73kQy/+5t34zNaxw96yA/aOJewjG7Cv7JQNmWS/g93gZfAq+BPuh6/DN+vSMLjpecE2Iuz+BYB1tVw=</latexit>

T12 =

2

4c✓2 �s✓2 l1s✓2 c✓2 00 0 1

3

5

<latexit sha1_base64="if25sovVsnczNVp7CObX9W9/KyA=">AAACBHicbVC7SgNBFJ2Nrxhfq4JNmsEgWIXdFGojhNhYJpAXJMsyO5lNhszOLjOzgbBsYeOv2CgoYmvlF9jZ+C3OJik08cDlHs65l5l7vIhRqSzry8itrW9sbuW3Czu7e/sH5uFRW4axwKSFQxaKrockYZSTlqKKkW4kCAo8Rjre+CbzOxMiJA15U00j4gRoyKlPMVJacs1if0Jw0k3hNWy6iWWnWbMrKay7FdcsWWVrBrhK7AUpVU8a3/Sp9lF3zc/+IMRxQLjCDEnZs61IOQkSimJG0kI/liRCeIyGpKcpRwGRTjI7IoVnWhlAPxS6uIIz9fdGggIpp4GnJwOkRnLZy8T/vF6s/CsnoTyKFeF4/pAfM6hCmCUCB1QQrNhUE4QF1X+FeIQEwkrnVtAh2Msnr5J2pWxflK2GTqMG5siDIjgF58AGl6AKbkEdtAAGd+ABPIMX4954NF6Nt/lozljsHIM/MN5/ANk+mf0=</latexit>

~X = T01T12P2

<latexit sha1_base64="zmjGx6Jnw7i/wvknRSNwNszVPek=">AAACYHicbZFLTwIxFIU7gw9EENCdbhqJBmNCZohRNyZENy4xkUfCENIpRRo6nUl7RyWEP+nOhRt/iR2YBQI3aXL63XvS9tSPBNfgON+WndnZ3dvPHuQO84WjYql83NZhrChr0VCEqusTzQSXrAUcBOtGipHAF6zjT56SfuedKc1D+QrTiPUD8ib5iFMCBg1KH58YX+IHjMXAxR4NtQdjBsRsrg2qL1B1haWyfoWx5+Wmq2bN5brZoO3m3KBUcWrOovCmcFNRQWk1B6UvbxjSOGASqCBa91wngv6MKOBUsHnOizWLCJ2QN9YzUpKA6f5sEdAcXxgyxKNQmSUBL+iqY0YCraeBbyYDAmO93kvgtl4vhtF9f8ZlFAOTdHnQKBYYQpykjYdcMQpiagShipu7YjomilAwf5KE4K4/eVO06zX3tua83FQaj2kcWXSGzlEVuegONdAzaqIWoujHylh5q2D92lm7aJeXo7aVek7Qv7JP/wDznq3c</latexit>

x = l1 cos ✓1 + l2 cos(✓1 + ✓2)

y = l1 sin ✓1 + l2 sin(✓1 + ✓2)f(�1, �2) =

�XY

⇥

Find transformations between frames P2 – coordinate of the end effector in frame 2What is the coordinate in frame 0 ?

<latexit sha1_base64="WzbVAofRnFsRwMgaeIGw4z09Dbw=">AAACKHicbVDLSgMxFM3U9/iqunQTFEEQykwX6kYsunFZwdpCZyiZ9E4bzGSG5I5YBv/CX3Djr7gRUaRbv8S0VfB1IOFwzr3JvSfKpDDoeUOnNDU9Mzs3v+AuLi2vrJbX1i9NmmsODZ7KVLciZkAKBQ0UKKGVaWBJJKEZXZ2O/OY1aCNSdYGDDMKE9ZSIBWdopU75uN6p0iMaSIixTYMIekIVCUMtbm6pK60ZBK43unw3ANX98txAi14fw05526t4Y9C/xP8k27WtYO9uWBvUO+XnoJvyPAGFXDJj2r6XYVgwjYJLsO/mBjLGr1gP2pYqloAJi/Git3THKl0ap9oehXSsfu8oWGLMIIlspZ2zb357I/E/r51jfBgWQmU5guKTj+JcUkzpKDXaFRo4yoEljGthZ6W8zzTjaLN1bQj+75X/kstqxd+veOc2jRMywTzZJFtkl/jkgNTIGamTBuHknjySF/LqPDhPzpsznJSWnM+eDfIDzvsHU1mnVg==</latexit>

P2 =

2

4l201

3

5

11

Kinematic Chains in 3D

• Additional joints possible (spherical, screw)• Additional offset parametes• Same idea: set up frame with each link • Define relationship between links (two rules):

- use Z-axis as an axis of a revolute joint- connect two axes shortest distance

In 2D we need only link length and joint angle to specify the transform

In 3D Denavit-Hartenberg parameters (see LaValle (chapter [3])

di, ⇥i, ai�1, �i�1

12

7

Example 3D SCARA manipulator

Transformation between stationary frameand tool frame

Adapted from Murray, Li, SastryRobotic Manipulation

13

Inverse kinematics

• In order to accomplish tasks, we need to know given some coordinates in the tool frame, how to compute the joint angle

• Simple 2D example • Use trigonometry to compute given [X, Y] of the end effector • Solution may not be unique• See handout notes for details

�1, �2

Figure adapted from K. Hauser, http://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html

14

8

Inverse kinematics

• For small problem angles can be calculated analytically, for larger chains more complex

• Some 3D manipulators – analytic solutions to IK• Redundant robots – IK sets of solutions • Numerical techniques

- Cyclic coordinate descent - Root finding methods - minimization methods

• For more details http://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html

15

Inverse Kinematics

Forward Kinematics (FK) Mathematically determining the position and angle of joints in a series of flexible, jointed objects after determining the position and orientation of the end effector.

In game design, inverse kinematics (IK) is typically used most often in character animation

16

9

Learning Locomotion[Schulman, Moritz, Levine, Jordan, Abbeel, 2015]

policy gradients, value function approximation

Slides courtesy P. Abbeel

17

Jacobians

• Kinematics enables us study what space is reachable• Given reachable points in space, how well can be motion of an arm

controlled near these points • We would like to establish relationship between velocities in joint space

and velocities in end-effector space• Given kinematics equations for two link arm

• The relationship between velocities is manipulator Jacobian

�xy

⇥=

⇤⇥x⇥�1

⇥x⇥�2

⇥y⇥�1

⇥y⇥�2

⌅ ��1

�2

⇥ �xy

⇥= J(�1, �2)

��1

�2

⇥

x = fx(�1, �2)y = fy(�1, �2)

J(�1, �2)

18

10

Manipulator Jacobian

• Determinant of the Jacobian• If determinant is 0, there is a singularity

• Manipulator kinematics: position of end effector can be determined knowing the joint angles

• Actuators: motors that drive the joint angles• Workspace concept (in the presence of constraints)

Joint angle constraints +/- 45

19

Locomotion of wheeled robots

• Power the motion from place to place• Differential Drive (two powered wheels)• Car Drive (Ackerman Steering)

y

roll

z motion

x

y

we also allow wheels torotate around the z axis

21

11

Locomotion of wheeled robots

• Differential Drive (two powered wheels)

• Each wheel is has its own motor• Two wheels can move at different speeds

22

Mobile robot kinematics

• Two wheels, with radius • Point P centered between two wheels is the origin of

the robot frame• Distance between the wheels

yI

xI

s(t)q

v(t)

€

l€

r

24

12

Differential Drive

R

ICCw

(x,y)

y

l/2

qx

vl

vr

• Controls: Instantaneous linear velocity of each wheel • Left and right wheel can move at different speed• Robots coordinate system, robot (heading in the x-direction)• Parameters, distance between the wheels l • Radius of each wheel r

vl,vr

vr = ψ r

ψ

x

y

r – wheel radiusvr – linear velocity of the right wheelvl – linear velocity of the left wheel

25

Differential Drive

R

ICCw

(x,y)

y

l/2

q

x

vl

vr

• Controls: Instantaneous linear velocity of each wheel• Motion of the robot • Turn in place• Go straight

vl,vr

x

y

vr = −vl → R = 0vr = vl →ω = 0

26

13

Differential Drive

R

ICCw

(x,y)

y

l/2

q

x

vl

vr

• Turn in place• Go straight• More general motion, turning and moving forward • There must be a point that lies on the wheel axis that

the robot rotates around

x

y

vr = −vl → R = 0vr = vl →ω = 0

27

Instantaneous Center of Curvature

ICC

§ When robot moves on a curve with particular linear and angular velocity at each instance there is a point called instantaneous center of curvature

28

14

Differential Drive

R

ICCw

(x,y)

y

l/2

qx

vl

vr

ω(R+ l / 2) = vrω(R− l / 2) = vl

]cos,sin[ICC qq RyRx +-=

Instantaneous linear velocity of each wheel

𝜔 is the angular velocity of the robot’s body framearound ICC

ω =dθdt

=VR

vl,vr

vr = ψ r

Forward velocity of theWheel of radius r as it turns with angular rate ψ

29

Differential Drive

R

ICCw

(x,y)

y

l/2

q

x

vl

vr

ω(R+ l / 2) = vrω(R− l / 2) = vl

R = l2(vl + vr)(vr − vl)

ω =vr − vll

v = vr + vl2

Instantaneous linear velocity of each wheel • Angular velocity are related via R radius of the curve

(subtract two equations for )• Linear velocity (add two equations for )

ω =dθdt

=VR

Angular velocity

vl,vr

vl,vrvl,vr

vLinear velocity

30

15

Differential Drive: Intuition• When both wheels turn with the same

speed robot goes straight• When one wheel turns faster then the

other robot turns• When the wheels turn in opposite direction

the robot turns in place

• We can solve for rate of rotation around ICC two special cases

• Turn in place• Go straight

ω(R+ l / 2) = vrω(R− l / 2) = vl

R = l2(vl + vr)(vr − vl)

ω =vr − vll

v = vr + vl2

vr = vl

ω

vr = vl →ω = 0vr = −vl → R = 0

vr = −vl

31

Differential Drive

• Linear and angular velocities in the robot body frame

ICC

(x,y)

vr

vx ,Rvy ,Rω

!

"

####

$

%

&&&&

=

12(vl + vr )

012(vr − vl )

!

"

######

$

%

&&&&&&

xRyR

vl

v = [vx ,vy ]

ω = θ

32

16

Representing Robot Pose

• Representing robot motion in inertial (global) frame• Previous the velocities were expressed in robot frame

– Inertial (global) frame:– Robot frame (axes)– Robot pose:– Robot velocities: – Previously the velocities were expressed in the robot

coordinate frame– Mapping from global reference frame to robot frame

– Example: Robot aligned with YI

{ }II YX ,

{ }RR YX ,

€

R θ( ) =

cosθ sinθ x−sinθ cosθ y0 0 1

$

%

& & &

'

(

) ) )

YR

XRYI

XI

q

P

YR

XR

q

YI

XI

[ ]TI yx qx =

[ ]TI yx qx =. . ..

33

Robot Motion – Differential Drive• Representing to robot within an arbitrary initial frame

– Initial frame:– Robot frame:– Robot pose:– We control in the robot frame– Differential robot drive instantaneously moves along x

axis – Velocities in the world frame are

[ ]TI yx qx =

{ }II YX ,

{ }RR YX ,

xy

!

"##

$

%&&= cosθ −sinθ

sinθ cosθ

!

"#

$

%&

v0

!

"#

$

%&

P

YR

XR

q

YI

XIv,ω

v = vx,vy!" #$T= [vx, 0]

T

θ =ω

<latexit sha1_base64="VPbAAnFJizJOpXbPKoJs64Jd7Vw=">AAACeHicbZHJTsMwEIadsJetwJHLiLIKUSUVAi5ICC4cQaKA1FSR4zrUquNE9qSiivIMvBs3HoQLJ9zlAIWRLP2emc/LP1EmhUHP+3Dcmdm5+YXFpcryyuraenVj89GkuWa8yVKZ6ueIGi6F4k0UKPlzpjlNIsmfot7NsP7U59qIVD3gIOPthL4oEQtG0abC6lvQSbF4LWEfLiGINWWFLotGCYf9UMMx9EN5BAFLDQTY5UghCCojZPAvIuHEItoiRqhpZLyd4uQvrhJWa17dGwX8Ff5E1Mgk7sLquz2b5QlXyCQ1puV7GbYLqlEwyctKkBueUdajL7xlpaIJN+1iZFwJezbTgTjVdimEUfYnUdDEmEES2c6EYtdM14bJ/2qtHOOLdiFUliNXbHxRnEvAFIZTgI7QnKEcWEGZFvatwLrUWoJ2VkMT/Okv/xWPjbp/VvfuT2tX1xM7Fsk22SGHxCfn5IrckjvSJIx8OtvOrrPnfLngHrhH41bXmTBb5Fe4jW/KybtR</latexit>

x =r

2(vr + vl) cos ✓

y =r

2(vl � vr) sin ✓

✓ =r

l(vl � vr)

34

17

Robot motion Differential Drive

– Velocities in the world frame are

– With the following controls

P

YR

XR

q

YI

XI

ω =vr − vll

v = vr + vl2

<latexit sha1_base64="VPbAAnFJizJOpXbPKoJs64Jd7Vw=">AAACeHicbZHJTsMwEIadsJetwJHLiLIKUSUVAi5ICC4cQaKA1FSR4zrUquNE9qSiivIMvBs3HoQLJ9zlAIWRLP2emc/LP1EmhUHP+3Dcmdm5+YXFpcryyuraenVj89GkuWa8yVKZ6ueIGi6F4k0UKPlzpjlNIsmfot7NsP7U59qIVD3gIOPthL4oEQtG0abC6lvQSbF4LWEfLiGINWWFLotGCYf9UMMx9EN5BAFLDQTY5UghCCojZPAvIuHEItoiRqhpZLyd4uQvrhJWa17dGwX8Ff5E1Mgk7sLquz2b5QlXyCQ1puV7GbYLqlEwyctKkBueUdajL7xlpaIJN+1iZFwJezbTgTjVdimEUfYnUdDEmEES2c6EYtdM14bJ/2qtHOOLdiFUliNXbHxRnEvAFIZTgI7QnKEcWEGZFvatwLrUWoJ2VkMT/Okv/xWPjbp/VvfuT2tX1xM7Fsk22SGHxCfn5IrckjvSJIx8OtvOrrPnfLngHrhH41bXmTBb5Fe4jW/KybtR</latexit>

x =r

2(vr + vl) cos ✓

y =r

2(vl � vr) sin ✓

✓ =r

l(vl � vr)

<latexit sha1_base64="7t68YIaie8zk7wjih98UrJitpBs=">AAACQnicbVDLSgMxFM34tr6qLt0Ei+KqzIioG0F041LBaqVTSia9bUMzyZDcEcvQb3PjF7jzA9y4UMStC9OHUFsPBE7OOTePEyVSWPT9F29qemZ2bn5hMbe0vLK6ll/fuLE6NRxKXEttyhGzIIWCEgqUUE4MsDiScBu1z3v+7T0YK7S6xk4C1Zg1lWgIztBJtfxdWNeYPXTpLj2hht7TkGtLQ2wBMhqGub7dGbGtUOP2YPuboaGOoclytXzBL/p90EkSDEmBDHFZyz+703gag0IumbWVwE+wmjGDgkvo5sLUQsJ4mzWh4qhiMdhq1q+gS3ecUqcNbdxSSPvq6ETGYms7ceSSMcOWHfd64n9eJcXGcTUTKkkRFB9c1EglRU17fdK6MMBRdhxh3Aj3VspbzDCOrvVeCcH4lyfJzX4xOCz6VweF07NhHQtki2yTPRKQI3JKLsglKRFOHskreScf3pP35n16X4PolDec2SR/4H3/ALMurUc=</latexit>

x = rv cos ✓

y = rv sin ✓

✓ = r!

36

Unicycle• Viewed as abstract version of differential drive• Parameters: wheel radius r, pedaling velocity, linear

velocity, angular velocity controlled directlyxR

yRψ

xI

xI

𝜃

<latexit sha1_base64="5yyndE315qPDNZFgSG3hUiwHn6g=">AAACO3icbVBNSwMxEM3W7/pV9eglWBRPZVdEvQiiF48qVgvdUrLptA3NJksyK5al/8uLf8KbFy8eFPHq3fQD0dYHgZf3ZiaZFyVSWPT9Zy83NT0zOze/kF9cWl5ZLayt31idGg5lrqU2lYhZkEJBGQVKqCQGWBxJuI06Z33/9g6MFVpdYzeBWsxaSjQFZ+ikeuEqbGjM7nt0hx7TOxpybWmIbUBGwzA/MLs/phVq3BxehxWhjqHF8vVC0S/5A9BJEoxIkYxwUS88uVE8jUEhl8zaauAnWMuYQcEl9PJhaiFhvMNaUHVUsRhsLRvs3qPbTmnQpjbuKKQD9XdHxmJru3HkKmOGbTvu9cX/vGqKzaNaJlSSIig+fKiZSoqa9oOkDWGAo+w6wrgR7q+Ut5lhHF3c/RCC8ZUnyc1eKTgo+Zf7xZPTURzzZJNskV0SkENyQs7JBSkTTh7IC3kj796j9+p9eJ/D0pw36tkgf+B9fQMrZasr</latexit>

x = v cos ✓

y = v sin ✓

✓ = !

<latexit sha1_base64="Pqfc0Iwtj5IaY0fbYJmrl5nv3J0=">AAAB+XicbVDLSsNAFL2pr1pfUZduBovgqiQi6kYounFZwT6gDWUynbRDJw9mbgol9E/cuFDErX/izr9x0mahrQcGDufcw71z/EQKjY7zbZXW1jc2t8rblZ3dvf0D+/CopeNUMd5ksYxVx6eaSxHxJgqUvJMoTkNf8rY/vs/99oQrLeLoCacJ90I6jEQgGEUj9W17Qm5JbxAj6SVaEFXp21Wn5sxBVolbkCoUaPTtLxNnacgjZJJq3XWdBL2MKhRM8lmll2qeUDamQ941NKIh1142v3xGzowyIEGszIuQzNXfiYyGWk9D30yGFEd62cvF/7xuisGNl4koSZFHbLEoSCXBmOQ1kIFQnKGcGkKZEuZWwkZUUYamrLwEd/nLq6R1UXOvas7jZbV+V9RRhhM4hXNw4Rrq8AANaAKDCTzDK7xZmfVivVsfi9GSVWSO4Q+szx+FFpJK</latexit>

v = r

37

18

Car Model • Car kinematics model (Ackerman steering)• Steering angle, forward speed

• Ingredients: how to characterize the pose, velocity • What are the parameters and control inputs • See: http://planning.cs.uiuc.edu/node657.html for

additional detailed derivations, e.g. tractor trailer

x = vx cosθ

y = vx sinθ

θ = tanφLvx

40

Bicycle Kinematic model

• Similar, slightly different steering mechanism• Bicycle model of the car• Hind wheels move with the same speed• Front wheels can be rotated• L distance between form and back wheels

ICC

(x,y)q

γ

x = vcosθy = vsinθ

θ = vLtanγ

xy

xcosθ − ysinθ = 0Nonholonomic velocity constraints

Cannot change orientation not moving with v

v = 0→ θ = vLtanγ→ θ = 0

41

19

Mobile Robot Kinematic Models

• Manipulator case – given joint angles, we can always tell where the end effector is

• Mobile robot basis – given wheel positions we cannot tell where the robot is

• We have to remember the history how it got there• Need to find relationship between velocities and

changes in pose• Presented on blackboard (see handout)• How is the wheel velocity affecting velocity of the

chassis

42

Differential Drive: Forward Kinematics

43

ICC

R

P(t)

P(t+dt)

• To compute the trajectory we need to integrate the equations

')]'()'([1)(

')]'(sin[)]'()'([21)(

')]'(cos[)]'()'([21)(

0

0

0

ò

ò

ò

-=

+=

+=

t

lr

t

lr

t

lr

dttvtvl

t

dtttvtvty

dtttvtvtx

q

q

q

43

20

Differential Drive

• Integral cannot be solved analytically • are functions of time• Option 1: consider special cases of straight line

motion and rotation only• Option 2: simulate the differential equation (see

notes)

ω(t),v(t)

44

Differential Drive

<latexit sha1_base64="6Z8EUzbg2gh5GDk7JAyiineVV90=">AAACeHicfZFLS8NAEMc38V1fVY9eBusTsSRF1IsgevGoYFVoSthsN3Zxswm7k2IJ+Qx+N29+EC+e3D4OWsWBhf/OzG8f/4kyKQx63rvjTk3PzM7NL1QWl5ZXVqtr6/cmzTXjTZbKVD9G1HApFG+iQMkfM81pEkn+ED1fDeoPPa6NSNUd9jPeTuiTErFgFG0qrL4GnRSLlxJ24RyCWFNW6LJolLDfCzUcQi+UBxCw1ECAXY4UgqAyRPp/IhKOLKItYoSaREbbf7lKWK15dW8Y8Fv4Y1Ej47gJq2/2bJYnXCGT1JiW72XYLqhGwSQvK0FueEbZM33iLSsVTbhpF0PjStixmQ7EqbZLIQyz34mCJsb0k8h2JhS7ZrI2SP5Va+UYn7ULobIcuWKji+JcAqYwmAJ0hOYMZd8KyrSwbwXWpdYStLMamOBPfvm3uG/U/ZO6d3tcu7gc2zFPNskW2Sc+OSUX5JrckCZh5MPZdLadHefTBXfPPRi1us6Y2SA/wm18AW+1uxc=</latexit>

x =r

2(vr + vl) cos ✓

y =r

2(vl � vr) sin ✓

✓ =r

2(vl � vr)

<latexit sha1_base64="tA7DmWsv9M1JcRBUccaNCJ4w2zE=">AAAConicbZFdS9xAFIYnsVZd27rqpRceXC2KuCQi6o0gFaGU4gd2VdgsYTI7cQcnkzBzsriE/LD+jd713zi7m2KNHhh45z3nmY9zokwKg57313FnPsx+nJtfaCx++vxlqbm8cmvSXDPeYalM9X1EDZdC8Q4KlPw+05wmkeR30ePZOH835NqIVP3CUcZ7CX1QIhaMorXC5u+gn2LxVMJXOIEg1pQVuiz2S9gehhp2YRjKHQhYaiDAAUcKQdCYIKN3EQl7FtEWMULVkem2xv2scVWttaaF1OZeTP3P1BA2W17bmwS8FX4lWqSKq7D5xx7C8oQrZJIa0/W9DHsF1SiY5GUjyA3PKHukD7xrpaIJN71i0uIStqzThzjVdimEifs/UdDEmFES2cqE4sDUc2PzvVw3x/i4VwiV5cgVm14U5xIwhfG8oC80ZyhHVlCmhX0rsAG1zUM71YZtgl//8ltxu9/2D9ve9UHr9FvVjnmyRjbINvHJETkl38kV6RDmrDvnzoVz6W66P9xr92Za6joVs0pehRs8A/aByFM=</latexit>

x =r

2(vr + vl) cos ✓

y =r

2(vl � vr) sin ✓

✓ =r

L(vl � vr)

vl = al

vr = ar

Kinematics First order model

Dynamics second order model

• Pick control input (in this case velocities or left and right wheel) and add equations for their derivatives• New control – angular accelerations <latexit sha1_base64="1aMWanavsWw3BPWj8dt0DgOcFAw=">AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5RERD0WvXisYD+gDWGy3bZLN5u4uxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwkRwbVz32ymsrK6tbxQ3S1vbO7t75f2Dpo5TRVmDxiJW7RA1E1yyhuFGsHaiGEahYK1wdDv1W09MaR7LBzNOmB/hQPI+p2is1MZAnBEMVFCuuFV3BrJMvJxUIEc9KH91ezFNIyYNFah1x3MT42eoDKeCTUrdVLME6QgHrGOpxIhpP5vdOyEnVumRfqxsSUNm6u+JDCOtx1FoOyM0Q73oTcX/vE5q+td+xmWSGibpfFE/FcTEZPo86XHFqBFjS5Aqbm8ldIgKqbERlWwI3uLLy6R5XvUuq+79RaV2k8dRhCM4hlPw4ApqcAd1aAAFAc/wCm/Oo/PivDsf89aCk88cwh84nz9Hho95</latexit>al, ar

45

21

Car

<latexit sha1_base64="npWdD6QMvLSwlY9pnHPY2A51Lnk=">AAACVXicbVHPS8MwGE3rr1l/bOrRS3AonkYrol6EoRcPHiY4HaylpFm6BdO0JF+Ho/Sf3EX8T7wIZlsPc/pBwst77+NLXqJMcA2u+2nZa+sbm1u1bWdnd2+/3jg4fNFprijr0lSkqhcRzQSXrAscBOtlipEkEuw1eruf6a9jpjRP5TNMMhYkZCh5zCkBQ4UN4Q9SKN5LfIZv8TjU2Kep2WDEgGDfd+byZEnWXK7Ki+PC48eK0MI4y+KxNEYicR4WfjbipeOEjabbcueF/wKvAk1UVSdsTM0AmidMAhVE677nZhAURAGngpWOn2uWEfpGhqxvoCQJ00ExT6XEp4YZ4DhVZknAc3a5oyCJ1pMkMs6EwEivajPyP62fQ3wTFFxmOTBJF4PiXGBI8SxiPOCKURATAwhV3NwV0xExuYD5iFkI3uqT/4KXi5Z31XKfLpvtuyqOGjpGJ+gceegatdED6qAuomiKvizLsq0P69tetzcXVtuqeo7Qr7LrPwVzsUE=</latexit>

x = vs cos ✓

y = vs sin ✓

✓ =vsL

tanu�

<latexit sha1_base64="WsUiEtNijLK1+lMzoNjPo+yi/DI=">AAACgHicbVFNS8QwEE3rd/1a9egluCgedG1FVARh0YsHDwquCtulpNnUDaZpSabiUvo7/F/e/DGCabvoujqQ8PLem0kyE6aCa3DdD8uemp6ZnZtfcBaXlldWG2vr9zrJFGUdmohEPYZEM8El6wAHwR5TxUgcCvYQPl+W+sMLU5on8g6GKevF5EnyiFMChgoab34/gfy1wDv4HL8EGvs0MRsMGBDs+04lD8dkzeWkXB9rjx8pQnPjLPLrwhiJxFmQ++mAF9/+Uq3MWeD9FKksNXvoOEGj6bbcKvBf4I1AE43iJmi8mzI0i5kEKojWXc9NoZcTBZwKVjh+pllK6DN5Yl0DJYmZ7uVVAwu8bZg+jhJllgRcseMZOYm1HsahccYEBnpSK8n/tG4G0Wkv5zLNgElaXxRlAkOCy2ngPleMghgaQKji5q2YDohpIZiZlU3wJr/8F9wftrzjlnt71GxfjNoxjzbRFtpFHjpBbXSFblAHUfRpNa09a9+27V37wPZqq22NcjbQr7DPvgDJ+71i</latexit>

x = vs cos ✓

y = vs sin ✓

✓ =vsL

tanu�

vs = u1

� = u2

State sPosition OrientationTranslational velocity Steering angle

State sPosition OrientationTranslational velocity Steering angle Translational accelerationSteering acceleration

<latexit sha1_base64="rEUF24W61IMG3p2TpN2N070h0RI=">AAAB+HicbVDLSsNAFJ34rPHRqEs3g0WoUEoioi6LblxWsA9oQ5lMJ+3QyYOZGzGGfokbF4q49VPc+TdO2iy09cCFwzn3cu89Xiy4Atv+NlZW19Y3Nktb5vbO7l7Z2j9oqyiRlLVoJCLZ9YhigoesBRwE68aSkcATrONNbnK/88Ck4lF4D2nM3ICMQu5zSkBLA6tcfaylNdyHMQNyapoDq2LX7RnwMnEKUkEFmgPrqz+MaBKwEKggSvUcOwY3IxI4FWxq9hPFYkInZMR6moYkYMrNZodP8YlWhtiPpK4Q8Ez9PZGRQKk08HRnQGCsFr1c/M/rJeBfuRkP4wRYSOeL/ERgiHCeAh5yySiIVBNCJde3YjomklDQWeUhOIsvL5P2Wd25qNt355XGdRFHCR2hY1RFDrpEDXSLmqiFKErQM3pFb8aT8WK8Gx/z1hWjmDlEf2B8/gBAkJGG</latexit>

(x, y, ✓)<latexit sha1_base64="dYMDEuaNQ7OetXUuuBbP6486hUc=">AAACA3icbVDLSgMxFM34rPVVdaebYBEqlDIjoi6LblxWsA9ohyGTZtrQTGZI7hTLUHDjr7hxoYhbf8Kdf2Om7UJbD1w4Oedecu/xY8E12Pa3tbS8srq2ntvIb25t7+wW9vYbOkoUZXUaiUi1fKKZ4JLVgYNgrVgxEvqCNf3BTeY3h0xpHsl7GMXMDUlP8oBTAkbyCoelh/KojDvQZ0DKeOhp84j7/DSf9wpFu2JPgBeJMyNFNEPNK3x1uhFNQiaBCqJ127FjcFOigFPBxvlOollM6ID0WNtQSUKm3XRywxifGKWLg0iZkoAn6u+JlIRaj0LfdIYE+nrey8T/vHYCwZWbchknwCSdfhQkAkOEs0BwlytGQYwMIVRxsyumfaIIBRNbFoIzf/IiaZxVnIuKfXderF7P4sihI3SMSshBl6iKblEN1RFFj+gZvaI368l6sd6tj2nrkjWbOUB/YH3+AGlTlXE=</latexit>

(x, y, ✓, vs,�)

<latexit sha1_base64="60RDTYUsifBDTk4afKhTj2M1R50=">AAAB9HicbVBNS8NAEJ3Urxq/qh69LBbBg5REinosevFYwX5AG8Jmu2mXbjZxd1Moob/DiwdFvPpjvPlv3LQ5aOuDgcd7M8zMCxLOlHacb6u0tr6xuVXetnd29/YPKodHbRWnktAWiXksuwFWlDNBW5ppTruJpDgKOO0E47vc70yoVCwWj3qaUC/CQ8FCRrA2kjfx1QVK/X4yYrbtV6pOzZkDrRK3IFUo0PQrX/1BTNKICk04VqrnOon2Miw1I5zO7H6qaILJGA9pz1CBI6q8bH70DJ0ZZYDCWJoSGs3V3xMZjpSaRoHpjLAeqWUvF//zeqkOb7yMiSTVVJDFojDlSMcoTwANmKRE86khmEhmbkVkhCUm2uSUh+Auv7xK2pc196rmPNSrjdsijjKcwCmcgwvX0IB7aEILCDzBM7zCmzWxXqx362PRWrKKmWP4A+vzBz9DkRo=</latexit>vs, u�<latexit sha1_base64="7r4r785hyQbIlJITJs/TvxGdbMg=">AAAB7HicbVBNS8NAEJ3Urxq/qh69LBbBU0lE1GPRi8cKpi20oWy2m3bpZhP2Qyihv8GLB0W8+oO8+W/ctDlo64OBx3szzMyLMs6U9rxvp7K2vrG5Vd12d3b39g9qh0dtlRpJaEBSnspuhBXlTNBAM81pN5MUJxGnnWhyV/idJyoVS8WjnmY0TPBIsJgRrK0UmIHvuoNa3Wt4c6BV4pekDiVag9pXf5gSk1ChCcdK9Xwv02GOpWaE05nbN4pmmEzwiPYsFTihKsznx87QmVWGKE6lLaHRXP09keNEqWkS2c4E67Fa9grxP69ndHwT5kxkRlNBFotiw5FOUfE5GjJJieZTSzCRzN6KyBhLTLTNpwjBX355lbQvGv5Vw3u4rDdvyziqcAKncA4+XEMT7qEFARBg8Ayv8OYI58V5dz4WrRWnnDmGP3A+fwBy+o3K</latexit>u1

<latexit sha1_base64="OzfO5mrHGSuw1vA72uLfS3ce2Ps=">AAAB7HicbVBNS8NAEJ31s8avqkcvi0XwVJIi6rHoxWMF0xbaUDbbTbt0swm7G6GE/gYvHhTx6g/y5r9x0+agrQ8GHu/NMDMvTAXXxnW/0dr6xubWdmXH2d3bPzisHh23dZIpynyaiER1Q6KZ4JL5hhvBuqliJA4F64STu8LvPDGleSIfzTRlQUxGkkecEmMlPxs0HGdQrbl1dw68SryS1KBEa1D96g8TmsVMGiqI1j3PTU2QE2U4FWzm9DPNUkInZMR6lkoSMx3k82Nn+NwqQxwlypY0eK7+nshJrPU0Dm1nTMxYL3uF+J/Xy0x0E+Rcpplhki4WRZnAJsHF53jIFaNGTC0hVHF7K6Zjogg1Np8iBG/55VXSbtS9q7r7cFlr3pZxVOAUzuACPLiGJtxDC3ygwOEZXuENSfSC3tHHonUNlTMn8Afo8wd0gI3L</latexit>u2

Kinematics First order model

Dynamics second order model

46

Generating motions

• Apply control inputs and integrate equations of motion• Start in some state • Apply controls over some time T

• Closed form integration when possible• Numerical integration

<latexit sha1_base64="KQX7cfTczAVSKdfLPkxG5fl10zs=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lE1GPRi8cK9gPaUDbbTbt0dxN2J0IJ/QtePCji1T/kzX9j0uagrQ8GHu/NMDMviKWw6LrfTmltfWNzq7xd2dnd2z+oHh61bZQYxlsskpHpBtRyKTRvoUDJu7HhVAWSd4LJXe53nrixItKPOI25r+hIi1AwirlkB25lUK25dXcOskq8gtSgQHNQ/eoPI5YorpFJam3Pc2P0U2pQMMlnlX5ieUzZhI54L6OaKm79dH7rjJxlypCEkclKI5mrvydSqqydqiDrVBTHdtnLxf+8XoLhjZ8KHSfINVssChNJMCL542QoDGcopxmhzIjsVsLG1FCGWTx5CN7yy6ukfVH3ruruw2WtcVvEUYYTOIVz8OAaGnAPTWgBgzE8wyu8Ocp5cd6dj0VrySlmjuEPnM8fOUaNsw==</latexit>s0<latexit sha1_base64="z8Xd7WXzteIuGvhWnlTdTiEaaTU=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE1GPRi8cq1hbaUDbbTbt0swm7E6GE/gMvHhTx6j/y5r9x0+agrQ8GHu/NMDMvSKQw6LrfTmlldW19o7xZ2dre2d2r7h88mjjVjLdYLGPdCajhUijeQoGSdxLNaRRI3g7GN7nffuLaiFg94CThfkSHSoSCUbTSfVrpV2tu3Z2BLBOvIDUo0OxXv3qDmKURV8gkNabruQn6GdUomOTTSi81PKFsTIe8a6miETd+Nrt0Sk6sMiBhrG0pJDP190RGI2MmUWA7I4ojs+jl4n9eN8Xwys+ESlLkis0XhakkGJP8bTIQmjOUE0so08LeStiIasrQhpOH4C2+vEwez+reRd29O681ros4ynAEx3AKHlxCA26hCS1gEMIzvMKbM3ZenHfnY95acoqZQ/gD5/MHF+ONEg==</latexit>u

<latexit sha1_base64="SHdECuDTjbcGaKcj2bSerIHJIvc=">AAACFXicbVDLSsNAFJ34rPVVdelmsAgVS0lE1E2h6MZlhT6ENobJdFKHTiZh5kYooT/hxl9x40IRt4I7/8ZJm4WvA/dyOOdeZu7xY8E12PanNTe/sLi0XFgprq6tb2yWtrY7OkoUZW0aiUhd+0QzwSVrAwfBrmPFSOgL1vVHF5nfvWNK80i2YBwzNyRDyQNOCRjJK1V1BQ5wHWvPxoe4zyV4KdTtyY3prQkOKplfxckBHkDRK5Xtmj0F/kucnJRRjqZX+ugPIpqETAIVROueY8fgpkQBp4JNiv1Es5jQERmynqGShEy76fSqCd43ygAHkTIlAU/V7xspCbUeh76ZDAnc6t9eJv7n9RIIztyUyzgBJunsoSARGCKcRYQHXDEKYmwIoYqbv2J6SxShYILMQnB+n/yXdI5qzknNvjouN87zOApoF+2hCnLQKWqgS9REbUTRPXpEz+jFerCerFfrbTY6Z+U7O+gHrPcvAACblg==</latexit>

s(t) = s0 +

Z t=T

t=0f(s(t), u)dt

47

22

Numerical Integration

<latexit sha1_base64="dO5VJLAc3gqa85sL3Qr/9roPArM=">AAACLXicbVDLSgMxFM3UV62vqks3wSK0oGVGRN0IRbtwWcE+oFNKJs20oZkHyR2xDPNDbvwVEVxUxK2/YaatoK0HAodzzuXmHicUXIFpjo3M0vLK6lp2PbexubW9k9/da6ggkpTVaSAC2XKIYoL7rA4cBGuFkhHPEazpDG9Sv/nApOKBfw+jkHU80ve5yykBLXXzVbsXQKyKUErwFXaLKTvGUQnbJAxl8IhtVxKqA3aVCSAYSvgEq6JZSuIfJcl18wWzbE6AF4k1IwU0Q62bf9VraeQxH6ggSrUtM4ROTCRwKliSsyPFQkKHpM/amvrEY6oTT65N8JFWetgNpH4+4In6eyImnlIjz9FJj8BAzXup+J/XjsC97MTcDyNgPp0uciOBIcBpdbjHJaMgRpoQKrn+K6YDousBXXBagjV/8iJpnJat87J5d1aoXM/qyKIDdIiKyEIXqIJuUQ3VEUVP6AWN0bvxbLwZH8bnNJoxZjP76A+Mr2+r/aVZ</latexit>

˙s(t) = f(s(t), u) ⇡ s(�t)� s(0)

�t

<latexit sha1_base64="SHdECuDTjbcGaKcj2bSerIHJIvc=">AAACFXicbVDLSsNAFJ34rPVVdelmsAgVS0lE1E2h6MZlhT6ENobJdFKHTiZh5kYooT/hxl9x40IRt4I7/8ZJm4WvA/dyOOdeZu7xY8E12PanNTe/sLi0XFgprq6tb2yWtrY7OkoUZW0aiUhd+0QzwSVrAwfBrmPFSOgL1vVHF5nfvWNK80i2YBwzNyRDyQNOCRjJK1V1BQ5wHWvPxoe4zyV4KdTtyY3prQkOKplfxckBHkDRK5Xtmj0F/kucnJRRjqZX+ugPIpqETAIVROueY8fgpkQBp4JNiv1Es5jQERmynqGShEy76fSqCd43ygAHkTIlAU/V7xspCbUeh76ZDAnc6t9eJv7n9RIIztyUyzgBJunsoSARGCKcRYQHXDEKYmwIoYqbv2J6SxShYILMQnB+n/yXdI5qzknNvjouN87zOApoF+2hCnLQKWqgS9REbUTRPXpEz+jFerCerFfrbTY6Z+U7O+gHrPcvAACblg==</latexit>

s(t) = s0 +

Z t=T

t=0f(s(t), u)dt

<latexit sha1_base64="mBcquUQ9Dsrm0z+A4ekUNIFk/4g=">AAACtXicbVFbi9NAFJ7E2xpvVR99GSzCykJJFnF9XNQHH1ewu4VOCJPJSTrsXMLMibSG/kOffPPfOGkrbrceGObj+879lK2SHtP0dxTfuXvv/oOjh8mjx0+ePhs9f3HpbecETIVV1s1K7kFJA1OUqGDWOuC6VHBVXn8a9Kvv4Ly05huuWsg1b4yspeAYqGL0k1UWe3+Mb9eU8bZ1dkmZghrnNGElNNL0mqOTyzVNlkVKGUtW2+9H+BIGpvrnwJxsFpjTk+SEss+gkFMM7CbdQTbaFczLRg8+TFhPGS4gRITUNxUvzb5iNTR8r/DfukkxGqeTdGP0EGQ7MCY7uyhGv8L4otNgUCju/TxLW8x77lAKBSFx56Hl4po3MA/QcA0+7zdbX9M3galobV14BumGvRnRc+39SpfBMzS68Le1gfyfNu+w/pD30rQdghHbQnWnKFo6nJBW0oFAtQqACydDr1QsuOMCw6GHJWS3Rz4El6eT7P0k/fpufP5xt44j8oq8JsckI2fknHwhF2RKRHQazSIelfFZnMdVXG9d42gX85LsWWz/AAJi0jQ=</latexit>

˙s(t) ⇡

2

4x0

y0z0

3

5++�t

2

4u�t cos ✓u�t sin ✓

u!

3

5

• For small step• Simple and efficient• Not very accurate

48

Numerical Integration

• Fourth order Runge-Kutta integration<latexit sha1_base64="T0eicfiTqVgJa1q6lyX6dRAkW+I=">AAACK3icbVDLSgMxFM34rPVVdekmWIQWocyoVJelunBZwT6gU4ZMmqnBzIPkjrUM8z9u/BUXuvCBW//DdNqFth7I5XDOvdzc40aCKzDND2NhcWl5ZTW3ll/f2NzaLuzstlQYS8qaNBSh7LhEMcED1gQOgnUiyYjvCtZ27y7GfvueScXD4AZGEev5ZBBwj1MCWnIKdbsfQqJKUE6xTaJIhg9YOSY+wrYnCU3sSyaAYEiTaloaOpY2hs5xVk+yelp2CkWzYmbA88SakiKaouEUXvRSGvssACqIUl3LjKCXEAmcCpbm7VixiNA7MmBdTQPiM9VLsltTfKiVPvZCqV8AOFN/TyTEV2rku7rTJ3CrZr2x+J/XjcE77yU8iGJgAZ0s8mKBIcTj4HCfS0ZBjDQhVHL9V0xvic4IdLx5HYI1e/I8aR1XrGrFvD4t1urTOHJoHx2gErLQGaqhK9RATUTRI3pGb+jdeDJejU/ja9K6YExn9tAfGN8/DeqkZg==</latexit>

˙s(t) ⇡ s0 +�t

6(w1 + w2 + w3 + w4)

<latexit sha1_base64="RhrVQX91tLjTdSXqlQMdEhwc4jc=">AAACiHicjZHLSgMxFIYz462Ot6pLNweLUlHKTK23hSDqwqWCVaFThkyaqcHMheSMpQx9Ft/JnW9jelG8LTwQ+PnPd0jynzCTQqPrvln21PTM7Fxp3llYXFpeKa+u3ek0V4w3WSpT9RBSzaVIeBMFSv6QKU7jUPL78Oli2L9/5kqLNLnFfsbbMe0mIhKMorGC8ksv8GAbTiGq6qq7swf5Dvi+0wvqX1zYBT9SlBX+JZdIAQdFfQBmcu+D3v8HXf+kG/+g98d0UK64NXdU8Ft4E1Ehk7oOyq9+J2V5zBNkkmrd8twM2wVVKJjkA8fPNc8oe6Jd3jIyoTHX7WIU5AC2jNOBKFXmJAgj9+tEQWOt+3FoyJjio/7ZG5p/9Vo5RsftQiRZjjxh44uiXAKmMNwKdITiDGXfCMqUMG8F9khNLGh255gQvJ9f/i3u6jXvsObeNCpn55M4SmSDbJIq8cgROSNX5Jo0CbNmrF2rYR3Yju3aR/bJGLWtycw6+Vb2+Ts0KreA</latexit>

w1 = f(s(0), u)

w2 = f(s(0) +�t

2w1, u)

w3 = f(s(0) +�t

2w2, u)

w4 = f(s(0) +�t

2w3, u)

49