Post on 14-May-2022
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