﻿ RCFS packages = ['numpy']

# Exercise 7Iterative linear quadratic regulator (iLQR)

In Exercise 6a, we have seen how LQR can be used to control a point-mass agent to pass through a set of viapoints. In Exercise 6b, we have seen how LQR can be used to control a robot with joint angle velocity commands to pass through a set of joint angle configurations.

In this exercise we will now see how an iterative linear quadratic regulator (iLQR) can be used to control a robot with joint angle velocity commands. We will consider a combination of viapoints in both joint space and task space. The goal will be to have the robot passing through a set of intermediate joint angle configurations and to have its end-effector passing through another set a intermediate viapoints in task space.

The goal of this exercise is to plan a path with iLQR to generate a motion passing through this combination of task space and joint space viapoints:

• The 1st viapoint param.viapoint1 is defined in the joint space, displayed as the orange robot.
• The 2nd viapoint param.viapoint2 is defined in the task space, displayed as the pink object.
• The 3rd viapoint param.viapoint3 is defined in the task space, displayed as the green object.

We consider a single integrator dynamical system with $\bm{x}_t$ and $\bm{u}_t$ representing joint angles and velocity commands at time step $t$, respectively. The cost function at a given time step $t$ is given by \begin{equation*} c(\bm{x}_t,\bm{u}_t) = \sum_{i=1}^3\bm{f}_i(\bm{x}_t)^{\!\trsp} \bm{Q}_{i,t} \bm{f}_i(\bm{x}_t) + \bm{u}_t^\trsp \bm{R}_t \, \bm{u}_t, \end{equation*} where $\bm{Q}_{i,t}$ and $\bm{R}_t$ are weight matrices trading off viapoints tracking and control effort. Such cost is quadratic on the residual functions $\bm{f}_i(\bm{x}_t)$ but nonquadratic on $\bm{x}_t$, which requires iLQR to be used instead of LQR.

The solution to this problem results in the following update of the control commands at each iteration of iLQR \begin{equation*} \Delta\bm{\hat{u}} \!=\! {\Bigg(\sum_{i=1}^3 \bm{S}_{\bm{u},i}^\trsp \bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{J}_i(\bm{x}) \bm{S}_{\bm{u},i} \!+\! \bm{R}\Bigg)}^{\!\!-1} \Bigg(- \sum_{i=1}^3 \bm{S}_{\bm{u},i}^\trsp \bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{f}_i(\bm{x}) - \bm{R} \, \bm{u} \Bigg), \end{equation*} where $\bm{J}_i(\bm{x})$ is the Jacobian matrix of $\bm{f}_i(\bm{x})$.

• By looking at the commented code snippets, modify the code in compute_fJ() so that the robot can pass through the set of viapoints defined in joint space and task space. You can verify your answer by moving the objects and changing the joint angle configurations of the robot for both initial pose (in light gray) and viapoint pose (in orange).
• Move the objects and modify the parameters tl, Q1, Q2 and Q3. Observe the resulting behaviors and the resulting cost.

# Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * 1E-6 # Precision matrices Q1 = np.eye(param.nbVarX) Q2 = np.eye(param.nbVarF) Q3 = np.eye(param.nbVarF) # Time occurrence of viapoints tl = np.array([33, 66, 100]) # Joint angle configuration at the first viapoint # print(x[:,tl[0]]) # End-effector position at the first viapoint # print(fkin(x[:,tl[0]], param)) # Corresponding Jacobian at the first viapoint # print(Jkin(x[:,tl[0]], param)) # print(param.viapoint1) #First viapoint # print(param.viapoint2) #Second viapoint # print(param.viapoint3) #Third viapoint def compute_fJ(x, param): # Residual when reaching the first viapoint (in joint space, orange robot) f1 = np.zeros(param.nbVarX) # To be replaced # Residual when reaching the second viapoint (in task space, pink object) f2 = np.zeros(param.nbVarF) # To be replaced # Residual when reaching the third viapoint (in task space, green object) f3 = np.zeros(param.nbVarF) # To be replaced # Jacobian of the residual when reaching the first viapoint (in joint space, orange robot) J1 = np.zeros((param.nbVarX, param.nbVarX)) # To be replaced # Jacobian of the residual when reaching the second viapoint (in task space, pink object) J2 = np.zeros((param.nbVarF, param.nbVarX)) # To be replaced # Jacobian of the residual when reaching the third viapoint (in task space, green object) J3 = np.zeros((param.nbVarF, param.nbVarX)) # To be replaced return f1,f2,f3,J1,J2,J3 update_iLQR()
# Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * 1E-6 # Precision matrix Q1 = np.eye(param.nbVarX) Q2 = np.eye(param.nbVarF) Q3 = np.eye(param.nbVarF) # Time occurrence of viapoints tl = np.array([33, 66, 100]) def compute_fJ(x, param): # Viapoint residuals f1 = x[:,tl[0]] - param.viapoint1 f2 = fkin(x[:,tl[1]], param) - param.viapoint2 f3 = fkin(x[:,tl[2]], param) - param.viapoint3 # Corresponding Jacobians J1 = np.eye(param.nbVarX) J2 = Jkin(x[:,tl[1]], param) J3 = Jkin(x[:,tl[2]], param) return f1,f2,f3,J1,J2,J3 update_iLQR()

