packages = ['numpy']

Exercise 5.a
Forward dynamics

Forward dynamics is the problem of computing the joint (angular) accelerations of a robot given the applied joint torques. This allows us to construct physical models that can predict the next steps of the motion when applying certain torques.

The goal of this exercise is to familiarize with the dynamics of a 2D planar manipulator. You can move the robot by dragging its joints using the left mouse button.

  • Use your mouse to perturb the robot and observe the effect of the dynamics.
  • Change param.l, param.m. param.dt and param.damping to see how these parameters affect the dynamics. Note that some parameters will produce instable behaviors (if this happens, you can reload the page to restart from a stable initial state).

x = np.array([-np.pi/4, -np.pi/2, np.pi/4, 0, 0, 0]) # Initial robot state (position and velocity) param.l = np.array([200.0, 200.0, 200.0]) # Robot links lengths param.m = np.array([1.0, 1.0, 1.0]) # Links masses param.dt = 1E-2 # Time step length param.damping = 20.0 # Viscous friction

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 # 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 def computeGCML(x, param): # Auxiliary matrices l = np.reshape(param.l/param.drawScale, [1, param.nbVarX]) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) m = np.reshape(param.m, [1, param.nbVarX]) Lm = np.triu(np.ones([m.shape[1], m.shape[1]])) * np.repeat(m, m.shape[1],0) # # Elementwise computation of G, C, and M # G = np.zeros(param.nbVarX) # M = np.zeros([param.nbVarX, param.nbVarX]) # C = np.zeros([param.nbVarX, param.nbVarX]) # for k in range(param.nbVarX): # G[k] = -sum(m[0,k:]) * param.gravity * l[0,k] * np.cos(L[k,:] @ x[:param.nbVarX]) # for i in range(param.nbVarX): # S = sum(m[0,k:param.nbVarX] * np.heaviside(np.array(range(k, param.nbVarX)) - i, 1)) # M[k,i] = l[0,k] * l[0,i] * np.cos(L[k,:] @ x[:param.nbVarX] - L[i,:] @ x[:param.nbVarX]) * S # C[k,i] = -l[0,k] * l[0,i] * np.sin(L[k,:] @ x[:param.nbVarX] - L[i,:] @ x[:param.nbVarX]) * S # Computation in matrix form of G, C, and M G = -np.sum(Lm,1) * param.l * np.cos(L @ x[:param.nbVarX]) * param.gravity C = -(l.T * l) * np.sin(np.reshape(L @ x[:param.nbVarX], [param.nbVarX,1]) - L @ x[:param.nbVarX]) * (Lm**.5 @ ((Lm**.5).T)) M = (l.T * l) * np.cos(np.reshape(L @ x[:param.nbVarX], [param.nbVarX,1]) - L @ x[:param.nbVarX]) * (Lm**.5 @ ((Lm**.5).T)) G = L.T @ G C = L.T @ C M = L.T @ M @ L return G,C,M,L def fdyn(x, u, param): G,C,M,L = computeGCML(x, param) ddx = np.linalg.inv(M) @ (u + G + C @ (L @ x[param.nbVarX:])**2 - x[param.nbVarX:] * param.damping) return ddx def externalPerturbation(x, param): u = np.zeros(param.nbVarX) if move_joint >= 0: f = fkin(x[:move_joint+1], param2) J = Jkin(x[:move_joint+1], param2) u[:move_joint+1] = J.T @ (mouse - f) * 1E-3 # Torque commands return u ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.nbVarX = 3 # State space dimension param.drawScale = 200.0 param.l = np.ones(param.nbVarX) * param.drawScale # Robot links lengths param.m = np.ones(param.nbVarX) # Robot links masses param.dt = 1E-2 # Time step length param.damping = 20.0 # Viscous friction param.gravity = 9.81 * 1E-2 # Gravity 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.1) 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.1) def onMouseDown(event): global mousedown, move_joint mousedown = 1 if hover_joint >= 0: f0 = fkin0(x[:param.nbVarX], 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) def draw_robot(x, color): global hover_joint ctx.setTransform(1, 0, 0, -1, canvas.width*0.5, canvas.height*0.1) f = fkin0(x, param) # 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: #console.log(i) 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]) def draw_selected_point(f, color): ctx.setTransform(1, 0, 0, -1, canvas.width*0.5, canvas.height*0.1) # Draw object obj = Path2D.new() obj.arc(0, 0, 6, 0, 2*np.pi) ctx.translate(f[0], f[1]) ctx.fillStyle = color ctx.fill(obj) ######################################################################################### def errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('errors') el.innerText = msg #el.textContent = msg ######################################################################################### x = np.zeros(2*param.nbVarX) # Initial robot state (position and velocity) async def main(): global hover_joint, x while True: try: u = externalPerturbation(x, param) # Torque commands ddx = fdyn(x, u, param) # Compute accelerations x += np.append(x[param.nbVarX:] + 0.5 * ddx * param.dt, ddx) * param.dt # Update state except Exception as e: errorHandler(e) x = np.zeros(2*param.nbVarX) # Reinit hovering variables hover_joint = -1 # Rendering clear_screen() draw_robot(x[:param.nbVarX], '#AAAAAA') if move_joint >= 0: f = fkin(x[:move_joint+1], param2) draw_selected_point(f, '#777777') await asyncio.sleep(0.00001) pyscript.run_until_complete(main())