Transcript Dynamics of rigid bodies and of articulated robots

Dynamics of
Articulated Robots
Kris Hauser
CS B659: Principles of Intelligent Robot Motion
Spring 2013
Agenda
• Basic elements of simulation
• Derive the standard form of the dynamics of an articulated
robot in joint space
• Also works for humans, biological systems, non-actuated
mechanical systems …
• Featherstone algorithm: fast method for computing forward
dynamics (torques to accelerations) and inverse dynamics
(accelerations to torques)
• Constrained dynamics
Rigid Body Dynamics
• The following can be derived from first principles
using Newton’s laws + rigidity assumption
• Parameters
•
•
•
•
•
CM translation c(t)
CM velocity v(t)
Rotation R(t)
Angular velocity w(t)
Mass m, local inertia tensor HL
Rigid body ordinary
differential equations
• We will express forces and torques in terms of terms of H (a
function of R), 𝜔, 𝑣 and 𝜔
• 𝑓 = 𝑚𝑣
• 𝜏 = [𝜔] 𝐻 𝜔 + 𝐻 𝜔
• Rearrange…
• 𝑣 = 𝑓/𝑚
• 𝜔 = 𝐻 −1 (𝜏 − 𝜔 𝐻 𝜔 )
• So knowing f(t) and τ(t), we can derive c(t), v(t), R(t), and w(t)
by solving an ordinary differential equation (ODE)
• dx/dt = f(x)
• x(0) = x0
• With x=(c,v,R,w) the state of the system
• Numerical integration, also known as simulation
Articulated body ODEs
• We will express joint torques 𝜏 in terms of terms of 𝑞, 𝑞, and 𝑞
and external forces f
• 𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝐺 𝑞 = 𝜏 + 𝐽𝑇 𝑞 𝑓
• Rearrange…
• 𝑞=𝐵 𝑞
−1
𝜏 + 𝐽𝑇 𝑞 𝑓 − 𝐶 𝑞, 𝑞 − 𝐺 𝑞
• An ODE in the state space x=(𝑞, 𝑞)
•
𝑑𝑥
𝑑𝑡
=
𝑑𝑞
𝑑𝑡
𝑑𝑞
𝑑𝑡
= 𝑓 𝑞, 𝑞 =
𝐵 𝑞
−1
𝑞
𝜏 + 𝐽𝑇 𝑞 𝑓 − 𝐶 𝑞, 𝑞 − 𝐺 𝑞
• Solve using numerical integration
Numerical integration of ODEs
• If dx/dt = f(x) and x(0) are known, then given a step size h,
• x(kh)  xk = xk-1 + h f’(xk-1)
• gives an approximate trajectory for k 1
• Provided f is smooth
• Accuracy depends on h
• Known as Euler’s method
• Better integration schemes are available
• (e.g., Runge-Kutta methods, implicit integration, adaptive step
sizes, etc)
• Beyond the scope of this course
DYNAMICS OF RIGID BODIES
Kinetic energy for rigid body
• Rigid body with velocity v, angular velocity w
• KE = ½ (m vTv + wT H w)
• World-space inertia tensor H = R HL RT
1/2
w
v
T
H 0
0 mI
w
v
Kinetic energy derivatives
•
𝜕𝐾𝐸
𝜕𝑣
= 𝑚𝑣
• Force (@CM) 𝑓 =
•
•
𝜕𝐾𝐸
𝜕𝜔
𝑑
H
𝑑𝑡
𝑑 𝜕𝐾𝐸
( )
𝑑𝑡 𝜕𝑣
= 𝑚𝑣
= 𝐻𝜔
= [w]H – H[w]
• Torque t =
𝑑 𝜕𝐾𝐸
𝑑𝑡 𝜕𝜔
= [w] H w + H 𝜔
Summary
•𝑓 = 𝑚𝑣
• 𝜏 = [𝜔] 𝐻 𝜔 + 𝐻 𝜔
Gyroscopic “force”
Force off of COM
F
x
Force off of COM
F
𝛿𝑥
x
Consider infinitesimal virtual displacement 𝛿𝑥 generated
by F.
(we don’t know what this is, exactly)
The virtual work performed by this displacement is FT𝛿𝑥
Generalized torque
f
Now consider the equivalent force f, torque τ at COM
Generalized torque
f
𝛿𝑥
𝛿𝑞
Now consider the equivalent force f, torque τ at COM
And an infinitesimal virtual displacement of R.B.
coordinates 𝛿𝑞
Generalized torque
f
𝛿𝑥
𝛿𝑞
Now consider the equivalent force f, torque τ at COM
And an infinitesimal virtual displacement of R.B.
coordinates 𝛿𝑞
Virtual work in configuration space is [fT,τT] 𝛿𝑞
Principle of virtual work
f
F
𝛿𝑥
𝛿𝑞
[fT,τT] 𝛿𝑞= FT 𝛿𝑥
Since 𝛿𝑥 = 𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞
Principle of virtual work
f
F
𝛿𝑥
𝛿𝑞
[fT,τT] 𝛿𝑞= FT 𝛿𝑥
Since 𝛿𝑥 = 𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞
Since this holds no matter what 𝛿𝑞 is, we have [fT,τT] =
FTJ(q),
f
Or JT(q) F = τ
ARTICULATED ROBOT DYNAMICS
Robot Dynamics
• Configuration 𝑞, velocity 𝑞  Rn
• Generalized forces u  Rm
• 𝑢 = 𝜏 + 𝑘 𝐽𝑘 𝑞 𝑇𝑓𝑘
• Joint torques 𝜏 and external forces 𝑓𝑘
• How does u relate to 𝒒 and 𝒒?
• Use Langrangian mechanics to find a link between
u and 𝑞
Lagrangian Mechanics
• 𝐿 𝑞, 𝑞 = 𝐾 𝑞, 𝑞 − 𝑃 𝑞
Kinetic energy
Potential energy
• The trajectory between two states (𝑞0 , 𝑞0 ),
𝑞𝑇 , 𝑞𝑇 is the one that minimizes the “action”
𝑇
𝑆 = 0 𝐿 𝑞, 𝑞 𝑑𝑡
• 𝐿 𝑞, 𝑞 is defined such that the path minimizing S is
equivalent to the one produced by Newton’s laws,
subject to the constraints that the system only moves
along coordinates q
Lagrangian Mechanics
• 𝐿 𝑞, 𝑞 = 𝐾 𝑞, 𝑞 − 𝑃 𝑞
• Minimum action condition => Euler-Lagrange
equations of motion:
•
𝑑 𝜕𝐿
𝑑𝑡 𝜕𝑞
−
𝜕𝐿
𝜕𝑞
=𝑢
A system of n partial differential
equations
• Note that P is independent of 𝑞, so
•
𝑑 𝜕𝐾
𝑑𝑡 𝜕𝑞
−
𝜕𝐾
𝜕𝑞
+
𝜕𝑃
𝜕𝑞
=𝑢
Example: Point Mass
• Coordinates q = (x,y)
• Potential field P(x,y)
1
2
• Lagrangian: 𝐿 𝑞, 𝑞 = 𝑚 𝑥 2 + 𝑦 2 − 𝑃 𝑥, 𝑦
• Equations of motion
•
•
𝑑
𝑑𝑡
𝑑
𝑑𝑡
𝜕𝑃
𝜕𝑥
𝜕𝑃
+
𝜕𝑦
𝜕𝑃
𝜕𝑥
𝜕𝑃
+
𝜕𝑦
𝑚𝑥 +
= 𝑚𝑥 +
= 𝑢𝑥
𝑚𝑦
= 𝑚𝑦
= 𝑢𝑦
Sanity check: Newton’s laws
Kinetic energy for articulated robot
• 𝐾(𝑞, 𝑞) = 𝑖 𝐾𝑖 (𝑞, 𝑞)
• Velocity of i’th rigid body
𝑝
• 𝑣𝑖 = 𝐽𝑖 𝑞 𝑞
• Angular velocity of i’th rigid body
• 𝜔𝑖 = 𝐽𝑖𝑟(𝑞)𝑞
• 𝐾𝑖 =
1 𝑇
𝑞 (𝑚𝑖𝐽𝑖𝑝𝑇𝐽𝑖𝑝
2
+ 𝐽𝑖𝑟𝑇𝐻𝑖𝐽𝑖𝑟)𝑞
• 𝐾(𝑞, 𝑞) = ½𝑞 𝑇 𝐵(𝑞)𝑞
Mass matrix:
symmetric positive definite
Derivative of K.E. w.r.t 𝑞
𝜕𝐾
•
𝜕𝑞
𝑞, 𝑞 = 𝐵 𝑞 𝑞
𝑑 𝜕𝐾
•
𝑑𝑡 𝜕𝑞
=𝐵 𝑞 𝑞+
• = 𝐵(𝑞) 𝑞 +
𝑖 𝑞𝑖
𝑑
𝐵
𝑑𝑡
𝜕
𝐵
𝜕𝑞𝑖
𝑞
𝑞
𝑞
𝑞
Derivative of K.E. w.r.t q
𝜕
𝐾 𝑞, 𝑞 = ½
𝜕𝑞
𝜕
𝐵(𝑞)𝑞
𝜕𝑞1
…
𝜕
𝑇
𝑞
𝐵(𝑞)𝑞
𝜕𝑞𝑛
𝑞𝑇
Potential energy for articulated
robot in gravity field
𝜕𝑃
•
𝜕𝑞
𝜕𝑃𝑖
•
𝜕𝑞
=
𝜕𝑃𝑖
𝑖 𝜕𝑞
= 𝑚𝑖
0
0
0 𝑣𝑖 = 𝑚𝑖 0 𝐽𝑖𝑝(𝑞)
𝑔
𝑔
• G(q)
Generalized gravity
Putting it all together
𝑑 𝜕𝐾
•
𝑑𝑡 𝜕𝑞
𝜕𝐾
−
𝜕𝑞
• 𝐵 𝑞 𝑞+
+
𝜕𝑃
𝜕𝑞
𝑑
𝐵(𝑞)𝑞
𝑑𝑡
=𝑢
–½
𝜕
𝑇
𝑞
𝐵(𝑞)𝑞
𝜕𝑞1
𝑞𝑇
…
+ 𝐺(𝑞) = 𝑢
𝜕
𝐵(𝑞)𝑞
𝜕𝑞𝑛
Group these terms together
Final canonical form
• 𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝐺(𝑞) = 𝑢
Generalized
inertia
Centrifugal/
coriolis forces
Generalized
gravity
Generalized forces
(joint torques +
external forces)
Forward/Inverse Dynamics
• Given 𝑢, 𝑞, and 𝑞, find 𝑞
• From torques to accelerations
• 𝑞 = 𝐵 𝑞 −1 (𝑢 − 𝐶(𝑞, 𝑞) − 𝐺(𝑞) )
• Given 𝑞, 𝑞, and 𝑞, find 𝑢
• From desired accelerations to necessary torques
• 𝑢 = 𝐵 𝑞 𝑞 + 𝐶(𝑞, 𝑞) + 𝐺(𝑞)
Example: RP manipulator
Application: Effective Inertia
• If a force 𝑓 is applied to a point 𝑝 on a robot, how much will 𝑝
accelerate?
Application: Effective Inertia
• If a force 𝑓 is applied to a point 𝑝 on a robot, how much will 𝑝
accelerate?
• Assume a stationary system, no acceleration when no force is
applied
• 𝑞0 = 𝐵 𝑞
−1
𝜏 − 𝐶 𝑞, 𝑞 − 𝐺 𝑞 =0
• With the force:
•𝑞= 𝐵 𝑞
−1 𝐽(𝑞)𝑇 𝑓
Application: Effective Inertia
• If a force 𝑓 is applied to a point 𝑝 on a robot, how much will 𝑝
accelerate?
• Assume a stationary system, no acceleration when no force is
applied
• 𝑞0 = 𝐵 𝑞
−1
𝜏 − 𝐶 𝑞, 𝑞 − 𝐺 𝑞 =0
• With the force:
• 𝑞 = 𝐵 𝑞 −1 𝐽(𝑞)𝑇 𝑓
• 𝑝 = 𝐽 𝑞 𝑞 = 𝐽(𝑞)𝐵 𝑞
• The matrix 𝐽 𝑞 𝐵 𝑞
inertia matrix
• 𝑓= 𝐽 𝑞 𝐵 𝑞
−1 𝐽
−1 𝐽
𝑞
𝑞
−1 𝐽(𝑞)𝑇 𝑓
𝑇 −1
is called the effective
𝑇 −1 𝑝
Can be infinite at singular configurations!
Application: Feedforward control
• Feedback control: let torques be a
function of the current error between
actual and desired configuration
• Problem: heavy arms require strong
torques, requiring a stiff system
• Stiff systems become unstable relatively
quickly
Application: Feedforward control
• Solution: include feedforward torques
to reduce reliance on feedback
• Estimate the torques that would
compensate for gravity and coriolis
forces, send those torques to the
motors
Feedforward Torques
• Given current 𝑞, 𝑞, desired 𝑞
• 1. Estimate B, C, G
• 2. Compute u
• 𝑢 = 𝐵 𝑞 𝑞 + 𝐶(𝑞, 𝑞) + 𝐺(𝑞)
• 3. Apply torques u
• How to compensate for errors in B,C,G? Combine
feedforward with feedback. More in later classes…
Newton-Euler Method
(Featherstone 1984)
• Explicitly solves a linear system for joint constraint forces and
accelerations, related via Newton’s equations
• No matrix larger than 6x6
• Faster forward/inverse dynamics for large chains (O(n) vs O(n3)
for direct matrix computations)
Forward Dynamics: Basic
Intuition
• Downward recursion: Starting from root, compute
“articulated body inertia matrix” for each link
• 6x6 matrix 𝐼𝑖𝐴 relating (𝑓, 𝜏) vectors to translational/angular
accelerations (a, 𝛼) respectively
• Also need a “bias force” 𝑃𝑖
• 𝑓, 𝜏 = 𝐼𝑖𝐴 𝛼, 𝑎 + 𝑃𝑖
• Upward recursion: Starting from leaves, compute
accelerations on links
• Given (𝑓, 𝜏) acting on i’th link, compute acceleration of joint i and
the joint constraint forces on the i-1’th link
• (𝑓, 𝜏) includes external forces + joint constraint forces from
downward links
Software
• Both Lagrangian dynamics and Newton-Euler methods are
implemented in KrisLibrary
• Lagrangian form is usually most mathematically convenient
representation
CONSTRAINED DYNAMICS
Constrained Systems
• Suppose the system is constrained by 𝐴 𝑞 𝑞 = 0
• E.g., closed-chains, contact constraints, rolling
constraints
• A is a k x n matrix (k constraints)
• How does 𝑞 evolve over time?
The Wrong Way
• Suppose the system is constrained by 𝐴 𝑞 𝑞 = 0
• E.g., closed-chains, contact constraints, rolling
constraints
• A is a k x n matrix (k constraints)
• How does 𝑞 evolve over time?
• Wrong way:
•
𝑑
𝑑𝑡
𝐴 𝑞 𝑞 =𝐴 𝑞 𝑞+𝐴 𝑞 𝑞 =0
• Solve for 𝑞 as usual, then project it onto the subspace that
satisfies this equation, obtaining 𝑞𝑐𝑜𝑛𝑠𝑡
• The correct answer will be a projection, but a very specific one!
The Right Way…
• Constrained system of equations:
• 𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝐺 𝑞 = 𝑢 + 𝐴 𝑞 𝑇 𝜆
•
𝑑
𝑑𝑡
𝐴 𝑞 𝑞 =𝐴 𝑞 𝑞+𝐴 𝑞 𝑞 =0
• Lagrange multipliers have been introduced
• 𝜆 = 𝜆1 , … , 𝜆𝑘 𝑇
• 𝜆 can be thought of as constraint forces
• Solve for n+k variables 𝑞, 𝜆
(1)
(2)
Solving…
• Constrained system of equations:
• 𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝐺 𝑞 = 𝑢 + 𝐴 𝑞 𝑇 𝜆
•
𝑑
𝑑𝑡
𝐴 𝑞 𝑞 =𝐴 𝑞 𝑞+𝐴 𝑞 𝑞 =0
(1)
(2)
• Solve for n+k variables 𝑞, 𝜆
• A solution must satisfy
• 𝑞 = 𝐵−1 𝑢 + 𝐴𝑇 𝜆 − 𝐶 − 𝐺
• 𝐴𝑞 + 𝐴𝐵−1 𝑢 + 𝐴𝑇 𝜆 − 𝐶 − 𝐺 = 0
• 𝜆 = 𝐴𝐵−1 𝐴𝑇
−1
(3) solve 1 for 𝑞
(4) subst (3) in (2)
𝐴𝑞 + 𝐴𝐵−1 𝐶 + 𝐺 − 𝑢
(5) solve for 𝜆 in (4), use −𝐴𝑞 = 𝐴𝑞 from (2)
• 𝑃𝑞 = 𝑃𝐵−1 (𝑢 − 𝐶 − 𝐺)
• With 𝑃 = 𝐼 − 𝐵−1 𝐴𝑇 𝐴𝐵−1 𝐴𝑇
(6) more manipulations..
−1 𝐴
Back to Pseudoinverses
• A pseudoinverse A# of the matrix A is a matrix such that
• A = AA#A
• A# = A#AA#
• Generalizes the concept of inverse to non-square, noninvertible
matrices
• Such a matrix exists (in fact, there are infinitely many)
• The Moore-Penrose pseudoinverse, denoted A+, can be
derived as
• A+ = (ATA)-1AT when ATA is invertible
• A+ = AT(AAT)-1 when AAT is invertible
(overconstrained)
(underconstrained)
Properties
• Note connection to least-squares formula
• Ax=b => x = A+b
• If system is overconstrained, this solution minimizes ||b-Ax||2
• If system is underconstrained, this solution minimizes ||x||2
• Note that (I-AA+)Ay = 0 is always satisfied
• (I-AA+) is a projection matrix
Weighted Pseudoinverse
• If (AAT)-1 exists, given any positive definite weighting matrix W,
we can derive a new pseudoinverse
• A# = W-1AT(AW-1AT)-1
• This is a weighted pseudoinverse
• Has the property that x=A#b is a solution to Ax = b such that
• x minimizes xTWx – a weighted norm
Weighted Pseudoinverse
• If (AAT)-1 exists, given any positive definite weighting matrix W,
we can derive a new pseudoinverse
• A# = W-1AT(AW-1AT)-1
• This is a weighted pseudoinverse
• Has the property that x=A#b is a solution to Ax = b such that
• x minimizes xTWx – a weighted norm
• Revisiting constrained dynamics…
• The P projection matrix solves for 𝒒 such that 𝒒𝑻 𝑩𝒒 is
minimized
• Constraint forces dissipate kinetic energy in a minimal fashion!
Rigid Body Simulators
• Articulated robots are often simulated as a set of connected
rigid bodies (Open Dynamics Engine, Bullet, etc)
• Connections give rise to constraints in the dynamics
• 𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝐺 𝑞 = 𝑢 + 𝐴 𝑞 𝑇 𝜆
•
𝑑
𝑑𝑡
𝐴 𝑞 𝑞 =𝐴 𝑞 𝑞+𝐴 𝑞 𝑞 =0
(1)
(2)
• Solve for n+k variables 𝑞, 𝜆
• (1), (2) are sparse systems and are solved using specialized
solvers
• More on frictional contact later…
Next class
• Feedback control
• Principles App J