31:T122c,function robotMotion(layout, {w=width, points, initialAngles, finalAngles}={}) { const { abs,sin,cos,PI } = Math; // Establish mm to pixel mapping const { xlo,xhi,ylo,yhi } = layout.extent; const aspect = (yhi - ylo) / (xhi - xlo), h = Math.ceil(aspect*w); const fx = d3.scaleLinear().domain([xlo,xhi]).range([0,w-1]); const fy = d3.scaleLinear().domain([yhi,ylo]).range([0,h-1]); const s = fx(1) - fx(0); // Initialize color map for triplets const colors = d3.schemePastel2; const tripletColor = d => colors[d.id.slice(5, 7)*1 % colors.length]; // Build the SVG DOM 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"); // Draw boundary polygon, if specified if(layout.boundary) { const lineGenerator = d3.line().x(d => fx(d[0])).y(d => fy(d[1])); g.append("path") .attr("d", lineGenerator(layout.boundary) + "Z") // "Z" closes the polygon .attr("fill", "none") .attr("stroke", "pink"); } // Draw (x,y) points, if specified if(points) { const P = g.append("g") .attr("fill", "red") .attr("stroke", "none"); P.selectAll("circle") .data(points) .join("circle") .attr("cx", d=>fx(d.x)) .attr("cy", d=>fy(d.y)) .attr("r", 3); } g.selectAll("g.robot") .data(layout.robots, d=>d.name) .join("g") .classed("robot", true) .attr("fill", tripletColor) .attr("stroke", tripletColor) // Translate to each robot's (x,y) and orientation relative to the IRIS origin .attr("transform", d => `translate(${fx(d.x0)},${fy(d.y0)}) rotate(${-d.T0},0,0)`) .call(R => { // Draw a line to the triplet center that aligns with T0 R.append("line") .attr("x2", d => -s*d.rpatrol) .attr("stroke", tripletColor); // Draw patrol radius R.append("circle") .attr("cx", 0) .attr("cy", 0) .attr("r", d => s*d.rpatrol) .attr("stroke-opacity", 0.5) .attr("fill-opacity", 0.2); // Draw theta hardstops const alpha = geometry.toverlap/2*PI/180 const ux = sin(alpha), uy = cos(alpha); R.append("line") .classed("hardstop1", true) .attr("x1", d => s*ux*d.rpatrol) .attr("x2", d => 0.9*s*ux*d.rpatrol) .attr("y1", d => -s*uy*d.rpatrol) .attr("y2", d => -0.9*s*uy*d.rpatrol); R.append("line") .classed("hardstop2", true) .attr("x1", d => -s*ux*d.rpatrol) .attr("x2", d => -0.9*s*ux*d.rpatrol) .attr("y1", d => -s*uy*d.rpatrol) .attr("y2", d => -0.9*s*uy*d.rpatrol); R.append("g") .classed("theta", true) // Rotate to this robot's theta motor angle .attr("transform", d => `rotate(${-d.T+geometry.rangle},0,0)`) .call(S => { S.append("line") .attr("x2", d => -s*d.rpatrol); S.append("g") .classed("rflex", true) .attr("transform", d => `translate(${s*d.rkeepout},0)`) .call(R => { // Keepout rectangle R.append("rect") .attr("x", -s*geometry.lkeep/2) .attr("y", -s*geometry.wflex/2) .attr("width", s*geometry.lkeep) .attr("height", s*geometry.wflex) .attr("fill", "blue") .attr("fill-opacity", 0.25) .append("title") .text(d => d.id); // Fiber tip R.append("circle") .attr("cx", -s*(geometry.xkeep + geometry.lkeep/2 - geometry.rfiber)) .attr("r", 4) .attr("fill", "blue"); }); }); }); function _setAngles(robots) { g.selectAll("g.theta") .data(robots, d => d.id) .join("g") .attr("transform", d => `rotate(${-d.T+geometry.rangle},0,0)`); g.selectAll("g.rflex") .data(robots, d => d.id) .join("g") .attr("transform", d => `translate(${s*d.rkeepout},0)`); } const model = Object.assign(svg.node(), { value: layout, setAngles(angles) { angles.forEach(({id,T,R}) => { if(this.value.index.has(id)) { const robot = this.value.index.get(id); robot.T = T; robot.R = R; } }); _setAngles(this.value.robots); model.dispatchEvent(new Event("input", {bubble:true})); }, }); return model; }32:Tac5,function getTargets(layout, {seed=1,pad=0.2}={}) { const { xlo,xhi,ylo,yhi } = layout.extent; const ngen = 50 * layout.robots.length; const x = getUniformFloat32(seed, ngen, {channel:0, type:Array}).map(u => xlo + u * (xhi - xlo)); const y = getUniformFloat32(seed, ngen, {channel:1, type:Array}).map(v => ylo + v * (yhi - ylo)); function reachableBy(x, y) { if(layout.boundary && !d3.polygonContains(layout.boundary, [x,y])) return [ ]; return layout.robots.filter(d => d.isReachable(x,y)).map(({id}) => id); } const fmt = d3.format(".1f"); const angles = new Map(); const unassigned = [ ], points = [ ]; for(let k = 0; k < ngen; ++k) { let assigned = false; // Is this location reachable by any robot in the layout? const ids = reachableBy(x[k], y[k]); if(ids.length == 0) continue; // Are any of the robots that can reach this location available? for(const id of ids) { if(angles.has(id)) continue; const robot = layout.index.get(id); // Remember this robot's original angles const Tsave = robot.T; const Rsave = robot.R; // Calculate angles at this new location const { T, R } = robot.getTR(x[k], y[k]); // Calculate keepout at this new location const keepout = robot.getKeepout({T, R, pad}); // Check if any keepout corner is outside of the boundary let inside = true; if(layout.boundary) { const corners = getCorners(keepout); for(const corner of corners) { if(!d3.polygonContains(layout.boundary, corner)) { inside = false; break; } } } if(!inside) continue; // Check if this new keepout position collides with any neighbors that are already assigned let collided = false; for(const neighborId of robot.neighbors) { if(!angles.has(neighborId)) continue; const {T:Tn,R:Rn} = angles.get(neighborId); const neighbor = layout.index.get(neighborId); const keepout2 = neighbor.getKeepout({T:Tn, R:Rn, pad}); let check = collisionDistanceRect(keepout, keepout2); if(check.distance <= 0) { collided = true; break; } } if(collided) continue; // If we get this far, assign this location to this robot angles.set(id, {id,x:x[k],y:y[k],T,R}); assigned = true; points.push(keepout); if(angles.size == layout.robots.length) return { angles, unassigned, points }; break; } if(!assigned) unassigned.push({x:x[k], y:y[k]}); } // If we get here, we do not have enough targets throw new Error( `Not enough targets (${ngen}) with only ${angles.size}/${layout.robots.length} robots assigned`); }33:T95a,/** * Rectangle representation: * * { * x, y, // center coordinates * ux, uy, // unit vector along the rectangle's local x-axis * hx, hy // half-width and half-height * } * * The local y-axis is [-uy, ux]. * * The caller must ensure: * ux*ux + uy*uy == 1 * hx >= 0 * hy >= 0 */ function collisionDistanceRect(poly1, poly2, {origin = [0, 0]} = {}) { const {abs, max} = Math; const [x0, y0] = origin; let ax0; let ay0; let dsmax = -Infinity; const projections = []; /* * Test one unit separating axis. r1 and r2 are the projection * half-widths of the two rectangles along that axis. */ const testAxis = (i, ax, ay, r1, r2) => { const s1 = (poly1.x - x0) * ax + (poly1.y - y0) * ay; const s2 = (poly2.x - x0) * ax + (poly2.y - y0) * ay; const s1min = s1 - r1; const s1max = s1 + r1; const s2min = s2 - r2; const s2max = s2 + r2; projections.push({ i, ax, ay, s1min, s1max, s2min, s2max }); // Preserve the signed-separation definition of collisionDistance(). const ds12 = s2min - s1max; const ds21 = s1min - s2max; const ds = max(ds12, ds21); if (ds > dsmax) { dsmax = ds; if (ds12 > ds21) { ax0 = ds * ax; ay0 = ds * ay; } else { ax0 = -ds * ax; ay0 = -ds * ay; } } }; /* * Relative orientation of the two rectangles. * * c = dot(poly1.u, poly2.u) * s = cross(poly1.u, poly2.u) * * Absolute values are sufficient when calculating projection radii. */ const c = poly1.ux * poly2.ux + poly1.uy * poly2.uy; const s = poly1.ux * poly2.uy - poly1.uy * poly2.ux; const ac = abs(c); const as = abs(s); // Axis 0: poly1 local x-axis. testAxis( 0, poly1.ux, poly1.uy, poly1.hx, poly2.hx * ac + poly2.hy * as ); // Axis 1: poly1 local y-axis. testAxis( 1, -poly1.uy, poly1.ux, poly1.hy, poly2.hx * as + poly2.hy * ac ); // Axis 2: poly2 local x-axis. testAxis( 2, poly2.ux, poly2.uy, poly1.hx * ac + poly1.hy * as, poly2.hx ); // Axis 3: poly2 local y-axis. testAxis( 3, -poly2.uy, poly2.ux, poly1.hx * as + poly1.hy * ac, poly2.hy ); return { offset: [ax0, ay0], distance: dsmax, projections }; }34:[{"id":"22d176af93ed9450b941069b3680829af3f35e5874220553a545cdb3fbb0328d817b42c77e813279e61ca2eff93afacb500e4204f604ffb02e3f4b7c4358a34f","href":"https://static.observableusercontent.com/files/22d176af93ed9450b941069b3680829af3f35e5874220553a545cdb3fbb0328d817b42c77e813279e61ca2eff93afacb500e4204f604ffb02e3f4b7c4358a34f","name":"pure-rand-8.4.0.js","type":"application/javascript","lastModified":1784853608232.247,"size":1873}] 2f:{"title":"Raft Motion Planning","cells":[{"id":0,"value":"# Raft Motion Planning","pinned":false,"hidden":false,"mode":"md"},{"id":37,"value":"motionPlot = robotMotion(raftLayout)","pinned":false,"hidden":false,"mode":"ojs"},{"id":384,"value":"viewof buttons = Inputs.form([\n Inputs.button(\"INITIAL\", { value: null, reduce: () => motionPlot.setAngles(initial.angles) }),\n Inputs.button(\"FINAL\", { value: null, reduce: () => motionPlot.setAngles(final.angles) }),\n])","pinned":false,"hidden":false,"mode":"ojs"},{"id":205,"value":"viewof seeds = Inputs.form({\n seed1: Inputs.number([0, 99], {step: 1, value: 48}),\n seed2: Inputs.number([0, 99], {step: 1, value: 99}),\n})","pinned":false,"hidden":false,"mode":"ojs"},{"id":247,"value":"Enter a random seed to calculate and display the corresponding collision-free assignment for all robots in a raft.","pinned":false,"hidden":false,"mode":"md"},{"id":509,"value":"### Motion Planning","pinned":false,"hidden":false,"mode":"md"},{"id":529,"value":"planPlot = Plot.plot({\n aspectRatio: 1,\n marks: [\n Plot.dot([...plan.values()], { x:\"tT\", y:\"tR\" }),\n Plot.frame(),\n ]\n})","pinned":true,"hidden":false,"mode":"ojs"},{"id":515,"value":"plan = getPlan(initial.angles, final.angles)","pinned":true,"hidden":false,"mode":"ojs"},{"id":513,"value":"function getPlan(initial, final) {\n const plan = new Map();\n const { cruiseStep, upRamp, downRamp } = motorParams;\n final.forEach(({id,T:Tf,R:Rf}) => {\n if(!initial.has(id)) throw new Error(`No matching initial angle for ${id}`);\n const {T:Ti,R:Ri} = initial.get(id);\n const dR = Rf - Ri;\n const dT = Tf - Ti;\n let movesR, movesT, tR=0, tT=0;\n if(Math.abs(dR) > 4) {\n movesR = motor(cruiseStep, upRamp, downRamp, dR);\n tR = movesR.length / clockSpeed;\n }\n if(Math.abs(dT) > 4) {\n movesT = motor(cruiseStep, upRamp, downRamp, dT);\n tT = movesT.length / clockSpeed;\n }\n plan.set(id, {id, Ti,Tf, Ri,Rf, tR, tT});\n });\n return plan;\n}","pinned":true,"hidden":false,"mode":"ojs"},{"id":228,"value":"### Collision Free Targets","pinned":false,"hidden":false,"mode":"md"},{"id":30,"value":"$31","pinned":false,"hidden":false,"mode":"ojs"},{"id":65,"value":"initial = getTargets(raftLayout, { seed: seeds.seed1 })","pinned":false,"hidden":false,"mode":"ojs"},{"id":374,"value":"final = getTargets(raftLayout, { seed: seeds.seed2 })","pinned":false,"hidden":false,"mode":"ojs"},{"id":454,"value":"printKeepout = k => {\n const s=[ ];\n const f = d3.format(\".2f\");\n Object.entries(k).forEach(([key,val]) => s.push(`${key}=${f(val)}`));\n return s.join(\",\");\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":23,"value":"$32","pinned":false,"hidden":false,"mode":"ojs"},{"id":225,"value":"### Raft Motion Utilities","pinned":false,"hidden":false,"mode":"md"},{"id":172,"value":"function getCorners(rect) {\n const {x,y,ux,uy,hx,hy} = rect;\n return [\n [ x + hx*ux - hy*uy, y + hx*uy + hy*ux ], // (+hx,+hy)\n [ x + hx*ux + hy*uy, y + hx*uy - hy*ux ], // (+hx,-hy)\n [ x - hx*ux - hy*uy, y - hx*uy + hy*ux ], // (-hx,+hy)\n [ x - hx*ux + hy*uy, y - hx*uy - hy*ux ], // (-hx,-hy)\n ];\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":35,"value":"raftLayout = {\n const robots = [ ];\n const fmt = d3.format(\"02d\");\n raftTriplets.forEach((triplet,k) => {\n //if(k > 0) return;\n // Calculate triplet center\n const xc = d3.mean(triplet, d => raftCoordinates[d].x), yc = d3.mean(triplet, d => raftCoordinates[d].y);\n for(let j = 0; j < 3; ++j) {\n const {x,y} = raftCoordinates[triplet[j]];\n const T0 = Math.atan2(y - yc, x - xc) * 180/Math.PI;\n const id = `IRFLX${fmt(k)}${['A','B','C'][j]}`;\n robots.push(new Robot({id, x0:x, y0:y, T0, T:0, R:0}));\n }\n });\n return new Layout(robots, {boundary:raftBoundary});\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":32,"value":"import { Robot, Layout, geometry } from \"@dkirkby/iris-rflex-kinematics\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":220,"value":"### Raft Geometry","pinned":false,"hidden":false,"mode":"md"},{"id":250,"value":"raftTriplets = [\n // Only valid when raftGeometry.num_edge_robots == 9\n [ 0, 10, 9 ], [11, 1, 2], [13, 3, 4], [5,15, 14], [16, 6, 7], [8, 18, 17],\n [19, 20, 28], [12, 22, 21], [23, 24, 32], [25, 34, 33], [26, 27, 35],\n [29, 37, 36], [30, 31, 38], [39, 40, 46], [41, 42, 48], [43, 44, 49],\n [45, 51, 50], [47, 53, 52], [54, 55, 58], [56, 57, 60], [59, 61, 62]\n]","pinned":false,"hidden":false,"mode":"ojs"},{"id":8,"value":"raftCoordinates = {\n const { num_edge_robots, spacing } = raftGeometry;\n const dy = spacing * Math.cos(Math.PI/6);\n let ysum = 0;\n const coords = [ ];\n for(let row = 0; row <= num_edge_robots; ++row) {\n const ncol = (row == 0) ? num_edge_robots : num_edge_robots + 2 - row;\n const y = row * dy;\n ysum += ncol * y;\n for(let col = 0; col < ncol; ++col) {\n const x = (col - (ncol - 1) / 2) * spacing;\n coords.push({ row, col, x, y });\n }\n }\n // Set (x,y) origin at centroid\n const yavg = ysum / coords.length;\n return coords.map(({y,...c}) => ({y:y-yavg, ...c}));\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":43,"value":"raftBoundary = d3.transpose([\n [ 27.378, 31.939, 4.561, -4.561, -31.939, -27.378],\n [-21.073, -13.173, 34.247, 34.247, -13.173, -21.073],\n])","pinned":false,"hidden":false,"mode":"ojs"},{"id":5,"value":"raftGeometry = ({\n num_edge_robots: 9,\n spacing: 6.2, // mm\n})","pinned":false,"hidden":false,"mode":"ojs"},{"id":498,"value":"### Motor Control","pinned":false,"hidden":false,"mode":"md"},{"id":520,"value":"motorParams = ({ cruiseStep: 33, upRamp: 12, downRamp: 12 })","pinned":false,"hidden":false,"mode":"ojs"},{"id":500,"value":"import {motor,plot as motorPlot,clockSpeed} from \"@dkirkby/motor-params\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":211,"value":"### Rectangle Collisions","pinned":false,"hidden":false,"mode":"md"},{"id":196,"value":"$33","pinned":false,"hidden":false,"mode":"ojs"},{"id":209,"value":"### Random Numbers","pinned":false,"hidden":false,"mode":"md"},{"id":113,"value":"function getUniformFloat32(seed, n, { channel=0, type=Float32Array}={}) {\n var rng = prand.xoroshiro128plus(seed);\n if(channel > 0) {\n const pureJump = prand.purify((rng) => rng.jump());\n for(let i=0; i