packages = ['numpy']

Exercise 6a
Planning with linear quadratic regulator

Previously, in exercises 4 and 5, we defined a desired target position \bm{x}_{\text{target}} defined either in joint space or task space and we wanted the robot to reach this target position. We did not specify the timings or the viapoints/paths that the robot should follow to reach these target positions.

Optimal control is the principled way of planning a robot motion by specifying timings and/or a set of viapoints or a reference trajectory to follow.

Linear quadratic regulator (LQR) is the most simple form of optimal control. In this exercise, the goal is to understand how LQR works and how it can be applied to robot planning problems. You can read more about this in Section 7 of the RCFS documentation.

This exercise allows you to specify the precision matrices used in LQR and to change the placement of two viapoints. The series of control commands generates a resulting path that can be visualized on the right figure (the black point shows the initial position). The graphs show the corresponding covariance matrices (inverses of precision matrices) for two viapoints (in pink and green).

  • Change the code so that the point mass system passes through any point on the pink line at timestep 20 and reaches the green point at time step 49. To verify your solution, you can change the positions of the pink and green objects by dragging them and change their shape by redefining the precision matrices.
  • Change the code by setting param.nbDeriv=2 so that the point-mass system is controlled with acceleration commands instead of velocity commands (system dynamics defined as a double integrator instead of a simple integrator). Set param.r to a higher value until you see that it does not reach the pink line anymore. Set param.nbDeriv=1 again to compare the smoothness of the two trajectories.
  • Change the code so that both keypoints should be reached at time step 49. What do you observe? Change the precision matrices so that one of the corresponding covariance is elongated horizontally and the other is elongated vertically (large along one direction and thin anlong the other direction). Try to change the locations of the two viapoints. What do you observe?

param.nbVarPos = 2 # Dimension of position param.nbDeriv = 1 # Number of derivatives (1 for velocity commands) param.nbVarX = param.nbVarPos * param.nbDeriv # Dimension of the state space param.r = 1e-2 # precision of the control command Sx, Su = compute_transfer_matrices(param) # precision matrix of the first viapoint Q1 = np.eye(param.nbVarX) # precision matrix of the second viapoint Q2 = np.eye(param.nbVarX) # Time occurrence of viapoints tl = np.array([24, 49]) idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)]) x = np.array([0, 0]) # Initial state


