RCFS
General information
About
License
2D sandboxes
Forward kinematics (FK)
Inverse kinematics (IK)
Bimanual robot
Humanoid robot (CoM and coordination matrix)
Iterative linear quadratic regulator (iLQR)
iLQR for car
iLQR for bicopter
Impedance control
3D sandboxes
Inverse kinematics (IK)
Coding exercises
01
Linear algebra in Python
02
Movement primitives and Newton's method
03
Gaussian Distributions
4a
Forward kinematics
4b
Inverse kinematics and nullspace control
5a
Forward dynamics
5b
Inverse dynamics and impedance control
6a
Planning with linear quadratic regulator
6b
Planning in joint space with LQR
07
Iterative linear quadratic regulator (iLQR)
08
Exploration with ergodic control
09
Orientation with Riemannian manifold
Online course
Table of contents
1 Introduction
2 Newton’s method for minimization
2.1 Gauss–Newton algorithm
2.2 Least squares
2.3 Least squares with constraints
3 Forward kinematics (FK) for a planar robot manipulator
4 Inverse kinematics (IK) for a planar robot manipulator
4.1 Numerical estimation of the Jacobian
4.2 Inverse kinematics (IK) with task prioritization
5 Encoding with basis functions
5.1 Univariate trajectories
5.2 Multivariate trajectories
5.3 Multidimensional inputs
5.4 Derivatives
5.5 Concatenated basis functions
5.6 Batch computation of basis functions coefficients
5.7 Recursive computation of basis functions coefficients
6 Linear quadratic tracking (LQT)
6.1 LQT with smoothness cost
6.2 LQT with control primitives
6.3 LQR with a recursive formulation
6.4 LQT with a recursive formulation and an augmented state space
6.5 Least squares formulation of recursive LQR
6.6 Dynamical movement primitives (DMP) reformulated as LQT with control primitives
7 iLQR optimization
7.1 Batch formulation of iLQR
7.2 Recursive formulation of iLQR
7.3 Least squares formulation of recursive iLQR
7.4 Updates by considering step sizes
7.5 iLQR with quadratic cost on f(x_{t})
7.6 iLQR with control primitives
7.7 iLQR for spacetime optimization
7.8 iLQR with offdiagonal elements in the precision matrix
7.9 Car steering
7.10 Bicopter
8 Forward dynamics (FD) for a planar robot manipulator
8.1 Robot manipulator with dynamics equation
References
A System dynamics at trajectory level
B Derivation of motion equation for a planar robot
C Linear systems used in the bimanual tennis serve example
D Equivalence between LQT and LQR with augmented state space
# Precision matrix Q = np.eye(param.nbVarF * param.nbPoints) # # Consider only position tracking (no orientation) # Q = np.kron(np.eye(param.nbPoints), np.diag([1, 1, 0])) # # Consider only last keypoint # Q = np.kron(np.diag([0, 1]), np.eye(3)) update_iLQR()
(click on the green run button to run the code; objects and joints can be moved with the mouse)
Object1 orientation
0.0
Object2 orientation
0.0
Objects width
50.0
Objects height
30.0
Simulation speed
10.0
Cost
Bounding box
from pyodide.ffi import create_proxy from js import Path2D, document, console import numpy as np import asyncio objects_angle = [document.getElementById('object0_angle'), document.getElementById('object1_angle')] # Objects angle objects_size = [document.getElementById('objects_width'), document.getElementById('objects_height')] # Objects size simulation_speed = document.getElementById('simulation_speed') # Simulation speed #track_orientation = document.getElementById('track_orientation') bounding_box = document.getElementById('use_boundingbox') 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') # Logarithmic map for R^2 x S^1 manifold def logmap(f, f0): position_error = f[:2,:] - f0[:2,:] orientation_error = np.imag(np.log(np.exp(f0[-1,:]*1j).conj().T * np.exp(f[-1,:]*1j).T)).conj() diff = np.vstack([position_error, orientation_error]) return diff # 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 return f # Forward kinematics for all joints (in robot coordinate system) def fkin0(x, param): xt, _ = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ L @ np.diag(param.l) @ np.cos(L @ xt), L @ np.diag(param.l) @ np.sin(L @ xt) ]) f = np.hstack([np.zeros([2,1]), f]) f[1] += 81 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 # Residual and Jacobian for a viapoints reaching task (in object coordinate system) def f_reach(x, param): f = logmap(fkin(x, param), param.Mu) J = np.zeros([param.nbPoints * param.nbVarF, param.nbPoints * param.nbVarX]) for t in range(param.nbPoints): f[:2,t] = param.A[:,:,t].T @ f[:2,t] # Object oriented residual Jtmp = Jkin(x[:,t], param) Jtmp[:2] = param.A[:,:,t].T @ Jtmp[:2] # Object centered Jacobian if param.useBoundingBox: for i in range(2): if abs(f[i,t]) < param.sz[i]/2: f[i,t] = 0 Jtmp[i] = 0 else: f[i,t] -= np.sign(f[i,t]) * param.sz[i]/2 J[t*param.nbVarF:(t+1)*param.nbVarF, t*param.nbVarX:(t+1)*param.nbVarX] = Jtmp 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[:,tl], 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[:,tl], 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 param, Q, x, cost_el # console.log("iLQR required") param.useBoundingBox = bounding_box.checked param.sz = [float(objects_size[0].value), float(objects_size[1].value)] for i in range(param.nbPoints): #obj[i].rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) param.Mu[2,i] = float(objects_angle[i].value) param.A[:,:,i] = np.asarray([ [np.cos(param.Mu[2,i]), -np.sin(param.Mu[2,i])], [np.sin(param.Mu[2,i]), np.cos(param.Mu[2,i])] ]) # if track_orientation.checked: # Q = np.identity(param.nbVarF * param.nbPoints) # else: # Q = np.kron(np.identity(param.nbPoints), np.diag([1, 1, 0])) x, cost = iLQR(x0, u, param) cost_el.textContent = '%.3f' % cost ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.dt = 1e-2 # Time step length param.nbData = 50 # Number of datapoints param.nbIter = 20 # Maximum number of iterations for iLQR param.nbPoints = 2 # Number of viapoints param.nbVarX = 3 # State space dimension (x1,x2,x3) param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3) param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation) param.l = [79., 96., 55.] # Robot links lengths param.sz = [float(objects_size[0].value), float(objects_size[1].value)] # Size of objects param.r = 1E-6 # Control weight term param.Mu = np.array([[100., 0, float(objects_angle[0].value)], [100., 100., float(objects_angle[1].value)]]).T # Viapoints param.A = np.zeros([2, 2, param.nbPoints]) # Object orientation matrices param.useBoundingBox = False # Consider bounding boxes for reaching cost # Object rotation matrices #obj = [Path2D.new(), Path2D.new()] for t in range(param.nbPoints): # obj[t].rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) orn_t = param.Mu[-1,t] param.A[:,:,t] = np.asarray([ [np.cos(orn_t), np.sin(orn_t)], [-np.sin(orn_t), np.cos(orn_t)] ]) # Precision matrix Q = np.identity(param.nbVarF * param.nbPoints) # Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * param.r # Time occurrence of viapoints tl = np.linspace(0, param.nbData, param.nbPoints+1) tl = np.rint(tl[1:]).astype(np.int64) - 1 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 ######################################################################################### # 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, hover0, x0 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: x0[move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onTouchMove(event): global mouse, mouse0, hover0, x0 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: x0[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, selected_obj, move_joint mousedown = 0 selected_obj = -1 move_joint = -1 update_iLQR() def onWheel(event): global hover_joint, hover_obj, x0, objects_angle #if mousedown==1: #document.getElementById('object0_angle').value = str(param.Mu[2,0] + 0.2 * (event.deltaY/106)) if hover_obj >= 0: objects_angle[hover_obj].value = float(objects_angle[hover_obj].value) + 0.2 * (event.deltaY/106) update_iLQR() if hover_joint >= 0: x0[hover_joint] -= 0.2 * (event.deltaY/106) update_iLQR() cost_el = document.getElementById('cost') 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): global hover_joint 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 = '#3399FF' hover_joint = 0 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 = '#FF9933' hover_joint = 1 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=='True' and ctx.isPointInPath(seg34_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#99FF33' hover_joint = 2 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() # # Draw skeleton of the kinematic chain # ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # ctx.lineCap = 'round' # ctx.lineJoin = 'round' # ctx.lineWidth = '2' # ctx.strokeStyle = '#FF8888' # f = fkin0(x, param) # ctx.beginPath() # ctx.moveTo(0, 81) # for i in range(param.nbVarX+1): # ctx.lineTo(f[0,i], f[1,i]) # ctx.stroke() 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 #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) ######################################################################################### x0 = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial robot state u = np.zeros(param.nbVarU * (param.nbData-1)) # Initial control commands x, cost = iLQR(x0, 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 += float(simulation_speed.value) 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(x0, '#EEEEEE', '#DDDDDD', '#CCCCCC', '#BBBBBB', True) draw_obj(0, param, '#FF3399', '#DD1177') draw_obj(1, param, '#33FF99', '#11DD77') draw_robot(x[:,t], '#CCCCCC', '#AAAAAA', '#222222', '#000000', False) # draw_obj(0, 'rgba(255,51,153,0.5)', 'rgba(221,17,119,0.5)') # draw_obj(1, 'rgba(51,255,153,0.5)', 'rgba(17,221,119,0.5)') # 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) await asyncio.sleep(0.0001) pyscript.run_until_complete(main())