Report

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