RCFS
General information
About
License
2D sandboxes
Forward kinematics (FK)
Inverse kinematics (IK)
Bimanual robot
Humanoid robot (CoM and coordination matrix)
Iterative linear quadratic regulator (iLQR)
iLQR for car
iLQR for bicopter
Impedance control
3D sandboxes
Inverse kinematics (IK)
Coding 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
09
Orientation with Riemannian manifold
Online course
Table of contents
1 Introduction
2 Newton’s method for minimization
2.1 Gauss–Newton algorithm
2.2 Least squares
2.3 Least squares with constraints
3 Forward kinematics (FK) for a planar robot manipulator
4 Inverse kinematics (IK) for a planar robot manipulator
4.1 Numerical estimation of the Jacobian
4.2 Inverse kinematics (IK) with task prioritization
5 Encoding with basis functions
5.1 Univariate trajectories
5.2 Multivariate trajectories
5.3 Multidimensional inputs
5.4 Derivatives
5.5 Concatenated basis functions
5.6 Batch computation of basis functions coefficients
5.7 Recursive computation of basis functions coefficients
6 Linear quadratic tracking (LQT)
6.1 LQT with smoothness cost
6.2 LQT with control primitives
6.3 LQR with a recursive formulation
6.4 LQT with a recursive formulation and an augmented state space
6.5 Least squares formulation of recursive LQR
6.6 Dynamical movement primitives (DMP) reformulated as LQT with control primitives
7 iLQR optimization
7.1 Batch formulation of iLQR
7.2 Recursive formulation of iLQR
7.3 Least squares formulation of recursive iLQR
7.4 Updates by considering step sizes
7.5 iLQR with quadratic cost on f(x_{t})
7.6 iLQR with control primitives
7.7 iLQR for spacetime optimization
7.8 iLQR with offdiagonal elements in the precision matrix
7.9 Car steering
7.10 Bicopter
8 Forward dynamics (FD) for a planar robot manipulator
8.1 Robot manipulator with dynamics equation
References
A System dynamics at trajectory level
B Derivation of motion equation for a planar robot
C Linear systems used in the bimanual tennis serve example
D Equivalence between LQT and LQR with augmented state space
\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())