packages = ['numpy']

Exercise 6b
Planning in joint space with LQR

This exercise extends the previous example with an optimal control problem in joint space. You can specify the precision matrices used in LQR, change the poses of two viapoints, and then visualize the resulting motion found by LQR.

  • Change the code in the LQR in joint space tab so that the first viapoint is a 'picking configuration' (pink object) and the second viapoint is a 'placing configuration' (green object).

param.nbVarPos = 3 # Dimension of position param.nbDeriv = 1 # Number of derivatives, 2 for acceleration control 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) param.Mu[:,0] = np.array([-np.pi/3, np.pi/2, np.pi]) # 1st viapoint param.Mu[:,1] = np.array([-np.pi/2, np.pi/2, np.pi]) # 2nd viapoint # 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([-np.pi/4, np.pi/2, np.pi/4]) # 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 = 3 # Control space dimension (dx1,dx2,dx3) param.nbPoints = 2 # Number of viapoints param.nbDeriv = 1 param.nbVarPos = 3 param.nbVarX = param.nbVarPos * param.nbDeriv param.l = [79, 96, 55] # Robot links lengths param.sz = [50, 30] # Size of objects param.Mu = np.zeros((param.nbPoints, param.nbVarPos)).T param.Mu[:,0] = np.array([-np.pi/3, np.pi/2, np.pi]) # 1st viapoint param.Mu[:,1] = np.array([-np.pi/2, np.pi/2, np.pi]) # 2nd viapoint param.r = 1e-2 # 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 hover_joint = -1 move_joint= -1 hover0 = np.zeros(2) active_robot_id = 0 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_robot(xt, color1, color2, color3, color4, selectable, robot_id): global hover_joint,active_robot_id ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # Draw base ctx.translate(0, 81) ctx.lineWidth = '1' ctx.strokeStyle = color3 ctx.fillStyle = color1 ctx.fill(base1_svg) ctx.stroke(base1_svg) # Outline ctx.stroke(base2_svg) # Draw seg1 ctx.rotate(xt[0]) ctx.fillStyle = color1 ctx.fill(seg11_svg) ctx.stroke(seg11_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg12_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#3399FF' hover_joint = 0 active_robot_id = robot_id ctx.fill(seg12_svg) ctx.stroke(seg12_svg) ctx.stroke(seg13_svg) ctx.stroke(seg14_svg) ctx.stroke(seg15_svg) # Draw seg2 ctx.translate(0, -79) ctx.rotate(xt[1]) ctx.fillStyle = color1 ctx.fill(seg21_svg) ctx.stroke(seg21_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg22_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#FF9933' hover_joint = 1 active_robot_id = robot_id ctx.fill(seg22_svg) ctx.stroke(seg22_svg) ctx.stroke(seg23_svg) ctx.stroke(seg24_svg) # Draw seg3 ctx.translate(0, -96) ctx.rotate(xt[2]) ctx.fillStyle = color1 ctx.fill(seg31_svg) ctx.stroke(seg31_svg) # Outline ctx.fill(seg32_svg) ctx.stroke(seg32_svg) # Outline ctx.fill(seg33_svg) ctx.stroke(seg33_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg34_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#99FF33' hover_joint = 2 active_robot_id = robot_id ctx.fill(seg34_svg) ctx.stroke(seg34_svg) ctx.stroke(seg35_svg) ctx.stroke(seg36_svg) ctx.stroke(seg37_svg) # Draw end-effector point ctx.translate(-20.5, -51) ctx.beginPath() ctx.arc(0, 0, 2, 0, 2 * np.pi) ctx.fillStyle = color4 ctx.fill() # # Draw skeleton of the kinematic chain # 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 = '#FF8888' # f = fkin0(x, param) # ctx.beginPath() # ctx.moveTo(0, 81) # for i in range(param.nbVarX+1): # ctx.lineTo(f[0,i], f[1,i]) # ctx.stroke() 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, -10) ctx.rotate(np.pi/2) if id == 1: ctx.translate(100, 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 errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('errors') el.innerText = msg #el.textContent = msg ######################################################################################### x = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial robot state u = np.zeros(param.nbVarX) Sx, Su = compute_transfer_matrices(param) async def main(): global hover_joint, hover_obj t0 = 0 t = 0 tf = 0 max_iter = 20 while True: t0 += 10 if t0 > 19: t0 = 0 if t > param.nbData-2: tf += 1 if tf > 10: #Stay some iterations at the final point before starting again t = 0 tf = 0 max_iter = 20 else: t += 1 max_iter = 5 x0 = np.append(x, np.zeros(param.nbVarX-param.nbVarPos)) xs = solve_LQR(x0, param) # Reinit hovering variables hover_joint = -1 hover_obj = -1 # Rendering clear_screen() draw_ground() draw_object(1, param,'#FF3399') draw_object(0, param,'#33FF99') draw_robot(param.Mu[:,0], '#EEEEEE', '#DDDDDD', '#CCCCCC', '#BBBBBB', False, 0) draw_robot(param.Mu[:,1], '#EEEEEE', '#DDDDDD', '#CCCCCC', '#BBBBBB', False, 1) draw_robot(xs[t], '#CCCCCC', '#AAAAAA', '#222222', '#000000', False, -1) await asyncio.sleep(0.0001) pyscript.run_until_complete(main())