def print(x): display(x, target='output') from pyodide.ffi import create_proxy from js import Path2D, document, console import numpy as np import asyncio from math import factorial object_angle = document.getElementById('object_angle') # Objects angle object_angle = lambda: None object_angle.value = 0 ######################################################################################### base1_svg = Path2D.new('m -40.741975,77.319831 c -0.47247,-4.03869 7.32825,-20.1653 10.1171,-22.57617 4.71807,-4.07862 14.00201,-4.3722 15.87822,-6.89366 1.16821,-1.06725 1.19306,-2.45846 1.19136,-4.984461 -0.005,-6.836939 0.0375,-38.9164375 -0.0588,-42.62054746 C -13.757555,-5.2728275 -9.8130348,-13.34661 -0.02248483,-13.67734 7.5903552,-13.93451 13.741895,-7.1292375 13.608255,-0.84839739 13.474625,5.4324325 13.073715,50.200081 13.741895,54.075491 c 0.66817,3.8754 3.0736,26.72695 3.0736,26.72695 l -53.47684,-0.23624 c -3.68777,-0.0163 -4.0806,-3.24637 -4.0806,-3.24637 z') base2_svg = Path2D.new('m -13.653647,45.770986 27.119789,-0.07088') seg11_svg = Path2D.new('M 1.1085815,-48.64595 C 2.8616565,-42.037584 12.141047,-7.3721658 13.181308,-3.8158258 14.730923,1.4818692 12.982058,10.29588 3.6015646,13.1191 -3.6924249,15.31437 -11.379603,10.30832 -12.856452,4.2020952 c -1.476846,-6.106188 -11.012844,-42.5297362 -12.082149,-45.6580692 -1.43181,-5.329295 -2.652606,-11.707828 -2.961653,-18.313541 -0.264086,-5.644652 2.111069,-7.347919 2.111069,-7.347919 2.624567,-3.183184 8.150604,-3.203987 10.333578,-6.275591 1.769697,-2.490098 1.823736,-5.627976 1.959877,-8.208118 0.347278,-6.581603 7.8818877,-11.888333 13.83865325,-11.31331 11.26196775,1.087146 13.17554475,9.678077 12.89920975,14.363762 -0.465778,7.897881 -5.8447437,11.223081 -10.8257944,12.5317 -4.0229212,1.0569 -4.0522977,5.558527 -3.6062254,8.077811 0.53206435,3.004955 1.69902315,6.035714 2.2984683,9.29523 z') seg12_svg = Path2D.new('m 0.05406256,-11.597507 c -6.39589386,0 -11.58398456,5.1988245 -11.58398456,11.60742169 0,6.40859681 5.1880907,11.60742231 11.58398456,11.60742231 6.39589414,0 11.58398444,-5.1988255 11.58398444,-11.60742231 0,-6.40859719 -5.1880903,-11.60742169 -11.58398444,-11.60742169 z') seg13_svg = Path2D.new('m 0.89874154,-90.983149 c -6.37570324,-0.50777 -11.96015354,4.262759 -12.46893054,10.651135 -0.508778,6.388373 4.2502031,11.982666 10.62590635,12.490434 6.37571205,0.507768 11.96015765,-4.262758 12.46893565,-10.65113 0.50878,-6.388376 -4.2501988,-11.982669 -10.62591146,-12.490439 z') seg14_svg = Path2D.new('M -24.784795,-41.659214 1.1085815,-48.64595') seg15_svg = Path2D.new('m -20.037453,-23.361462 c 0,0 0.150891,-2.736177 2.859936,-3.928038 2.698441,-1.058633 15.064238,-4.832856 18.5649072,-5.023273 3.4151981,-0.800461 4.5404475,1.903276 4.5404475,1.903276') seg21_svg = Path2D.new('m 1.0846146,-63.378335 c 0.2455591,-2.834423 3.4523451,-16.559449 4.0431711,-18.415736 1.4726648,-4.271726 5.7043363,-7.554682 6.9088533,-12.676592 0.896166,-8.180737 -5.5218419,-14.075707 -11.67006058,-13.690757 -5.14680322,0.32229 -11.25729142,3.07163 -11.71005642,12.988353 -0.245696,5.381384 2.1556935,6.934579 1.261502,10.892576 -1.067995,4.72731 -3.306673,16.43352 -4.123841,19.092346 -1.013352,3.297141 -2.321128,5.411066 -6.454795,11.635385 -4.133667,6.224321 -5.394419,14.031661 -6.200979,18.250843 -0.80656,4.219183 -2.639059,14.959257 -1.769749,20.046047 0.662189,3.874813 5.317911,7.0872532 8.194376,7.8656925 2.799342,0.6504765 3.517742,0.6405013 5.007603,2.5337107 1.489861,1.8932084 1.467073,4.13299795 2.141633,7.605938 0.4829,3.1674976 4.2207359,9.9421608 11.3304401,10.8558018 C 5.1524174,14.518915 14.875984,8.7881742 13.263942,-1.6038057 11.604726,-12.299883 3.6744317,-12.710682 0.92067775,-13.632854 -1.5420631,-15.114186 -2.6268693,-19.519275 -1.8747035,-22.72879 -1.1225409,-25.938308 1.196278,-37.889572 1.3340625,-40.676542 1.8762966,-51.644393 -0.30239687,-54.622686 1.0846146,-63.378335 Z') seg22_svg = Path2D.new('M -11.586565,0.93074939 C -11.083534,7.3068272 -5.4927791,12.069965 0.89597241,11.565935 7.284721,11.061904 12.059397,5.4810033 11.556367,-0.89507457 11.053335,-7.2711624 5.4625836,-12.034299 -0.92616504,-11.530269 -7.3149165,-11.02624 -12.089595,-5.4453385 -11.586565,0.93074939 Z') seg23_svg = Path2D.new('m -26.640574,-36.592971 c 5.304398,1.031726 26.42204728,5.61535 26.42204728,5.61535') seg24_svg = Path2D.new('m -18.97242,-7.0296766 c 0,0 5.357638,0.9161489 6.790283,-0.3224518 0,0 1.645529,-2.0773004 2.9224726,-3.1740806 1.2245317,-1.051764 3.0335173,-2.07985 3.0335173,-2.07985 1.9028326,-1.212528 2.2666634,-4.627153 3.1812597,-7.476594 1.7216337,-5.363774 1.9197573,-6.250728 1.9197573,-6.250728') seg31_svg = Path2D.new('m -28.6797,-26.841855 c -1.2675,3.57197 -1.218858,4.557009 -1.595581,8.234518 -0.376722,3.677509 -0.09415,6.442577 -0.0095,8.568278 -0.253944,2.7250156 1.116106,5.225167 1.12849,7.9985227 -0.113818,2.61245518 -0.732443,4.5287742 -1.461378,6.6813667 -0.049,4.0362406 -0.269163,8.1196006 0.283769,12.1263916 0.524743,2.889586 3.777418,3.398207 6.006756,4.487809 3.000431,1.151299 5.962802,2.459036 9.011639,3.446545 2.908512,0.626882 4.197412,-2.375507 4.231736,-4.87884 0.0854,-2.479073 0.335025,-4.760767 2.8765686,-5.44487 3.9560009,-1.216619 8.05245912,-1.946456 12.0010307,-2.99019 5.703849,-2.0129894 9.4239807,-8.5502843 7.7887937,-14.4529723 -1.270267,-5.5243102 -6.867591,-9.6714567 -12.54557065,-9.0219797 -3.01008665,0.221201 -5.63894195,1.895241 -8.24502045,3.5658663 -2.0818469,1.3351245 -1.6868669,-3.2534803 -1.7460679,-4.8326393 -0.0013,-3.276304 0.21006,-3.084655 0.0062,-4.979716 -0.203891,-1.895062 -0.264478,-4.611901 -1.494343,-8.479035 -5.412496,-0.0097 -15.221678,-0.05267 -15.221678,-0.05267 z') seg32_svg = Path2D.new('m -14.015345,-33.566241 c -1.232867,-1.390966 -2.465733,-2.781932 -3.698599,-4.172898 0.0038,-3.646334 0.02928,-7.293353 0.01923,-10.939249 -0.02501,-0.949144 -0.522837,-2.078703 -1.513796,-2.119205 -0.942425,0.01577 -1.897362,-0.08159 -2.832194,0.04493 -0.950302,0.333999 -1.133628,1.580185 -1.115778,2.522511 -0.04848,3.474219 -0.09695,6.948437 -0.145432,10.422655 -1.213181,1.407781 -2.426362,2.815561 -3.639543,4.223342 4.308705,0.006 8.617409,0.01194 12.926114,0.01791 z') seg33_svg = Path2D.new('m -12.412129,-26.867866 v -4.799995 c 0,-1.919999 -0.435344,-1.878396 -0.888867,-1.876655 -3.030562,0.01164 -14.262729,-0.07064 -14.523962,-0.04334 -0.467055,0 -0.934111,0 -0.883228,1.904343 0.01098,0.410814 0.0013,4.808677 0.0013,4.808677') seg34_svg = Path2D.new('M -10.345869,0.79321884 C -9.8879044,6.4881727 -4.8873495,10.735439 0.81892316,10.276563 6.5251942,9.8176865 10.782785,4.8259161 10.324819,-0.86903645 9.8668498,-6.5639995 4.8662942,-10.811265 -0.8399769,-10.35239 -6.5462504,-9.8935134 -10.803835,-4.901743 -10.345869,0.79321884 Z') seg35_svg = Path2D.new('m -10.926083,-10.640947 c -12.932836,-0.04585 -19.378158,-0.0931 -19.378158,-0.0931') seg36_svg = Path2D.new('M -9.9187154,15.300602 C -29.124234,15.272545 -30.475824,15.251842 -30.475824,15.251842') seg37_svg = Path2D.new('m -23.186087,-46.845579 h 5.542233 v 0') # Logarithmic map for R^2 x S^1 manifold def logmap(f, f0): diff = np.zeros(3) diff[:2] = f[:2] - f0[:2] diff[2] = np.imag(np.log(np.exp(f0[-1]*1j).conj().T * np.exp(f[-1]*1j).T)).conj() return diff # Apply angle offsets to match robot kinematic chain def emulate_DH_params(x): xt = np.copy(x) xt[0] = xt[0] - np.pi/2 orient = np.mod(np.sum(xt,0)+np.pi, 2*np.pi) - np.pi xt[2] = xt[2] - np.arctan(20.5/51) return xt, orient # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): xt, orient = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ param.l @ np.cos(L @ xt), param.l @ np.sin(L @ xt), orient ]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot) f[1] += 81 return f.flatten() # Forward kinematics for all joints (in robot coordinate system) def fkin0(x, param): xt, _ = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ L @ np.diag(param.l) @ np.cos(L @ xt), L @ np.diag(param.l) @ np.sin(L @ xt) ]) f = np.hstack([np.zeros([2,1]), f]) f[1] += 81 return f # Jacobian with analytical computation (for single time step) def Jkin(xt, param): xt, _ = emulate_DH_params(xt) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) J = np.vstack([ -np.sin(L @ xt).T @ np.diag(param.l) @ L, np.cos(L @ xt).T @ np.diag(param.l) @ L, np.ones([1,param.nbVarX]) ]) return J def compute_transfer_matrices(param): A1d = np.zeros((param.nbDeriv,param.nbDeriv)) B1d = np.zeros((param.nbDeriv,1)) for i in range(param.nbDeriv): A1d += np.diag( np.ones(param.nbDeriv-i) ,i ) * param.dt**i * 1/factorial(i) B1d[param.nbDeriv-i-1] = param.dt**(i+1) * 1/factorial(i+1) A = np.kron(A1d,np.identity(param.nbVarPos)) B = np.kron(B1d,np.identity(param.nbVarPos)) # Build Sx and Su transfer matrices Su = np.zeros((param.nbVarX*param.nbData,param.nbVarPos * (param.nbData-1))) Sx = np.kron(np.ones((param.nbData,1)),np.eye(param.nbVarX,param.nbVarX)) M = B for i in range(1,param.nbData): Sx[i*param.nbVarX:param.nbData*param.nbVarX,:] = np.dot(Sx[i*param.nbVarX:param.nbData*param.nbVarX,:], A) Su[param.nbVarX*i:param.nbVarX*i+M.shape[0],0:M.shape[1]] = M M = np.hstack((np.dot(A,M),B)) # [0,nb_state_var-1] return Sx, Su def solve_LQR(x0, param): Q = np.identity(param.nbVarX * param.nbPoints) Q[:param.nbVarX, :param.nbVarX] = Q1 Q[param.nbVarX:, param.nbVarX:] = Q2 R = I * param.r # Batch LQR Reproduction # ===================================== xd = np.vstack([param.Mu, np.zeros((param.nbVarX-param.nbVarPos, param.nbPoints))]).T.flatten() u_hat = np.linalg.inv(Su[idx.flatten()].T @ Q @ Su[idx.flatten()] + R) @ Su[idx.flatten()].T @ Q @ (xd - Sx[idx.flatten()] @ x0) x_hat = (Sx @ x0 + Su @ u_hat).reshape((param.nbData, -1)) return x_hat ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.dt = 1e-1 # Time step length param.nbData = 50 # Number of datapoints param.nbVarU = 2 # Control space dimension (dx1,dx2,dx3) param.nbPoints = 2 # Number of viapoints param.nbDeriv = 1 param.nbVarPos = 2 param.nbVarX = param.nbVarPos * param.nbDeriv param.l = [79, 96, 55] # Robot links lengths param.sz = [50, 30] # Size of objects param.Mu = np.array([[100, 0], [100, 100]]).T # Viapoints initial_viapoints = param.Mu.copy() param.r = 1e-2 param.tab = 'question1-tab-pane' param.prev_tab = 'question1-tab-pane' x_vias = np.zeros((2, param.nbVarX)) # Precision matrix Q1 = np.eye(param.nbVarX)*1e0 Q2 = np.eye(param.nbVarX)*1e0 # Control weight matrix I = np.identity((param.nbData-1) * param.nbVarU) # Time occurrence of viapoints tl = np.array([24, 49]) idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)]) ######################################################################################### # GUI scaling_factor = 2 # General scaling factor for rendering # Mouse events mouse0 = np.zeros(2) mouse = np.zeros(2) mousedown = 0 selected_obj = -1 hover_obj = -1 hover0 = np.zeros(2) def onMouseMove(event): global mouse, mouse0, hover0, x ,active_robot_id, x_vias offset = canvas.getBoundingClientRect() mouse0[0] = (event.clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.clientY - offset.y) * canvas.height / canvas.clientHeight mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x_vias[active_robot_id, move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onTouchMove(event): global mouse, mouse0, hover0, x,active_robot_id, x_vias offset = event.target.getBoundingClientRect() mouse0[0] = (event.touches.item(0).clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.touches.item(0).clientY - offset.y) * canvas.height / canvas.clientHeight mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x_vias[active_robot_id, move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onMouseDown(event): global mousedown, move_joint, hover0 mousedown = 1 if hover_joint >= 0: move_joint = hover_joint hover0 = np.copy(mouse0) def onMouseUp(event): global mousedown, selected_obj, move_joint mousedown = 0 selected_obj = -1 move_joint = -1 def onWheel(event): global hover_joint, hover_obj, x, object_angle, x_vias if hover_obj == 0: #param.Mu[2] += 0.2 * (event.deltaY/106) object_angle.value = (float)(object_angle.value) + 0.2 * (event.deltaY/106) if hover_joint >= 0: x[hover_joint] -= 0.2 * (event.deltaY/106) document.addEventListener('mousemove', create_proxy(onMouseMove)) #for standard mouse document.addEventListener('touchmove', create_proxy(onTouchMove)) #for mobile interfaces document.addEventListener('mousedown', create_proxy(onMouseDown)) #for standard mouse #document.addEventListener('pointerdown', create_proxy(onMouseDown)) #for mobile interfaces document.addEventListener('touchstart', create_proxy(onMouseDown)) #for mobile interfaces document.addEventListener('mouseup', create_proxy(onMouseUp)) #for standard mouse #document.addEventListener('pointerup', create_proxy(onMouseUp)) #for mobile interfaces document.addEventListener('touchend', create_proxy(onMouseUp)) #for mobile interfaces document.addEventListener('wheel', create_proxy(onWheel)) #for standard mouse ######################################################################################### canvas = document.getElementById('canvas') ctx = canvas.getContext('2d') def clear_screen(): ctx.setTransform(1, 0, 0, 1, 0, 0) # Reset transformation to identity ctx.fillStyle = 'white' ctx.fillRect(0, 0, canvas.width, canvas.height) def draw_ground(): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.beginPath() ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '5' ctx.strokeStyle = '#CCCCCC' ctx.moveTo(-200, 164) ctx.lineTo(200, 164) ctx.stroke() def draw_viapoint(id, param, color, color2): global selected_obj, hover_obj ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.translate(param.Mu[0, id], param.Mu[1, id]) if id == 0: s, U = np.linalg.eig(np.linalg.inv(Q1[:2, :2])) else: s, U = np.linalg.eig(np.linalg.inv(Q2[:2, :2])) # Draw object obj = Path2D.new() al = np.linspace(-np.pi, np.pi, 50) D = np.diag(s)*100 R = np.real(U @ np.sqrt(D+0j)) msh = (R @ np.array([np.cos(al),np.sin(al)])).T #+ param.Mu[:2,id] obj = Path2D.new() obj.moveTo(msh[0,0], msh[0,1]) for i in range(msh.shape[0]-1): obj.lineTo(msh[i+1,0], msh[i+1,1]) obj.closePath() ctx.strokeStyle = color2 ctx.stroke(obj) ctx.fillStyle = color ctx.fill(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_obj = id if ctx.isPointInPath(obj, mouse0[0], mouse0[1]) and mousedown==1: selected_obj = id def draw_object(id, param, color): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation if id == 0: ctx.translate(-150, 140) if id == 1: ctx.translate(150, 140) obj = Path2D.new() obj.rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) ctx.fillStyle = color ctx.fill(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_obj = id if ctx.isPointInPath(obj, mouse0[0], mouse0[1]) and mousedown==1: selected_obj = id def draw_lqr_path(xs, param, color): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # Draw initial point ctx.fillStyle = color ctx.beginPath() ctx.arc(xs[0,0], xs[0,1], 5, 0, 2*np.pi) ctx.fill() # Draw path ctx.lineWidth = '3' ctx.strokeStyle = color ctx.beginPath() ctx.moveTo(xs[0,0], xs[0,1]) for i in range(param.nbData-1): ctx.lineTo(xs[i+1,0], xs[i+1,1]) ctx.stroke() def draw_line(color): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '2' ctx.strokeStyle = color ctx.beginPath() ctx.moveTo(-50, -1000) ctx.lineTo(-50, 1000) ctx.stroke() def inverse_kinematics(x, t, fs, param, max_iter): x_next = x.copy() for i in range(max_iter): f_ee = fkin(x_next, param) J = Jkin(x_next, param) u = np.linalg.pinv(J) @ logmap(fs[t], f_ee) x_next += u * param.dt return x_next ######################################################################################### def errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('errors') el.innerText = msg #el.textContent = msg ######################################################################################### x = np.array([0,0]) # Initial robot state u = np.zeros(param.nbVarX) Sx, Su = compute_transfer_matrices(param) async def main(): global hover_obj, param while True: x0 = np.append(x, np.zeros(param.nbVarX-param.nbVarPos)) xs = solve_LQR(x0, param) # Reinit hovering variables hover_obj = -1 # Rendering clear_screen() draw_line('#FF77DD') draw_viapoint(0, param, '#FF3399', '#DD1177') draw_viapoint(1, param, '#33FF99', '#11DD77') draw_lqr_path(xs, param, '#000000') # Object selection if selected_obj>= 0: param.Mu[:2,selected_obj] = mouse param.Mu[0,selected_obj] = max(min(param.Mu[0,selected_obj],225), -225) param.Mu[1,selected_obj] = max(min(param.Mu[1,selected_obj],175), -175) await asyncio.sleep(0.0001) pyscript.run_until_complete(main())