1.Dynamics of Articulated Robots
Kris Hauser
CS B659: Principles of Intelligent Robot Motion
Spring 2013
2.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
3.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
4.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
5.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
6.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
7.Dynamics of Rigid Bodies
8.Kinetic energy for rigid body
Rigid body with velocity v, angular velocity w about COM
KE = ½ (m vTv + wT H w)
World-space inertia tensor H = R HL RT
w
v
T
w
v
H 0
0 m I
1/2
9.Kinetic energy derivatives
𝜕𝐾𝐸 𝜕𝑣 = 𝑚 𝑣
Force (@CM) 𝑓 = 𝑑 𝑑𝑡 ( 𝜕𝐾𝐸 𝜕𝑣 )= 𝑚 𝑣
𝜕𝐾𝐸 𝜕𝜔 =𝐻𝜔
𝑑 𝑑𝑡 H = [w]H – H[w]
Torque t = 𝑑 𝑑𝑡 𝜕𝐾𝐸 𝜕𝜔 = [w] H w + H 𝜔
12.Force off of COM
x
F
Consider infinitesimal virtual displacement 𝛿𝑥 generated by F.
(we don’t know what this is, exactly)
The virtual work performed by this displacement is FT𝛿𝑥
𝛿𝑥
13.Generalized torque
f
Now consider the equivalent force f, torque τ at COM
14.Generalized torque
f
Now consider the equivalent force f, torque τ at COM
And an infinitesimal virtual displacement of R.B. coordinates 𝛿𝑞
𝛿𝑥
𝛿𝑞
15.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] 𝛿𝑞
16.Principle of virtual work
f
𝛿𝑥
𝛿𝑞
[fT,τT] 𝛿𝑞= FT 𝛿𝑥
Since 𝛿𝑥=𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞
F
17.Principle of virtual work
f
𝛿𝑥
𝛿𝑞
[fT,τT] 𝛿𝑞= FT 𝛿𝑥
Since 𝛿𝑥=𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞
Since this holds no matter what 𝛿𝑞 is, we have [fT,τT] = FTJ(q),
Or JT(q) F =
F
f
τ
18.Articulated Robot Dynamics
19.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 𝑞
20.Lagrangian Mechanics
𝐿 𝑞, 𝑞 =𝐾 𝑞, 𝑞 −𝑃 𝑞
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
Kinetic energy
Potential energy
21.Lagrangian Mechanics
𝐿 𝑞, 𝑞 =𝐾 𝑞, 𝑞 −𝑃 𝑞
Minimum action condition => Euler-Lagrange equations of motion:
𝑑 𝑑𝑡 𝜕𝐿 𝜕 𝑞 − 𝜕𝐿 𝜕𝑞 =𝑢
Note that P is independent of 𝑞 , so
𝑑 𝑑𝑡 𝜕𝐾 𝜕 𝑞 − 𝜕𝐾 𝜕𝑞 + 𝜕𝑃 𝜕𝑞 =𝑢
A system of n partial differential equations
29.Forward/Inverse Dynamics
Given 𝑢, 𝑞, and 𝑞 , find 𝑞
From torques to accelerations
𝑞 = 𝐵 𝑞 −1 (𝑢−𝐶(𝑞, 𝑞 )−𝐺(𝑞) )
Given 𝑞, 𝑞 , and 𝑞 , find 𝑢
From desired accelerations to necessary torques
𝑢=𝐵 𝑞 𝑞 +𝐶(𝑞, 𝑞 )+𝐺(𝑞)
30.Example: RP manipulator
31.Application: Effective Inertia
If a force 𝑓 is applied to a point 𝑝 on a robot, how much will 𝑝 accelerate?
32.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 𝐽(𝑞) 𝑇 𝑓
33.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 𝐽(𝑞) 𝑇 𝑓
𝑝 =𝐽 𝑞 𝑞 =𝐽(𝑞)𝐵 𝑞 −1 𝐽(𝑞) 𝑇 𝑓
The matrix 𝐽 𝑞 𝐵 𝑞 −1 𝐽 𝑞 𝑇 −1 is called the effective inertia matrix
𝑓= 𝐽 𝑞 𝐵 𝑞 −1 𝐽 𝑞 𝑇 −1 𝑝
Can be infinite at singular configurations!
34.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
35.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
36.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…
37.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)
38.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
39.Software
Both Lagrangian dynamics and Newton-Euler methods are implemented in KrisLibrary
Lagrangian form is usually most mathematically convenient representation
40.Constrained Dynamics
41.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?
42.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!
43.The Right Way…
Constrained system of equations:
𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1)
𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2)
Lagrange multipliers have been introduced
𝜆= 𝜆 1 ,…, 𝜆 𝑘 𝑇
𝜆 can be thought of as constraint forces
Solve for n+k variables 𝑞 ,𝜆
44.Solving…
Constrained system of equations:
𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1)
𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2)
Solve for n+k variables 𝑞 ,𝜆
A solution must satisfy
𝑞 = 𝐵 −1 𝑢+ 𝐴 𝑇 𝜆−𝐶−𝐺 (3) solve 1 for 𝑞
𝐴 𝑞 +𝐴 𝐵 −1 𝑢+ 𝐴 𝑇 𝜆−𝐶−𝐺 =0 (4) subst (3) in (2)
𝜆= 𝐴 𝐵 −1 𝐴 𝑇 −1 𝐴 𝑞 +𝐴 𝐵 −1 𝐶+𝐺−𝑢
(5) solve for 𝜆 in (4), use − 𝐴 𝑞 =𝐴 𝑞 from (2)
𝑃 𝑞 =𝑃 𝐵 −1 (𝑢−𝐶−𝐺) (6) more manipulations..
With 𝑃=𝐼− 𝐵 −1 𝐴 𝑇 𝐴 𝐵 −1 𝐴 𝑇 −1 𝐴
45.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 (overconstrained)
A+ = AT(AAT)-1 when AAT is invertible (underconstrained)
46.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
47.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
48.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!
49.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
𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1)
𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2)
Solve for n+k variables 𝑞 ,𝜆
(1), (2) are sparse systems and are solved using specialized solvers
More on frictional contact later…