* Your assessment is very important for improving the workof artificial intelligence, which forms the content of this project
Download Document
Survey
Document related concepts
Computational fluid dynamics wikipedia , lookup
Routhian mechanics wikipedia , lookup
Plateau principle wikipedia , lookup
Genetic algorithm wikipedia , lookup
Selection algorithm wikipedia , lookup
Fast Fourier transform wikipedia , lookup
Fisher–Yates shuffle wikipedia , lookup
Simplex algorithm wikipedia , lookup
Computational electromagnetics wikipedia , lookup
Smith–Waterman algorithm wikipedia , lookup
Molecular dynamics wikipedia , lookup
Dijkstra's algorithm wikipedia , lookup
Algorithm characterizations wikipedia , lookup
Factorization of polynomials over finite fields wikipedia , lookup
Transcript
Articulated Body Dynamics The Basics Comp 768 October 23, 2007 Will Moss Overview • Motivation • Background / Notation • Articulate Dynamics Algorithms – – – – Newton-Euler Algorithm Composite-Rigid Body Algorithm Articulated-Body Algorithm (Featherstone) Lagrange Multiplier approach (Baraff) October, 23 2007 2 History • Originally a problem from robotics – Given a robotic arm with a series of joints that can apply forces to themselves (called motors), find the forces to get the robot arm into the desired configuration October, 23 2007 3 Applications • Computer Graphics – Humans, animals, birds, robots, etc. – Wires, chains, ropes, etc. – Trees, grass, etc. – Many more • October, 23 2007 http://vrlab.epfl.ch/~alegarcia/VHOntology/long.html 4 Basics • An articulated body is a group of rigid bodies (called links) connected by joints • Multiple types of joints – Revolute (1 degree of freedom) – Ball joint (3 degrees of freedom) – Prismatic, screw, etc. October, 23 2007 5 Notation • In rigid body dynamics we had two equations F(t) Ma(t) τ(t) I(t)ω(t) F (t ) I (t )a (t ) s s s – Fs is the vector of spatial forces – Is is the spatial inertia matrix (6 x 6) – as is the spatial acceleration • This is called spatial algebra – Combines the linear and angular components of the physical quantities into one 6 dimensional vector October, 23 2007 6 Notation • Transitioning this to articulated bodies n n n j 1 j 1 k 1 Qi H ij qj Cijk q j qk gi – Qi is the force on link i – H is the joint-space inertia matrix (n x n) – q, q and q are the coordinates, velocities and accelerations of the joints – C term produces the vector of forces that produce zero acceleration October, 23 2007 7 Forward vs. Inverse Dynamics • Inverse Dynamics – The calculation of forces given a set of accelerations • Forward Dynamics – The calculation of accelerations given a set of forces n n n j 1 j 1 k 1 Qi H ij q j Cijk q j q k g i October, 23 2007 8 Algorithms • Inverse Dynamics – Newton-Euler Algorithm • Forward Dynamics – Composite-Rigid-Body Algorithm – Articulated-Body Algorithm – Lagrange Multiplier Algorithm October, 23 2007 9 Newton-Euler Algorithm • Goal – Given the accelerations and velocities at the joints, find the forces required at the joints to generate those accelerations • Recursive approach – Finds the accelerations and velocities of link i in terms of link i - 1 October, 23 2007 10 Newton-Euler Algorithm • Method 1.Calculate the velocities and accelerations at each link 2.Calculate the required net force acting on each link to generate those accelerations 3.Calculate the joint forces required to generate the net forces on each link October, 23 2007 11 Newton-Euler Algorithm 1. Find the velocities and accelerations of the links where s is a unit vecto r describing the vi vi - 1 si qi (v0 0) ai ai-1 vi si qi si qi (a0 0) i allowed motion of joint i, q, q and q are the physical variables for the joint and v and a are the velocity and acceleration of the link October, 23 2007 12 Newton-Euler Algorithm 2. Find the forces on each link d fi I i vi I i ai vi I i vi dt l October, 23 2007 13 Newton-Euler Algorithm 3. Find the forces on the joints j j l f i f i 1 f i fi fi1 fi where f n f n j j l j l • This can be reformulated in link coordinates to speed up the calculation • Runs in O(n) October, 23 2007 14 Forward vs. Inverse Dynamics • Inverse Dynamics – The calculation of forces given a set of accelerations • Forward Dynamics – The calculation of accelerations given a set of forces October, 23 2007 15 Composite-Rigid-Body Algorithm n n n j 1 j 1 k 1 Qi H ij qj Cijk q j qk gi C(q, q ) Q H(q) q – – – – Q is the vector of the forces on the links H is the joint-space inertia matrix (n x n) C vector of forces that produce zero acceleration q, q and q are the coordinates, velocities and accelerations • Algorithm – Calculate the elements of C – Calculate the elements of H – Solve the set of simultaneous equations October, 23 2007 16 Composite-Rigid-Body Algorithm • Solve for C ) – Setting the acceleration to zero, we get Q C(q, q – We can, therefore, interpret C as the forces which produce no acceleration – We can use a forward-dynamics solver (like NewtonEuler) to solve for the forces given the position, velocity and an acceleration of zero October, 23 2007 17 Composite-Rigid-Body Algorithm • Solve for H – If we set C to 0, we observe that H (q)q is the vector of joint forces that will impart an acceleration of q onto a stationary robot • Therefore, the ith column of H is the vector of forces required to produce a unit of acceleration about joint i and no other acceleration. – Treat the links i…n as a rigid-body with inertia defined n by C C I i I j I i 1 I i j i – Treat the links from 1…i-1 are therefore unmoving October, 23 2007 18 Composite-Rigid-Body Algorithm • Solve for H (cont.) – Given that C f i I i si where f i is a unit force at joint i and si is a unit acceleration – Since none of the links from 1 … i-1 are moving, every joint transmits f i onto the subsequent link, so we can solve for H by solving S H ji s j fi for j i Where s S is the spacial transpose of s – Which is a complete solution for H since it is symmetric – Runs in O(n2) October, 23 2007 19 Composite-Rigid-Body Algorithm • Once you have H and C, solve the system of equations using any solver – O(n3), but the constant is small enough that for n less than ~12 the O(n2) term dominates • Like Newton-Euler, this can be reformulated in link coordinates – Faster for n ≤ 16 October, 23 2007 20 Articulated-Body Algorithm • (Re)consider the equation of motion of an articulated body f I a p A where f is a force applied to a link, A I is the articulate body intertia and p is the bias force, or the force required to keep the link from accelerating • This is true for any link in the articulated body October, 23 2007 21 Articulated-Body Algorithm • Consider an articulate robot as a single joint attached to an articulated body – The problem simplifies to the forward dynamics of a one-joint robot (much simpler than the general case) – The first joint is simply a one-joint robot – The second joint is a one-joint robot with a moving base (slightly more complicated, but still much simpler that the general case) – Solving this requires two tasks • Solving the one-joint robot forward dynamics problem • Finding the articulated-body inertias (I) and bias forces (p) October, 23 2007 22 Articulated-Body Algorithm • Solving the one-joint robot problem S A Q s I ab vb s q p q S A s I s I articulated body inertia of the A p, Q, q , q October, 23 2007 rest of the robot p bias force s is the unit vecto r describing the allowed motion of the joint S s spatial transpose of s v b velocity of the base a b acceleration of the base Q force at the joint q velocity at the joint q acceleration at the joint 23 Articulated-Body Algorithm • Finding the articulated-body inertia (IA) and bias force (p) A I2ss S I2 I I1 I 2 S s I1 s S v v Qs p p1 p2 I 2 v1 v2 s Q, p October, 23 2007 v I 2 v1 v2 p2 S s I2s v Where pi is called the velocity-product force and is defined to be v pi vi I i vi 24 Articulated-Body Algorithm • These formulas so allow us A recursively, can be reformulated to find I i and pi in terms of only I i 1 A and pi 1 • Our algorithm is then – Calculate the series of articulated body inertias and bias forces – Using these inertias and bias forces, calculate the joint accelerations • Since these are both defined recursively, they each take O(n), making the entire algorithm O(n) October, 23 2007 25 Lagrange-Multiplier Method • The preceding methods are reducedcoordinate formulations – These methods remove some of the dof’s by enforcing a set of constraints (a joint can only rotate in a certain direction constraining the motion of the joint and the link) – Finding a parameterization for the generalized coordinates in terms of the reduced coordinates is not always easy • The Lagrange Multiplier Method considers all the d.o.f.’s of the system October, 23 2007 26 Lagrange-Multiplier Method • Consider the equation of motion of i bodies Mi v i Fi – M describes the mass properties of the system is an d x d matrix where d is the number of dof’s of body i when not constrained • Also consider a constraint i that removes m dof’s from the system, we can write it as ji1 v 1 jik v k jin v n ci 0 – Where each jik is a m x d matrix that represents the constraint on link k where • d is again the number of dof’s of body k and • m is the number of dof’s removed by the constraint October, 23 2007 27 Lagrange-Multiplier Method • To simplify the notation, we replace the q individual constraint equations j11 v 1 j1k v k j1n v n c1 0 j21 v 1 j2k v k j2n v n c 2 0 jq1 v 1 jqk v k jqn v n c q 0 • With Jv c 0 – Where J is a q x n matrix of the individual jik matrices • Where q is the total number of constraints on the system and • n is the number of bodies – c is a q dimensional vector October, 23 2007 28 Lagrange-Multiplier Method • Just as we did when solving the constrained particle dynamics problems, we require that the constraint does no work. This results is a constraint force of the form: jT i1 c Fi λ i J T λ jT in – Where λi is an m (dof’s removed by constraint i) dimensional column vector and is referred to as the Lagrange multiplier • The problem is now just to find a λ so the constraint forces and any external forces satisfy the constraints October, 23 2007 29 Lagrange-Multiplier Method • If we introduce an external force acting on the system and combine the equations, we get Mv J T λ F ext • Solving for v and plugging into our constraint equation, we get JM 1J T λ JM 1F ext c • For constraints that act on two bodies, the matrix system is tightly banded and can be solved in O(n) – Using banded Cholesky decomposition, for example October, 23 2007 30 Lagrange-Multiplier Method • For more complicated constraints, JM 1J T is no longer sparse and we reformulate the equation as 0 M J T y 1 ext J 0 λ JM F c • If we required acyclic constraints, then sparsematrix theory tells us that MJ 0J has perfect elimination order T M JT – This means that if we factor J 0 into three matrices LDLT, L will be as sparse as H and can be computed in O(n) y 0 – We can then solve LDLT for λ, by solving each piece of LDLT 1 ext λ JM F c separately, each also in O(n) time and combining the solutions October, 23 2007 31 Summary • Inverse Dynamics – Newton-Euler is the standard implementation in O(n) • Forward dynamics – Composite-rigid-body algorithm is simpler and faster for n < 9, runs in O(n3) – Articulated-body algorithm is faster for n > 9, runs in O(n) – Lagrange multiplier method is somewhat simpler than ABA and speed is comparable, runs in O(n) October, 23 2007 32 References / Thanks • R. Featherstone, Robot Dynamics Algorithms, Boston/Dordrecht/Lancaster: Kluwer Academic Publishers, 1987. • D. Baraff, "Linear-Time Dynamics using Lagrange Multipliers," Proc. SIGGRAPH '96, pp. 137-146, New Orleans, August 1996. • Thanks to Nico for his slides from last year October, 23 2007 33