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}).

These are physical models that allow us to control the motion of a robot. You can read about inverse dynamics in Section 6 of the RCFS documentation.

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

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

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

  • Set \bm{q}, \bm{\dot{q}} and \bm{\ddot{q}} so that \bm{\tau} = \bm{g}(\bm{q}). Perturb the robot with the mouse and observe the result of the perturbation.
  • Set \bm{q}, \bm{\dot{q}} and \bm{\ddot{q}} so that \bm{\tau} = \bm{C}(\bm{q},\bm{\dot{q}}) + \bm{g}(\bm{q}). Perturb the robot with the mouse and observe the result of the perturbation.
  • We would like the robot to reach an upright position q_target = np.array([np.pi/2, 0]) and stay there with zero velocity dq_target = np.array([0, 0]). Set \bm{q}, \bm{\dot{q}} and \bm{\ddot{q}} such that we implement the following impedance controller to achieve our goal: \bm{\tau} = \bm{M}(\bm{q})(\bm{K}_p(\bm{q}^d - \bm{q}) + \bm{K}_v(\bm{\dot{q}}^d - \bm{\dot{q}})) + \bm{C}(\bm{q},\bm{\dot{q}}) + \bm{g}(\bm{q}). You should see that it stabilizes around the upright position. To check how stable is your controller, 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/2, 0]) dq_target = np.array([0, 0]) Kp = 10 Kv = 1 def controlCommand(state, param=param): # Question 1: Gravity Compensation Term (g(q)) q = np.zeros(param.nbVarX) # Implement here dq = np.zeros(param.nbVarX) # Implement here ddq = np.zeros(param.nbVarX) # Implement here u = inverse_dynamics(q, dq, ddq, param=param) # Question 2: Gravity compensation and coriolis terms (c(q,dq) + g(q)) q = np.zeros(param.nbVarX) # Implement here dq = np.zeros(param.nbVarX) # Implement here ddq = np.zeros(param.nbVarX) # Implement here u = inverse_dynamics(q, dq, ddq, param=param) # Question 3: Joint impedance control for reaching upright position q = np.zeros(param.nbVarX) # Implement here dq = np.zeros(param.nbVarX) # Implement here ddq = np.zeros(param.nbVarX) # Implement here u = inverse_dynamics(q, dq, ddq, param=param) # implement here return u print(state)
