### 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
```