RCFS
Sandboxes
Forward kinematics (FK)
Inverse kinematics (IK)
Bimanual robot
Humanoid robot (CoM and coordination matrix)
Iterative linear quadratic regulator (iLQR)
Exercises
01
Linear algebra in Python
02
Movement primitives and Newton's method
03
Gaussian Distributions
4a
Forward kinematics
4b
Inverse kinematics and nullspace control
5a
Forward dynamics
5b
Inverse dynamics and impedance control
6a
Planning with linear quadratic regulator
6b
Planning in joint space with LQR
07
Iterative linear quadratic regulator (iLQR)
08
Exploration with ergodic control
$\bm{J}(\bm{x})=\begin{bmatrix} \frac{\color{#00AA00}\partial f_1}{\color{#CC0000}\partial x_1} & {\color{#CCCCCC}\frac{\partial f_1}{\partial x_2}} & {\color{#CCCCCC}\frac{\partial f_1}{\partial x_3}} \\[2mm] \frac{\color{#0000CC}\partial f_2}{\color{#CC0000}\partial x_1} & {\color{#CCCCCC}\frac{\partial f_2}{\partial x_2}} & {\color{#CCCCCC}\frac{\partial f_2}{\partial x_3}} \end{bmatrix}$
$\bm{J}(\bm{x})=\begin{bmatrix} {\color{#CCCCCC}\frac{\partial f_1}{\partial x_1}} & \frac{\color{#00AA00}\partial f_1}{\color{#CC5500}\partial x_2} & {\color{#CCCCCC}\frac{\partial f_1}{\partial x_3}} \\[2mm] {\color{#CCCCCC}\frac{\partial f_2}{\partial x_1}} & \frac{\color{#0000CC}\partial f_2}{\color{#CC5500}\partial x_2} & {\color{#CCCCCC}\frac{\partial f_2}{\partial x_3}} \end{bmatrix}$
$\bm{J}(\bm{x})=\begin{bmatrix} {\color{#CCCCCC}\frac{\partial f_1}{\partial x_1}} & {\color{#CCCCCC}\frac{\partial f_1}{\partial x_2}} & \frac{\color{#00AA00}\partial f_1}{\color{#CC9900}\partial x_3} \\[2mm] {\color{#CCCCCC}\frac{\partial f_2}{\partial x_1}} & {\color{#CCCCCC}\frac{\partial f_2}{\partial x_2}} & \frac{\color{#0000CC}\partial f_2}{\color{#CC9900}\partial x_3} \end{bmatrix}$
$\color{#CC0000}x_1$
$\color{#CC5500}x_2$
$\color{#CC9900}x_3$
$\color{#00AA00}f_1$
$\color{#0000CC}f_2$
from pyodide.ffi import create_proxy from js import Path2D, document, console import numpy as np import asyncio # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = [param.l @ np.cos(L @ x), param.l @ np.sin(L @ x)] return f # Forward kinematics for all joints (in robot coordinate system) def fkin0(x, param): L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ L @ np.diag(param.l) @ np.cos(L @ x), L @ np.diag(param.l) @ np.sin(L @ x) ]) f = np.hstack([np.zeros([2,1]), f]) return f # Jacobian with analytical computation (for single time step) def Jkin(x, param): L = np.tril(np.ones([param.nbVarX, param.nbVarX])) J = np.vstack([ -np.sin(L @ x).T @ np.diag(param.l) @ L, np.cos(L @ x).T @ np.diag(param.l) @ L ]) return J ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.dt = 1E-1 # Time step length param.nbVarX = 3 # State space dimension (x1,x2,x3) param.l = [320, 280, 160] # Robot links lengths param2 = lambda: None # Lazy way to define an empty class in python ######################################################################################### # Mouse events mouse0 = np.zeros(2) mouse = np.zeros(2) mousedown = 0 hover_joint = -1 move_joint= -1 def onMouseMove(event): global mouse, mouse0 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.2) mouse[1] = -(mouse0[1] - canvas.height * 0.9) def onTouchMove(event): global mouse, mouse0 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.2) mouse[1] = -(mouse0[1] - canvas.height * 0.9) def onMouseDown(event): global mousedown, move_joint, param2, formula_el mousedown = 1 # if hover_joint >= 0: # f0 = fkin0(x, param) # param2.l = np.append(param.l[:hover_joint], np.linalg.norm(f0[:,hover_joint] - mouse)) # param2.nbVarX = hover_joint+1 # move_joint = hover_joint # for i in range(param.nbVarX): # formula_el[i].setAttribute('hidden', 'hidden') # formula_el[hover_joint].removeAttribute('hidden') def onMouseUp(event): global mousedown, move_joint mousedown = 0 move_joint = -1 def onWheel(event): global x # 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') x_el = [] formula_el = [] for i in range(param.nbVarX): formula_el.append(document.getElementById('formula%d' % i)) x_el.append(document.getElementById('x%d' % i)) f_el = [document.getElementById('f0'), document.getElementById('f1')] def clear_screen(): ctx.setTransform(1, 0, 0, 1, 0, 0) ctx.fillStyle = 'white' ctx.fillRect(0, 0, canvas.width, canvas.height) ctx.setTransform(1, 0, 0, -1, canvas.width*0.2, canvas.height*0.9) def draw_robot(x): global hover_joint, x_el f = fkin0(x, param) # Draw axes ctx.lineWidth = '2' ctx.fillStyle = '#DDDDDD' ctx.strokeStyle = '#DDDDDD' ctx.beginPath() ctx.moveTo(650,0) ctx.lineTo(0,0) ctx.lineTo(0,500) ctx.stroke() # Draw arrow tips ctx.beginPath() ctx.moveTo(650,0) ctx.lineTo(630,-10) ctx.lineTo(630,10) ctx.fill() ctx.beginPath() ctx.moveTo(0,500) ctx.lineTo(-10,480) ctx.lineTo(10,480) ctx.fill() # Draw base ctx.translate(f[0,0], f[1,0]) ctx.lineWidth = '4' ctx.strokeStyle = 'white' ctx.fillStyle = '#AAAAAA' ctx.beginPath() ctx.arc(0, 0, 40, 0, np.pi) ctx.rect(-40, 0, 80, -40) ctx.fill() ctx.strokeStyle = '#AAAAAA' for i in range(5): ctx.beginPath() ctx.moveTo(-30+i*15, -40) ctx.lineTo(-40+i*15, -60) ctx.stroke() # Draw links and articulations obj_articulation = Path2D.new() obj_articulation.arc(0, 0, 12, 0, 2*np.pi) ctx.lineCap = 'round' ctx.lineJoin = 'round' for i in range(param.nbVarX): if i < param.nbVarX: # Draw links outlines ctx.lineWidth = '46' ctx.strokeStyle = 'white' ctx.beginPath() ctx.lineTo(f[0,i], f[1,i]) ctx.lineTo(f[0,i+1], f[1,i+1]) ctx.stroke() # Draw links obj = Path2D.new() obj.lineTo(f[0,i], f[1,i]) obj.lineTo(f[0,i+1], f[1,i+1]) ctx.lineWidth = '38' ctx.strokeStyle = '#AAAAAA' ctx.stroke(obj) if ctx.isPointInStroke(obj, mouse0[0], mouse0[1]) and move_joint < 0: hover_joint = i # Draw articulations ctx.lineWidth = '4' ctx.strokeStyle = 'white' ctx.translate(f[0,i], f[1,i]) ctx.stroke(obj_articulation) ctx.translate(-f[0,i], -f[1,i]) # # Draw link lengths # ctx.font = '38px serif' # ctx.fillStyle = 'rgb(0, 160, 0)' # ctx.strokeStyle = 'rgb(0, 160, 0)' # ctx.setLineDash([2, 6]) # for i in range(param.nbVarX): # ctx.beginPath() # ctx.moveTo(f[0,i], f[1,i]) # ctx.lineTo(f[0,i+1], f[1,i+1]) # ctx.stroke() # ctx.save() # xtmp = [np.mean([f[0,i], f[0,i+1]]), np.mean([f[1,i], f[1,i+1]])] # dtmp = f[:,i+1] - f[:,i] # dtmp = [dtmp[1], -dtmp[0]] / np.linalg.norm(dtmp) # ctx.translate(xtmp[0]+dtmp[0]*30-15, xtmp[1]+dtmp[1]*30-15) # ctx.scale(1, -1) # ctx.fillText('l' + chr(8321 + i), 0, 0) # Display subscript with unicode # ctx.restore() # Draw joint angles ctx.setLineDash([]) colors = ['#CC0000','#CC5500','#CC9900'] colors2 = ['#CC000033','#CC550033','#CC990033'] r = 80 ctx.font = '48px serif' ctx.setLineDash([2, 6]) for i in range(param.nbVarX): a = np.sort([np.sum(x[:i]), np.sum(x[:(i+1)])]) ctx.translate(f[0,i], f[1,i]) # Draw sector ctx.fillStyle = colors2[i] ctx.beginPath() ctx.moveTo(0, 0) ctx.arc(0, 0, r*.9, a[0], a[1]) ctx.lineTo(0, 0) ctx.fill() # Draw sector boundaries ctx.strokeStyle = colors[i] ctx.beginPath() ctx.moveTo(0, 0) ctx.lineTo(np.cos(a[0])*r, np.sin(a[0])*r) ctx.stroke() ctx.beginPath() ctx.moveTo(0, 0) ctx.lineTo(np.cos(a[1])*r, np.sin(a[1])*r) ctx.stroke() # # Draw joint angle name (with canvas) # ctx.fillStyle = colors[i] # ctx.save() # ctx.translate(np.cos(np.mean(a))*(r+20)-15, np.sin(np.mean(a))*(r+20)-15) # ctx.scale(1, -1) # ctx.fillText('x' + chr(8321 + i), 0, 0) # Display subscript with unicode # ctx.restore() # Draw joint angle name (with latex) xtmp = np.zeros(2) xtmp[0] = (f[0,i] + np.cos(np.mean(a))*(r+20)-15 + canvas.width * 0.2) * canvas.clientWidth / canvas.width xtmp[1] = (f[1,i] + np.sin(np.mean(a))*(r+20)-15 + canvas.height * 0.1) * canvas.clientHeight / canvas.height x_el[i].setAttribute('style', 'position:absolute; left:%dpx; bottom:%dpx;' % (xtmp[0],xtmp[1])) ctx.translate(-f[0,i], -f[1,i]) # Draw robot end-effector ctx.setLineDash([]) obj = Path2D.new() obj.arc(0, 0, 6, 0, 2*np.pi) ctx.fillStyle = 'rgb(0, 0, 0)' ctx.translate(f[0,-1], 0) ctx.fill(obj) ctx.translate(0, f[1,-1]) ctx.fill(obj) ctx.translate(-f[0,-1], 0) ctx.fill(obj) ctx.translate(0, -f[1,-1]) def draw_trace(fhist): # Draw trace ctx.lineWidth = '8' ctx.strokeStyle = '#555555' ctx.beginPath() for i in range(fhist_id+1, fhist.shape[1]): ctx.lineTo(fhist[0,i], fhist[1,i]) for i in range(fhist_id): ctx.lineTo(fhist[0,i], fhist[1,i]) ctx.stroke() # Draw trace on horizontal axis ctx.strokeStyle = '#00AA00' ctx.fillStyle = '#00AA00' ctx.beginPath() for i in range(fhist_id+1, fhist.shape[1]): ctx.lineTo(fhist[0,i], 0) for i in range(fhist_id): ctx.lineTo(fhist[0,i], 0) ctx.stroke() # # Draw text on horizontal axis # ctx.save() # ctx.translate(fhist[0,fhist_id-1]-15, -45) # ctx.scale(1, -1) # ctx.fillText('f' + chr(8321), 0, 0) # Display subscript with unicode # ctx.restore() # Draw trace on vertical axis ctx.strokeStyle = '#0000CC' ctx.fillStyle = '#0000CC' ctx.beginPath() for i in range(fhist_id+1, fhist.shape[1]): ctx.lineTo(0, fhist[1,i]) for i in range(fhist_id): ctx.lineTo(0, fhist[1,i]) ctx.stroke() # # Draw text on vertical axis (with canvas) # ctx.save() # ctx.translate(-45, fhist[1,fhist_id-1]-15) # ctx.scale(1, -1) # ctx.fillText('f' + chr(8322), 0, 0) # Display subscript with unicode # ctx.restore() # Draw text on axes (with latex) xtmp = np.zeros(2) xtmp[0] = (fhist[0,fhist_id-1] - 15 + canvas.width * 0.2) * canvas.clientWidth / canvas.width xtmp[1] = (- 60 + canvas.height * 0.1) * canvas.clientHeight / canvas.height f_el[0].setAttribute('style', 'position:absolute; left:%dpx; bottom:%dpx;' % (xtmp[0],xtmp[1])) # Horizontal axis xtmp[0] = (- 60 + canvas.width * 0.2) * canvas.clientWidth / canvas.width xtmp[1] = (fhist[1,fhist_id-1] - 15 + canvas.height * 0.1) * canvas.clientHeight / canvas.height f_el[1].setAttribute('style', 'position:absolute; left:%dpx; bottom:%dpx;' % (xtmp[0],xtmp[1])) # Vertical axis def draw_selected_point(f): obj = Path2D.new() obj.arc(0, 0, 6, 0, 2*np.pi) ctx.translate(f[0], f[1]) ctx.fillStyle = '#999999' ctx.fill(obj) ctx.translate(-f[0], -f[1]) def errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('repl-err') el.innerText = msg #el.textContent = msg def controlCommand(x, param): u = np.zeros(param.nbVarX) if move_joint >= 0: # Residual and Jacobian df = (mouse - fkin(x[:move_joint+1], param2)) * 5 J = Jkin(x[:move_joint+1], param2) J = np.hstack((J, np.zeros([2,param.nbVarX-move_joint-1]))) # Augmented form # IK pinvJ = np.linalg.inv(J.T @ J + np.eye(J.shape[1]) * 1e4) @ J.T # Damped pseudoinverse u = pinvJ @ df # Control commands return u def wiggleJoint(joint_id, t): u = np.zeros(param.nbVarX) u[joint_id] = np.sin(np.pi * 4 * t / tmax) * .08 return u ######################################################################################### x = [np.pi/3, -np.pi/4, -np.pi/2] # Initial robot state fhist = np.tile(np.array(fkin(x,param)).reshape(-1,1), [1,100]) fhist_id = 0 joint_id = 0 t = 0 tmax = 200 async def main(): global hover_joint, x, fhist, fhist_id, t, joint_id, formula_el while True: # u = controlCommand(x, param) u = wiggleJoint(joint_id, t) x += u * param.dt # Update robot state f = fkin(x, param) fhist[:,fhist_id] = f fhist_id = (fhist_id+1) % 100 t += 1 if t > tmax: t = 0 joint_id = (joint_id+1) % param.nbVarX fhist = np.tile(np.array(fkin(x,param)).reshape(-1,1), [1,100]) for i in range(param.nbVarX): formula_el[i].setAttribute('hidden', 'hidden') formula_el[joint_id].removeAttribute('hidden') # # Reinit hovering variables # hover_joint = -1 # Rendering clear_screen() draw_robot(x) draw_trace(fhist) # if move_joint >= 0: # f = fkin(x[:move_joint+1], param2) # draw_selected_point(f) await asyncio.sleep(0.0001) pyscript.run_until_complete(main())