Published
Edited
Jan 23, 2021
1 star
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
// ANCHOR ROTATION CONFIG
ros = ([
135, 45, // L1 R1
180, 0, // L2 R2
-135, -45 // L3 R3
]).map(r => r * Math.PI / 180) // convert to radian
Insert cell
// ANCHOR COORDINATE CONFIG
anchors = [
[-3, 5, 0], [ 3, 5, 0], // L1 R1
[-5, 0, 0], [ 5, 0, 0], // L2 R2
[-3, -5, 0], [ 3, -5, 0] // L3 R3
]
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
Insert cell
// CALCULATE EACH LEG TIPS TARGET
legs = {
// Set Normalized Position
const leg0 = [- frs / 2, l / 2, -h]
const leg2 = [ - ms / 2, 0, -h]
const leg4 = [- frs / 2, - l / 2, -h]
const leg1 = [ frs / 2, l / 2, -h]
const leg3 = [ ms / 2, 0, -h]
const leg5 = [ frs / 2, - l / 2, -h]
const ls = [leg0, leg1, leg2, leg3, leg4, leg5]
// Perform Pitch, Roll, Yaw
return ls
.map(([x, y, z]) => ([
x,
y * Math.cos(rx) - z * Math.sin(rx),
y * Math.sin(rx) + z * Math.cos(rx)
]))
.map(([x, y, z]) => ([
x * Math.cos(ry) + z * Math.sin(ry),
y,
- x * Math.sin(ry) + z * Math.cos(ry)
]))
.map(([x, y, z]) => ([
x * Math.cos(rz) - y * Math.sin(rz),
x * Math.sin(rz) + y * Math.cos(rz),
z
]))
}
Insert cell
// CALCULATE EACH SERVOS ANGLE OF EACH LEGS
legTipPos = {
const p = []

// Calculating each tips target with Inverse Kinematics (setTipPos)
for (let legIndex = 0; legIndex < 6; legIndex++) {
p.push(setTipPos(
ros[legIndex],
anchors[legIndex],
coxa,
femur,
tibia,
legs[legIndex][0],
legs[legIndex][1],
legs[legIndex][2]
));
}
return p
}
Insert cell
// CALCULATE EACH POINTS TO DRAW IN SIMULATION
legPoints = {
const p = []

// Calculating drawable points of each leg joints with Forward Kinematics (createLegPoints)
for (let legIndex = 0; legIndex < 6; legIndex++) {
p.push(createLegPoints(
ros[legIndex],
anchors[legIndex],
coxa,
femur,
tibia,
legTipPos[legIndex][0],
legTipPos[legIndex][1],
legTipPos[legIndex][2]
));
}
return p
}
Insert cell
// FORWARD KINEMATICS
function createLegPoints (ro, anchor, coxa, femur, tibia, teta, beta, alpha) {
const p0 = anchor

const p1 = [
p0[0] + coxa * Math.cos(ro + teta),
p0[1] + coxa * Math.sin(ro + teta),
p0[2]
]

const p2 = [
p1[0] + femur * Math.cos(beta) * Math.cos(ro + teta),
p1[1] + femur * Math.cos(beta) * Math.sin(ro + teta),
p1[2] + femur * Math.sin(beta),
]

const omega = alpha - (Math.PI/2 - beta)
const Z = - tibia * Math.cos(omega)
const R = tibia * Math.sin(omega)

const p3 = [
p2[0] + tibia * Math.sin(alpha - (Math.PI/2 - beta)) * Math.cos(ro + teta),
p2[1] + tibia * Math.sin(alpha - (Math.PI/2 - beta)) * Math.sin(ro + teta),
p2[2] - tibia * Math.cos(alpha - (Math.PI/2 - beta))
]

return [p0, p1, p2, p3]
}
Insert cell
// INVERSE KINEMATICS
function setTipPos (ro, anchor, coxa, femur, tibia, x, y, z) {
const diffX = x - anchor[0]
const diffY = y - anchor[1]
const diffZ = z - anchor[2]

const rotatedX = diffX * Math.cos(ro) + diffY * Math.sin(ro)
const rotatedY = diffY * Math.cos(ro) - diffX * Math.sin(ro)

const r = Math.sqrt(Math.pow(rotatedY, 2) + Math.pow(rotatedX, 2))
const p = r - coxa
const u = Math.sqrt(Math.pow(p, 2) + Math.pow(diffZ, 2))
const gama = p > 0 ? Math.atan(-diffZ / p) : Math.PI + Math.atan(-diffZ / p)

return [
Math.atan(rotatedY / rotatedX),
Math.acos((Math.pow(u, 2) + Math.pow(femur, 2) - Math.pow(tibia, 2)) / (2 * femur * u)) - gama,
Math.acos((Math.pow(tibia, 2) + Math.pow(femur, 2) - Math.pow(u, 2)) / (2 * tibia * femur))
]
}
Insert cell
// 3D SIMULATION
THREE = {
const THREE = await require("three@0.95.0/build/three.min.js")

const scene = new THREE.Scene()

const gridHelper = new THREE.GridHelper(60, 20)
const axesHelper = new THREE.AxesHelper(5)
scene.add(gridHelper)
scene.add(axesHelper)
const randomColors = [
0x3498db,
0x1abc9c,
0xf1c40f,
0xe67e22,
0xe74c3c,
0x9b59b6
]

const SCALE = 2

function createPoint ([x, y, z]) {
// x -> x, y -> z, z -> y
const sx = x * SCALE
const sy = z * SCALE
const sz = y * SCALE
return new THREE.Vector3(sx, sy, sz)
}

const robot = new THREE.Group()

const firstPoints = []

for (let legIndex = 0; legIndex < 6; legIndex++) {
const [teta, beta, alpha] = setTipPos(
ros[legIndex],
anchors[legIndex],
coxa,
femur,
tibia,
legs[legIndex][0],
legs[legIndex][1],
legs[legIndex][2]
);

const legPoints = createLegPoints(
ros[legIndex],
anchors[legIndex],
coxa,
femur,
tibia,
teta,
beta,
alpha
);

firstPoints.push(legPoints[0])

const points = [];
legPoints.forEach(r => points.push(createPoint(r)))

{
const material = new THREE.LineBasicMaterial({ color: 0xffffff, linewidth: 4 })
const geometry = new THREE.BufferGeometry().setFromPoints(points)
const line = new THREE.Line(geometry, material)
robot.add(line)
}

{
points.forEach((vector, i) => {
const geometry = new THREE.SphereGeometry(.4)
const material = new THREE.MeshBasicMaterial({ color: randomColors[i] })
const sphere = new THREE.Mesh(geometry, material)
geometry.translate(vector.x, vector.y, vector.z)
robot.add(sphere)
})
}
}

{
const geometry = new THREE.Geometry()
const sortedAnchorPoints = [
firstPoints[0],
firstPoints[2],
firstPoints[4],
firstPoints[5],
firstPoints[3],
firstPoints[1],
firstPoints[0]
]

sortedAnchorPoints.forEach(r => geometry.vertices.push(createPoint(r)))

const material = new THREE.LineBasicMaterial({ color: 0xdddddd, linewidth: 2 })
const line = new THREE.Line(geometry, material)
robot.add(line)
}
robot.translateY(h * SCALE);
robot.rotateX(rx);
robot.rotateZ(ry);
robot.rotateY(rz);
scene.add(robot)

const camera = new THREE.PerspectiveCamera(60, 600 / 400, 1, 500)
camera.position.y = 60 * Math.sin(rotateCameraUp)
camera.position.x = 60 * Math.cos(rotateCameraUp) * Math.sin(rotateCamera)
camera.position.z = 60 * Math.cos(rotateCameraUp) * Math.cos(rotateCamera)
camera.lookAt(0, 0, 0)

const canvas = document.getElementById("simulation")
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true })
renderer.setSize(600, 400)

let running = true

invalidation.then(() => {
renderer.dispose()
running = false
})

function resizeRendererToDisplaySize() {
const pixelRatio = window.devicePixelRatio
const width = canvas.clientWidth * pixelRatio
const height = canvas.clientHeight * pixelRatio
const needResize = canvas.width !== width || canvas.height !== height

if (needResize) {
renderer.setSize(width, height, false)
camera.aspect = canvas.clientWidth / canvas.clientHeight
camera.updateProjectionMatrix()
}
}

if (running) {
resizeRendererToDisplaySize()
renderer.render(scene, camera)
await Promises.tick(1000 / 10)
}

return "3D Simulation Rendered by Three.js"
}
Insert cell

Purpose-built for displays of data

Observable is your go-to platform for exploring data and creating expressive data visualizations. Use reactive JavaScript notebooks for prototyping and a collaborative canvas for visual data exploration and dashboard creation.
Learn more