Dynamics and Numerical Integration

Transcription

1 Dynamics and Numerical Integration EECS 398 Intro. to Autonomous Robotics autorob.org ME/EECS 567 ROB 510 Robot Modeling and Control Fall 2018

2

3

4 Administrivia Project 1 ( Path Planning ) due next Monday Sep 24 git pizza extended office hours Friday 5-7pm, Beyster 3725 If you are not on course slack group, me (ocj) Send me point to your git repository (and your picture) Anyone still looking for permission to enroll from waitlist?

5 Example: Motorized Pendulum

6 Example: Motorized Pendulum Pendulum of length l with point mass m

7 Example: Motorized Pendulum Angle expresses pendulum range of motion Pendulum of length l with point mass m

8 Example: Motorized Pendulum Motor produces torque (angular force) Angle expresses pendulum range of motion Pendulum of length l with point mass m

9 Example: Motorized Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) Angle expresses pendulum range of motion Pendulum of length l with point mass m

10 Dynamics Equation of motion (with rotational inertia I) Motorized Pendulum Controls Motor produces torque (angular force) State (or Configuration) Angle expresses pendulum range of motion System Pendulum of length l with point mass m

11 State Defining (or Configuration) State State comprised of degrees-of-freedom (DOFs) DOFs describe translational and rotational axes for joints How many DOFs in the example pendulum? Airplane DOFs? Note: particles vs. rigid bodies, COM, geom, inertia Note: types of joints, global vs. local DOFs

12 Defining State State comprised of degrees-of-freedom (DOFs) DOFs describe translational and rotational axes of system Humanoid DOFs? joint angles global positioning

13 DOFs and Coordinate Spaces Each body has its own coordinate system link frame Joints connect two links (rigid bodies) Hinge (1 rotational DOF) Prismatic (1 translational DOF) Ball-socket (3 DOFs) A motor exerts force along a DOF axis Linear transformations used to relate coordinate systems of bodies and joints Spatial geometry attached to each link, but does not affect the body s coordinate frame

14 DOFs and Coordinate Spaces Each body has its own coordinate system link frame Joints connect two links (rigid bodies) Hinge (1 rotational DOF) Prismatic (1 translational DOF) Ball-socket (3 DOFs) A motor exerts force along a DOF axis Linear transformations used to relate coordinate systems of bodies and joints Spatial geometry attached to each link, but does not affect the body s coordinate frame

15 DOFs and Coordinate Spaces Each body has its own coordinate system link frame Joints connect two links (rigid bodies) Hinge (1 rotational DOF) Prismatic (1 translational DOF) Ball-socket (3 DOFs) A motor exerts force along a DOF axis Linear transformations used to relate coordinate systems of bodies and joints Spatial geometry attached to each link, but does not affect the body s coordinate frame

16 DOFs and Coordinate Spaces Each body has its own coordinate system link frame Joints connect two links (rigid bodies) Hinge (1 rotational DOF) Prismatic (1 translational DOF) Ball-socket (3 DOFs) A motor exerts force along a DOF axis Linear transformations used to relate coordinate systems of bodies and joints Spatial geometry attached to each link, but does not affect the body s coordinate frame

17 DOFs and Coordinate Spaces Each body has its own coordinate system link frame Joints connect two links (rigid bodies) Hinge (1 rotational DOF) Prismatic (1 translational DOF) Ball-socket (3 DOFs) A motor exerts force along a DOF axis Linear transformations used to relate coordinate systems of bodies and joints Spatial geometry attached to each link, but does not affect the body s coordinate frame

18 Robotic machines are comprised of N joints and N+1 links DLR Lightweight arm Joints and links form a tree hierarchy of articulated motion The base is the root link of this hierarchy A link has one parent (inboard) joint and potentially zero, one, or multiple child (outboard) joints A serial chain is a robot where every link has only one child joint Dynamixel robot kit

19 Planar 2-DOF 2-link Arm

20 SCARA Arm Selective Compliance Assembly Robot Arm Tech Museum of Innovation

21 Fetch robot

22 Fetch links Fetch joints

23

24 Atlas robot WPI

25 Physical Simulation (Gazebo) Technion

26 Why simulate robots? Real robots are expensive Improper controllers can physically break robots Inexpensive to experiment and test robot controllers in simulation Predictive model of dynamics Necessary for some types of control (e.g., optimal control)

27 For example, NASA Valkyrie

28 Valkyrie for real -

29 NASA Valkyrie started development in simulation

30 Valkyrie in simulation -

31 Our first robot: Pendularm!

32 Project 2: 1 DOF Pendularm model, simulate, and control 1 DoF robot arm

33 Dynamics Equation of motion (with rotational inertia I) Example: Pendulum Controls Motor produces torque (angular force) State (or Configuration) Angle expresses pendulum range of motion System Pendulum of length l with point mass m

34 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) motor torque Angle expresses pendulum range of motion Pendulum of length l with point mass m