Cost:
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 hover_joint = -1 selected_obj = -1 ######################################################################################### base1_svg = Path2D.new('m -40.741975,77.319831 c -0.47247,-4.03869 7.32825,-20.1653 10.1171,-22.57617 4.71807,-4.07862 14.00201,-4.3722 15.87822,-6.89366 1.16821,-1.06725 1.19306,-2.45846 1.19136,-4.984461 -0.005,-6.836939 0.0375,-38.9164375 -0.0588,-42.62054746 C -13.757555,-5.2728275 -9.8130348,-13.34661 -0.02248483,-13.67734 7.5903552,-13.93451 13.741895,-7.1292375 13.608255,-0.84839739 13.474625,5.4324325 13.073715,50.200081 13.741895,54.075491 c 0.66817,3.8754 3.0736,26.72695 3.0736,26.72695 l -53.47684,-0.23624 c -3.68777,-0.0163 -4.0806,-3.24637 -4.0806,-3.24637 z') base2_svg = Path2D.new('m -13.653647,45.770986 27.119789,-0.07088') seg11_svg = Path2D.new('M 1.1085815,-48.64595 C 2.8616565,-42.037584 12.141047,-7.3721658 13.181308,-3.8158258 14.730923,1.4818692 12.982058,10.29588 3.6015646,13.1191 -3.6924249,15.31437 -11.379603,10.30832 -12.856452,4.2020952 c -1.476846,-6.106188 -11.012844,-42.5297362 -12.082149,-45.6580692 -1.43181,-5.329295 -2.652606,-11.707828 -2.961653,-18.313541 -0.264086,-5.644652 2.111069,-7.347919 2.111069,-7.347919 2.624567,-3.183184 8.150604,-3.203987 10.333578,-6.275591 1.769697,-2.490098 1.823736,-5.627976 1.959877,-8.208118 0.347278,-6.581603 7.8818877,-11.888333 13.83865325,-11.31331 11.26196775,1.087146 13.17554475,9.678077 12.89920975,14.363762 -0.465778,7.897881 -5.8447437,11.223081 -10.8257944,12.5317 -4.0229212,1.0569 -4.0522977,5.558527 -3.6062254,8.077811 0.53206435,3.004955 1.69902315,6.035714 2.2984683,9.29523 z') seg12_svg = Path2D.new('m 0.05406256,-11.597507 c -6.39589386,0 -11.58398456,5.1988245 -11.58398456,11.60742169 0,6.40859681 5.1880907,11.60742231 11.58398456,11.60742231 6.39589414,0 11.58398444,-5.1988255 11.58398444,-11.60742231 0,-6.40859719 -5.1880903,-11.60742169 -11.58398444,-11.60742169 z') seg13_svg = Path2D.new('m 0.89874154,-90.983149 c -6.37570324,-0.50777 -11.96015354,4.262759 -12.46893054,10.651135 -0.508778,6.388373 4.2502031,11.982666 10.62590635,12.490434 6.37571205,0.507768 11.96015765,-4.262758 12.46893565,-10.65113 0.50878,-6.388376 -4.2501988,-11.982669 -10.62591146,-12.490439 z') seg14_svg = Path2D.new('M -24.784795,-41.659214 1.1085815,-48.64595') seg15_svg = Path2D.new('m -20.037453,-23.361462 c 0,0 0.150891,-2.736177 2.859936,-3.928038 2.698441,-1.058633 15.064238,-4.832856 18.5649072,-5.023273 3.4151981,-0.800461 4.5404475,1.903276 4.5404475,1.903276') seg21_svg = Path2D.new('m 1.0846146,-63.378335 c 0.2455591,-2.834423 3.4523451,-16.559449 4.0431711,-18.415736 1.4726648,-4.271726 5.7043363,-7.554682 6.9088533,-12.676592 0.896166,-8.180737 -5.5218419,-14.075707 -11.67006058,-13.690757 -5.14680322,0.32229 -11.25729142,3.07163 -11.71005642,12.988353 -0.245696,5.381384 2.1556935,6.934579 1.261502,10.892576 -1.067995,4.72731 -3.306673,16.43352 -4.123841,19.092346 -1.013352,3.297141 -2.321128,5.411066 -6.454795,11.635385 -4.133667,6.224321 -5.394419,14.031661 -6.200979,18.250843 -0.80656,4.219183 -2.639059,14.959257 -1.769749,20.046047 0.662189,3.874813 5.317911,7.0872532 8.194376,7.8656925 2.799342,0.6504765 3.517742,0.6405013 5.007603,2.5337107 1.489861,1.8932084 1.467073,4.13299795 2.141633,7.605938 0.4829,3.1674976 4.2207359,9.9421608 11.3304401,10.8558018 C 5.1524174,14.518915 14.875984,8.7881742 13.263942,-1.6038057 11.604726,-12.299883 3.6744317,-12.710682 0.92067775,-13.632854 -1.5420631,-15.114186 -2.6268693,-19.519275 -1.8747035,-22.72879 -1.1225409,-25.938308 1.196278,-37.889572 1.3340625,-40.676542 1.8762966,-51.644393 -0.30239687,-54.622686 1.0846146,-63.378335 Z') seg22_svg = Path2D.new('M -11.586565,0.93074939 C -11.083534,7.3068272 -5.4927791,12.069965 0.89597241,11.565935 7.284721,11.061904 12.059397,5.4810033 11.556367,-0.89507457 11.053335,-7.2711624 5.4625836,-12.034299 -0.92616504,-11.530269 -7.3149165,-11.02624 -12.089595,-5.4453385 -11.586565,0.93074939 Z') seg23_svg = Path2D.new('m -26.640574,-36.592971 c 5.304398,1.031726 26.42204728,5.61535 26.42204728,5.61535') seg24_svg = Path2D.new('m -18.97242,-7.0296766 c 0,0 5.357638,0.9161489 6.790283,-0.3224518 0,0 1.645529,-2.0773004 2.9224726,-3.1740806 1.2245317,-1.051764 3.0335173,-2.07985 3.0335173,-2.07985 1.9028326,-1.212528 2.2666634,-4.627153 3.1812597,-7.476594 1.7216337,-5.363774 1.9197573,-6.250728 1.9197573,-6.250728') seg31_svg = Path2D.new('m -28.6797,-26.841855 c -1.2675,3.57197 -1.218858,4.557009 -1.595581,8.234518 -0.376722,3.677509 -0.09415,6.442577 -0.0095,8.568278 -0.253944,2.7250156 1.116106,5.225167 1.12849,7.9985227 -0.113818,2.61245518 -0.732443,4.5287742 -1.461378,6.6813667 -0.049,4.0362406 -0.269163,8.1196006 0.283769,12.1263916 0.524743,2.889586 3.777418,3.398207 6.006756,4.487809 3.000431,1.151299 5.962802,2.459036 9.011639,3.446545 2.908512,0.626882 4.197412,-2.375507 4.231736,-4.87884 0.0854,-2.479073 0.335025,-4.760767 2.8765686,-5.44487 3.9560009,-1.216619 8.05245912,-1.946456 12.0010307,-2.99019 5.703849,-2.0129894 9.4239807,-8.5502843 7.7887937,-14.4529723 -1.270267,-5.5243102 -6.867591,-9.6714567 -12.54557065,-9.0219797 -3.01008665,0.221201 -5.63894195,1.895241 -8.24502045,3.5658663 -2.0818469,1.3351245 -1.6868669,-3.2534803 -1.7460679,-4.8326393 -0.0013,-3.276304 0.21006,-3.084655 0.0062,-4.979716 -0.203891,-1.895062 -0.264478,-4.611901 -1.494343,-8.479035 -5.412496,-0.0097 -15.221678,-0.05267 -15.221678,-0.05267 z') seg32_svg = Path2D.new('m -14.015345,-33.566241 c -1.232867,-1.390966 -2.465733,-2.781932 -3.698599,-4.172898 0.0038,-3.646334 0.02928,-7.293353 0.01923,-10.939249 -0.02501,-0.949144 -0.522837,-2.078703 -1.513796,-2.119205 -0.942425,0.01577 -1.897362,-0.08159 -2.832194,0.04493 -0.950302,0.333999 -1.133628,1.580185 -1.115778,2.522511 -0.04848,3.474219 -0.09695,6.948437 -0.145432,10.422655 -1.213181,1.407781 -2.426362,2.815561 -3.639543,4.223342 4.308705,0.006 8.617409,0.01194 12.926114,0.01791 z') seg33_svg = Path2D.new('m -12.412129,-26.867866 v -4.799995 c 0,-1.919999 -0.435344,-1.878396 -0.888867,-1.876655 -3.030562,0.01164 -14.262729,-0.07064 -14.523962,-0.04334 -0.467055,0 -0.934111,0 -0.883228,1.904343 0.01098,0.410814 0.0013,4.808677 0.0013,4.808677') seg34_svg = Path2D.new('M -10.345869,0.79321884 C -9.8879044,6.4881727 -4.8873495,10.735439 0.81892316,10.276563 6.5251942,9.8176865 10.782785,4.8259161 10.324819,-0.86903645 9.8668498,-6.5639995 4.8662942,-10.811265 -0.8399769,-10.35239 -6.5462504,-9.8935134 -10.803835,-4.901743 -10.345869,0.79321884 Z') seg35_svg = Path2D.new('m -10.926083,-10.640947 c -12.932836,-0.04585 -19.378158,-0.0931 -19.378158,-0.0931') seg36_svg = Path2D.new('M -9.9187154,15.300602 C -29.124234,15.272545 -30.475824,15.251842 -30.475824,15.251842') seg37_svg = Path2D.new('m -23.186087,-46.845579 h 5.542233 v 0') ## Parameters # =============================== simulation_speed = 10 param = lambda: None # Lazy way to define an empty class in python param.dt = 1E-2 # Time step length param.nbData = 101 # Number of datapoints param.nbIter = 20 # Maximum number of iterations for iLQR param.nbPoints = 3 # Number of viapoints param.nbVarX = 3 # State space dimension (x1,x2,x3) param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3) param.nbVarF = 2 # Objective function dimension (f1,f2) param.l = [79, 96, 55] # Robot links lengths param.sz = [50, 50] # Size of objects param.r = 1e-6 # Control weight term param.Mu = np.asarray([[150, 0, 0], [150, 120, 0]]).T # Objects location param.viapoint1 = np.array([-np.pi/8, 3*np.pi/4, np.pi/2]) # Viapoint in joint space param.viapoint2 = np.array([150, 0]) # Viapoint in task space param.viapoint3 = np.array([150, 120]) # Viapoint in task space x_vias = np.zeros([param.nbVarX, 2]) # Initial and intermediate robot states x_vias[:,0] = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial robot state x_vias[:,1] = param.viapoint1 # Intermediate robot state # Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * param.r # Precision matrix Q1 = np.eye(param.nbVarX) Q2 = np.eye(param.nbVarF) Q3 = np.eye(param.nbVarF) Q = np.eye(param.nbVarX + param.nbVarF * 2) Q[:param.nbVarX, :param.nbVarX] = Q1 Q[param.nbVarX:param.nbVarX+param.nbVarF, param.nbVarX:param.nbVarX+param.nbVarF] = Q2 Q[param.nbVarX+param.nbVarF:param.nbVarX+2*param.nbVarF, param.nbVarX+param.nbVarF:param.nbVarX+2*param.nbVarF] = Q3 # Time occurrence of viapoints tl = np.array([34, 70, 100]) idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)]) # Transfer matrices (for linear system as single integrator) Su0 = np.vstack([ np.zeros([param.nbVarX, param.nbVarX*(param.nbData-1)]), np.tril(np.kron(np.ones([param.nbData-1, param.nbData-1]), np.eye(param.nbVarX) * param.dt)) ]) Sx0 = np.kron(np.ones(param.nbData), np.identity(param.nbVarX)).T Su = Su0[idx.flatten()] # We remove the lines that are out of interest # Apply angle offsets to match robot kinematic chain def emulate_DH_params(x): xt = np.copy(x) xt[0] = xt[0] - np.pi/2 orient = np.mod(np.sum(xt,0)+np.pi, 2*np.pi) - np.pi xt[2] = xt[2] - np.arctan(20.5/51) return xt, orient # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): xt, orient = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ param.l @ np.cos(L @ xt), param.l @ np.sin(L @ xt), #orient ]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot) f[1] += 81 if f.shape[-1] == 1: f = f[:,0] return f # Jacobian with analytical computation (for single time step) def Jkin(xt, param): xt, _ = emulate_DH_params(xt) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) J = np.vstack([ -np.sin(L @ xt).T @ np.diag(param.l) @ L, np.cos(L @ xt).T @ np.diag(param.l) @ L, #np.ones([1,param.nbVarX]) ]) return J def compute_fJ(x, param): # Viapoints residuals f1 = np.zeros(param.nbVarX) f2 = np.zeros(param.nbVarF) f3 = np.zeros(param.nbVarF) # Corresponding Jacobians J1 = np.zeros((param.nbVarX, param.nbVarX)) J2 = np.zeros((param.nbVarF, param.nbVarX)) J3 = np.zeros((param.nbVarF, param.nbVarX)) return f1,f2,f3,J1,J2,J3 # Residual and Jacobian for a viapoints reaching task def f_reach(x, param): f1,f2,f3,J1,J2,J3 = compute_fJ(x, param) f = np.append(np.append(f1, f2), f3) J = np.zeros([param.nbVarX + 2 * param.nbVarF, 3 * param.nbVarX]) J[:param.nbVarX, :param.nbVarX] = J1 J[param.nbVarX:param.nbVarX+param.nbVarF, param.nbVarX:2*param.nbVarX] = J2 J[param.nbVarX+param.nbVarF:, 2*param.nbVarX:] = J3 return f, J # iLQR in batch form def iLQR(x0, u, param): for i in range(param.nbIter): x = Su0 @ u + Sx0 @ x0 # System evolution x = x.reshape([param.nbVarX, param.nbData], order='F') f, J = f_reach(x, param) # Residuals and Jacobians du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten('F') - u * param.r) # Gauss-Newton update # Estimate step size with backtracking line search method alpha = 1 cost0 = f.flatten('F').T @ Q @ f.flatten('F') + np.linalg.norm(u)**2 * param.r # Cost while True: utmp = u + du * alpha xtmp = Su0 @ utmp + Sx0 @ x0 # System evolution xtmp = xtmp.reshape([param.nbVarX, param.nbData], order='F') ftmp, _ = f_reach(xtmp, param) # Residuals cost = ftmp.flatten('F').T @ Q @ ftmp.flatten('F') + np.linalg.norm(utmp)**2 * param.r # Cost if cost < cost0 or alpha < 1e-3: u = utmp #console.log("Iteration {}, cost: {}".format(i,cost)) break alpha /= 2 if np.linalg.norm(du * alpha) < 1E-2: break # Stop iLQR iterations when solution is reached return x, cost def update_iLQR(): global Su, Q, x, cost_el idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)]) Su = Su0[idx.flatten()] # We remove the lines that are out of interest Q[:param.nbVarX, :param.nbVarX] = Q1 Q[param.nbVarX:param.nbVarX+param.nbVarF, param.nbVarX:param.nbVarX+param.nbVarF] = Q2 Q[param.nbVarX+param.nbVarF:param.nbVarX+2*param.nbVarF, param.nbVarX+param.nbVarF:param.nbVarX+2*param.nbVarF] = Q3 x, cost = iLQR(x_vias[:,0], u, param) cost_el.textContent = '%.3f' % cost ######################################################################################### # GUI scaling_factor = 2 # General scaling factor for rendering # Mouse events mouse0 = np.zeros(2) mouse = np.zeros(2) mousedown = 0 move_joint= -1 hover0 = np.zeros(2) #def onMouseMove(event): # global mouse, mouse0 # mouse0[0] = event.offsetX # mouse0[1] = event.offsetY # mouse[0] = (event.offsetX - canvas.width * 0.5) / scaling_factor #(event.offsetX / (window.innerWidth*.5)) * 2 - 1 # mouse[1] = (event.offsetY - canvas.height * 0.5) / scaling_factor #-(event.offsetY / (window.innerHeight*.5)) * 2 + 1 def onMouseMove(event): global mouse, mouse0, hover0, x, active_robot_id, x_vias offset = canvas.getBoundingClientRect() mouse0[0] = (event.clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.clientY - offset.y) * canvas.width / canvas.clientWidth mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x_vias[move_joint, active_robot_id] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onTouchMove(event): global mouse, mouse0, hover0, x, active_robot_id, x_vias bcr = event.target.getBoundingClientRect() mouse0[0] = event.touches.item(0).clientX - bcr.x mouse0[1] = event.touches.item(0).clientY - bcr.y mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x_vias[move_joint, active_robot_id] -= 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, selected_obj, move_joint mousedown = 0 selected_obj = -1 move_joint = -1 update_iLQR() def onWheel(event): global hover_joint, x_vias #if mousedown==1: #document.getElementById('object0_angle').value = str(param.Mu[2,0] + 0.2 * (event.deltaY/106)) if hover_joint >= 0: x_vias[hover_joint, active_robot_id] -= 0.2 * (event.deltaY/106) update_iLQR() cost_el = document.getElementById('cost') debug_el = document.getElementById('debug') 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) # Reset transformation to identity ctx.fillStyle = 'white' ctx.fillRect(0, 0, canvas.width, canvas.height) def draw_ground(): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.beginPath() ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '5' ctx.strokeStyle = '#CCCCCC' ctx.moveTo(-200, 164) ctx.lineTo(200, 164) ctx.stroke() def draw_robot(xt, color1, color2, color3, color4, selectable, robot_id): global hover_joint,active_robot_id ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # Draw base ctx.translate(0, 81) ctx.lineWidth = '1' ctx.strokeStyle = color3 ctx.fillStyle = color1 ctx.fill(base1_svg) ctx.stroke(base1_svg) # Outline ctx.stroke(base2_svg) # Draw seg1 ctx.rotate(xt[0]) ctx.fillStyle = color1 ctx.fill(seg11_svg) ctx.stroke(seg11_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg12_svg, mouse0[0], mouse0[1]): ctx.fillStyle = color4 hover_joint = 0 active_robot_id = robot_id ctx.fill(seg12_svg) ctx.stroke(seg12_svg) ctx.stroke(seg13_svg) ctx.stroke(seg14_svg) ctx.stroke(seg15_svg) # Draw seg2 ctx.translate(0, -79) ctx.rotate(xt[1]) ctx.fillStyle = color1 ctx.fill(seg21_svg) ctx.stroke(seg21_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg22_svg, mouse0[0], mouse0[1]): ctx.fillStyle = color4 hover_joint = 1 active_robot_id = robot_id ctx.fill(seg22_svg) ctx.stroke(seg22_svg) ctx.stroke(seg23_svg) ctx.stroke(seg24_svg) # Draw seg3 ctx.translate(0, -96) ctx.rotate(xt[2]) ctx.fillStyle = color1 ctx.fill(seg31_svg) ctx.stroke(seg31_svg) # Outline ctx.fill(seg32_svg) ctx.stroke(seg32_svg) # Outline ctx.fill(seg33_svg) ctx.stroke(seg33_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg34_svg, mouse0[0], mouse0[1]): ctx.fillStyle = color4 hover_joint = 2 active_robot_id = robot_id ctx.fill(seg34_svg) ctx.stroke(seg34_svg) ctx.stroke(seg35_svg) ctx.stroke(seg36_svg) ctx.stroke(seg37_svg) # Draw end-effector point ctx.translate(-20.5, -51) ctx.beginPath() ctx.arc(0, 0, 2, 0, 2 * np.pi) ctx.fillStyle = color4 ctx.fill() def draw_obj(id, param, color, colortxt): global selected_obj, hover_obj ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.translate(param.Mu[0,id], param.Mu[1,id]) ctx.rotate(param.Mu[2,id]) # Draw object ctx.fillStyle = color obj = Path2D.new() obj.rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) ctx.fill(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_obj = id if ctx.isPointInPath(obj, mouse0[0], mouse0[1]) and mousedown==1: selected_obj = id #debug_el.innerText = str(selected_obj) #ctx.fillRect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) if param.sz[0] > 39 and param.sz[1] > 19: ctx.textAlign = 'center' ctx.textBaseline = 'middle' ctx.font = '10px Permanent Marker' ctx.fillStyle = colortxt ctx.fillText('Move me!', 0, 0) ######################################################################################### def errorHandler(e): msg = 'Error: ' + str(e) console.error(msg) el = document.getElementById('errors') el.innerText = msg #el.textContent = msg ######################################################################################### u = np.zeros(param.nbVarU * (param.nbData-1)) # Initial control commands x, cost = iLQR(x_vias[:,0], u, param) cost_el.textContent = '%.3f' % cost async def main(): global hover_joint, hover_obj, param t0 = 0 t = 0 tf = 0 while True: # time = performance.now() * 0.0005; t0 += simulation_speed if t0 > 19: t0 = 0 if t > param.nbData-2: tf += 1 if tf > 10: #Stay some iterations at the final point before starting again t = 0 tf = 0 else: t += 1 # Reinit hovering variables hover_joint = -1 hover_obj = -1 # Rendering clear_screen() draw_ground() draw_robot(x_vias[:,0], '#EEEEEE', '#DDDDDD', '#CCCCCC', '#BBBBBB', True, 0) # x0 draw_robot(x_vias[:,1], '#FF9933', '#EE8822', '#DD7711', '#CC6600', True, 1) # Viapose draw_obj(0, param, '#FF3399', '#DD1177') draw_obj(1, param, '#33FF99', '#11DD77') draw_robot(x[:,t], '#CCCCCC', '#AAAAAA', '#222222', '#000000', False, -1) # Object selection if selected_obj>= 0: param.Mu[:2,selected_obj] = mouse param.Mu[0,selected_obj] = max(min(param.Mu[0,selected_obj],225), -225) param.Mu[1,selected_obj] = max(min(param.Mu[1,selected_obj],175), -175) param.viapoint1 = x_vias[:,1] param.viapoint2 = param.Mu[:2,0] param.viapoint3 = param.Mu[:2,1] await asyncio.sleep(0.0001) pyscript.run_until_complete(main())