Public
Edited
Mar 18
Insert cell
Insert cell
class RobotArm{
constructor(fixedPoint, segmentLengths){
this.fixedPoint = fixedPoint;
this.joints = [vecCopy(fixedPoint)];
this.distances = [];
this.angles = [];
this.globalAngles = [];
for (let i = 0; i < segmentLengths.length; ++i) {
this.distances.push(segmentLengths[i]);
this.joints.push(vecFromAdd(this.joints[i], {x:segmentLengths[i], y:0, z:0}));
this.angles.push(0);
this.globalAngles.push(0);
}
this.angles.push(0);
this.globalAngles.push(0);
}

reset(){
for (let i = 1; i < this.joints.length; ++i) {
this.joints[i] = vecFromAdd(this.joints[i-1], {x:this.distances[i-1], y:0, z:0})
}
this.angles.fill(0);
this.globalAngles.fill(0);
}
}
Insert cell
Insert cell
RobotArm.prototype.fabrik = function(target, reset = false, allowedError = 1, maxTries = 999) {
if (reset) this.reset(); // starting from the default positions
let tries = 0;
while (vecMag(vecFromSub(target, this.joints[this.joints.length - 1])) > allowedError && tries < maxTries) {
// forward
this.joints[this.joints.length - 1] = vecCopy(target);
for (let i = this.joints.length - 2; i > -1; --i) {
const tar = this.joints[i], anchor = this.joints[i+1];
distanceConstrain(anchor, tar, this.distances[i])
}

// backward
this.joints[0] = vecCopy(this.fixedPoint);
for (let i = 1; i < this.joints.length; i++) {
const tar = this.joints[i], anchor = this.joints[i-1];
distanceConstrain(anchor, tar, this.distances[i-1]);
}

tries++;
}
//update angles
this.angles[0] = vecHeading(vecFromSub(this.joints[1], this.joints[0]));
let lastG = this.angles[0];
for (let i = 1; i < this.joints.length; ++i) {
let a = vecHeading(vecFromSub(this.joints[i + 1] || target, this.joints[i])); // angle in global coordinate
this.angles[i] = modAngle2(a - lastG);
lastG = a;
}
}
Insert cell
Insert cell
Insert cell
RobotArm.prototype.fabrik2 = function (target, angleConstraints = null, reset = false, allowedError = 1, maxTries = 1) {
const ac = angleConstraints || this.angleConstraints; // [[joint0CCW(left),joint0CW(right)]]
if (ac === undefined) return this.fabrik(target, reset, allowedError, maxTries);
if (reset) this.reset();

let tries = 0;
while (vecMag(vecFromSub(target, this.joints[this.joints.length - 1])) > allowedError && tries < maxTries) {
const lastJointIdx = this.joints.length - 1;
//forward
this.globalAngles[lastJointIdx] = modAngle(vecHeading(vecFromSub(target, this.joints[lastJointIdx])));
this.joints[lastJointIdx] = vecCopy(target);
for (let i = lastJointIdx - 1; i > -1; --i) {
let vec = vecFromSub(this.joints[i], this.joints[i+1]);
const currAng = modAngle(vecHeading(vec)); // 0 - 2PI
const ref = modAngle(this.globalAngles[i + 1] + Math.PI);
const finalAng = constrainAngle(currAng, ref, ac[i+1]);
vec = vecFromAngle(finalAng);
vecSetMag(vec, this.distances[i]);
this.joints[i].x = this.joints[i+1].x + vec.x;
this.joints[i].y = this.joints[i+1].y + vec.y;
this.joints[i].z = this.joints[i+1].z + vec.z;
this.globalAngles[i] = modAngle(finalAng + Math.PI);
}
//backward
this.joints[0] = vecCopy(this.fixedPoint);
for (let i = 1; i < this.joints.length; ++i) {
let vec = vecFromSub(this.joints[i], this.joints[i-1]);
const currAng = modAngle(vecHeading(vec)); // 0 - 2PI
const ref = this.globalAngles[i-2] || 0;
const finalAng = constrainAngle(currAng, ref, ac[i-1]);
vec = vecFromAngle(finalAng);
vecSetMag(vec, this.distances[i-1]);
this.joints[i].x = this.joints[i-1].x + vec.x;
this.joints[i].y = this.joints[i-1].y + vec.y;
this.joints[i].z = this.joints[i-1].z + vec.z;
this.globalAngles[i-1] = modAngle(finalAng);
}
this.globalAngles[lastJointIdx] = modAngle(vecHeading(vecFromSub(target, this.joints[lastJointIdx])));
tries++;
}
//update angles
this.angles[0] = this.globalAngles[0];
for (let i = 1; i < this.joints.length; ++i) {
this.angles[i] = modAngle2(this.globalAngles[i]-this.globalAngles[i-1]);
}
}
Insert cell
Insert cell
Insert cell
class RobotArm3D {
constructor(ThreeEnv, fixedPoint, segmentLengths, limits){
this.fixedPoint = fixedPoint || new THREE.Vector3(); // should be a Three.js Vector3 object
this.ball = new THREE.SphereGeometry(5, 16, 16);
this.sphere = new THREE.MeshLambertMaterial("0x999999");
this.joints = [];
this.env = ThreeEnv;
this.addJoint(ThreeEnv.scene, {x:0,y:0,z:0}, limits[0]);
for (let i = 0; i < segmentLengths.length; ++i) {
this.addJoint(this.joints[i], {x:0, y:segmentLengths[i], z:0}, limits[i+1]);
}
}

/**
@param {Three Object} base
@param {Vector3} position: {x, y, z}
@param {Array} limits [[]]
**/
addJoint(base, position, limits){
let joint = new THREE.Group();
joint.base = base;
joint.position.set(position.x, position.y, position.z);
joint.ori = new THREE.Vector3(0,1,0); //pointing up
joint.limits = limits;
this.joints.push(joint);
let ball = new THREE.Mesh(this.sphere, this.color);
joint.add(ball);
ball.position.set(0,0,0);
}

display(){
}
}
Insert cell
t1 = new ThreeEnv(600,300)
Insert cell
Insert cell
t1.renderer.domElement
Insert cell
Insert cell
class CCDRobotArm{
/**
@param {ThreeEnv} ThreeEnv
@param {Vector3} fixedPoint
@parem {Object[]} segments For each segment, {pos(Vector3), axis(Vector3), limits(Array), boxSize(Vector3), boxPos(Vector3)}
**/
constructor(ThreeEnv, fixedPoint, segments) {
this.env = ThreeEnv;
this.boxGeometry = new THREE.BoxBufferGeometry(1, 1, 1);
this.color = new THREE.MeshLambertMaterial({ color: 0x888888 });

this.fixedPoint = fixedPoint;
this.joints = [];
for (let i = 0; i < segments.length; ++i) {
const base = this.joints.length > 0 ? this.joints[this.joints.length - 1]: this.env.scene;
this.addJoint(base, segments[i].pos, segments[i].axis, segments[i].limits, segments[i].boxSize, segments[i].boxPos);
}
this.endEffector = new THREE.Group();
this.joints[this.joints.length - 1].add(this.endEffector);
this.endEffector.position.set(0,1,0);
}

addJoint(base, pos, axis, limits, sizes, boxPos){
let joint = new THREE.Group();
this.base.add(joint);
joint.position.set(pos.x, pos.y, pos.z); // the rotation point position
joint.axis = new THREE.Vector3(axis.x, axis.y, axis.z); // the rotation axis
joint.minLimit = -1 * limits[0]; // CCW
joint.maxLimit = limits[1]; // CW
this.joints.push(joint);
let box = new THREE.Mesh(this.boxGeometry, this.color);
box.scale.set(sizes.x, sizes.y, sizes.z);
box.position.set(boxPos.x, boxPos.y, boxPos.z);
box.castShadow = true;
joint.add(box);
return joint;
}

CCDIK(targetPos){
let tipPos = new THREE.Vector3();
let rotation = new THREE.Quaternion();
let quaternionDiff = new THREE.Quaternion();
for (let i = this.joints.length - 1; i > -1; --i) {
this.joints[i].updateMatrixWorld();
this.endEffector.getWorldPosition(tipPos);
// Rotate toward target
let direction = this.joints[i].worldToLocal(tipPos.clone()).normalize();
let targetDirection = this.joints[i].worldToLocal(targetPos.clone()).normalize();
quaternionDiff.setFromUnitVectors(direction, targetDirection);
this.joints[i].quaternion.multiply(quaternionDiff);
// Find the rotation from here to the parent, and rotate the axis by it...
// This ensures that were always rotating with the hinge
let inversedQuaternion = this.joints[i].quaternion.inverse();
let parentAxis = this.joints[i].axis.clone.applyQuaternion(inversedQuaternion);
quaternionDiff.setFromUnitVectors(this.joints[i].axis, parentAxis);
this.joints[i].quaternion.multiply(quaternionDiff);
// Clamp the rotation with limitation
let clampedRotation = this.joints[i].rotation.toVector3().clampScalar(this.joints[i].minLimit, this.joints[i].maxLimit);
this.joints[i].rotation.setFromVector3(clampedRotation);

this.joints[i].updateMatrixWorld();
}
}
}
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

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