35 Example device Control torque of motor as a proportion of current applied Decrease resistance to increase force DC motor Potentiometer irinakl Increase resistance to decrease force

36 Actuator (electric) Servos have: actuators to produce motion proprioception to sense pose controller to regulate current to motor Proprioception (optical encoder) Controller (4 channel H-Bridges)

37

38 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) angular acceleration (second time derivative) Angle expresses pendulum range of motion Pendulum of length l with point mass m

39 First derivative with approximation after step length h Second derivative Integration inverse operation of differentiation

40 Power rule Time derivatives (velocity, acceleration) or or Product rule Partial derivative Chain rule Total derivative

41 Math.sin(A) Angles A, B, C Lengths a, b, c Math.cos(A) Math.tan(A) Math.atan2(a,b)

42 Example: Pendulum Equation of motion (with rotational inertia I) How was this derived? Motor produces torque (angular force) Angle expresses pendulum range of motion Pendulum of length l with point mass m

43 Example: Pendulum Equation of motion (with rotational inertia I) Lagrangian is kinetic minus potential energy used to generate equation of motion Motor produces torque (angular force) Angle expresses pendulum range of motion Pendulum of length l with point mass m

44 Lagrangian Dynamics for Pendulum Equation of motion (with rotational inertia I) Kinetic Energy Potential Energy Lagrangian Equation of motion

45 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) with Parallel Axis Theorem (I=ml 2 ) Angle expresses pendulum range of motion Pendulum of length l with point mass m

46 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) with Parallel Axis Theorem (I=ml 2 ) Angle expresses pendulum range of motion gravity term motor term Pendulum of length l with point mass m

47 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) with Parallel Axis Theorem (I=ml 2 ) Angle expresses pendulum range of motion Numerical integration over time Pendulum of length l with point mass m

48 Example: Pendulum Equation of motion (with rotational inertia I) with Parallel Axis Theorem (I=ml2) What is this? Numerical integration over time Motor produces torque (angular force) Angle expresses pendulum range of motion Hint Pendulum of length l with point mass m

49 Hidden Figures (2017)

50 Example: Pendulum Equation of motion (with rotational inertia I) Motor produces torque (angular force) with Parallel Axis Theorem (I=ml 2 ) Angle expresses pendulum range of motion Numerical integration over time Euler integration Pendulum of length l with point mass m

51 Let s see what happens

52 Euler integration would kill this man Walter Lewin -

53

54 Given initial condition and

55 Given initial condition and time state

56 Given initial condition differential equation and in the form velocity system dynamics

57 Given initial condition differential equation and in the form how to estimate of future state at simulation iteration n?

58 Given initial condition differential equation and in the form how to estimate of future state at simulation iteration n? numerical integration over timestep iterations next state current state integration over timestep

59 Given initial condition differential equation and in the form how to estimate of future state at simulation iteration n? Euler s method next state current state advance time timestep (dt)

60 Example Euler integration of 2D point over 2 time steps

61 Second-order state State in Newtonian physics has both position ( ) and velocity ( )

62 Euler s method acceleration assumes second order system f(y,y,y ) treats velocity y = f(y) and acceleration y = f (y ) individually

63 Euler s method acceleration Why is the pendulum going unstable?

64 Given initial condition differential equation and in the form how to estimate of future state at time n? integral approximation Euler s method

65 where Euler s method ended up where we should have ended up integral approximation

66 Example Euler integration of 2D point over 4 time steps

67 Example Euler integration of 2D point over 4 time steps Euler integration True evolution of state

68 Example Euler integration of 2D point over 4 time steps Euler integration True evolution of state

69 Initial tangent Euler integration True evolution of state

70 Extend by timestep Euler integration True evolution of state

71 Extend by timestep Euler integration True evolution of state

72 Add to current state Euler integration True evolution of state

73 Euler integration True evolution of state Integration error

74 Euler integration True evolution of state Integration error

75

76 integration error

77 Can we improve this integration over time? integration error

78 Can we improve this integration over time? Option 1: Reduce timestep Option 2: Use a better integrator integration error

79 Can we improve this integration over time? Option 1: Reduce timestep Option 2: Use a better integrator integration error

80 Verlet Integration Initialize Iterate position Iterate velocity (optional) update notation: x is used as state

81 Verlet Integration

82 Verlet Integration

83 Verlet Integration

84 Verlet Integration Initialize Iterate position Iterate velocity (optional) Do not forget to initialize

85 Let s see what happens

86 Verlet Integration Initialize Iterate position Iterate velocity (optional)

87 Velocity Verlet

88 Let s see what happens

89 Runge-Kutta think of f() as callable function for dy/dt

90 Runge-Kutta Butcher tableau describes family of RK methods s : order of RK formula

91 Runge-Kutta 1 Euler s Method when s=1 and b 1 =1