def controlCommand(state, param=param): # Question 1: Gravity Compensation Term (g(q)) q = state[:param.nbVarX] dq = np.zeros(param.nbVarX) ddq = np.zeros(param.nbVarX) u = inverse_dynamics(q, dq, ddq, param=param) return u print(state)
def controlCommand(state, param=param): # Question 2: Gravity compensation and coriolis terms (c(q,dq) + g(q)) q = state[:param.nbVarX] dq = state[param.nbVarX:] ddq = np.zeros(param.nbVarX) u = inverse_dynamics(q, dq, ddq, param=param) return u print(state)
q_target = np.array([np.pi/2, 0]) dq_target = np.array([0, 0]) Kp = 10 Kv = 1 def controlCommand(state, param=param): # Question 3: Joint impedance control for reaching upright position q = state[:param.nbVarX] dq = state[param.nbVarX:] ddq = Kp * (q_target - q) + Kv * (dq_target - dq) u = inverse_dynamics(q, dq, ddq, param=param) return u print(state)





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 def inverse_dynamics(q, dq, ddq, param): l = np.reshape(param.l, [1,param.nbVarX]) m = np.reshape(param.l_m, [1,param.nbVarX]) T = np.tril(np.ones([param.nbVarX, param.nbVarX])) Tm = np.multiply(np.triu(np.ones([m.shape[1], m.shape[1]])), np.repeat(m, m.shape[1],0)) ## Computation in matrix form of G,M, and C #G =-np.reshape(np.sum(Tm, 1), [param.nbVarX, 1]) * l.T * np.cos(T @ np.reshape(q, [param.nbVarX, 1])) * param.gravity #G = T.T @ G #M = (l.T * l) * np.cos(np.reshape(T @ q, [param.nbVarX, 1]) - T @ q) * (Tm**.5 @ ((Tm**.5).T)) #M = T.T @ M @ T #C = -(l.T * l) * np.sin(np.reshape(T @ q, [param.nbVarX, 1]) - T @ q) * (Tm**.5 @ ((Tm**.5).T)) # Elementwise computation of G, M, and C G = np.zeros([param.nbVarX, 1]) M = np.zeros([param.nbVarX, param.nbVarX]) C = np.zeros([param.nbVarX, param.nbVarX]) for k in range(param.nbVarX): G[k,0] = -sum(m[0,k:]) * param.gravity * l[0,k] * np.cos(T[k,:] @ q) 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(T[k,:] @ q - T[i,:] @ q) * S C[k,i] = -l[0,k] * l[0,i] * np.sin(T[k,:] @ q - T[i,:] @ q) * S G = T.T @ G M = T.T @ M @ T param.inertia_matrix = M return M @ ddq - T.T @ C @ (T @ dq)**2 - T @ dq * param.damping - G[:,0] # Forward kinematics for end-effector (in robot coordinate system) def fdyn(x,u, param): l = np.reshape(param.l, [1,param.nbVarX]) m = np.reshape(param.l_m, [1,param.nbVarX]) T = np.tril(np.ones([param.nbVarX, param.nbVarX])) Tm = np.multiply(np.triu(np.ones([m.shape[1], m.shape[1]])), np.repeat(m, m.shape[1],0)) ## Computation in matrix form of G,M, and C #G =-np.reshape(np.sum(Tm, 1), [param.nbVarX, 1]) * l.T * np.cos(T @ np.reshape(x[0:param.nbVarX], [param.nbVarX, 1])) * param.gravity #G = T.T @ G #M = (l.T * l) * np.cos(np.reshape(T @ x[:param.nbVarX], [param.nbVarX, 1]) - T @ x[:param.nbVarX]) * (Tm**.5 @ ((Tm**.5).T)) #M = T.T @ M @ T #C = -(l.T * l) * np.sin(np.reshape(T @ x[:param.nbVarX], [param.nbVarX, 1]) - T @ x[:param.nbVarX]) * (Tm**.5 @ ((Tm**.5).T)) # Elementwise computation of G, M, and C G = np.zeros([param.nbVarX, 1]) M = np.zeros([param.nbVarX, param.nbVarX]) C = np.zeros([param.nbVarX, param.nbVarX]) for k in range(param.nbVarX): G[k,0] = -sum(m[0,k:]) * param.gravity * l[0,k] * np.cos(T[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(T[k,:] @ x[:param.nbVarX] - T[i,:] @ x[:param.nbVarX]) * S C[k,i] = -l[0,k] * l[0,i] * np.sin(T[k,:] @ x[:param.nbVarX] - T[i,:] @ x[:param.nbVarX]) * S G = T.T @ G M = T.T @ M @ T param.inertia_matrix = M # Compute acceleration tau = np.reshape(u, [param.nbVarX, 1]) inv_M = np.linalg.inv(M) ddq = inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[param.nbVarX:],[param.nbVarX, 1]))**2 - \ T @ np.reshape(x[param.nbVarX:],[param.nbVarX, 1])*param.damping) ddq = ddq[:, 0] # compute the next step dq = np.clip(x[param.nbVarX:] + ddq * param.dt , -10., 10.) q = x[:param.nbVarX] + x[param.nbVarX:] * param.dt + 0.5 * ddq * (param.dt**2) xt = np.append(q, dq) return xt # 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 ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.nbVarX = 2 # State space dimension (x1,x2,x3) param.l = [2, 2] # Robot links lengths param.l_m = [1, 1] # Link masses param.damping = 8. # Damping param.gravity = 9.81 # Gravity param.dt = 5e-2 # Time step length param.noise_scale = 0. ######################################################################################### # Mouse events mouse0 = np.zeros(2) mousedown = 0 hover_joint = -1 move_joint= -1 hover0 = np.zeros(2) def onMouseMove(event): global mouse, mouse0, hover0, x offset = canvas.getBoundingClientRect() mouse0[0] = (event.clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.clientY - offset.y) * canvas.height / canvas.clientHeight if move_joint >= 0: x[move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onTouchMove(event): global mouse, mouse0, hover0, x 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 if move_joint >= 0: x[move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onMouseDown(event): global mousedown, move_joint, hover0 mousedown = 1 if hover_joint >= 0: move_joint = hover_joint hover0 = np.copy(mouse0) 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_ground(): ctx.setTransform(1, 0, 0, -1, canvas.width*0.5, canvas.height*0.9) ctx.translate(0, -40) ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '5' ctx.strokeStyle = '#CCCCCC' ctx.beginPath() ctx.moveTo(-400, 0) ctx.lineTo(400, 0) ctx.stroke() def draw_robot(x, color): global hover_joint ctx.setTransform(1, 0, 0, -1, canvas.width*0.5, canvas.height*0.5) f = fkin0(x, param)*100 # 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 ctx.lineCap = 'round' ctx.lineJoin = 'round' for i in range(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 ctx.lineWidth = '38' ctx.strokeStyle = color ctx.beginPath() ctx.lineTo(f[0,i], f[1,i]) ctx.lineTo(f[0,i+1], f[1,i+1]) ctx.stroke() # Draw articulations obj = Path2D.new() obj.arc(0, 0, 12, 0, 2*np.pi) ctx.lineWidth = '4' ctx.strokeStyle = 'white' for i in range(param.nbVarX+1): ctx.translate(f[0,i], f[1,i]) ctx.stroke(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_joint = i ctx.translate(-f[0,i], -f[1,i]) def draw_tip(f, color): ctx.setTransform(1, 0, 0, -1, canvas.width*0.5, canvas.height*0.9) # Draw object obj = Path2D.new() obj.arc(0, 0, 16, 0, 2*np.pi) ctx.translate(f[0], f[1]) ctx.fillStyle = color ctx.fill(obj) def controlCommand(state, param=param): u = inverse_dynamics(q=state[:param.nbVarX], dq=np.zeros(param.nbVarX), ddq=np.zeros(param.nbVarX), param=param) # Gravity compensation return u ######################################################################################### def errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('errors') el.innerText = msg #el.textContent = msg ######################################################################################### x = np.array([np.pi/3, np.pi/4]) # Initial robot state x_target = np.array([np.pi/2, np.pi/5]) state = np.append(x, np.zeros(param.nbVarX)) u = np.zeros(param.nbVarX) async def main(): global hover_joint, x, state while True: try: u = controlCommand(state+np.random.randn(param.nbVarX*2)*param.noise_scale) except Exception as e: errorHandler(e) u = np.zeros(param.nbVarX) state = fdyn(state, u, param) x = state[:param.nbVarX] # Reinit hovering variables hover_joint = -1 # Rendering clear_screen() # draw_ground() draw_robot(x, '#AAAAAA') #draw_tip(f, '#FF3399') await asyncio.sleep(0.01) pyscript.run_until_complete(main())