dynamics of articulated robots kris hauser cs b659: principles of intelligent robot motion spring...
TRANSCRIPT
Dynamics of Articulated RobotsKris HauserCS B659: Principles of Intelligent Robot MotionSpring 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…
• 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…
• An ODE in the state space x=()
• 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
wv
T
wv
H 0 0 m I
1/2
Kinetic energy derivatives
• Force (@CM)
• H = [w]H – H[w]• Torque t = = [w] H w + H
Summary
Gyroscopic “force”
Force off of COM
x
F
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
𝛿𝑥
Generalized torque
f
Now consider the equivalent force f, torque τ at COM
Generalized torque
f
Now consider the equivalent force f, torque τ at COMAnd an infinitesimal virtual displacement of R.B. coordinates
𝛿𝑥
𝛿𝑞
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]
Principle of virtual work
f𝛿𝑥
𝛿𝑞
[fT,τT] = FT
Since we have [fT,τT] = FT
F
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τ
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
• 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
Lagrangian Mechanics
• Minimum action condition => Euler-Lagrange equations of motion:
• Note that P is independent of , so
A system of n partial differential equations
Sanity check: Newton’s laws
Example: Point Mass• Coordinates q = (x,y)• Potential field P(x,y)• Lagrangian: • Equations of motion
Kinetic energy for articulated robot
• Velocity of i’th rigid body
• Angular velocity of i’th rigid body
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
• G(q)
Generalized gravity
Putting it all together
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
• 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
• With the force:
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!
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
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 • 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 • 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!
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
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
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)
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
• (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