92 An RK 2: The Midpoint Method Midpoint Method s=2 b 1 =0 b 2 =1 c 1 c 2 a 21 c 2 =0.5 a 21 =0.5 b 1 b 2

93 2) Evaluate tangent at trial midpoint 3) Step again from start using trial midpoint tangent 1) Take trial step to midpoint

94

95 Euler Midpoint

96 The RK4 : Simpson s Rule a widely used version of fourth order Runge-Kutta (known as RK4 ) implements Simpson s rule: numerical approximation of definite integrals by evaluating integral extents and midpoint

97 RK4 uses s=4 a 21 =1/2, a 32 =1/2, a 43 =1, c 1 =0, c 2 =1/2, c 3 =1/2, c 4 =1 b 1 =1/6, b 2 =1/3, b 3 =1/3, b 4 =1/6

98 Runge-Kutta: RK4 Slope at beginning of interval Slope at trial midpoint hk 1 /2 Slope at trial midpoint hk 2 /2 Slope at trial end of interval

99

100 k (x1) = x t + 0 k (v1) = v t + 0 c 1 =0 remember: 2 nd order ODE can represented as 1 st order

101 integrate position from t to trial midpoint at t+c 2 h using k (v1) (c 2 =1/2) k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) a 21 =1/2

102 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) integrate velocity from t to trial midpoint at t+c 2 h acceleration depends on system state (i.e., position of pendulum)

103 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) a 32 =1/2 integrate position from t to second trial midpoint at t+c 3 h using k (v2) (c 3 =1/2)

104 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) k (v3) = k (v1) + (a 32 * accel(k (x2) ) * h) acceleration depends on system state

105 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) k (v3) = k (v1) + (a 32 * accel(k (x2) ) * h) k (x4) = k (x1) + (a 43 * k (v3) * h) a 43 =1 integrate position from t to trial endpoint at t+c 4 h using k (v3) (c 4 =1)

106 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) k (v3) = k (v1) + (a 32 * accel(k (x2) ) * h) k (x4) = k (x1) + (a 43 * k (v3) * h) k (v4) = k (v1) + (a 43 * accel(k (x3) ) * h)

107 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) k (v3) = k (v1) + (a 32 * accel(k (x2) ) * h) k (x4) = k (x1) + (a 43 * k (v3) * h) k (v4) = k (v1) + (a 43 * accel(k (x3) ) * h) b 1 =1/6, b 2 =1/3, b 3 =1/3, b 4 =1/6 x t+h = x t + h*(b 1 *k (v1) +b 2 *k (v2) +b 3 *k (v3) +b 4 *k (v4) )

108 k (x1) = x t + 0 k (v1) = v t + 0 k (x2) = k (x1) + (a 21 * k (v1) * h) k (v2) = k (v1) + (a 21 * accel(k (x1) ) * h) k (x3) = k (x1) + (a 32 * k (v2) * h) k (v3) = k (v1) + (a 32 * accel(k (x2) ) * h) k (x4) = k (x1) + (a 43 * k (v3) * h) k (v4) = k (v1) + (a 43 * accel(k (x3) ) * h) velocity depends on acceleration a(x) v t+h = v t + h*[b 1 *a(k (x1) )+b 2 *a(k (x2) )+b 3 *a(k (x3) ) +b 4 *a(k (x4) )]

109 Let s see what happens

110 Can we add another link? Double pendulum

111 Locations of pendulum bobs Lagrangian of pendulum bob positions

112 Lagrangian in generalized coordinates (joint angles)

113 Lagrangian equations of motion Lagrangian EOM for i=1

114 Lagrangian equations of motion Lagrangian EOM for i=2

115 The 2 equations of motion for double pendulum Substitute equations into each other to solve for and What happened to the torques?

116 Pendularm2 by oharib

117 Project 2: Pendularm function init() {... pendulum = { // pendulum object length:2.0, mass:2.0, angle:math.pi/2, angle_dot:0.0}; gravity = 9.81; // Earth gravity t = 0; dt = 0.05; // init time pendulum.control = 0; // motor kineval-stencil-f16/master/ project_pendularm/pendularm1.html // next lecture: PID control pendulum.desired = -Math.PI/2.5; pendulum.desired_dot = 0; pendulum.servo = {kp:0, kd:0, ki:0};...}

118 function animate() {... if (numerical_integrator === "euler") { // STENCIL: Euler integrator } else if (numerical_integrator === "verlet") { // STENCIL: basic Verlet integration } else if (numerical_integrator === "velocity verlet") { // STENCIL: velocity Verlet } else if (numerical_integrator === "runge-kutta") { // STENCIL: Runge-Kutta 4 integrator } else { } // set the angle of the pendulum pendulum.geom.rotation.y = pendulum.angle; t = t + dt; // advance time...} function pendulum_acceleration(p,g) { // STENCIL: return acceleration from equations of motion }

119 Next up: Control (although no Janet Jackson)