packages = ['numpy']


param.nbVarX = 3 # State space dimension param.l = [200, 200, 120] # Robot links lengths x = [3*np.pi/4, -np.pi/2, -np.pi/4] # Initial robot state # param.nbVarX = 8 # State space dimension # param.l = np.ones(param.nbVarX) * 800 / param.nbVarX # Robot links lengths # x = -np.ones(param.nbVarX) * np.pi / param.nbVarX # Initial robot state # x[0] = x[0] + np.pi

(click on the green run button to run the code; objects and joints can be moved with the mouse)

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 = [200., 200., 120.] # 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.5) 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.5) mouse[1] = -(mouse0[1] - canvas.height * 0.9) def onMouseDown(event): global mousedown, move_joint, param2 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 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') 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.5, canvas.height*0.9) def draw_robot(x, color): global hover_joint f = fkin0(x, param) # x = np.append(0, x) # Draw base ctx.translate(f[0,0], f[1,0]) ctx.lineWidth = '4' ctx.strokeStyle = 'white' ctx.fillStyle = color ctx.beginPath() ctx.arc(0, 0, 40, 0, np.pi) ctx.rect(-40, 0, 80, -40) ctx.fill() ctx.strokeStyle = color 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 = color 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 cursive' 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() ctx.setLineDash([]) # Draw joint angles r = 80 ctx.strokeStyle = 'rgb(200, 0, 0)' 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 = 'rgba(200, 0, 0, .2)' 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.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 ctx.fillStyle = 'rgb(200, 0, 0)' ctx.save() ctx.translate(np.cos(np.mean(a))*r-15, np.sin(np.mean(a))*r-15) ctx.scale(1, -1) ctx.fillText('x' + chr(8321 + i), 0, 0) # Display subscript with unicode ctx.restore() ctx.translate(-f[0,i], -f[1,i]) ctx.setLineDash([]) def draw_tip(f, color): # Draw object obj = Path2D.new() obj.arc(0, 0, 12, 0, 2*np.pi) ctx.translate(f[0], f[1]) ctx.fillStyle = color ctx.fill(obj) ctx.translate(-f[0], -f[1]) def draw_selected_point(f, color): obj = Path2D.new() obj.arc(0, 0, 6, 0, 2*np.pi) ctx.translate(f[0], f[1]) ctx.fillStyle = color 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 ######################################################################################### x = [3*np.pi/4, -np.pi/2, -np.pi/4] # Initial robot state async def main(): global hover_joint, x while True: try: u = controlCommand(x, param) x += u * param.dt # Update robot state f = fkin(x, param) except Exception as e: errorHandler(e) f = np.zeros(param.nbVarX) # Reinit hovering variables hover_joint = -1 # Rendering clear_screen() draw_robot(x, '#AAAAAA') # draw_tip(f, '#FF3399') draw_tip(f, '#000000') if move_joint >= 0: f = fkin(x[:move_joint+1], param2) draw_selected_point(f, '#777777') await asyncio.sleep(0.0001) pyscript.run_until_complete(main())