brepjs 18.80.0 → 18.81.1
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/dist/brepjs.cjs +8 -3
- package/dist/brepjs.js +7 -4
- package/dist/index.d.ts +1 -0
- package/dist/operations/ikFns.d.ts +58 -0
- package/dist/operations/jointFns.d.ts +6 -1
- package/dist/operations.cjs +3 -1
- package/dist/operations.d.ts +1 -0
- package/dist/operations.js +2 -2
- package/dist/{threadFns-B7a1EVpS.cjs → threadFns-By0Gy2f9.cjs} +316 -4
- package/dist/{threadFns-Cra0yHSF.js → threadFns-YzJhd0Kz.js} +305 -5
- package/package.json +1 -1
package/dist/brepjs.cjs
CHANGED
|
@@ -17,7 +17,7 @@ const require_arrayAccess = require("./arrayAccess-e4H9cBfh.cjs");
|
|
|
17
17
|
const require_surfaceBuilders = require("./surfaceBuilders-B8aVZamB.cjs");
|
|
18
18
|
const require_primitiveFns = require("./primitiveFns-DEBQdEkG.cjs");
|
|
19
19
|
const require_healingFns = require("./healingFns--PtL9j2K.cjs");
|
|
20
|
-
const require_threadFns = require("./threadFns-
|
|
20
|
+
const require_threadFns = require("./threadFns-By0Gy2f9.cjs");
|
|
21
21
|
const require_blueprintSketcher = require("./blueprintSketcher-BJPBKhF3.cjs");
|
|
22
22
|
const require_helpers = require("./helpers-B8mE35Fm.cjs");
|
|
23
23
|
const require_drawFns = require("./drawFns-DTpCthM5.cjs");
|
|
@@ -2444,8 +2444,11 @@ function solveMate(c, ref, dep) {
|
|
|
2444
2444
|
switch (c.type) {
|
|
2445
2445
|
case "concentric": return solveConcentric(ref, dep);
|
|
2446
2446
|
case "angle": return solveAngle(ref, dep, (c.value ?? 0) * Math.PI / 180);
|
|
2447
|
-
|
|
2448
|
-
|
|
2447
|
+
default: {
|
|
2448
|
+
const pose = solveTranslational(ref, dep, c.type === "distance" ? c.value ?? 0 : 0);
|
|
2449
|
+
if (!pose) throw new Error(`solveMate: unsupported entity pair escaped filter: ${ref.type}-${dep.type}`);
|
|
2450
|
+
return pose;
|
|
2451
|
+
}
|
|
2449
2452
|
}
|
|
2450
2453
|
}
|
|
2451
2454
|
/**
|
|
@@ -6742,6 +6745,7 @@ exports.intersect2D = require_boolean2D.intersect2D;
|
|
|
6742
6745
|
exports.intersectBlueprints = require_boolean2D.intersectBlueprints;
|
|
6743
6746
|
exports.intersectWithEvolution = require_healingFns.intersectWithEvolution;
|
|
6744
6747
|
exports.invalidateShapeCache = require_topologyQueryFns.invalidateShapeCache;
|
|
6748
|
+
exports.inverseKinematics = require_threadFns.inverseKinematics;
|
|
6745
6749
|
Object.defineProperty(exports, "io", {
|
|
6746
6750
|
enumerable: true,
|
|
6747
6751
|
get: function() {
|
|
@@ -6793,6 +6797,7 @@ exports.iterSolids = require_topologyQueryFns.iterSolids;
|
|
|
6793
6797
|
exports.iterTopo = require_faceFns.iterTopo;
|
|
6794
6798
|
exports.iterVertices = require_topologyQueryFns.iterVertices;
|
|
6795
6799
|
exports.iterWires = require_topologyQueryFns.iterWires;
|
|
6800
|
+
exports.jointTrajectory = require_threadFns.jointTrajectory;
|
|
6796
6801
|
exports.jointTransform = require_threadFns.jointTransform;
|
|
6797
6802
|
exports.kernelCall = require_topologyQueryFns.kernelCall;
|
|
6798
6803
|
exports.kernelCallRaw = require_topologyQueryFns.kernelCallRaw;
|
package/dist/brepjs.js
CHANGED
|
@@ -15,7 +15,7 @@ import { n as getAtOrThrow, r as lastOrThrow, t as firstOrThrow } from "./arrayA
|
|
|
15
15
|
import { _ as makeThreePointArc, d as makeCircle, h as makeLine, l as makeBSplineInterpolation, n as fill, r as makeFace, s as assembleWire } from "./surfaceBuilders-hTXdNCYm.js";
|
|
16
16
|
import { A as cutAll, C as threePointArc, D as wireLoop, E as wire, F as sectionToFace$1, I as slice$1, L as split$1, M as fuseAll, N as intersect$2, O as booleanPipeline, P as section$1, S as tangentArc, T as vertex, _ as polygon, a as circle, b as sphere$1, c as cylinder, d as ellipsoid, f as face, g as offsetFace, h as line, i as bsplineApprox, j as fuse$2, k as cut$2, l as ellipse, m as helix, n as bezier, o as compound, p as filledFace, r as box, s as cone, t as addHoles, u as ellipseArc, v as sewShells, w as torus$1, x as subFace, y as solid } from "./primitiveFns-BSKbI4Kl.js";
|
|
17
17
|
import { A as edgesOfFace, C as shellWithEvolution, D as getNurbsCurveData, E as fuseAllBisect, F as chamferDistAngle, I as toBufferGeometryData, L as toGroupedBufferGeometryData, M as sharedEdges, N as verticesOfEdge, O as getNurbsSurfaceData, P as wiresOfFace, R as toLODGeometryData, S as intersectWithEvolution, T as cutAllBisect, _ as positionOnCurve, a as healFace, b as filletWithEvolution, c as isValid$1, d as draft$1, f as fillet$1, g as variableFillet, h as thicken$1, i as heal$1, j as facesOfEdge, k as adjacentFaces, l as solidFromShell, m as shell$1, n as fixSelfIntersection, o as healSolid, p as offset$1, r as fixShape, s as healWire, t as autoHeal, u as chamfer$1, v as chamferWithEvolution, w as checkBoolean, x as fuseWithEvolution, y as cutWithEvolution, z as toLineGeometryData } from "./healingFns-Bm-NdBj_.js";
|
|
18
|
-
import { A as
|
|
18
|
+
import { A as quatFromAxisAngle, B as walkAssembly, C as mechanismDOF, D as setJointValue, E as revoluteJoint, F as countNodes, G as createAssembly, I as createAssemblyNode, L as findNode, M as quatRotate, N as addChild, O as setJointValues, P as collectShapes, R as removeChild, S as jointTransform, T as prismaticJoint, U as linearPattern, V as circularPattern, W as exportAssemblySTEP, _ as inverseKinematics, a as deserializeHistory, b as cylindricalJoint, c as modifyStep, d as replayFrom, f as replayHistory, g as undoLast, h as stepsFrom, i as createRegistry, j as quatFromTo, k as sphericalJoint, l as registerOperation, m as stepCount, n as addStep, o as findStep, p as serializeHistory, r as createHistory, s as getShape, t as thread, u as registerShape, v as jointTrajectory, w as planarJoint, x as forwardKinematics, y as addJoint, z as updateNode } from "./threadFns-YzJhd0Kz.js";
|
|
19
19
|
import { n as BaseSketcher2d, r as organiseBlueprints, t as BlueprintSketcher } from "./blueprintSketcher-EYqKWQh0.js";
|
|
20
20
|
import { a as createTypedFinder, i as wireFinder, n as edgeFinder, r as faceFinder, t as getSingleFace } from "./helpers-CD8EMZ5l.js";
|
|
21
21
|
import { A as sketchEllipse, D as makeBaseBox, E as deserializeDrawing, F as sketchRectangle, I as sketchRoundedRectangle, L as FaceSketcher, M as sketchHelix, N as sketchParametricFunction, O as polysideInnerRadius, P as sketchPolysides, R as Sketcher, S as drawText, _ as drawPolysides, a as drawingIntersect, b as drawSingleCircle, c as rotateDrawing, d as drawFaceOutline, f as drawProjection, g as drawPointsInterpolation, h as drawParametricFunction, i as drawingFuse, j as sketchFaceOffset, k as sketchCircle, l as scaleDrawing, m as drawEllipse, n as drawingCut, o as drawingToSketchOnPlane, p as drawCircle, r as drawingFillet, s as mirrorDrawing, t as drawingChamfer, u as translateDrawing, v as drawRectangle, w as draw, x as drawSingleEllipse, y as drawRoundedRectangle } from "./drawFns-D_LwehLr.js";
|
|
@@ -2455,8 +2455,11 @@ function solveMate(c, ref, dep) {
|
|
|
2455
2455
|
switch (c.type) {
|
|
2456
2456
|
case "concentric": return solveConcentric(ref, dep);
|
|
2457
2457
|
case "angle": return solveAngle(ref, dep, (c.value ?? 0) * Math.PI / 180);
|
|
2458
|
-
|
|
2459
|
-
|
|
2458
|
+
default: {
|
|
2459
|
+
const pose = solveTranslational(ref, dep, c.type === "distance" ? c.value ?? 0 : 0);
|
|
2460
|
+
if (!pose) throw new Error(`solveMate: unsupported entity pair escaped filter: ${ref.type}-${dep.type}`);
|
|
2461
|
+
return pose;
|
|
2462
|
+
}
|
|
2460
2463
|
}
|
|
2461
2464
|
}
|
|
2462
2465
|
/**
|
|
@@ -6454,4 +6457,4 @@ var csg_exports = /* @__PURE__ */ __exportAll({
|
|
|
6454
6457
|
withEvaluator: () => withEvaluator
|
|
6455
6458
|
});
|
|
6456
6459
|
//#endregion
|
|
6457
|
-
export { BaseSketcher2d, BlueprintSketcher, BrepBugError, BrepErrorCode, BrepWrapperError, BrepkitAdapter, CompoundSketch, DEFAULT_CAPABILITIES, DEG2RAD, DisposalScope, EXACT_BREP_CAPABILITIES, FaceSketcher, HASH_CODE_MAX, OK, OcctWasmAdapter, RAD2DEG, Sketch, Sketcher, Sketches, addChild, addHoles, addJoint, addMate, addStep, adjacentFaces, all, andThen, applyGlue, applyMatrix, approximateCurve, as2D, as3D, asTopo, assignRoles, autoHeal, bezier, blueprintToDXF, booleanPipeline, booleans_exports as booleans, boss, box, bsplineApprox, bug, cameraFromPlane, cameraLookAt, captureHint, cast, castShape, castShape3D, chamfer, chamferDistAngle as chamferDistAngleShape, chamferWithEvolution, checkAllInterferences, checkBoolean, checkInterference, circle, circularPattern, classifyPointOnFace, clearMeshCache, clone, closedWire, collect, collectShapes, colorFaces, colorShape, complexExtrude, composeTransforms, compound, compoundSketchExtrude, compoundSketchFace, compoundSketchLoft, compoundSketchRevolve, computationError, computeStraightSkeleton, cone, construction_exports as construction, convexHull, cornerFinder, countNodes, createAssembly, createAssemblyNode, createBlueprint, createCamera, createCompound, createCompoundBlueprint, createDistanceQuery, createEdge, createFace, createHandle, createHistory, createKernelHandle, createMeshCache, createNamedPlane, createOperationRegistry, createPlane, createRef, createRegistry, createShell, createSolid, createTaskQueue, createVertex, createWire, createWorkerClient, createWorkerHandler, csg_exports as csg, currentQuality, curve2dBoundingBox, curve2dDistanceFrom, curve2dFirstPoint, curve2dIsOnCurve, curve2dLastPoint, curve2dParameter, curve2dSplitAt, curve2dTangentAt, curveAxis, curveEndPoint, curveIsClosed, curveIsPeriodic, curveLength, curvePeriod, curvePointAt, curveStartPoint, curveTangentAt, cut, cut2D, cutAll, cutAllBisect, cutBlueprints, cutWithEvolution, cylinder, cylindricalJoint, defaultScorer, dequeueTask, describe, deserializeDrawing, deserializeHistory, fromBREP as deserializeShape, downcast, draft, draw, drawCircle, drawEllipse, drawFaceOutline, drawParametricFunction, drawPointsInterpolation, drawPolysides, drawProjection, drawRectangle, drawRoundedRectangle, drawSingleCircle, drawSingleEllipse, drawText, drawingChamfer, drawingCut, drawingFillet, drawingFuse, drawingIntersect, drawingToSketchOnPlane, drill, edgeFinder, edgesOfFace, ellipse, ellipseArc, ellipsoid, enqueueTask, err, exportAssemblySTEP, exportDXF, exportGlb, exportGltf, exportIGES, exportOBJ, exportSTEP, exportSTEPConfigured, exportSTL, exportThreeMF, extrude, extrudeAll, face, faceAxis, faceCenter, faceFinder, faceGeomType, faceOrientation, facesOfEdge, fieldBoolean, fieldContour, fieldOffset, fieldReinit, fieldShell, fill, filledFace, fillet, filletWithEvolution, findFacesByTag, findNode, findStep, fixSelfIntersection, fixShape, flatMap, flatten, flipFaceOrientation, flipOrientation, fontMetrics, forwardKinematics, fromBREP$1 as fromBREP, fromKernelDir, fromKernelPnt, fromKernelVec, fromNullable, fuse, fuse2D, fuseAll, fuseAllBisect, fuseBlueprints, fuseWithEvolution, gearGeometry, getActiveVoxelId, getBounds, getBounds2D, getCompSolids, getCurveType, getDisposalStats, getEdges, getFaceColor, getFaceOrigins, getFaceTags, getFaces, getFont, getHashCode, getShape as getHistoryShape, getKernel, getKernelCapabilities, getKernelTier, getNurbsCurveData, getNurbsSurfaceData, getOrientation, getOrientation2D, getPerformanceStats, getShapeColor, getShapeKind, getShells, getSingleFace, getSolids, getSurfaceType, getTagMetadata, getVertices, getVoxel, getWires, guidedSweep, heal, healFace, healSolid, healWire, helix, hull, importDXF, importGLB, importIGES, importOBJ, importSTEP, importSTL, importSVG, importSVGPathD, importThreeMF, init, initFromManifold, initFromOC, initVoxel, innerWires, interpolateCurve, intersect, intersect2D, intersectBlueprints, intersectWithEvolution, invalidateShapeCache, ioNs_exports as io, ioError, is2D, is3D, isChamferRadius, isClosedWire, isCompSolid, isCompound, isDisposeRequest, isEdge, isEmpty, isEqualShape, isErr, isErrorResponse, isFace, isFilletRadius, isInitRequest, isInside2D, isLive, isManifoldShell, isNumber, isOk, isOperationRequest, isOrientedFace, isPlanarFace, isPlanarWire, isProjectionPlane, isEmpty$1 as isQueueEmpty, isSameShape, isShape1D, isShape3D, isShell, isSolid, isSuccessResponse, isValid, isValidSolid, isVertex, isWire, iterCompSolids, iterEdges, iterFaces, iterShells, iterSolids, iterTopo, iterVertices, iterWires, jointTransform, kernelCall, kernelCallRaw, kernelCallScoped, kernelError, latticeInfill, latticeInfillShape, line, linearPattern, loadFont, loft, loftAll, makeBaseBox, makeExternalGear, makeInternalGear, makePlane, makePlanetaryGear, makeProjectedEdges, manifoldShell, map, mapBoth, mapErr, match, measureArea, measureCurvatureAt, measureCurvatureAtMid, measureDistance, measureDistanceProps, measureLength, measureLinearProps, measureSurfaceProps, measureVolume, measureVolumeProps, measurement_exports as measurement, mechanismDOF, mesh, meshEdges, meshMultiLOD, minkowski, mirror, mirror2D, mirrorDrawing, mirrorJoin, modifiers_exports as modifiers, modifyStep, moduleInitError, multiSectionSweep, normalAt, offset, offsetFace, offsetMesh, offsetShape, offsetWire2D, ok, or, orElse, organiseBlueprints, orientedFace, outerWire, patterns_exports as patterns, pendingCount, pipeline, pivotPlane, planarFace, planarJoint, planarWire, planetPlacements, pocket, pointOnSurface, pointsInside, polygon, polyhedron, polysideInnerRadius, polysidesBlueprint, positionOnCurve, prewarm, primitives_exports as primitives, prismaticJoint, projectEdges, projectPointOnFace, query_exports as query, queryError, rectangularPattern, registerHandler, registerKernel, registerKernelTier, registerOperation, registerShape, registerVoxel, rejectAll, removeChild, removeHolesFromFace, repairMesh, replayFrom, replayHistory, resetDisposalStats, resetPerformanceStats, resize, resolve, resolve3D, resolveDirection, resolvePlane, resolveRef, reverseCurve, revoluteJoint, revolve, roof, rotate, rotate2D, rotateDrawing, roundedRectangleBlueprint, scale, scale2D, scaleDrawing, box$1 as sdfBox, capsule as sdfCapsule, cone$1 as sdfCone, cylinder$1 as sdfCylinder, fieldAxialRamp as sdfFieldAxialRamp, fieldClamp as sdfFieldClamp, fieldConst as sdfFieldConst, fieldFromSdf as sdfFieldFromSdf, fieldRadialRamp as sdfFieldRadialRamp, lattice as sdfLattice, plane as sdfPlane, roundedBox as sdfRoundedBox, sphere as sdfSphere, strutLattice as sdfStrutLattice, sweep as sdfSweep, torus as sdfTorus, section, sectionToFace, serializeHistory, setJointValue, setJointValues, setShapeOrigin, setTagMetadata, sewShells, shape, shapeToMeshInput, shapeType, sharedEdges, shell, shellMesh, shellShape, shellWithEvolution, simplify, sketchCircle, sketchEllipse, sketchExtrude, sketchFace, sketchFaceOffset, sketchHelix, sketchLoft, sketchOnFace2D, sketchOnPlane2D, sketchParametricFunction, sketchPolysides, sketchRectangle, sketchRevolve, sketchRoundedRectangle, sketchSweep, sketchText, sketchWires, sketcherStateError, slice, solid, solidFromShell, solveAssembly, sphere$1 as sphere, sphericalJoint, split, stepCount, stepsFrom, stretch2D, subFace, supportExtrude, supportsConstraintSketch, supportsProjection, surfaceFromGrid, surfaceFromImage, sweep$1 as sweep, tagFaces, tangentArc, tap, tapErr, textBlueprints, textMetrics, thicken, thread, threePointArc, toBREP, toBufferGeometryData, toGroupedBufferGeometryData, toKernelVec, toLODGeometryData, toLineGeometryData, toSVGPathD, toVec2, toVec3, torus$1 as torus, tpmsLattice, transformCopy, transforms_exports as transforms, translate, translate2D, translateDrawing, translatePlane, tryCatch, tryCatchAsync, twistExtrude, typeCastError, undoLast, unsupportedError, unwrap, unwrapErr, unwrapOr, unwrapOrElse, updateNode, updateRoles, uvBounds, uvCoordinates, validSolid, validatePlanetary, validationError, variableFillet, vecAdd, vecAngle, vecCross, vecDistance, vecDot, vecEquals, vecIsZero, vecLength, vecLengthSq, vecNegate, vecNormalize, vecProjectToPlane, vecRepr, vecRotate, vecScale, vecSub, vertex, vertexFinder, vertexPosition, verticesOfEdge, voxelBoolean, voxelBooleanField, voxelBooleanFieldShapes, voxelBooleanShapes, voxelField, voxelFieldFromShape, walkAssembly, windingNumbers, wire, wireFinder, wireLoop, wiresOfFace, withKernel, withKernelDir, withKernelPnt, withKernelVec, withQuality, withScope, withScopeResult, withScopeResultAsync, withTier, zip as zipResults };
|
|
6460
|
+
export { BaseSketcher2d, BlueprintSketcher, BrepBugError, BrepErrorCode, BrepWrapperError, BrepkitAdapter, CompoundSketch, DEFAULT_CAPABILITIES, DEG2RAD, DisposalScope, EXACT_BREP_CAPABILITIES, FaceSketcher, HASH_CODE_MAX, OK, OcctWasmAdapter, RAD2DEG, Sketch, Sketcher, Sketches, addChild, addHoles, addJoint, addMate, addStep, adjacentFaces, all, andThen, applyGlue, applyMatrix, approximateCurve, as2D, as3D, asTopo, assignRoles, autoHeal, bezier, blueprintToDXF, booleanPipeline, booleans_exports as booleans, boss, box, bsplineApprox, bug, cameraFromPlane, cameraLookAt, captureHint, cast, castShape, castShape3D, chamfer, chamferDistAngle as chamferDistAngleShape, chamferWithEvolution, checkAllInterferences, checkBoolean, checkInterference, circle, circularPattern, classifyPointOnFace, clearMeshCache, clone, closedWire, collect, collectShapes, colorFaces, colorShape, complexExtrude, composeTransforms, compound, compoundSketchExtrude, compoundSketchFace, compoundSketchLoft, compoundSketchRevolve, computationError, computeStraightSkeleton, cone, construction_exports as construction, convexHull, cornerFinder, countNodes, createAssembly, createAssemblyNode, createBlueprint, createCamera, createCompound, createCompoundBlueprint, createDistanceQuery, createEdge, createFace, createHandle, createHistory, createKernelHandle, createMeshCache, createNamedPlane, createOperationRegistry, createPlane, createRef, createRegistry, createShell, createSolid, createTaskQueue, createVertex, createWire, createWorkerClient, createWorkerHandler, csg_exports as csg, currentQuality, curve2dBoundingBox, curve2dDistanceFrom, curve2dFirstPoint, curve2dIsOnCurve, curve2dLastPoint, curve2dParameter, curve2dSplitAt, curve2dTangentAt, curveAxis, curveEndPoint, curveIsClosed, curveIsPeriodic, curveLength, curvePeriod, curvePointAt, curveStartPoint, curveTangentAt, cut, cut2D, cutAll, cutAllBisect, cutBlueprints, cutWithEvolution, cylinder, cylindricalJoint, defaultScorer, dequeueTask, describe, deserializeDrawing, deserializeHistory, fromBREP as deserializeShape, downcast, draft, draw, drawCircle, drawEllipse, drawFaceOutline, drawParametricFunction, drawPointsInterpolation, drawPolysides, drawProjection, drawRectangle, drawRoundedRectangle, drawSingleCircle, drawSingleEllipse, drawText, drawingChamfer, drawingCut, drawingFillet, drawingFuse, drawingIntersect, drawingToSketchOnPlane, drill, edgeFinder, edgesOfFace, ellipse, ellipseArc, ellipsoid, enqueueTask, err, exportAssemblySTEP, exportDXF, exportGlb, exportGltf, exportIGES, exportOBJ, exportSTEP, exportSTEPConfigured, exportSTL, exportThreeMF, extrude, extrudeAll, face, faceAxis, faceCenter, faceFinder, faceGeomType, faceOrientation, facesOfEdge, fieldBoolean, fieldContour, fieldOffset, fieldReinit, fieldShell, fill, filledFace, fillet, filletWithEvolution, findFacesByTag, findNode, findStep, fixSelfIntersection, fixShape, flatMap, flatten, flipFaceOrientation, flipOrientation, fontMetrics, forwardKinematics, fromBREP$1 as fromBREP, fromKernelDir, fromKernelPnt, fromKernelVec, fromNullable, fuse, fuse2D, fuseAll, fuseAllBisect, fuseBlueprints, fuseWithEvolution, gearGeometry, getActiveVoxelId, getBounds, getBounds2D, getCompSolids, getCurveType, getDisposalStats, getEdges, getFaceColor, getFaceOrigins, getFaceTags, getFaces, getFont, getHashCode, getShape as getHistoryShape, getKernel, getKernelCapabilities, getKernelTier, getNurbsCurveData, getNurbsSurfaceData, getOrientation, getOrientation2D, getPerformanceStats, getShapeColor, getShapeKind, getShells, getSingleFace, getSolids, getSurfaceType, getTagMetadata, getVertices, getVoxel, getWires, guidedSweep, heal, healFace, healSolid, healWire, helix, hull, importDXF, importGLB, importIGES, importOBJ, importSTEP, importSTL, importSVG, importSVGPathD, importThreeMF, init, initFromManifold, initFromOC, initVoxel, innerWires, interpolateCurve, intersect, intersect2D, intersectBlueprints, intersectWithEvolution, invalidateShapeCache, inverseKinematics, ioNs_exports as io, ioError, is2D, is3D, isChamferRadius, isClosedWire, isCompSolid, isCompound, isDisposeRequest, isEdge, isEmpty, isEqualShape, isErr, isErrorResponse, isFace, isFilletRadius, isInitRequest, isInside2D, isLive, isManifoldShell, isNumber, isOk, isOperationRequest, isOrientedFace, isPlanarFace, isPlanarWire, isProjectionPlane, isEmpty$1 as isQueueEmpty, isSameShape, isShape1D, isShape3D, isShell, isSolid, isSuccessResponse, isValid, isValidSolid, isVertex, isWire, iterCompSolids, iterEdges, iterFaces, iterShells, iterSolids, iterTopo, iterVertices, iterWires, jointTrajectory, jointTransform, kernelCall, kernelCallRaw, kernelCallScoped, kernelError, latticeInfill, latticeInfillShape, line, linearPattern, loadFont, loft, loftAll, makeBaseBox, makeExternalGear, makeInternalGear, makePlane, makePlanetaryGear, makeProjectedEdges, manifoldShell, map, mapBoth, mapErr, match, measureArea, measureCurvatureAt, measureCurvatureAtMid, measureDistance, measureDistanceProps, measureLength, measureLinearProps, measureSurfaceProps, measureVolume, measureVolumeProps, measurement_exports as measurement, mechanismDOF, mesh, meshEdges, meshMultiLOD, minkowski, mirror, mirror2D, mirrorDrawing, mirrorJoin, modifiers_exports as modifiers, modifyStep, moduleInitError, multiSectionSweep, normalAt, offset, offsetFace, offsetMesh, offsetShape, offsetWire2D, ok, or, orElse, organiseBlueprints, orientedFace, outerWire, patterns_exports as patterns, pendingCount, pipeline, pivotPlane, planarFace, planarJoint, planarWire, planetPlacements, pocket, pointOnSurface, pointsInside, polygon, polyhedron, polysideInnerRadius, polysidesBlueprint, positionOnCurve, prewarm, primitives_exports as primitives, prismaticJoint, projectEdges, projectPointOnFace, query_exports as query, queryError, rectangularPattern, registerHandler, registerKernel, registerKernelTier, registerOperation, registerShape, registerVoxel, rejectAll, removeChild, removeHolesFromFace, repairMesh, replayFrom, replayHistory, resetDisposalStats, resetPerformanceStats, resize, resolve, resolve3D, resolveDirection, resolvePlane, resolveRef, reverseCurve, revoluteJoint, revolve, roof, rotate, rotate2D, rotateDrawing, roundedRectangleBlueprint, scale, scale2D, scaleDrawing, box$1 as sdfBox, capsule as sdfCapsule, cone$1 as sdfCone, cylinder$1 as sdfCylinder, fieldAxialRamp as sdfFieldAxialRamp, fieldClamp as sdfFieldClamp, fieldConst as sdfFieldConst, fieldFromSdf as sdfFieldFromSdf, fieldRadialRamp as sdfFieldRadialRamp, lattice as sdfLattice, plane as sdfPlane, roundedBox as sdfRoundedBox, sphere as sdfSphere, strutLattice as sdfStrutLattice, sweep as sdfSweep, torus as sdfTorus, section, sectionToFace, serializeHistory, setJointValue, setJointValues, setShapeOrigin, setTagMetadata, sewShells, shape, shapeToMeshInput, shapeType, sharedEdges, shell, shellMesh, shellShape, shellWithEvolution, simplify, sketchCircle, sketchEllipse, sketchExtrude, sketchFace, sketchFaceOffset, sketchHelix, sketchLoft, sketchOnFace2D, sketchOnPlane2D, sketchParametricFunction, sketchPolysides, sketchRectangle, sketchRevolve, sketchRoundedRectangle, sketchSweep, sketchText, sketchWires, sketcherStateError, slice, solid, solidFromShell, solveAssembly, sphere$1 as sphere, sphericalJoint, split, stepCount, stepsFrom, stretch2D, subFace, supportExtrude, supportsConstraintSketch, supportsProjection, surfaceFromGrid, surfaceFromImage, sweep$1 as sweep, tagFaces, tangentArc, tap, tapErr, textBlueprints, textMetrics, thicken, thread, threePointArc, toBREP, toBufferGeometryData, toGroupedBufferGeometryData, toKernelVec, toLODGeometryData, toLineGeometryData, toSVGPathD, toVec2, toVec3, torus$1 as torus, tpmsLattice, transformCopy, transforms_exports as transforms, translate, translate2D, translateDrawing, translatePlane, tryCatch, tryCatchAsync, twistExtrude, typeCastError, undoLast, unsupportedError, unwrap, unwrapErr, unwrapOr, unwrapOrElse, updateNode, updateRoles, uvBounds, uvCoordinates, validSolid, validatePlanetary, validationError, variableFillet, vecAdd, vecAngle, vecCross, vecDistance, vecDot, vecEquals, vecIsZero, vecLength, vecLengthSq, vecNegate, vecNormalize, vecProjectToPlane, vecRepr, vecRotate, vecScale, vecSub, vertex, vertexFinder, vertexPosition, verticesOfEdge, voxelBoolean, voxelBooleanField, voxelBooleanFieldShapes, voxelBooleanShapes, voxelField, voxelFieldFromShape, walkAssembly, windingNumbers, wire, wireFinder, wireLoop, wiresOfFace, withKernel, withKernelDir, withKernelPnt, withKernelVec, withQuality, withScope, withScopeResult, withScopeResultAsync, withTier, zip as zipResults };
|
package/dist/index.d.ts
CHANGED
|
@@ -122,6 +122,7 @@ export { linearPattern, circularPattern } from './operations/patternFns.js';
|
|
|
122
122
|
export { createAssemblyNode, addChild, removeChild, updateNode, findNode, walkAssembly, countNodes, collectShapes, type AssemblyNode, type AssemblyNodeOptions, } from './operations/assemblyFns.js';
|
|
123
123
|
export { addMate, solveAssembly, type MateConstraint, type MateEntity, type AssemblySolveResult, } from './operations/mateFns.js';
|
|
124
124
|
export { revoluteJoint, prismaticJoint, cylindricalJoint, planarJoint, sphericalJoint, setJointValue, setJointValues, jointTransform, addJoint, forwardKinematics, mechanismDOF, type Joint, type JointDOF, type JointAxis, type JointType, type JointPose, type JointOptions, type CylindricalOptions, type PlanarOptions, type SphericalOptions, } from './operations/jointFns.js';
|
|
125
|
+
export { inverseKinematics, jointTrajectory, type IKTarget, type IKOptions, type IKResult, type TrajectorySample, } from './operations/ikFns.js';
|
|
125
126
|
export { createHistory, addStep, undoLast, findStep, getShape as getHistoryShape, stepCount, stepsFrom, registerShape, createRegistry, registerOperation, replayHistory, replayFrom, modifyStep, serializeHistory, deserializeHistory, type OperationStep, type ModelHistory, type SerializedHistory, type OperationFn, type OperationRegistry as HistoryOperationRegistry, } from './operations/historyFns.js';
|
|
126
127
|
export { measureVolume, measureArea, measureLength, measureDistance, measureDistanceProps, createDistanceQuery, measureVolumeProps, measureSurfaceProps, measureLinearProps, type PhysicalProps, type VolumeProps, type SurfaceProps, type LinearProps, type DistanceProps, measureCurvatureAt, measureCurvatureAtMid, type CurvatureResult, } from './measurement/measureFns.js';
|
|
127
128
|
export { checkInterference, checkAllInterferences, type InterferenceResult, type InterferencePair, } from './measurement/interferenceFns.js';
|
|
@@ -0,0 +1,58 @@
|
|
|
1
|
+
import { Vec3 } from '../core/types.js';
|
|
2
|
+
import { AssemblyNode } from './assemblyFns.js';
|
|
3
|
+
import { JointPose } from './jointFns.js';
|
|
4
|
+
type Quat = readonly [number, number, number, number];
|
|
5
|
+
/** A target for the end-effector: a world position, optionally an orientation. */
|
|
6
|
+
export interface IKTarget {
|
|
7
|
+
readonly position: Vec3;
|
|
8
|
+
/** Target orientation `[w, x, y, z]`. Omit for position-only IK. */
|
|
9
|
+
readonly rotation?: Quat;
|
|
10
|
+
}
|
|
11
|
+
export interface IKOptions {
|
|
12
|
+
/** Maximum solver iterations. Default 200. */
|
|
13
|
+
maxIterations?: number;
|
|
14
|
+
/** Convergence threshold on the residual norm. Default 1e-5. */
|
|
15
|
+
tolerance?: number;
|
|
16
|
+
/** Damping factor λ for the least-squares step. Default 0.05. */
|
|
17
|
+
damping?: number;
|
|
18
|
+
/** Initial joint values, keyed by child node (number or per-DOF array). */
|
|
19
|
+
seed?: Readonly<Record<string, number | readonly number[]>>;
|
|
20
|
+
/** Local point on the end-effector node to drive to the target. Default origin. */
|
|
21
|
+
tip?: Vec3;
|
|
22
|
+
}
|
|
23
|
+
export interface IKResult {
|
|
24
|
+
/** Solved joint values, keyed by child node, one entry per DOF. */
|
|
25
|
+
readonly values: Record<string, number[]>;
|
|
26
|
+
readonly converged: boolean;
|
|
27
|
+
readonly iterations: number;
|
|
28
|
+
/** Final residual norm (position, plus orientation when targeted). */
|
|
29
|
+
readonly error: number;
|
|
30
|
+
}
|
|
31
|
+
/**
|
|
32
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
33
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
34
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
35
|
+
*
|
|
36
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
37
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
38
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
39
|
+
* target, returns `converged: false` with the best configuration found.
|
|
40
|
+
*/
|
|
41
|
+
export declare function inverseKinematics(assembly: AssemblyNode, endEffector: string, target: IKTarget, options?: IKOptions): IKResult;
|
|
42
|
+
export interface TrajectorySample {
|
|
43
|
+
/** Normalized path parameter in `[0, 1]`. */
|
|
44
|
+
readonly t: number;
|
|
45
|
+
/** Interpolated joint values at this step, keyed by child node. */
|
|
46
|
+
readonly values: Record<string, number[]>;
|
|
47
|
+
/** Forward-kinematics world poses for every node at this step. */
|
|
48
|
+
readonly poses: Map<string, JointPose>;
|
|
49
|
+
}
|
|
50
|
+
/**
|
|
51
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
52
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
53
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
54
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
55
|
+
* their stored value at both ends.
|
|
56
|
+
*/
|
|
57
|
+
export declare function jointTrajectory(assembly: AssemblyNode, from: Readonly<Record<string, number | readonly number[]>>, to: Readonly<Record<string, number | readonly number[]>>, steps: number): TrajectorySample[];
|
|
58
|
+
export {};
|
|
@@ -50,7 +50,12 @@ export interface JointOptions {
|
|
|
50
50
|
export interface CylindricalOptions {
|
|
51
51
|
/** Rotation DOF (degrees). Default range -180..180. */
|
|
52
52
|
rotation?: JointOptions;
|
|
53
|
-
/**
|
|
53
|
+
/**
|
|
54
|
+
* Translation DOF (length). Default range 0..100, matching `prismaticJoint`
|
|
55
|
+
* (both model a slide along an axis). This is deliberately asymmetric with
|
|
56
|
+
* `planarJoint`'s in-plane translations, which default to -100..100 because
|
|
57
|
+
* an unanchored in-plane slide is naturally bidirectional.
|
|
58
|
+
*/
|
|
54
59
|
translation?: JointOptions;
|
|
55
60
|
}
|
|
56
61
|
/** Per-DOF ranges for a planar joint (two in-plane translations + a rotation). */
|
package/dist/operations.cjs
CHANGED
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
Object.defineProperty(exports, Symbol.toStringTag, { value: "Module" });
|
|
2
|
-
const require_threadFns = require("./threadFns-
|
|
2
|
+
const require_threadFns = require("./threadFns-By0Gy2f9.cjs");
|
|
3
3
|
const require_loftFns = require("./loftFns-DiSsI2PM.cjs");
|
|
4
4
|
exports.addChild = require_threadFns.addChild;
|
|
5
5
|
exports.addJoint = require_threadFns.addJoint;
|
|
@@ -19,6 +19,8 @@ exports.findStep = require_threadFns.findStep;
|
|
|
19
19
|
exports.forwardKinematics = require_threadFns.forwardKinematics;
|
|
20
20
|
exports.getHistoryShape = require_threadFns.getShape;
|
|
21
21
|
exports.gridPattern = require_threadFns.gridPattern;
|
|
22
|
+
exports.inverseKinematics = require_threadFns.inverseKinematics;
|
|
23
|
+
exports.jointTrajectory = require_threadFns.jointTrajectory;
|
|
22
24
|
exports.jointTransform = require_threadFns.jointTransform;
|
|
23
25
|
exports.linearPattern = require_threadFns.linearPattern;
|
|
24
26
|
exports.mechanismDOF = require_threadFns.mechanismDOF;
|
package/dist/operations.d.ts
CHANGED
|
@@ -11,6 +11,7 @@ export { thread, type ThreadOptions } from './operations/threadFns.js';
|
|
|
11
11
|
export { linearPattern, circularPattern, gridPattern } from './operations/patternFns.js';
|
|
12
12
|
export { createAssemblyNode, addChild, removeChild, updateNode, findNode, walkAssembly, countNodes, collectShapes, type AssemblyNode, type AssemblyNodeOptions, } from './operations/assemblyFns.js';
|
|
13
13
|
export { revoluteJoint, prismaticJoint, cylindricalJoint, planarJoint, sphericalJoint, setJointValue, setJointValues, jointTransform, addJoint, forwardKinematics, mechanismDOF, type Joint, type JointDOF, type JointAxis, type JointType, type JointPose, type JointOptions, type CylindricalOptions, type PlanarOptions, type SphericalOptions, } from './operations/jointFns.js';
|
|
14
|
+
export { inverseKinematics, jointTrajectory, type IKTarget, type IKOptions, type IKResult, type TrajectorySample, } from './operations/ikFns.js';
|
|
14
15
|
export { exportAssemblySTEP, type ShapeOptions, type SupportedUnit, } from './operations/exporterFns.js';
|
|
15
16
|
export { createHistory, addStep, undoLast, findStep, getShape as getHistoryShape, stepCount, stepsFrom, registerShape, createRegistry, registerOperation, replayHistory, replayFrom, modifyStep, type OperationStep, type ModelHistory, type OperationFn, type OperationRegistry as HistoryOperationRegistry, } from './operations/historyFns.js';
|
|
16
17
|
export { type AssemblyExporter, createAssembly } from './operations/exporters.js';
|
package/dist/operations.js
CHANGED
|
@@ -1,3 +1,3 @@
|
|
|
1
|
-
import { B as
|
|
1
|
+
import { B as walkAssembly, C as mechanismDOF, D as setJointValue, E as revoluteJoint, F as countNodes, G as createAssembly, H as gridPattern, I as createAssemblyNode, L as findNode, N as addChild, O as setJointValues, P as collectShapes, R as removeChild, S as jointTransform, T as prismaticJoint, U as linearPattern, V as circularPattern, W as exportAssemblySTEP, _ as inverseKinematics, b as cylindricalJoint, c as modifyStep, d as replayFrom, f as replayHistory, g as undoLast, h as stepsFrom, i as createRegistry, k as sphericalJoint, l as registerOperation, m as stepCount, n as addStep, o as findStep, r as createHistory, s as getShape, t as thread, u as registerShape, v as jointTrajectory, w as planarJoint, x as forwardKinematics, y as addJoint, z as updateNode } from "./threadFns-YzJhd0Kz.js";
|
|
2
2
|
import { d as twistExtrude, l as supportExtrude, o as complexExtrude, u as sweep } from "./loftFns-CsHOwded.js";
|
|
3
|
-
export { addChild, addJoint, addStep, circularPattern, collectShapes, complexExtrude, countNodes, createAssembly, createAssemblyNode, createHistory, createRegistry, cylindricalJoint, exportAssemblySTEP, findNode, findStep, forwardKinematics, getShape as getHistoryShape, gridPattern, jointTransform, linearPattern, mechanismDOF, modifyStep, planarJoint, prismaticJoint, registerOperation, registerShape, removeChild, replayFrom, replayHistory, revoluteJoint, setJointValue, setJointValues, sphericalJoint, stepCount, stepsFrom, supportExtrude, sweep, thread, twistExtrude, undoLast, updateNode, walkAssembly };
|
|
3
|
+
export { addChild, addJoint, addStep, circularPattern, collectShapes, complexExtrude, countNodes, createAssembly, createAssemblyNode, createHistory, createRegistry, cylindricalJoint, exportAssemblySTEP, findNode, findStep, forwardKinematics, getShape as getHistoryShape, gridPattern, inverseKinematics, jointTrajectory, jointTransform, linearPattern, mechanismDOF, modifyStep, planarJoint, prismaticJoint, registerOperation, registerShape, removeChild, replayFrom, replayHistory, revoluteJoint, setJointValue, setJointValues, sphericalJoint, stepCount, stepsFrom, supportExtrude, sweep, thread, twistExtrude, undoLast, updateNode, walkAssembly };
|
|
@@ -565,7 +565,7 @@ function jointTransform(joint, value = joint.value) {
|
|
|
565
565
|
const overrides = Array.isArray(value) ? value : void 0;
|
|
566
566
|
const primary = overrides ? void 0 : value;
|
|
567
567
|
const origin = joint.axis.origin;
|
|
568
|
-
let pose = IDENTITY_POSE;
|
|
568
|
+
let pose = IDENTITY_POSE$1;
|
|
569
569
|
for (let i = 0; i < joint.dofs.length; i++) {
|
|
570
570
|
const dof = joint.dofs[i];
|
|
571
571
|
if (!dof) continue;
|
|
@@ -582,7 +582,7 @@ function addJoint(assembly, joint) {
|
|
|
582
582
|
joints: [...existing, joint]
|
|
583
583
|
};
|
|
584
584
|
}
|
|
585
|
-
var IDENTITY_POSE = {
|
|
585
|
+
var IDENTITY_POSE$1 = {
|
|
586
586
|
position: [
|
|
587
587
|
0,
|
|
588
588
|
0,
|
|
@@ -639,7 +639,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
639
639
|
names.add(j.child);
|
|
640
640
|
}
|
|
641
641
|
const poses = /* @__PURE__ */ new Map();
|
|
642
|
-
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE);
|
|
642
|
+
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE$1);
|
|
643
643
|
const pending = [...joints];
|
|
644
644
|
let progress = true;
|
|
645
645
|
while (progress && pending.length > 0) {
|
|
@@ -656,7 +656,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
656
656
|
poses.set(j.child, composePose(parentPose, jointTransform(j, value)));
|
|
657
657
|
}
|
|
658
658
|
}
|
|
659
|
-
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE);
|
|
659
|
+
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE$1);
|
|
660
660
|
return poses;
|
|
661
661
|
}
|
|
662
662
|
/**
|
|
@@ -669,6 +669,306 @@ function mechanismDOF(assembly) {
|
|
|
669
669
|
return collectJoints(assembly).reduce((sum, j) => sum + j.dofs.length, 0);
|
|
670
670
|
}
|
|
671
671
|
//#endregion
|
|
672
|
+
//#region src/operations/ikFns.ts
|
|
673
|
+
var IDENTITY_POSE = {
|
|
674
|
+
position: [
|
|
675
|
+
0,
|
|
676
|
+
0,
|
|
677
|
+
0
|
|
678
|
+
],
|
|
679
|
+
rotation: [
|
|
680
|
+
1,
|
|
681
|
+
0,
|
|
682
|
+
0,
|
|
683
|
+
0
|
|
684
|
+
]
|
|
685
|
+
};
|
|
686
|
+
function applyPose(pose, p) {
|
|
687
|
+
const r = quatRotate(pose.rotation, p);
|
|
688
|
+
return [
|
|
689
|
+
r[0] + pose.position[0],
|
|
690
|
+
r[1] + pose.position[1],
|
|
691
|
+
r[2] + pose.position[2]
|
|
692
|
+
];
|
|
693
|
+
}
|
|
694
|
+
function quatConjugate(q) {
|
|
695
|
+
return [
|
|
696
|
+
q[0],
|
|
697
|
+
-q[1],
|
|
698
|
+
-q[2],
|
|
699
|
+
-q[3]
|
|
700
|
+
];
|
|
701
|
+
}
|
|
702
|
+
/**
|
|
703
|
+
* The rotation vector (axis · angle) taking orientation `from` to `to`, i.e. the
|
|
704
|
+
* angular error that drives `from` toward `to`. Returns the zero vector when the
|
|
705
|
+
* orientations coincide.
|
|
706
|
+
*/
|
|
707
|
+
function rotationError(from, to) {
|
|
708
|
+
let [w, x, y, z] = quatMultiply(to, quatConjugate(from));
|
|
709
|
+
const norm = Math.hypot(w, x, y, z) || 1;
|
|
710
|
+
w /= norm;
|
|
711
|
+
x /= norm;
|
|
712
|
+
y /= norm;
|
|
713
|
+
z /= norm;
|
|
714
|
+
if (w < 0) {
|
|
715
|
+
w = -w;
|
|
716
|
+
x = -x;
|
|
717
|
+
y = -y;
|
|
718
|
+
z = -z;
|
|
719
|
+
}
|
|
720
|
+
const s = Math.hypot(x, y, z);
|
|
721
|
+
if (s < 1e-12) return [
|
|
722
|
+
0,
|
|
723
|
+
0,
|
|
724
|
+
0
|
|
725
|
+
];
|
|
726
|
+
const k = 2 * Math.atan2(s, w) / s;
|
|
727
|
+
return [
|
|
728
|
+
x * k,
|
|
729
|
+
y * k,
|
|
730
|
+
z * k
|
|
731
|
+
];
|
|
732
|
+
}
|
|
733
|
+
/** Joints from the root down to `endEffector`, in root→leaf order. */
|
|
734
|
+
function chainTo(assembly, endEffector) {
|
|
735
|
+
const joints = [];
|
|
736
|
+
walkAssembly(assembly, (n) => {
|
|
737
|
+
if (n.joints) joints.push(...n.joints);
|
|
738
|
+
});
|
|
739
|
+
const byChild = /* @__PURE__ */ new Map();
|
|
740
|
+
for (const j of joints) byChild.set(j.child, j);
|
|
741
|
+
const chain = [];
|
|
742
|
+
const seen = /* @__PURE__ */ new Set();
|
|
743
|
+
let cur = endEffector;
|
|
744
|
+
while (cur && byChild.has(cur) && !seen.has(cur)) {
|
|
745
|
+
seen.add(cur);
|
|
746
|
+
const j = byChild.get(cur);
|
|
747
|
+
if (!j) break;
|
|
748
|
+
chain.push(j);
|
|
749
|
+
cur = j.parent;
|
|
750
|
+
}
|
|
751
|
+
return chain.reverse();
|
|
752
|
+
}
|
|
753
|
+
/** Read a Float64Array element as a definite number (dense matrices are full). */
|
|
754
|
+
function el(a, i) {
|
|
755
|
+
return a[i] ?? 0;
|
|
756
|
+
}
|
|
757
|
+
/** Solve `A x = b` for an `n×n` system by Gauss-Jordan with partial pivoting. */
|
|
758
|
+
function solveLinear(A, b, n) {
|
|
759
|
+
const w = n + 1;
|
|
760
|
+
const M = new Float64Array(n * w);
|
|
761
|
+
for (let r = 0; r < n; r++) {
|
|
762
|
+
for (let c = 0; c < n; c++) M[r * w + c] = el(A, r * n + c);
|
|
763
|
+
M[r * w + n] = el(b, r);
|
|
764
|
+
}
|
|
765
|
+
for (let col = 0; col < n; col++) {
|
|
766
|
+
let piv = col;
|
|
767
|
+
for (let r = col + 1; r < n; r++) if (Math.abs(el(M, r * w + col)) > Math.abs(el(M, piv * w + col))) piv = r;
|
|
768
|
+
if (Math.abs(el(M, piv * w + col)) < 1e-12) return null;
|
|
769
|
+
if (piv !== col) for (let k = col; k < w; k++) {
|
|
770
|
+
const tmp = el(M, col * w + k);
|
|
771
|
+
M[col * w + k] = el(M, piv * w + k);
|
|
772
|
+
M[piv * w + k] = tmp;
|
|
773
|
+
}
|
|
774
|
+
const d = el(M, col * w + col);
|
|
775
|
+
for (let k = col; k < w; k++) M[col * w + k] = el(M, col * w + k) / d;
|
|
776
|
+
for (let r = 0; r < n; r++) {
|
|
777
|
+
if (r === col) continue;
|
|
778
|
+
const f = el(M, r * w + col);
|
|
779
|
+
if (f === 0) continue;
|
|
780
|
+
for (let k = col; k < w; k++) M[r * w + k] = el(M, r * w + k) - f * el(M, col * w + k);
|
|
781
|
+
}
|
|
782
|
+
}
|
|
783
|
+
const x = new Float64Array(n);
|
|
784
|
+
for (let r = 0; r < n; r++) x[r] = el(M, r * w + n);
|
|
785
|
+
return x;
|
|
786
|
+
}
|
|
787
|
+
/** Flatten a chain's DOFs into a parameter vector with bounds, applying the seed. */
|
|
788
|
+
function flattenChain(chain, seed) {
|
|
789
|
+
const segments = [];
|
|
790
|
+
const q = [];
|
|
791
|
+
const lo = [];
|
|
792
|
+
const hi = [];
|
|
793
|
+
for (const j of chain) {
|
|
794
|
+
const s = seed?.[j.child];
|
|
795
|
+
segments.push({
|
|
796
|
+
child: j.child,
|
|
797
|
+
count: j.dofs.length
|
|
798
|
+
});
|
|
799
|
+
j.dofs.forEach((dof, i) => {
|
|
800
|
+
const v = (Array.isArray(s) ? s[i] : i === 0 ? s : void 0) ?? dof.value;
|
|
801
|
+
q.push(Math.min(dof.max, Math.max(dof.min, v)));
|
|
802
|
+
lo.push(dof.min);
|
|
803
|
+
hi.push(dof.max);
|
|
804
|
+
});
|
|
805
|
+
}
|
|
806
|
+
return {
|
|
807
|
+
segments,
|
|
808
|
+
q: Float64Array.from(q),
|
|
809
|
+
lo: Float64Array.from(lo),
|
|
810
|
+
hi: Float64Array.from(hi)
|
|
811
|
+
};
|
|
812
|
+
}
|
|
813
|
+
/** Slice a parameter vector back into per-joint value arrays keyed by child. */
|
|
814
|
+
function overridesOf(segments, q) {
|
|
815
|
+
const out = {};
|
|
816
|
+
let i = 0;
|
|
817
|
+
for (const seg of segments) {
|
|
818
|
+
const vals = [];
|
|
819
|
+
for (let k = 0; k < seg.count; k++) vals.push(el(q, i++));
|
|
820
|
+
out[seg.child] = vals;
|
|
821
|
+
}
|
|
822
|
+
return out;
|
|
823
|
+
}
|
|
824
|
+
/** Residual twist `e` (target − current) for a pose; returns its norm. */
|
|
825
|
+
function residualTwist(out, pose, target, tip, m) {
|
|
826
|
+
const pos = applyPose(pose, tip);
|
|
827
|
+
out[0] = target.position[0] - pos[0];
|
|
828
|
+
out[1] = target.position[1] - pos[1];
|
|
829
|
+
out[2] = target.position[2] - pos[2];
|
|
830
|
+
if (m === 6 && target.rotation) {
|
|
831
|
+
const r = rotationError(pose.rotation, target.rotation);
|
|
832
|
+
out[3] = r[0];
|
|
833
|
+
out[4] = r[1];
|
|
834
|
+
out[5] = r[2];
|
|
835
|
+
}
|
|
836
|
+
let s = 0;
|
|
837
|
+
for (let i = 0; i < m; i++) s += el(out, i) ** 2;
|
|
838
|
+
return Math.sqrt(s);
|
|
839
|
+
}
|
|
840
|
+
/**
|
|
841
|
+
* Finite-difference Jacobian: column `j` is the end-effector twist from δq[j].
|
|
842
|
+
* The probe steps *inward* from a bound — `forwardKinematics` clamps each DOF to
|
|
843
|
+
* its range, so a forward `+eps` at the upper limit would yield a zero column and
|
|
844
|
+
* trap the solver at the ceiling. Stepping `-eps` there (and dividing by the
|
|
845
|
+
* signed step) keeps every column a true one-sided derivative.
|
|
846
|
+
*/
|
|
847
|
+
function fillJacobian(J, q, n, m, lo, hi, base, tip, eps, tipPose) {
|
|
848
|
+
const basePos = applyPose(base, tip);
|
|
849
|
+
for (let j = 0; j < n; j++) {
|
|
850
|
+
const saved = el(q, j);
|
|
851
|
+
const h = saved + eps > el(hi, j) && saved - eps >= el(lo, j) ? -eps : eps;
|
|
852
|
+
q[j] = saved + h;
|
|
853
|
+
const p2 = tipPose(q);
|
|
854
|
+
q[j] = saved;
|
|
855
|
+
const pos2 = applyPose(p2, tip);
|
|
856
|
+
J[j] = (pos2[0] - basePos[0]) / h;
|
|
857
|
+
J[n + j] = (pos2[1] - basePos[1]) / h;
|
|
858
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / h;
|
|
859
|
+
if (m === 6) {
|
|
860
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
861
|
+
J[3 * n + j] = dr[0] / h;
|
|
862
|
+
J[4 * n + j] = dr[1] / h;
|
|
863
|
+
J[5 * n + j] = dr[2] / h;
|
|
864
|
+
}
|
|
865
|
+
}
|
|
866
|
+
}
|
|
867
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
868
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
869
|
+
const A = new Float64Array(m * m);
|
|
870
|
+
const lam2 = lambda * lambda;
|
|
871
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
872
|
+
let s = 0;
|
|
873
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
874
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
875
|
+
}
|
|
876
|
+
const y = solveLinear(A, e, m);
|
|
877
|
+
if (!y) return null;
|
|
878
|
+
const dq = new Float64Array(n);
|
|
879
|
+
for (let j = 0; j < n; j++) {
|
|
880
|
+
let v = 0;
|
|
881
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
882
|
+
dq[j] = v;
|
|
883
|
+
}
|
|
884
|
+
return dq;
|
|
885
|
+
}
|
|
886
|
+
/**
|
|
887
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
888
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
889
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
890
|
+
*
|
|
891
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
892
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
893
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
894
|
+
* target, returns `converged: false` with the best configuration found.
|
|
895
|
+
*/
|
|
896
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
897
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
898
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
899
|
+
const lambda = options.damping ?? .05;
|
|
900
|
+
const tip = options.tip ?? [
|
|
901
|
+
0,
|
|
902
|
+
0,
|
|
903
|
+
0
|
|
904
|
+
];
|
|
905
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
906
|
+
const eps = 1e-6;
|
|
907
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
908
|
+
const n = q.length;
|
|
909
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
910
|
+
const e = new Float64Array(m);
|
|
911
|
+
const J = new Float64Array(m * n);
|
|
912
|
+
let pose = tipPose(q);
|
|
913
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
914
|
+
let iter = 0;
|
|
915
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
916
|
+
fillJacobian(J, q, n, m, lo, hi, pose, tip, eps, tipPose);
|
|
917
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
918
|
+
if (!dq) break;
|
|
919
|
+
for (let j = 0; j < n; j++) {
|
|
920
|
+
const next = el(q, j) + el(dq, j);
|
|
921
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
922
|
+
}
|
|
923
|
+
pose = tipPose(q);
|
|
924
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
925
|
+
}
|
|
926
|
+
return {
|
|
927
|
+
values: overridesOf(segments, q),
|
|
928
|
+
converged: err <= tolerance,
|
|
929
|
+
iterations: iter,
|
|
930
|
+
error: err
|
|
931
|
+
};
|
|
932
|
+
}
|
|
933
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
934
|
+
function valuesOf(joint, spec) {
|
|
935
|
+
return joint.dofs.map((dof, i) => {
|
|
936
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
937
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
938
|
+
});
|
|
939
|
+
}
|
|
940
|
+
/**
|
|
941
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
942
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
943
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
944
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
945
|
+
* their stored value at both ends.
|
|
946
|
+
*/
|
|
947
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
948
|
+
const joints = [];
|
|
949
|
+
walkAssembly(assembly, (n) => {
|
|
950
|
+
if (n.joints) joints.push(...n.joints);
|
|
951
|
+
});
|
|
952
|
+
const ends = joints.map((j) => ({
|
|
953
|
+
child: j.child,
|
|
954
|
+
a: valuesOf(j, from[j.child]),
|
|
955
|
+
b: valuesOf(j, to[j.child])
|
|
956
|
+
}));
|
|
957
|
+
const count = Math.max(1, Math.floor(steps));
|
|
958
|
+
const samples = [];
|
|
959
|
+
for (let s = 0; s <= count; s++) {
|
|
960
|
+
const t = s / count;
|
|
961
|
+
const values = {};
|
|
962
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
963
|
+
samples.push({
|
|
964
|
+
t,
|
|
965
|
+
values,
|
|
966
|
+
poses: forwardKinematics(assembly, values)
|
|
967
|
+
});
|
|
968
|
+
}
|
|
969
|
+
return samples;
|
|
970
|
+
}
|
|
971
|
+
//#endregion
|
|
672
972
|
//#region src/operations/historyFns.ts
|
|
673
973
|
/** Create a new empty history. */
|
|
674
974
|
function createHistory() {
|
|
@@ -1044,6 +1344,18 @@ Object.defineProperty(exports, "gridPattern", {
|
|
|
1044
1344
|
return gridPattern;
|
|
1045
1345
|
}
|
|
1046
1346
|
});
|
|
1347
|
+
Object.defineProperty(exports, "inverseKinematics", {
|
|
1348
|
+
enumerable: true,
|
|
1349
|
+
get: function() {
|
|
1350
|
+
return inverseKinematics;
|
|
1351
|
+
}
|
|
1352
|
+
});
|
|
1353
|
+
Object.defineProperty(exports, "jointTrajectory", {
|
|
1354
|
+
enumerable: true,
|
|
1355
|
+
get: function() {
|
|
1356
|
+
return jointTrajectory;
|
|
1357
|
+
}
|
|
1358
|
+
});
|
|
1047
1359
|
Object.defineProperty(exports, "jointTransform", {
|
|
1048
1360
|
enumerable: true,
|
|
1049
1361
|
get: function() {
|
|
@@ -565,7 +565,7 @@ function jointTransform(joint, value = joint.value) {
|
|
|
565
565
|
const overrides = Array.isArray(value) ? value : void 0;
|
|
566
566
|
const primary = overrides ? void 0 : value;
|
|
567
567
|
const origin = joint.axis.origin;
|
|
568
|
-
let pose = IDENTITY_POSE;
|
|
568
|
+
let pose = IDENTITY_POSE$1;
|
|
569
569
|
for (let i = 0; i < joint.dofs.length; i++) {
|
|
570
570
|
const dof = joint.dofs[i];
|
|
571
571
|
if (!dof) continue;
|
|
@@ -582,7 +582,7 @@ function addJoint(assembly, joint) {
|
|
|
582
582
|
joints: [...existing, joint]
|
|
583
583
|
};
|
|
584
584
|
}
|
|
585
|
-
var IDENTITY_POSE = {
|
|
585
|
+
var IDENTITY_POSE$1 = {
|
|
586
586
|
position: [
|
|
587
587
|
0,
|
|
588
588
|
0,
|
|
@@ -639,7 +639,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
639
639
|
names.add(j.child);
|
|
640
640
|
}
|
|
641
641
|
const poses = /* @__PURE__ */ new Map();
|
|
642
|
-
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE);
|
|
642
|
+
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE$1);
|
|
643
643
|
const pending = [...joints];
|
|
644
644
|
let progress = true;
|
|
645
645
|
while (progress && pending.length > 0) {
|
|
@@ -656,7 +656,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
656
656
|
poses.set(j.child, composePose(parentPose, jointTransform(j, value)));
|
|
657
657
|
}
|
|
658
658
|
}
|
|
659
|
-
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE);
|
|
659
|
+
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE$1);
|
|
660
660
|
return poses;
|
|
661
661
|
}
|
|
662
662
|
/**
|
|
@@ -669,6 +669,306 @@ function mechanismDOF(assembly) {
|
|
|
669
669
|
return collectJoints(assembly).reduce((sum, j) => sum + j.dofs.length, 0);
|
|
670
670
|
}
|
|
671
671
|
//#endregion
|
|
672
|
+
//#region src/operations/ikFns.ts
|
|
673
|
+
var IDENTITY_POSE = {
|
|
674
|
+
position: [
|
|
675
|
+
0,
|
|
676
|
+
0,
|
|
677
|
+
0
|
|
678
|
+
],
|
|
679
|
+
rotation: [
|
|
680
|
+
1,
|
|
681
|
+
0,
|
|
682
|
+
0,
|
|
683
|
+
0
|
|
684
|
+
]
|
|
685
|
+
};
|
|
686
|
+
function applyPose(pose, p) {
|
|
687
|
+
const r = quatRotate(pose.rotation, p);
|
|
688
|
+
return [
|
|
689
|
+
r[0] + pose.position[0],
|
|
690
|
+
r[1] + pose.position[1],
|
|
691
|
+
r[2] + pose.position[2]
|
|
692
|
+
];
|
|
693
|
+
}
|
|
694
|
+
function quatConjugate(q) {
|
|
695
|
+
return [
|
|
696
|
+
q[0],
|
|
697
|
+
-q[1],
|
|
698
|
+
-q[2],
|
|
699
|
+
-q[3]
|
|
700
|
+
];
|
|
701
|
+
}
|
|
702
|
+
/**
|
|
703
|
+
* The rotation vector (axis · angle) taking orientation `from` to `to`, i.e. the
|
|
704
|
+
* angular error that drives `from` toward `to`. Returns the zero vector when the
|
|
705
|
+
* orientations coincide.
|
|
706
|
+
*/
|
|
707
|
+
function rotationError(from, to) {
|
|
708
|
+
let [w, x, y, z] = quatMultiply(to, quatConjugate(from));
|
|
709
|
+
const norm = Math.hypot(w, x, y, z) || 1;
|
|
710
|
+
w /= norm;
|
|
711
|
+
x /= norm;
|
|
712
|
+
y /= norm;
|
|
713
|
+
z /= norm;
|
|
714
|
+
if (w < 0) {
|
|
715
|
+
w = -w;
|
|
716
|
+
x = -x;
|
|
717
|
+
y = -y;
|
|
718
|
+
z = -z;
|
|
719
|
+
}
|
|
720
|
+
const s = Math.hypot(x, y, z);
|
|
721
|
+
if (s < 1e-12) return [
|
|
722
|
+
0,
|
|
723
|
+
0,
|
|
724
|
+
0
|
|
725
|
+
];
|
|
726
|
+
const k = 2 * Math.atan2(s, w) / s;
|
|
727
|
+
return [
|
|
728
|
+
x * k,
|
|
729
|
+
y * k,
|
|
730
|
+
z * k
|
|
731
|
+
];
|
|
732
|
+
}
|
|
733
|
+
/** Joints from the root down to `endEffector`, in root→leaf order. */
|
|
734
|
+
function chainTo(assembly, endEffector) {
|
|
735
|
+
const joints = [];
|
|
736
|
+
walkAssembly(assembly, (n) => {
|
|
737
|
+
if (n.joints) joints.push(...n.joints);
|
|
738
|
+
});
|
|
739
|
+
const byChild = /* @__PURE__ */ new Map();
|
|
740
|
+
for (const j of joints) byChild.set(j.child, j);
|
|
741
|
+
const chain = [];
|
|
742
|
+
const seen = /* @__PURE__ */ new Set();
|
|
743
|
+
let cur = endEffector;
|
|
744
|
+
while (cur && byChild.has(cur) && !seen.has(cur)) {
|
|
745
|
+
seen.add(cur);
|
|
746
|
+
const j = byChild.get(cur);
|
|
747
|
+
if (!j) break;
|
|
748
|
+
chain.push(j);
|
|
749
|
+
cur = j.parent;
|
|
750
|
+
}
|
|
751
|
+
return chain.reverse();
|
|
752
|
+
}
|
|
753
|
+
/** Read a Float64Array element as a definite number (dense matrices are full). */
|
|
754
|
+
function el(a, i) {
|
|
755
|
+
return a[i] ?? 0;
|
|
756
|
+
}
|
|
757
|
+
/** Solve `A x = b` for an `n×n` system by Gauss-Jordan with partial pivoting. */
|
|
758
|
+
function solveLinear(A, b, n) {
|
|
759
|
+
const w = n + 1;
|
|
760
|
+
const M = new Float64Array(n * w);
|
|
761
|
+
for (let r = 0; r < n; r++) {
|
|
762
|
+
for (let c = 0; c < n; c++) M[r * w + c] = el(A, r * n + c);
|
|
763
|
+
M[r * w + n] = el(b, r);
|
|
764
|
+
}
|
|
765
|
+
for (let col = 0; col < n; col++) {
|
|
766
|
+
let piv = col;
|
|
767
|
+
for (let r = col + 1; r < n; r++) if (Math.abs(el(M, r * w + col)) > Math.abs(el(M, piv * w + col))) piv = r;
|
|
768
|
+
if (Math.abs(el(M, piv * w + col)) < 1e-12) return null;
|
|
769
|
+
if (piv !== col) for (let k = col; k < w; k++) {
|
|
770
|
+
const tmp = el(M, col * w + k);
|
|
771
|
+
M[col * w + k] = el(M, piv * w + k);
|
|
772
|
+
M[piv * w + k] = tmp;
|
|
773
|
+
}
|
|
774
|
+
const d = el(M, col * w + col);
|
|
775
|
+
for (let k = col; k < w; k++) M[col * w + k] = el(M, col * w + k) / d;
|
|
776
|
+
for (let r = 0; r < n; r++) {
|
|
777
|
+
if (r === col) continue;
|
|
778
|
+
const f = el(M, r * w + col);
|
|
779
|
+
if (f === 0) continue;
|
|
780
|
+
for (let k = col; k < w; k++) M[r * w + k] = el(M, r * w + k) - f * el(M, col * w + k);
|
|
781
|
+
}
|
|
782
|
+
}
|
|
783
|
+
const x = new Float64Array(n);
|
|
784
|
+
for (let r = 0; r < n; r++) x[r] = el(M, r * w + n);
|
|
785
|
+
return x;
|
|
786
|
+
}
|
|
787
|
+
/** Flatten a chain's DOFs into a parameter vector with bounds, applying the seed. */
|
|
788
|
+
function flattenChain(chain, seed) {
|
|
789
|
+
const segments = [];
|
|
790
|
+
const q = [];
|
|
791
|
+
const lo = [];
|
|
792
|
+
const hi = [];
|
|
793
|
+
for (const j of chain) {
|
|
794
|
+
const s = seed?.[j.child];
|
|
795
|
+
segments.push({
|
|
796
|
+
child: j.child,
|
|
797
|
+
count: j.dofs.length
|
|
798
|
+
});
|
|
799
|
+
j.dofs.forEach((dof, i) => {
|
|
800
|
+
const v = (Array.isArray(s) ? s[i] : i === 0 ? s : void 0) ?? dof.value;
|
|
801
|
+
q.push(Math.min(dof.max, Math.max(dof.min, v)));
|
|
802
|
+
lo.push(dof.min);
|
|
803
|
+
hi.push(dof.max);
|
|
804
|
+
});
|
|
805
|
+
}
|
|
806
|
+
return {
|
|
807
|
+
segments,
|
|
808
|
+
q: Float64Array.from(q),
|
|
809
|
+
lo: Float64Array.from(lo),
|
|
810
|
+
hi: Float64Array.from(hi)
|
|
811
|
+
};
|
|
812
|
+
}
|
|
813
|
+
/** Slice a parameter vector back into per-joint value arrays keyed by child. */
|
|
814
|
+
function overridesOf(segments, q) {
|
|
815
|
+
const out = {};
|
|
816
|
+
let i = 0;
|
|
817
|
+
for (const seg of segments) {
|
|
818
|
+
const vals = [];
|
|
819
|
+
for (let k = 0; k < seg.count; k++) vals.push(el(q, i++));
|
|
820
|
+
out[seg.child] = vals;
|
|
821
|
+
}
|
|
822
|
+
return out;
|
|
823
|
+
}
|
|
824
|
+
/** Residual twist `e` (target − current) for a pose; returns its norm. */
|
|
825
|
+
function residualTwist(out, pose, target, tip, m) {
|
|
826
|
+
const pos = applyPose(pose, tip);
|
|
827
|
+
out[0] = target.position[0] - pos[0];
|
|
828
|
+
out[1] = target.position[1] - pos[1];
|
|
829
|
+
out[2] = target.position[2] - pos[2];
|
|
830
|
+
if (m === 6 && target.rotation) {
|
|
831
|
+
const r = rotationError(pose.rotation, target.rotation);
|
|
832
|
+
out[3] = r[0];
|
|
833
|
+
out[4] = r[1];
|
|
834
|
+
out[5] = r[2];
|
|
835
|
+
}
|
|
836
|
+
let s = 0;
|
|
837
|
+
for (let i = 0; i < m; i++) s += el(out, i) ** 2;
|
|
838
|
+
return Math.sqrt(s);
|
|
839
|
+
}
|
|
840
|
+
/**
|
|
841
|
+
* Finite-difference Jacobian: column `j` is the end-effector twist from δq[j].
|
|
842
|
+
* The probe steps *inward* from a bound — `forwardKinematics` clamps each DOF to
|
|
843
|
+
* its range, so a forward `+eps` at the upper limit would yield a zero column and
|
|
844
|
+
* trap the solver at the ceiling. Stepping `-eps` there (and dividing by the
|
|
845
|
+
* signed step) keeps every column a true one-sided derivative.
|
|
846
|
+
*/
|
|
847
|
+
function fillJacobian(J, q, n, m, lo, hi, base, tip, eps, tipPose) {
|
|
848
|
+
const basePos = applyPose(base, tip);
|
|
849
|
+
for (let j = 0; j < n; j++) {
|
|
850
|
+
const saved = el(q, j);
|
|
851
|
+
const h = saved + eps > el(hi, j) && saved - eps >= el(lo, j) ? -eps : eps;
|
|
852
|
+
q[j] = saved + h;
|
|
853
|
+
const p2 = tipPose(q);
|
|
854
|
+
q[j] = saved;
|
|
855
|
+
const pos2 = applyPose(p2, tip);
|
|
856
|
+
J[j] = (pos2[0] - basePos[0]) / h;
|
|
857
|
+
J[n + j] = (pos2[1] - basePos[1]) / h;
|
|
858
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / h;
|
|
859
|
+
if (m === 6) {
|
|
860
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
861
|
+
J[3 * n + j] = dr[0] / h;
|
|
862
|
+
J[4 * n + j] = dr[1] / h;
|
|
863
|
+
J[5 * n + j] = dr[2] / h;
|
|
864
|
+
}
|
|
865
|
+
}
|
|
866
|
+
}
|
|
867
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
868
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
869
|
+
const A = new Float64Array(m * m);
|
|
870
|
+
const lam2 = lambda * lambda;
|
|
871
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
872
|
+
let s = 0;
|
|
873
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
874
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
875
|
+
}
|
|
876
|
+
const y = solveLinear(A, e, m);
|
|
877
|
+
if (!y) return null;
|
|
878
|
+
const dq = new Float64Array(n);
|
|
879
|
+
for (let j = 0; j < n; j++) {
|
|
880
|
+
let v = 0;
|
|
881
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
882
|
+
dq[j] = v;
|
|
883
|
+
}
|
|
884
|
+
return dq;
|
|
885
|
+
}
|
|
886
|
+
/**
|
|
887
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
888
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
889
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
890
|
+
*
|
|
891
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
892
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
893
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
894
|
+
* target, returns `converged: false` with the best configuration found.
|
|
895
|
+
*/
|
|
896
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
897
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
898
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
899
|
+
const lambda = options.damping ?? .05;
|
|
900
|
+
const tip = options.tip ?? [
|
|
901
|
+
0,
|
|
902
|
+
0,
|
|
903
|
+
0
|
|
904
|
+
];
|
|
905
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
906
|
+
const eps = 1e-6;
|
|
907
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
908
|
+
const n = q.length;
|
|
909
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
910
|
+
const e = new Float64Array(m);
|
|
911
|
+
const J = new Float64Array(m * n);
|
|
912
|
+
let pose = tipPose(q);
|
|
913
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
914
|
+
let iter = 0;
|
|
915
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
916
|
+
fillJacobian(J, q, n, m, lo, hi, pose, tip, eps, tipPose);
|
|
917
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
918
|
+
if (!dq) break;
|
|
919
|
+
for (let j = 0; j < n; j++) {
|
|
920
|
+
const next = el(q, j) + el(dq, j);
|
|
921
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
922
|
+
}
|
|
923
|
+
pose = tipPose(q);
|
|
924
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
925
|
+
}
|
|
926
|
+
return {
|
|
927
|
+
values: overridesOf(segments, q),
|
|
928
|
+
converged: err <= tolerance,
|
|
929
|
+
iterations: iter,
|
|
930
|
+
error: err
|
|
931
|
+
};
|
|
932
|
+
}
|
|
933
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
934
|
+
function valuesOf(joint, spec) {
|
|
935
|
+
return joint.dofs.map((dof, i) => {
|
|
936
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
937
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
938
|
+
});
|
|
939
|
+
}
|
|
940
|
+
/**
|
|
941
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
942
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
943
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
944
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
945
|
+
* their stored value at both ends.
|
|
946
|
+
*/
|
|
947
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
948
|
+
const joints = [];
|
|
949
|
+
walkAssembly(assembly, (n) => {
|
|
950
|
+
if (n.joints) joints.push(...n.joints);
|
|
951
|
+
});
|
|
952
|
+
const ends = joints.map((j) => ({
|
|
953
|
+
child: j.child,
|
|
954
|
+
a: valuesOf(j, from[j.child]),
|
|
955
|
+
b: valuesOf(j, to[j.child])
|
|
956
|
+
}));
|
|
957
|
+
const count = Math.max(1, Math.floor(steps));
|
|
958
|
+
const samples = [];
|
|
959
|
+
for (let s = 0; s <= count; s++) {
|
|
960
|
+
const t = s / count;
|
|
961
|
+
const values = {};
|
|
962
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
963
|
+
samples.push({
|
|
964
|
+
t,
|
|
965
|
+
values,
|
|
966
|
+
poses: forwardKinematics(assembly, values)
|
|
967
|
+
});
|
|
968
|
+
}
|
|
969
|
+
return samples;
|
|
970
|
+
}
|
|
971
|
+
//#endregion
|
|
672
972
|
//#region src/operations/historyFns.ts
|
|
673
973
|
/** Create a new empty history. */
|
|
674
974
|
function createHistory() {
|
|
@@ -936,4 +1236,4 @@ function thread(options) {
|
|
|
936
1236
|
}
|
|
937
1237
|
}
|
|
938
1238
|
//#endregion
|
|
939
|
-
export {
|
|
1239
|
+
export { quatFromAxisAngle as A, walkAssembly as B, mechanismDOF as C, setJointValue as D, revoluteJoint as E, countNodes as F, createAssembly as G, gridPattern as H, createAssemblyNode as I, findNode as L, quatRotate as M, addChild as N, setJointValues as O, collectShapes as P, removeChild as R, jointTransform as S, prismaticJoint as T, linearPattern as U, circularPattern as V, exportAssemblySTEP as W, inverseKinematics as _, deserializeHistory as a, cylindricalJoint as b, modifyStep as c, replayFrom as d, replayHistory as f, undoLast as g, stepsFrom as h, createRegistry as i, quatFromTo as j, sphericalJoint as k, registerOperation as l, stepCount as m, addStep as n, findStep as o, serializeHistory as p, createHistory as r, getShape as s, thread as t, registerShape as u, jointTrajectory as v, planarJoint as w, forwardKinematics as x, addJoint as y, updateNode as z };
|