30:T80c,function planProgressBar({w=width, h=50, plotFraction=false, colorProximity=false, maxSteps=300}={}) { console.log(`planProgressBar: plotFraction ${plotFraction} colorProximity ${colorProximity}`); const svg = d3.create("svg") .attr("width", w) .attr("height", h) .attr("viewBox", [0, 0, w, h]) .attr("stroke", "black"); const X = d3.scaleLinear().domain([0,maxSteps]).range([0,w-1]); const Y = d3.scaleLinear().domain([0,1]).range([h-1,0]); const gProxs = svg.append("g"); const gFracs = svg.append("g"); const fracs = Array(maxSteps).fill(NaN); const proxs = Array(maxSteps).fill("lightgray"); const rectWidth = X(1)-X(0); // Progress bar colored by min proximity after each step. gProxs.selectAll("rect") .data(proxs) .join("rect") .attr("x", (_,i) => X(i)) .attr("y", () => 0) .attr("width", (_,i) => rectWidth) .attr("height", h) .attr("stroke", "lightgray") .attr("fill", "lightgray"); if(plotFraction) { // Graph of fraction of robots on target after each step. gFracs.append("path") .attr("fill", "none") .attr("stroke", "black"); } // border svg.append("g") .append("rect") .attr("x", 0).attr("y", 0) .attr("width", w).attr("height", h) .attr("fill", "none") .attr("stroke-width", 2) .attr("stroke", "gray"); // Return a dynamic SVG node with an update method. return Object.assign(svg.node(), { update(P) { if(plotFraction) { fracs[P.step] = P.fracOnTarget; gFracs.select("path").attr("d", d => { const p = d3.path(); p.moveTo(X(0), Y(0)); for(let istep=0; istep d) .attr("fill", d => d); } }); }31:T110d,function* movePlanner(layout, { maxSteps=300, tolT=0.1, tolP=0.1, linearRadius=10, linearSlope=3.0, retract=false, retractDTDP=0.5, tuckP=130, anticollision=false, maxAntiCollisionMove=5 /*deg*/, traverseSlope=3.0, beta=0.5, /*momentum*/ }={}) { console.log(`movePlanner: maxSteps ${maxSteps} rectract ${retract} anticollision ${anticollision}`); const {min,max,exp,abs,sqrt,sign} = Math; const fmt = d3.format(".1f"); let gradT = 0, gradP = 0; // Initialize the tracking state for each robot. layout.robots.forEach(robot => { const rotateT = retractDTDP * (tuckP - robot.P); const T1 = min(robot.maxT, max(robot.minT, robot.T - rotateT)); const T2 = robot.goalT == null ? null : min(robot.maxT, max(robot.minT, robot.goalT - rotateT)); robot.state = {momT: 0, momP:0, T1, T2, spin:null}; }); for(let nSteps = 0; nSteps < maxSteps; nSteps++) { const newAngles = []; const t = nSteps / (maxSteps - 1); // Calculate the proximities of all robots at the start of this step. const proximities = layout.getProximity(); const minProximity = d3.min([...proximities.values()], d=>d.proximity.value); let numOnTarget = 0; layout.robots.forEach(robot => { const {id,T,P,goalT,goalP,state} = robot; // Skip any robots that do not have a new goal. if(goalT == null || goalP == null) return; // Count robots on target. Note that on-target robots might later // move off target due to a nearby neighbor. if(abs(T-goalT) < tolT && abs(P-goalP) < tolP) numOnTarget++; // Select the target angles for this step. let targetT, targetP; if(t < 1/3 && retract) { targetT = state.T1, targetP = tuckP; } else if(t < 2/3 && retract) { targetT = state.T2, targetP = tuckP; } else { targetT = goalT, targetP = goalP; } // Calculate the gradient pulling this robot towards its goal. const dT = T - targetT, dP = P - targetP; const dR = sqrt(dT*dT+dP*dP); if(dR > 1e-3) { const gradNorm = (dR >= linearRadius) ? linearSlope : linearSlope/linearRadius*dR; gradT = gradNorm * dT / dR; gradP = gradNorm * dP / dR; } else { gradT = gradP = 0; } // Add the tucking gradient, which turns off smoothly at late times and for large P. //const tuckSigmoid = 1 / (1 + exp(50 * (t - tuckDuration))) / (1 + exp(2 * (P - tuckP))); //gradP = -tuckSlope * tuckSigmoid + (1 - tuckSigmoid) * gradP; if(anticollision) { // Implement just-in-time collision avoidance. const {dT:proxdT, dP:proxdP, prox0} = proximityGradient( id, layout, {proximities, maxMove:maxAntiCollisionMove}); const dotProd = proxdT * gradT + proxdP * gradP; if(dotProd > 0) { // We are within 2*bufferDistance of another robot. if(state.spin === null) { state.spin = sign(-proxdT); console.log(`${id} new collision: set spin ${state.spin}`); } const scale = state.spin * traverseSlope / sqrt(proxdT*proxdT + proxdP*proxdP); let traverseT = +proxdP * scale, traverseP = -proxdT * scale; if(!robot.inBounds(T + traverseT, P + traverseP)) { // We have traversed into a hard stop, so reverse the spin. state.spin *= -1, traverseT *= -1, traverseP *= -1; console.log(`${id} reversing spin`); } gradT = -proxdT - traverseT; gradP = -proxdP - traverseP; } } if(nSteps > 0) { // Apply momentum corrections. gradT = beta * state.momT + (1-beta) * gradT; gradP = beta * state.momP + (1-beta) * gradP; } // Update momentum. state.momT = gradT; state.momP = gradP; // Calculate new angles after this step, with limits applied. // TODO: move limits into Robot setter? const newT = min(robot.maxT, max(robot.minT, T-gradT)); const newP = min(robot.maxP, max(robot.minP, P-gradP)); newAngles.push({id, T:newT, P:newP}); }); // Finish early if there are no more updates required. if(newAngles.length == 0) break; //console.log(nSteps, newAngles); yield {step:nSteps, angles:newAngles, minProximity, fracOnTarget:numOnTarget/layout.robots.length}; } }32:T1068,function robotAnglesPlot(id0, layout, { w=width/2, nT=60, nP=30, contours=true, gradient=true, label=true}={}) { const {round,floor} = Math; if(!layout.index.has(id0)) throw new Error(`No such id ${id0} in this layout.`); const robot0 = layout.index.get(id0); const {proxT, proxP} = proximityGrid(id0, layout, {nT, nP}); const h = w / 2; const svg = d3.create("svg") .attr("width", w) .attr("height", h) .attr("viewBox", [0, 0, w, h]); // Angles to pixels. const T2x = d3.scaleLinear().domain([robot0.minT,robot0.maxT]).range([0,w]); const P2y = d3.scaleLinear().domain([robot0.minP,robot0.maxP]).range([h,0]); const xAxisTop = g => g .call(d3.axisBottom(T2x).ticks(w / h * 10)) .call(g => g.selectAll(".tick").filter(d => T2x.domain().includes(d)).remove()); const xAxisBottom = g => g .attr("transform", `translate(0,${h})`) .call(d3.axisTop(T2x).ticks(w / h * 10)) .call(g => g.selectAll(".tick").filter(d => T2x.domain().includes(d)).remove()); const yAxisLeft = g => g .call(d3.axisRight(P2y)) .call(g => g.selectAll(".tick").filter(d => P2y.domain().includes(d)).remove()); const yAxisRight = g => g .attr("transform", `translate(${w},0)`) .call(d3.axisLeft(P2y)) .call(g => g.selectAll(".tick").filter(d => P2y.domain().includes(d)).remove()); if(contours) { // Convert from grid indices to screen coords. const grid2x = d3.scaleLinear().domain([0,nT]).range([-1,w+1]); const grid2y = d3.scaleLinear().domain([0,nP]).range([h+1,-1]); const grid2xy = ({type, value, coordinates:gridCoords}) => { const xyCoords = gridCoords.map( rings => rings.map( points => points.map( ([ix,iy]) => ([grid2x(ix),grid2y(iy)])))); return {type, value, coordinates:xyCoords}; }; // Calculate and draw theta and phi contours. const contourF = d3.contours().size([nT,nP]).thresholds([2*bufferDistance,0]); const contourT = contourF(proxT).map(grid2xy); const contourP = contourF(proxP).map(grid2xy); const P = d3.geoPath(); const gT = svg.append("g"); gT.selectAll("path") .data(contourT) .join("path") .attr("fill", (_,i) => i==0 ? "none":"red") .attr("stroke", (_,i) => i==0 ? "black":"red") .attr("stroke-dasharray", (_,i) => i==0 ? [1,2]:[]) .attr("fill-opacity", (_,i) => i==0 ? 1:0.2) .attr("d", (d,i) => P(d)); const gP = svg.append("g"); gP.selectAll("path") .data(contourP) .join("path") .attr("fill", (_,i) => i==0 ? "none":"red") .attr("stroke", (_,i) => i==0 ? "black":"red") .attr("stroke-dasharray", (_,i) => i==0 ? [1,2]:[]) .attr("fill-opacity", (_,i) => i==0 ? 1:0.2) .attr("d", (d,i) => P(d)); } const gPos = svg.append("g") .append("circle") .attr("fill", "none") .attr("stroke", "blue") .attr("stroke-width", 2) .attr("cx", T2x(robot0.T)) .attr("cy", P2y(robot0.P)) .attr("r", w/100); if(robot0.goalT !== null && robot0.goalP !== null) { const gGoal = svg.append("g") .append("circle") .attr("fill", "none") .attr("stroke", "lightseagreen") .attr("stroke-width", 1) .attr("cx", T2x(robot0.goalT)) .attr("cy", P2y(robot0.goalP)) .attr("r", w/100); } if(gradient) { const {dT, dP} = proximityGradient(id0, layout); if(dT != 0 || dP != 0) { const x1 = T2x(robot0.T), y1 = P2y(robot0.P); const x2 = T2x(robot0.T + dT), y2 = P2y(robot0.P + dP); const gGrad = svg.append("g") .append("line") .attr("x1", x1) .attr("y1", y1) .attr("x2", x2) .attr("y2", y2) .attr("stroke", "red"); } } if(label) { const labelText = (label === true) ? id0 : label; const inset=30, size=16; svg.append("text").attr("x", inset).attr("y", inset) .attr("font-size", size).attr("alignment-baseline", "top") .attr("fill", "blue").text(labelText); } svg.append("g").call(xAxisTop); svg.append("g").call(xAxisBottom); svg.append("g").call(yAxisLeft); svg.append("g").call(yAxisRight); return svg.node(); }33:T6bd,function proximityGradient(id0, layout, { maxMove=5.0, /*max allowed norm of (dT,dP) in deg*/ stepT=0.5, stepP=0.5, /*step size for finite-diff partial derivatives in deg*/ proximities=null}={}) { const {sqrt} = Math; if(!layout.index.has(id0)) throw new Error(`Requested id ${id0} not in this layout.`); // Get this robot's current angles. const {T:T0,P:P0} = layout.index.get(id0); // Get the current proximities if needed. if(proximities===null) proximities = layout.getProximity(); const prox0 = proximities.get(id0).proximity.value; // No need to estimate partial derivatives if we are already outside the buffer zone. if(prox0 > 0) return { id0, dT:0, dP:0, T0, P0, prox0 }; // Estimate partial derivatives using first-order finite differences. // TODO: check if this works when T+-dT falls outside [-180,180] etc const outputIds = [id0]; let gradT = 0, gradP = 0; for(const sign of [+1,-1]) { const T = T0 + sign * stepT; const delta = layout.getProximity({angles:[{id:id0,T,P:P0}], outputIds}).get(id0); gradT += sign * delta.proximity.value; } gradT /= (2 * stepT); for(const sign of [+1,-1]) { const P = P0 + sign * stepP; const delta = layout.getProximity({angles:[{id:id0,T:T0,P}], outputIds}).get(id0); gradP += sign * delta.proximity.value; } gradP /= (2 * stepP); // The maximum gradNorm should be 6mm * pi/180 ~ 0.1 mm/deg. const gradNormSq = gradT*gradT + gradP*gradP; const scale = -prox0 / gradNormSq; let dT = scale * gradT, dP = scale * gradP; const norm = dT*dT + dP*dP; if(norm > maxMove*maxMove) { const scale = maxMove / sqrt(norm); dT *= scale; dP *= scale; } return { id0, dT, dP, T0, P0, prox0 }; }34:T1ea5,function robotModel(layout, { w=width, labels=true, buffers=true, labelSizeMM=1.5, labelColor = (id) => "cyan" }={}) { const {min,max,round,cos,sin,PI} = Math; const deg2rad = PI/180; // Use the requested width and scale the height to the coordinate extent. const {xlo,xhi,ylo,yhi} = layout.extent; const h = round(w*(yhi-ylo)/(xhi-xlo)); const X = d3.scaleLinear().domain([xlo,xhi]).range([0,w-1]); const Y = d3.scaleLinear().domain([ylo,yhi]).range([h-1,0]); const X0 = X(0), Y0 = Y(0); const labelSize = X(xlo + labelSizeMM) - X(xlo); // equivalent in pixels // Helper function to draw a keepout polygon. function drawPoly(P) { const p = d3.path(); d3.transpose([P.x,P.y]).forEach(([x,y],i) => { if(i) p.lineTo(X(x),Y(y)); else p.moveTo(X(x),Y(y)); }); p.closePath(); return p; } const armLineWidth = max(1, min(4, X(0.2)-X0)); const goalRadius = min(w / 150, X(0.5)-X0); // Build the SVG plot. console.log("initializing svg..."); const svg = d3.create("svg") .attr("width", w) .attr("height", h) .attr("viewBox", [0, 0, w, h]); const g = svg.append("g") .attr("fill", "lightgray") .attr("stroke", "black"); const gg = svg.append("g") .attr("fill", "none") .attr("stroke", "lightseagreen") .attr("stroke-width", 1); //.attr("stroke-dasharray", [2,2]); // Draw the robots. g.selectAll("g.robot") .data(layout.robots, d=>d.id) .join("g") .classed("robot", true) // Translate to each robots (x,y) and theta origins. .attr("transform", d => `translate(${X(d.x)-X0},${Y(d.y)-Y0}) rotate(${-d.T0},${X0},${Y0})`) .call(R => { R.append("g") .classed("theta", true) // Rotate to this robot's current internal theta. .attr("transform", d => `rotate(${-d.T},${X0},${Y0})`) .call(S => { S.append("path") .classed("theta", true) .attr("d", drawPoly(physicalKeepouts.theta)); if(buffers) { S.append("path") .attr("fill", "none") .attr("stroke-dasharray", [1,2]) .attr("d", drawPoly(bufferedKeepouts.theta)); } const phi=S.append("g") // Translate to this robot's phi arm axis. .attr("transform", d => `translate(${X(d.r1)-X0},0)`) .append("g") .classed("phi", true) // Rotate to this robot's current external phi. .attr("transform", d => `rotate(${-(d.P+d.P0)},${X0},${Y0})`); phi.append("path") .classed("phi", true) .attr("d", drawPoly(physicalKeepouts.phi)); if(buffers) { phi.append("path") .attr("fill", "none") .attr("stroke-dasharray", [1,2]) .attr("d", drawPoly(bufferedKeepouts.phi)); } phi.append("line") .attr("stroke-width", armLineWidth) .attr("stroke-linecap", "round") .attr("x1", X0) .attr("y1", Y0) .attr("x2", d => X(d.r2)) .attr("y2", d => Y0); S.append("line") .attr("stroke-width", armLineWidth) .attr("stroke-linecap", "round") .attr("x1", X0) .attr("y1", Y0) .attr("x2", d => X(d.r1)) .attr("y2", d => Y0); }); // Draw theta stop region relative to robot's (x,y) and theta origin. R.append("path") .attr("stroke", "blue") .attr("fill", "none") .attr("d", d => { const p = d3.path(); p.moveTo(X(-2*d.cosTstop), Y(2*d.sinTstop)); p.lineTo(X0, Y0); p.lineTo(X(-2*d.cosTstop), Y(-2*d.sinTstop)); return p; }); if(labels) { // Display an ID label for this robot. R.append("text") .attr("transform", d => `rotate(${d.T0},${X0},${Y0})`) .attr("x", X0) .attr("y", Y0) .attr("stroke", "black") .attr("fill", d => labelColor(d.id)) .attr("font-size", labelSize) .attr("font-weight", "bold") .attr("opacity", 0.3) .attr("text-anchor", "middle") .attr("dominant-baseline", "middle") .text(d => d.id); } }); // Update the angles with minimal changes to the DOM. function _setAngles(angles) { g.selectAll("g.theta") .data(angles, d=>d.id) .join( enter => { if(enter.size()) { const newIDs = enter.nodes().map(n=>n.__data__.id); console.log("Ignoring angles for new robots:", newIDs); } }, update => { update.attr("transform", d => `rotate(${-d.T},${X0},${Y0})`); }, exit => { } ); g.selectAll("g.phi") .data(angles, d=>d.id) .join( enter => { }, update => { update.attr("transform", d => `rotate(${-(d.P+d.P0)},${X0},${Y0})`); }, exit => { } ); } // Update the keepout polygon colors based on their proximity values. function _setProximityColors(proximities) { proximities = [...proximities.values()]; g.selectAll("path.theta") .data(proximities, d=>d.id) .join( enter => { }, update => { update.attr("fill", d => proximityColor(d.proximity.theta)); }, exit => { } ); g.selectAll("path.phi") .data(proximities, d=>d.id) .join( enter => { }, update => { update.attr("fill", d => proximityColor(d.proximity.phi)); }, exit => { } ); } const proximities = layout.getProximity(); _setProximityColors(proximities); // Update the goal angles. function _setGoals(goals) { goals = goals.filter(d => d.goalT !== null && d.goalP !== null); const goalT = goals.map(R => (R.goalT+R.T0)*deg2rad); const goalTP = goals.map(R => (R.goalT+R.T0+R.goalP+R.P0)*deg2rad); const cosT = goalT.map(T => cos(T)), sinT = goalT.map(T => sin(T)); const cosTP = goalTP.map(TP => cos(TP)), sinTP = goalTP.map(TP => sin(TP)); gg.selectAll("circle") .data(goals, d=>d.id) .join("circle") .attr("cx", (d,i) => X(d.x + d.r1 * cosT[i] + d.r2 * cosTP[i])) .attr("cy", (d,i) => Y(d.y + d.r1 * sinT[i] + d.r2 * sinTP[i])) .attr("r", goalRadius); gg.selectAll("path") .data(goals, d=>d.id) .join("path") .attr("d", (d,i) => { const p = d3.path(); p.moveTo(X(d.x), Y(d.y)); p.lineTo(X(d.x + d.r1 * cosT[i]), Y(d.y + d.r1 * sinT[i])); p.lineTo(X(d.x + d.r1 * cosT[i] + d.r2 * cosTP[i]), Y(d.y + d.r1 * sinT[i] + d.r2 * sinTP[i])); return p; }); } _setGoals(layout.robots); const model = Object.assign(svg.node(), { value: layout, setAngles(angles) { angles.forEach(({id,T,P}) => { if(this.value.index.has(id)) { const robot = this.value.index.get(id); robot.T = T; robot.P = P; //console.log(`robotModel: set T=${T} P=${P} for ${id}`); } }); _setAngles(this.value.robots); const proximities = this.value.getProximity(); _setProximityColors(proximities); model.dispatchEvent(new Event("input", {bubble:true})); }, setGoals(goals) { goals.forEach(({id,T,P}) => { if(this.value.index.has(id)) { const robot = this.value.index.get(id); robot.goalT = T; robot.goalP = P; } }); _setGoals(this.value.robots); model.dispatchEvent(new Event("input", {bubble:true})); } }); return model; }35:T15cc,class Layout { constructor(robots, {maxNeighborDist=15, extentPad=7.1, rng=null}={}) { this.robots = robots; this.rng = rng; // Index robots by id (and check they are unique). this.index = d3.index(robots, d=>d.id); // Calculate the (x,y) coordinate extent of this layout. const pad = 7.1; this.extent = { xlo: d3.min(robots, d=>d.x-extentPad), xhi: d3.max(robots, d=>d.x+extentPad), ylo: d3.min(robots, d=>d.y-extentPad), yhi: d3.max(robots, d=>d.y+extentPad) }; // Identify neighbors of each robot. this.neighborPairs = []; const rsqMax = maxNeighborDist**2; const nrobots = robots.length; for(let i1=1; i1 rsqMax) continue; this.neighborPairs.push([robot2.id, robot1.id]); robot1.neighbors = robot1?.neighbors ?? []; robot2.neighbors = robot2?.neighbors ?? []; robot1.neighbors.push(robot2.id); robot2.neighbors.push(robot1.id); } } } getInput(ids=null) { if(ids === null) ids = [...this.index.keys()]; else ids = ids.filter(id => this.index.has(id)); return view`

${['angles', ids.map(id => this.index.get(id).getInput())]}`; } randomizeGoals({minProximity=0, maxTries=200}={}) { // Generate random (x,y) targets uniformly over the range of each robot. const goals = this.robots.map(robot => robot.randomizeGoal(this.rng)); const goalsIndex = d3.index(goals, R => R.id); // Calculate proximities for these random targets. const proximities = this.getProximity({angles:goals}); let nTries = 0; while(nTries < maxTries) { nTries++; // Find the most collided robot. const worstId = d3.least([...proximities.values()], (a,b) => a.proximity.value - b.proximity.value).id; const proxMin = proximities.get(worstId).proximity.value; if(proxMin >= minProximity) return goals; // Re-randomize the goal for the most collided robot. const {T,P} = this.index.get(worstId).randomizeGoal(this.rng); Object.assign(goalsIndex.get(worstId), {T,P}); // Update proximities after changing this single robot. const outputIds = [worstId, ...this.index.get(worstId).neighbors]; const update = this.getProximity({angles:goals, outputIds}); update.forEach(({id,proximity}) => proximities.get(id).proximity = proximity); } throw new Error(`randomizeGoals: failed after ${maxTries} tries.`); } getProximity({angles=[], outputIds=null, returnKeepouts=false}={}) { // Calculate the proximities of each robot's arms to all other robot arms // in this layout. Uses the current angles (T,P) for each robot, by default, // but any values {id,T,P} in the angles array will override these. // Returns an array of {id,T,P,proximity} values for all robots in the // layout, or only for robots specified in outputIds. const {min} = Math; const BIG = Number.MAX_VALUE; // Create a map of angles to use for each robot, indexed by id. angles = angles?.filter(({id}) => this.index.has(id)) ?? []; angles = new Map([ ...d3.index(this.robots.map(({id,T,P}) => ({id,T,P})), R => R.id), ...d3.index(angles, R => R.id)]); // Initialize outputs. const outputs = new Map(), keepoutIds = new Set(); if(outputIds === null) { for(const id of this.index.keys()) { const {T,P} = angles.get(id); outputs.set(id, {id,T,P,proximity:{theta:BIG, phi:BIG}}); keepoutIds.add(id); } } else { // Nothing to do if no changes specified. for(const id of outputIds) { if(!this.index.has(id)) continue; const {T,P} = angles.get(id); outputs.set(id, {id,T,P,proximity:{theta:BIG, phi:BIG}}); keepoutIds.add(id); // We also need keepouts for each out robot's neighbors. this.index.get(id).neighbors.forEach(id2 => keepoutIds.add(id2)); } } // Precompute the keepouts for each robot used in the calculation. const keepouts = new Map(); [...keepoutIds].forEach(id => { const {T,P} = angles.get(id); keepouts.set(id, this.index.get(id).keepouts(T, P)); }); // Loop over neighboring pairs of robots in the full layout. for(const [id1, id2] of this.neighborPairs) { const out1 = outputs.get(id1), out2 = outputs.get(id2); if(!out1 && !out2) continue; // Lookup keepouts for this pair. const {theta:TK1,phi:PK1} = keepouts.get(id1); const {theta:TK2,phi:PK2} = keepouts.get(id2); // Calculate proximities between each pair of arms. const T1P2 = collisionDistance(TK1, PK2); const P1T2 = collisionDistance(PK1, TK2); const P1P2 = collisionDistance(PK1, PK2); if(out1) { out1.proximity.theta = min(out1.proximity.theta, T1P2.distance); out1.proximity.phi = min(out1.proximity.phi, P1T2.distance, P1P2.distance); } if(out2) { out2.proximity.theta = min(out2.proximity.theta, P1T2.distance); out2.proximity.phi = min(out2.proximity.phi, T1P2.distance, P1P2.distance); } } // Use the min proximity in theta and phi as the overall proximity value. outputs.forEach(({id,proximity}) => { proximity.value = min(proximity.theta, proximity.phi); if(returnKeepouts) proximity.keepouts = keepouts.get(id); }); return outputs; } }36:Td77,class Robot { static _count = 0; static idFormat = d3.format("04d"); static angleFormat = d3.format(".1f"); static nextId = () => `M${Robot.idFormat(Robot._count)}`; constructor({id, x=0, y=0, r1=3, r2=3, T=0, P=150, T0=0, P0=0, rangeT=390, rangeP=190, goalT=null, goalP=null}={}) { id = id || Robot.nextId(); Object.assign(this, {id,x,y,r1,r2,T,P,T0,P0,rangeT,rangeP,goalT,goalP}); this.minT = -rangeT / 2, this.maxT = +rangeT / 2; this.minP = 185 - rangeP, this.maxP = 185; // Precompute cos,sin of theta-stop angle. const {cos,sin,PI} = Math; const Tstop = (rangeT - 360)/2 * Math.PI/180; this.cosTstop = cos(Tstop), this.sinTstop = sin(Tstop); // Initialize keepout cache. this.lastT = this.lastP = null; Robot._count++; } inBounds(T, P) { return (T >= this.minT) && (T <= this.maxT) && (P >= this.minP) && (P <= this.maxP); } keepouts(T, P, {buffered=true}={}) { // Return the keepout polygons for the specified angles. // Only the buffered keepouts are cached. if(T != this.lastT || P != this.lastP || !buffered) { const {sin,cos,PI} = Math, deg2rad = PI / 180; const t = (T + this.T0) * deg2rad, tp = (T + this.T0 + P + this.P0) * deg2rad; const cost = cos(t), sint = sin(t), costp = cos(tp), sintp = sin(tp); const {x:xT, y:yT, r1} = this; const polys = buffered ? bufferedKeepouts : physicalKeepouts; const {x:xpolyT, y:ypolyT} = polys.theta; const theta = { x: xpolyT.map((x,i) => xT + x * cost - ypolyT[i] * sint), y: ypolyT.map((y,i) => yT + xpolyT[i] * sint + y * cost), length: xpolyT.length }; const xP = xT + r1 * cost, yP = yT + r1 * sint; const {x:xpolyP, y:ypolyP} = polys.phi; const phi = { x: xpolyP.map((x,i) => xP + x * costp - ypolyP[i] * sintp), y: ypolyP.map((y,i) => yP + xpolyP[i] * sintp + y * costp), length: xpolyP.length }; this._keepouts = { theta, phi }; this.lastT = T, this.lastP = P; } return this._keepouts; } getInput(step=0.1) { return view`

${["_id", Inputs.input(this.id)]} ${["T", Inputs.range([this.minT,this.maxT],{value:this.T, step:step, label:`${this.id}-T`})]} ${["P", Inputs.range([this.minP,this.maxP],{value:this.P, step:step, label:`${this.id}-P`})]} `; } randomizeGoal(rng=null) { const {sqrt,cos,sin,PI,acos,atan2,max} = Math; const random = (rng === null) ? d3.randomUniform(0,1) : d3.randomUniform.source(rng)(0,1); const {id,r1,r2} = this, r12 = r1 + r2; // Generate a random point uniformly distributed within a circle of radius r1+r2. const goalR = r12 * sqrt(random()), goalPhi = 2 * PI * random(); const goalX = goalR * cos(goalPhi), goalY = goalR * sin(goalPhi); // Convert to angles. const Rsq = goalR * goalR, r1sq = r1 * r1, r2sq = r2 * r2; const s = (Rsq - r2sq + r1sq) / (2 * goalR); const a = sqrt(max(0, r1sq - s * s)); const beta = atan2(a, s); // Calculate T in the range (-180,+180). let goalT = (goalPhi - beta) * 180 / PI; if(goalT > 180) goalT -= 360; // Calculate P in the range (0,180). const goalP = acos((Rsq - r1sq - r2sq) / (2 * r1 * r2)) * 180 / PI; return {id, T:goalT, P:goalP}; } toString() { return `${this.id}(T=${Robot.angleFormat(this.T)},P=${Robot.angleFormat(this.P)})`; } }37:[] 2e:{"title":"DESI Robots","cells":[{"id":0,"value":"# DESI Robots\n\nVisualization and calculations for the 5000 robots that position optical fibers to capture light from distant objects at the focal plane of the [Dark Energy Spectroscopic Instrument](https://www.desi.lbl.gov/) (DESI).\n\nEach robot is represented by a pair of polygons and has two angular degrees of freedom. Adjust these angles for the pair of robots below to see how they move. Notice how each robot's polygons are colored to indicate proximity to the other robot's polygons. The real robots cannot pass through each other, so their paths must be planned to avoid any collisions.\n\nTry setting new target goals using the button below, then manually guiding the central robots to their targets (indicated with dashed lines).","pinned":false,"hidden":false,"mode":"md"},{"id":551,"value":"viewof newGoals = Inputs.button(\"Set New Goals\");","pinned":false,"hidden":false,"mode":"ojs"},{"id":985,"value":"viewof sliders = layout.getInput([\"M0004\",\"M0005\"])","pinned":false,"hidden":false,"mode":"ojs"},{"id":138,"value":"viewof model = robotModel(layout, {labels:true, buffers:true})","pinned":false,"hidden":false,"mode":"ojs"},{"id":1017,"value":"md`---`","pinned":false,"hidden":false,"mode":"ojs"},{"id":1663,"value":"model","pinned":true,"hidden":false,"mode":"ojs"},{"id":527,"value":"{\n if(newGoals) {\n const start = new Date();\n const goals = layout.randomizeGoals({maxTries:1000});\n const elapsed = new Date() - start;\n const fmt = d3.format(\".1f\");\n console.log(`randomizeGoals took ${fmt(elapsed)} ms`);\n viewof model.setGoals(goals);\n }\n return md`Ready to set new goals...`;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":96,"value":"{\n viewof model.setAngles(sliders.angles);\n return md`Angles updating...`;\n}","pinned":true,"hidden":false,"mode":"ojs"},{"id":988,"value":"layout = createLayout()","pinned":false,"hidden":false,"mode":"ojs"},{"id":564,"value":"md`---`","pinned":false,"hidden":false,"mode":"ojs"},{"id":1595,"value":"md`Display a progress bar during a move sequence`","pinned":false,"hidden":false,"mode":"ojs"},{"id":1598,"value":"$30","pinned":false,"hidden":false,"mode":"ojs"},{"id":1592,"value":"md`Plan a sequence of small moves for all robots in a layout that converges on the most recently set\ngoal angles for each robot.`","pinned":false,"hidden":false,"mode":"ojs"},{"id":1585,"value":"$31","pinned":false,"hidden":false,"mode":"ojs"},{"id":1588,"value":"md`Plot the control space (T,P) for a single robot in a layout, where its own angles\nare represented by a single point and the geometry of all other robots are represented by\ncontours showing where collisions occur.`","pinned":false,"hidden":false,"mode":"ojs"},{"id":712,"value":"$32","pinned":false,"hidden":false,"mode":"ojs"},{"id":874,"value":"md`Calculate an angle update (dT,dP) that will keep robot id0 a fixed buffer distance ${tex`\\Pi_b`} (in mm) from all other robots in the layout. If id0 is already outside the buffer zone, returns dT=dP=0. If id0 is inside the buffer zone, (dT,dP) is aligned with the local proximity gradient, ${tex`\\vec{\\nabla}\\Pi_0`} using a first-order Taylor expansion of the proximity function\n${tex.block`\n\\Pi(T_0+dT,P_0+dP) \\simeq \\Pi_0 + (dT,dP)\\cdot \\vec{\\nabla}\\Pi_0\n`}\nto obtain\n${tex.block`\n\\begin{aligned}\n\\Pi(T_0+dT,P_0+dP) &= \\Pi_b \\\\\n\\Rightarrow (dT,dP) &= \\frac{\\Pi_b - \\Pi_0}{\\left|\\vec{\\nabla}\\Pi_0\\right|^2}\\,\\vec{\\nabla}\\Pi_0\n\\end{aligned}\n`}\nwith\n${tex.block`\n\\Pi_0 \\equiv \\Pi(T_0,P_0) \\quad, \\quad\n\\vec{\\nabla}\\Pi_0 \\equiv \\left(\\partial_T\\Pi(T_0,P_0), \\partial_P\\Pi(T_0,P_0)\\right) \\; .\n`}\n`","pinned":false,"hidden":false,"mode":"ojs"},{"id":812,"value":"$33","pinned":false,"hidden":false,"mode":"ojs"},{"id":604,"value":"function proximityGrid(id0, layout, {nT=60, nP=30, dtype=Array}={}) {\n // Tabulate the proximities for the robot with the specified id using the layout provided.\n // Return grids are flattened, with T increasing fastest.\n if(!layout.index.has(id0)) throw new Error(`Requested id ${id0} not in this layout.`);\n const {minT,maxT,minP,maxP} = layout.index.get(id0);\n const makeGrid = (lo,hi,n) => { const step=(hi-lo)/(n-1); return d3.range(n).map(i => lo + step*i); };\n const gridT = makeGrid(minT, maxT, nT);\n const gridP = makeGrid(minP, maxP, nP);\n const ngrid = nT * nP;\n const proxT = new dtype(ngrid), proxP = new dtype(ngrid);\n const outputIds = [id0];\n let k = 0;\n for(const P of gridP) {\n for(const T of gridT) {\n const {proximity} = layout.getProximity({angles:[{id:id0,T,P}], outputIds}).get(id0);\n proxT[k] = -proximity.theta;\n proxP[k] = -proximity.phi;\n k++;\n }\n }\n return {proxT, proxP, gridT, gridP};\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":12,"value":"$34","pinned":false,"hidden":false,"mode":"ojs"},{"id":1533,"value":"proximityColor = d3.scaleLinear()\n .domain([0,-2*bufferDistance])\n .interpolate(d3.interpolateHsl)\n .range([d3.color(\"green\"), d3.color(\"red\")])\n .clamp(true)","pinned":false,"hidden":false,"mode":"ojs"},{"id":1034,"value":"function createLayout({nrows=3, ncols=4, separation=10.4, staggerStart=true, seed=1}={}) {\n const {cos,sin,PI}=Math;\n const rng = d3.randomLcg(seed);\n const genT0 = d3.randomUniform.source(rng)(-180, 180);\n const S=separation, Sx=S*cos(PI/3), Sy=S*sin(PI/3);\n const row0 = staggerStart ? 1:0;\n const robots = [];\n const idFormat = d3.format(\"04d\");\n const nextId = () => `M${Robot.idFormat(robots.length)}`;\n for(let row=0; row