packages = ['numpy']

Exercise 5.b
Inverse dynamics and impedance control

Inverse dynamics is the problem of computing the joint torques of a robot given its joint positions, velocities and accelerations: \bm{\tau} = \bm{M}(\bm{q})\bm{\ddot{q}}+ \bm{C}(\bm{q},\bm{\dot{q}}) + \bm{g}(\bm{q}).

The goal of this exercise is to understand the different terms in the inverse dynamics problem formulation and to control the robot to reach a desired position using joint space impedance control.

The function inverse_dynamics(x, ddq, param) implements the inverse dynamics model for a planar manipulator. Its outputs are joint torques.

The function controlCommand(x, param) takes the current state of the robot \bm{x}=[\bm{q}, \bm{\dot{q}}] as input and outputs joint torque commands u that are sent to the robot.

  • You can first run the code with zero torque control commands u and observe the result.
  • Specify the control commands so that \bm{\tau} = \bm{g}(\bm{q}) by exploiting the function inverse_dynamics(x, ddq, param) to compute the gravity compensation torques \bm{g}(\bm{q}). Perturb the robot with the mouse (at the articulations level) and observe the result of the perturbation.
  • We would now like the robot to reach an upright position q_target and stay there with zero velocity. First, use the gravity compensation controller that you coded in the above to bring your robot close to an upright position. You can then specify the torque control commands that will be sent to the robot so that the resulting closed-loop robot behavior corresponds to a mass-spring-damper system. You should see that it stabilizes around the upright position. To check how stable your controller is, try to perturb the robot with the mouse. Modify the values of stiffness and damping to see the effect of perturbations on the impedance controller.

q_target = np.array([-np.pi/4, -np.pi/2, np.pi/4]) kP = 400.0 # Stiffness gain kV = 10.0 # Damping gain def controlCommand(x, param): q = x[:param.nbVarX] dq = x[param.nbVarX:] u = np.zeros(param.nbVarX) # Question 1: Gravity compensation # u = inverse_dynamics(x, ddq, param) # Gravity compensation and Coriolis force compensation # Question 2: Joint impedance control for reaching upright position # u = ... # Implement here return u
def controlCommand(x, param): # Question 1: Gravity Compensation Term (g(q)) xtmp = np.append(x[:param.nbVarX], np.zeros(param.nbVarX)) u = inverse_dynamics(xtmp, np.zeros(param.nbVarX), param) return u
q_target = np.array([-np.pi/4, -np.pi/2, np.pi/4]) kP = 400.0 # Stiffness gain kV = 10.0 # Damping gain def controlCommand(x, param): # Question 2: Joint impedance control for reaching upright position q = x[:param.nbVarX] dq = x[param.nbVarX:] u = kP * (q_target - q) - kV * dq + inverse_dynamics(x, np.zeros(param.nbVarX), param) return u





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 inverse_dynamics(x, ddx, param): G,C,M,L = computeGCML(x, param) # u = M @ ddx - G - C @ (L @ x[param.nbVarX:])**2 + x[param.nbVarX:] * param.damping # With gravity, Coriolis and viscous friction compensation models u = M @ ddx - G - C @ (L @ x[param.nbVarX:])**2 # With gravity and Coriolis models return u 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 controlCommand(x, param): u = inverse_dynamics(x, np.zeros(param.nbVarX), param) # Torques for gravity and Coriolis force compensation # u = np.zeros(param.nbVarX) return u 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) # pinvJ = np.linalg.inv(J.T @ J + np.eye(param2.nbVarX) * 1E-4) @ J.T # Damped pseudoinverse u[:move_joint+1] = J.T @ (mouse - f) * 1E-3 # Torque commands # u[:move_joint+1] = pinvJ @ (mouse - f) * 1E1 # Torque commands # u[:move_joint] = J[:,:move_joint].T @ (mouse - f) * 1E-1 # 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) x = np.array([-np.pi/4, -np.pi/2, np.pi/4, 0, 0, 0]) # Initial robot state (position and velocity) async def main(): global hover_joint, x while True: try: u = controlCommand(x, param) + 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) u = np.zeros(param.nbVarX) 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())