![Page 1: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/1.jpg)
Dynamics of Articulated RobotsKris HauserCS B659: Principles of Intelligent Robot MotionSpring 2013
![Page 2: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/2.jpg)
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
![Page 3: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/3.jpg)
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
![Page 4: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/4.jpg)
Rigid body ordinary differential equations• We will express forces and torques in terms of terms of H (a
function of R), , and
• Rearrange…
• 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
![Page 5: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/5.jpg)
Articulated body ODEs• We will express joint torques in terms of terms of and and
external forces f
• Rearrange…
• An ODE in the state space x=()
• Solve using numerical integration
![Page 6: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/6.jpg)
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
![Page 7: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/7.jpg)
DYNAMICS OF RIGID BODIES
![Page 8: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/8.jpg)
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
wv
T
wv
H 0 0 m I
1/2
![Page 9: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/9.jpg)
Kinetic energy derivatives
• Force (@CM)
• H = [w]H – H[w]• Torque t = = [w] H w + H
![Page 10: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/10.jpg)
Summary
Gyroscopic “force”
![Page 11: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/11.jpg)
Force off of COM
x
F
![Page 12: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/12.jpg)
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
𝛿𝑥
![Page 13: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/13.jpg)
Generalized torque
f
Now consider the equivalent force f, torque τ at COM
![Page 14: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/14.jpg)
Generalized torque
f
Now consider the equivalent force f, torque τ at COMAnd an infinitesimal virtual displacement of R.B. coordinates
𝛿𝑥
𝛿𝑞
![Page 15: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/15.jpg)
Generalized torque
f𝛿𝑥
𝛿𝑞
Now consider the equivalent force f, torque τ at COMAnd an infinitesimal virtual displacement of R.B. coordinates Virtual work in configuration space is [fT,τT]
![Page 16: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/16.jpg)
Principle of virtual work
f𝛿𝑥
𝛿𝑞
[fT,τT] = FT
Since we have [fT,τT] = FT
F
![Page 17: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/17.jpg)
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τ
![Page 18: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/18.jpg)
ARTICULATED ROBOT DYNAMICS
![Page 19: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/19.jpg)
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
![Page 20: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/20.jpg)
Lagrangian Mechanics
• The trajectory between two states , is the one that minimizes the “action”
• 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
![Page 21: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/21.jpg)
Lagrangian Mechanics
• Minimum action condition => Euler-Lagrange equations of motion:
• Note that P is independent of , so
A system of n partial differential equations
![Page 22: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/22.jpg)
Sanity check: Newton’s laws
Example: Point Mass• Coordinates q = (x,y)• Potential field P(x,y)• Lagrangian: • Equations of motion
![Page 23: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/23.jpg)
Kinetic energy for articulated robot
• Velocity of i’th rigid body
• Angular velocity of i’th rigid body
Mass matrix:symmetric positive definite
![Page 24: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/24.jpg)
Derivative of K.E. w.r.t
•
![Page 25: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/25.jpg)
Derivative of K.E. w.r.t q
𝜕𝜕𝑞
𝐾 (𝑞 ,�̇� )=½(�̇�𝑇 𝜕𝜕𝑞1
𝐵(𝑞) �̇�
…
�̇�𝑇 𝜕𝜕𝑞𝑛
𝐵(𝑞) �̇�)
![Page 26: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/26.jpg)
Potential energy for articulated robot in gravity field
• G(q)
Generalized gravity
![Page 27: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/27.jpg)
Putting it all together
Group these terms together
![Page 28: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/28.jpg)
Final canonical form
𝐵 (𝑞) �̈�+𝐶 (𝑞 , �̇�)+𝐺(𝑞)=𝑢Generalized inertia
Centrifugal/coriolis forces
Generalized gravity
Generalized forces (joint torques + external forces)
![Page 29: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/29.jpg)
Forward/Inverse Dynamics
• Given , , and , find • From torques to accelerations
• Given , , and , find • From desired accelerations to necessary torques
![Page 30: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/30.jpg)
Example: RP manipulator
![Page 31: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/31.jpg)
Application: Effective Inertia• If a force is applied to a point on a robot, how much will
accelerate?
![Page 32: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/32.jpg)
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
• With the force:
![Page 33: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/33.jpg)
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
• With the force:
• The matrix is called the effective inertia matrix
Can be infinite at singular configurations!
![Page 34: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/34.jpg)
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
![Page 35: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/35.jpg)
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
![Page 36: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/36.jpg)
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…
![Page 37: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/37.jpg)
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)
![Page 38: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/38.jpg)
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
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
![Page 39: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/39.jpg)
Software• Both Lagrangian dynamics and Newton-Euler methods are
implemented in KrisLibrary
• Lagrangian form is usually most mathematically convenient representation
![Page 40: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/40.jpg)
CONSTRAINED DYNAMICS
![Page 41: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/41.jpg)
Constrained Systems
• Suppose the system is constrained by • E.g., closed-chains, contact constraints, rolling
constraints• A is a k x n matrix (k constraints)
• How does evolve over time?
![Page 42: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/42.jpg)
The Wrong Way
• Suppose the system is constrained by • E.g., closed-chains, contact constraints, rolling
constraints• A is a k x n matrix (k constraints)
• How does evolve over time?• Wrong way:
• 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!
![Page 43: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/43.jpg)
The Right Way…
• Constrained system of equations:• (1)
• (2)
• Lagrange multipliers have been introduced
• can be thought of as constraint forces• Solve for n+k variables
![Page 44: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/44.jpg)
Solving…
• Constrained system of equations:• (1)
• (2)
• Solve for n+k variables • A solution must satisfy
• (3) solve 1 for
• (4) subst (3) in (2)
(5) solve for in (4), use from (2)
• (6) more manipulations..
• With
![Page 45: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/45.jpg)
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)
![Page 46: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/46.jpg)
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
![Page 47: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/47.jpg)
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
![Page 48: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/48.jpg)
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!
![Page 49: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/49.jpg)
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)
• (2)
• Solve for n+k variables • (1), (2) are sparse systems and are solved using specialized
solvers
• More on frictional contact later…
![Page 50: Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013](https://reader030.vdocuments.us/reader030/viewer/2022032703/56649d215503460f949f5e9f/html5/thumbnails/50.jpg)
Next class• Feedback control
• Principles App J