自创的机械臂新算法，因为是AI写的，暂时，并不智能，但目前支持任何段数

<!DOCTYPE html>

<head>

<title>三段机械臂抓取演示</title>

<style>

body { margin: 0; display: flex; flex-direction: column; align-items: center; background: #f0f0f0; }

#canvas { background: #fff; border: 2px solid #333; }

.controls { margin: 10px 0; display: flex; gap: 10px; align-items: center; flex-wrap: wrap; }

.arm-config { display: flex; gap: 10px; align-items: center; margin-bottom: 5px; }

input { width: 80px; padding: 5px; }

button { padding: 5px 15px; cursor: pointer; }

#tip { color: red; margin-top: 5px; }

</style>

</head>

<body>

<label>机械臂段数: <input type="number" id="armSegments" value="3" min="1" max="5" step="1"></label>

<label>角度最小值(弧度): <input type="number" id="angleMin" value="0" min="0" max="6.28" step="0.1"></label>

<label>角度最大值(弧度): <input type="number" id="angleMax" value="1.57" min="0" max="6.28" step="0.1"></label>

</div>

</div>

const canvas = document.getElementById('canvas');

const ctx = canvas.getContext('2d');

const [W, H] = [canvas.width, canvas.height];

const center = { x: W/2, y: H/2 };

let target = null;

let isRunning = false;

let currentPhase = 'shrink';

let arm = {

len: [120, 100, 80],

angle: [Math.random()*Math.PI/2, Math.random()*Math.PI/2, Math.random()*Math.PI/2],

joint: [{}, {}, {}, {}]

};

// 原算法: 积分函数 o()

function F(x, k) { return 1 + Math.pow(x, k) + x; }

function o(eq, a, b, d=0.1, K=5) {

const g = new Function('x', `return ${eq};`);

let s = [[a, b]], r = 0.0;

while (s.length > 0) {

let n = [];

for (let [x0, x1] of s) {

const dx = x1 - x0;

if (dx < d) {

const m = (x0 + x1)/2;

const m_scaled = m/(2*Math.PI);

const f_values = Array.from({length:K}, (_,k) => F(m_scaled, k+1));

let W = 0.0, V = 0.0;

const g_m = g(m);

for (let k = 0; k < K; k++) {

const w = 1/(f_values[k] + 1e-12);

W += w; V += w * g_m;

}

r += dx * (W ? V/W : g_m);

continue;

}

const u = g(x0), v = g(x1), mid = (x0+x1)/2, mv = g(mid);

if (Math.abs(u-v) < 1e-8 && Math.abs(mv-u) < 1e-8) {

r += dx * mv; continue;

}

const h = dx/10;

for (let i = 0; i < 10; i++) n.push([x0+i*h, x0+(i+1)*h]);

}

s = n;

}

return r;

}

// 原算法: 排序函数 f()

function c(seg) { let cnt = 0; for (let i=0;i<seg.length;i++) for(let j=i+1;j<seg.length;j++) if(seg[i]>seg[j]) cnt++; return cnt; }

function w(arr, wa) {

let a = [...arr], n = a.length;

if (n <=1) return a;

const exp_wa = Array.from({length:n}, (_,i) => Math.pow(wa, i+1));

for (let i=0;i<n;i++) {

let swapped = false;

for (let j=0;j<n-i-1;j++) {

const v1 = a[j]*(1 + wa + exp_wa[j]);

const v2 = a[j+1]*(1 + wa + exp_wa[j+1]);

if (Math.abs(v1-v2) <1e-8) {

const eq = ` ${a\[j\]}\*x -$ {a[j+1]}*x`;

const r = o(eq, 0, 1);

if (r>0) { [a[j],a[j+1]] = [a[j+1],a[j]]; swapped=true; }

} else if (v1>v2) { [a[j],a[j+1]] = [a[j+1],a[j]]; swapped=true; }

}

if (!swapped) break;

}

return a;

}

function f(arr) {

let n = arr.length;

if (n<=1) return [...arr];

let segs = [], i=0;

while (i < n) {

let l=1;

while (i+l <n && arr[i+l]>=arr[i+l-1]) l++;

const s = arr.slice(i, i+l);

const r = l>1 ? c(s)/(l-1) : 0;

segs.push({s, r});

i += l;

}

let n_segs = [];

for (let {s, r} of segs) {

if (r>0 && s.length>1) {

const avg = s.reduce((a,b)=>a+b)/s.length;

const wa_sum = Array.from({length:20}, (_,k) => 1/(F(avg, k+1)+1e-12)).reduce((a,b)=>a+b);

n_segs.push({s:w(s, wa_sum), r: c(w(s, wa_sum))/(s.length-1)});

} else n_segs.push({s, r});

}

return n_segs.flatMap(item => item.s);

}

// 机械臂关节坐标计算 - 适配任意段数

function updateArmJoint() {

let [x, y] = [center.x, center.y];

arm.joint = [];

arm.joint.push({x, y});

for (let i=0; i<arm.len.length; i++) {

x += arm.len[i] * Math.cos(arm.angle[i]);

y += arm.len[i] * Math.sin(arm.angle[i]);

arm.joint.push({x, y});

}

// 速度归一化

function normalizeSpeed(speed) {

return Math.max(0.1, Math.min(speed, 2)) * 0.01;

}

// 缩臂阶段

function shrinkArm() {

if (!target) return false;

const end = arm.joint[arm.joint.length - 1];

const dx = target.x - end.x;

const dy = target.y - end.y;

const dist = Math.hypot(dx, dy);

if (dist < 5) { currentPhase = 'extend'; return true; }

const eq = `Math.hypot(x*Math.cos( ${arm.angle\[arm.angle.length-1\]}) -$ {dx}, x*Math.sin( ${arm.angle\[arm.angle.length-1\]}) -$ {dy})`;

const delta = o(eq, -0.5, 0.5);

const speed = normalizeSpeed(Number(document.getElementById('shrinkSpeed').value));

// 差异化角度更新 - 适配任意段数

arm.angle = arm.angle.map((ang, idx) => {

const ratio = idx === 0 ? 1.2 : idx === arm.angle.length-1 ? 1 : 0.8;

return ang + delta * speed * ratio;

});

return dist > 20;

}

// 缓伸阶段

function extendArm() {

if (!target) return false;

const end = arm.joint[arm.joint.length - 1];

const points = Array.from({length:10}, (_,i) => Math.hypot(end.x - target.x + i, end.y - target.y + i));

const sortedPoints = f(points);

const avgDist = sortedPoints.reduce((a,b)=>a+b)/sortedPoints.length;

const speed = normalizeSpeed(Number(document.getElementById('extendSpeed').value));

// 杆长和角度更新 - 适配任意段数

arm.len = arm.len.map(l => l + (avgDist < 10 ? -speed*2 : speed));

arm.angle = arm.angle.map(ang => ang + speed * 0.5);

return Math.hypot(end.x - target.x, end.y - target.y) > 3;

}

// 绘制 - 适配任意段数

function draw() {

ctx.clearRect(0, 0, W, H);

if (target) {

ctx.fillStyle = 'blue';

ctx.beginPath();

ctx.arc(target.x, target.y, 10, 0, 2*Math.PI);

ctx.fill();

}

updateArmJoint();

ctx.strokeStyle = '#333';

ctx.lineWidth = 8;

for (let i=0; i<arm.joint.length-1; i++) {

const p1 = arm.joint[i];

const p2 = arm.joint[i+1];

ctx.beginPath();

ctx.moveTo(p1.x, p1.y);

ctx.lineTo(p2.x, p2.y);

ctx.stroke();

}

ctx.fillStyle = 'red';

ctx.beginPath();

ctx.arc(arm.joint[arm.joint.length-1].x, arm.joint[arm.joint.length-1].y, 5, 0, 2*Math.PI);

ctx.fill();

}

// 动画循环

function animate() {

if (!isRunning) return;

let keepRunning = true;

if (currentPhase === 'shrink') {

keepRunning = shrinkArm();

} else if (currentPhase === 'extend') {

keepRunning = extendArm();

}

if (!keepRunning) {

isRunning = false;

document.getElementById('tip').textContent = keepRunning ? '' : '抓取完成/未抓到目标，请重新放置';

}

draw();

requestAnimationFrame(animate);

}

// 应用机械臂配置

function applyArmConfig() {

const segments = parseInt(document.getElementById('armSegments').value);

const angleMin = parseFloat(document.getElementById('angleMin').value);

const angleMax = parseFloat(document.getElementById('angleMax').value);

if (angleMin >= angleMax) {

document.getElementById('tip').textContent = '角度最小值必须小于最大值';

return;

}

// 生成对应段数的长度和角度

arm.len = Array.from({length: segments}, (_, i) => 120 - i*20);

arm.angle = Array.from({length: segments}, () => angleMin + Math.random()*(angleMax - angleMin));

document.getElementById('tip').textContent = '';

draw();

}

// 事件监听

canvas.addEventListener('click', (e) => {

const rect = canvas.getBoundingClientRect();

target = {

x: e.clientX - rect.left,

y: e.clientY - rect.top

};

document.getElementById('tip').textContent = '';

draw();

});

document.getElementById('startBtn').addEventListener('click', () => {

if (!target) {

document.getElementById('tip').textContent = '请先点击画布放置目标物体';

return;

}

isRunning = true;

currentPhase = 'shrink';

animate();

});

document.getElementById('stopBtn').addEventListener('click', () => {

isRunning = false;

document.getElementById('tip').textContent = '已停止';

});

document.getElementById('applyConfig').addEventListener('click', applyArmConfig);

// 初始绘制

updateArmJoint();

draw();

</script>

</body>

</html>