brepjs 18.80.0 → 18.81.0
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 +3 -1
- package/dist/brepjs.js +2 -2
- package/dist/index.d.ts +1 -0
- package/dist/operations/ikFns.d.ts +58 -0
- package/dist/operations.cjs +3 -1
- package/dist/operations.d.ts +1 -0
- package/dist/operations.js +2 -2
- package/dist/{threadFns-Cra0yHSF.js → threadFns-D_CxgFXg.js} +298 -5
- package/dist/{threadFns-B7a1EVpS.cjs → threadFns-ubwexS-v.cjs} +309 -4
- 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-ubwexS-v.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");
|
|
@@ -6742,6 +6742,7 @@ exports.intersect2D = require_boolean2D.intersect2D;
|
|
|
6742
6742
|
exports.intersectBlueprints = require_boolean2D.intersectBlueprints;
|
|
6743
6743
|
exports.intersectWithEvolution = require_healingFns.intersectWithEvolution;
|
|
6744
6744
|
exports.invalidateShapeCache = require_topologyQueryFns.invalidateShapeCache;
|
|
6745
|
+
exports.inverseKinematics = require_threadFns.inverseKinematics;
|
|
6745
6746
|
Object.defineProperty(exports, "io", {
|
|
6746
6747
|
enumerable: true,
|
|
6747
6748
|
get: function() {
|
|
@@ -6793,6 +6794,7 @@ exports.iterSolids = require_topologyQueryFns.iterSolids;
|
|
|
6793
6794
|
exports.iterTopo = require_faceFns.iterTopo;
|
|
6794
6795
|
exports.iterVertices = require_topologyQueryFns.iterVertices;
|
|
6795
6796
|
exports.iterWires = require_topologyQueryFns.iterWires;
|
|
6797
|
+
exports.jointTrajectory = require_threadFns.jointTrajectory;
|
|
6796
6798
|
exports.jointTransform = require_threadFns.jointTransform;
|
|
6797
6799
|
exports.kernelCall = require_topologyQueryFns.kernelCall;
|
|
6798
6800
|
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-D_CxgFXg.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";
|
|
@@ -6454,4 +6454,4 @@ var csg_exports = /* @__PURE__ */ __exportAll({
|
|
|
6454
6454
|
withEvaluator: () => withEvaluator
|
|
6455
6455
|
});
|
|
6456
6456
|
//#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 };
|
|
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, 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 {};
|
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-ubwexS-v.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-D_CxgFXg.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,299 @@ 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
|
+
/** Finite-difference Jacobian: column `j` is the end-effector twist from δq[j]. */
|
|
841
|
+
function fillJacobian(J, q, n, m, base, tip, eps, tipPose) {
|
|
842
|
+
const basePos = applyPose(base, tip);
|
|
843
|
+
for (let j = 0; j < n; j++) {
|
|
844
|
+
const saved = el(q, j);
|
|
845
|
+
q[j] = saved + eps;
|
|
846
|
+
const p2 = tipPose(q);
|
|
847
|
+
q[j] = saved;
|
|
848
|
+
const pos2 = applyPose(p2, tip);
|
|
849
|
+
J[j] = (pos2[0] - basePos[0]) / eps;
|
|
850
|
+
J[n + j] = (pos2[1] - basePos[1]) / eps;
|
|
851
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / eps;
|
|
852
|
+
if (m === 6) {
|
|
853
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
854
|
+
J[3 * n + j] = dr[0] / eps;
|
|
855
|
+
J[4 * n + j] = dr[1] / eps;
|
|
856
|
+
J[5 * n + j] = dr[2] / eps;
|
|
857
|
+
}
|
|
858
|
+
}
|
|
859
|
+
}
|
|
860
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
861
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
862
|
+
const A = new Float64Array(m * m);
|
|
863
|
+
const lam2 = lambda * lambda;
|
|
864
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
865
|
+
let s = 0;
|
|
866
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
867
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
868
|
+
}
|
|
869
|
+
const y = solveLinear(A, e, m);
|
|
870
|
+
if (!y) return null;
|
|
871
|
+
const dq = new Float64Array(n);
|
|
872
|
+
for (let j = 0; j < n; j++) {
|
|
873
|
+
let v = 0;
|
|
874
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
875
|
+
dq[j] = v;
|
|
876
|
+
}
|
|
877
|
+
return dq;
|
|
878
|
+
}
|
|
879
|
+
/**
|
|
880
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
881
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
882
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
883
|
+
*
|
|
884
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
885
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
886
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
887
|
+
* target, returns `converged: false` with the best configuration found.
|
|
888
|
+
*/
|
|
889
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
890
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
891
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
892
|
+
const lambda = options.damping ?? .05;
|
|
893
|
+
const tip = options.tip ?? [
|
|
894
|
+
0,
|
|
895
|
+
0,
|
|
896
|
+
0
|
|
897
|
+
];
|
|
898
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
899
|
+
const eps = 1e-6;
|
|
900
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
901
|
+
const n = q.length;
|
|
902
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
903
|
+
const e = new Float64Array(m);
|
|
904
|
+
const J = new Float64Array(m * n);
|
|
905
|
+
let pose = tipPose(q);
|
|
906
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
907
|
+
let iter = 0;
|
|
908
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
909
|
+
fillJacobian(J, q, n, m, pose, tip, eps, tipPose);
|
|
910
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
911
|
+
if (!dq) break;
|
|
912
|
+
for (let j = 0; j < n; j++) {
|
|
913
|
+
const next = el(q, j) + el(dq, j);
|
|
914
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
915
|
+
}
|
|
916
|
+
pose = tipPose(q);
|
|
917
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
918
|
+
}
|
|
919
|
+
return {
|
|
920
|
+
values: overridesOf(segments, q),
|
|
921
|
+
converged: err <= tolerance,
|
|
922
|
+
iterations: iter,
|
|
923
|
+
error: err
|
|
924
|
+
};
|
|
925
|
+
}
|
|
926
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
927
|
+
function valuesOf(joint, spec) {
|
|
928
|
+
return joint.dofs.map((dof, i) => {
|
|
929
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
930
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
931
|
+
});
|
|
932
|
+
}
|
|
933
|
+
/**
|
|
934
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
935
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
936
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
937
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
938
|
+
* their stored value at both ends.
|
|
939
|
+
*/
|
|
940
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
941
|
+
const joints = [];
|
|
942
|
+
walkAssembly(assembly, (n) => {
|
|
943
|
+
if (n.joints) joints.push(...n.joints);
|
|
944
|
+
});
|
|
945
|
+
const ends = joints.map((j) => ({
|
|
946
|
+
child: j.child,
|
|
947
|
+
a: valuesOf(j, from[j.child]),
|
|
948
|
+
b: valuesOf(j, to[j.child])
|
|
949
|
+
}));
|
|
950
|
+
const count = Math.max(1, Math.floor(steps));
|
|
951
|
+
const samples = [];
|
|
952
|
+
for (let s = 0; s <= count; s++) {
|
|
953
|
+
const t = s / count;
|
|
954
|
+
const values = {};
|
|
955
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
956
|
+
samples.push({
|
|
957
|
+
t,
|
|
958
|
+
values,
|
|
959
|
+
poses: forwardKinematics(assembly, values)
|
|
960
|
+
});
|
|
961
|
+
}
|
|
962
|
+
return samples;
|
|
963
|
+
}
|
|
964
|
+
//#endregion
|
|
672
965
|
//#region src/operations/historyFns.ts
|
|
673
966
|
/** Create a new empty history. */
|
|
674
967
|
function createHistory() {
|
|
@@ -936,4 +1229,4 @@ function thread(options) {
|
|
|
936
1229
|
}
|
|
937
1230
|
}
|
|
938
1231
|
//#endregion
|
|
939
|
-
export {
|
|
1232
|
+
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 };
|
|
@@ -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,299 @@ 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
|
+
/** Finite-difference Jacobian: column `j` is the end-effector twist from δq[j]. */
|
|
841
|
+
function fillJacobian(J, q, n, m, base, tip, eps, tipPose) {
|
|
842
|
+
const basePos = applyPose(base, tip);
|
|
843
|
+
for (let j = 0; j < n; j++) {
|
|
844
|
+
const saved = el(q, j);
|
|
845
|
+
q[j] = saved + eps;
|
|
846
|
+
const p2 = tipPose(q);
|
|
847
|
+
q[j] = saved;
|
|
848
|
+
const pos2 = applyPose(p2, tip);
|
|
849
|
+
J[j] = (pos2[0] - basePos[0]) / eps;
|
|
850
|
+
J[n + j] = (pos2[1] - basePos[1]) / eps;
|
|
851
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / eps;
|
|
852
|
+
if (m === 6) {
|
|
853
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
854
|
+
J[3 * n + j] = dr[0] / eps;
|
|
855
|
+
J[4 * n + j] = dr[1] / eps;
|
|
856
|
+
J[5 * n + j] = dr[2] / eps;
|
|
857
|
+
}
|
|
858
|
+
}
|
|
859
|
+
}
|
|
860
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
861
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
862
|
+
const A = new Float64Array(m * m);
|
|
863
|
+
const lam2 = lambda * lambda;
|
|
864
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
865
|
+
let s = 0;
|
|
866
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
867
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
868
|
+
}
|
|
869
|
+
const y = solveLinear(A, e, m);
|
|
870
|
+
if (!y) return null;
|
|
871
|
+
const dq = new Float64Array(n);
|
|
872
|
+
for (let j = 0; j < n; j++) {
|
|
873
|
+
let v = 0;
|
|
874
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
875
|
+
dq[j] = v;
|
|
876
|
+
}
|
|
877
|
+
return dq;
|
|
878
|
+
}
|
|
879
|
+
/**
|
|
880
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
881
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
882
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
883
|
+
*
|
|
884
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
885
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
886
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
887
|
+
* target, returns `converged: false` with the best configuration found.
|
|
888
|
+
*/
|
|
889
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
890
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
891
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
892
|
+
const lambda = options.damping ?? .05;
|
|
893
|
+
const tip = options.tip ?? [
|
|
894
|
+
0,
|
|
895
|
+
0,
|
|
896
|
+
0
|
|
897
|
+
];
|
|
898
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
899
|
+
const eps = 1e-6;
|
|
900
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
901
|
+
const n = q.length;
|
|
902
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
903
|
+
const e = new Float64Array(m);
|
|
904
|
+
const J = new Float64Array(m * n);
|
|
905
|
+
let pose = tipPose(q);
|
|
906
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
907
|
+
let iter = 0;
|
|
908
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
909
|
+
fillJacobian(J, q, n, m, pose, tip, eps, tipPose);
|
|
910
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
911
|
+
if (!dq) break;
|
|
912
|
+
for (let j = 0; j < n; j++) {
|
|
913
|
+
const next = el(q, j) + el(dq, j);
|
|
914
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
915
|
+
}
|
|
916
|
+
pose = tipPose(q);
|
|
917
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
918
|
+
}
|
|
919
|
+
return {
|
|
920
|
+
values: overridesOf(segments, q),
|
|
921
|
+
converged: err <= tolerance,
|
|
922
|
+
iterations: iter,
|
|
923
|
+
error: err
|
|
924
|
+
};
|
|
925
|
+
}
|
|
926
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
927
|
+
function valuesOf(joint, spec) {
|
|
928
|
+
return joint.dofs.map((dof, i) => {
|
|
929
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
930
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
931
|
+
});
|
|
932
|
+
}
|
|
933
|
+
/**
|
|
934
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
935
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
936
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
937
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
938
|
+
* their stored value at both ends.
|
|
939
|
+
*/
|
|
940
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
941
|
+
const joints = [];
|
|
942
|
+
walkAssembly(assembly, (n) => {
|
|
943
|
+
if (n.joints) joints.push(...n.joints);
|
|
944
|
+
});
|
|
945
|
+
const ends = joints.map((j) => ({
|
|
946
|
+
child: j.child,
|
|
947
|
+
a: valuesOf(j, from[j.child]),
|
|
948
|
+
b: valuesOf(j, to[j.child])
|
|
949
|
+
}));
|
|
950
|
+
const count = Math.max(1, Math.floor(steps));
|
|
951
|
+
const samples = [];
|
|
952
|
+
for (let s = 0; s <= count; s++) {
|
|
953
|
+
const t = s / count;
|
|
954
|
+
const values = {};
|
|
955
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
956
|
+
samples.push({
|
|
957
|
+
t,
|
|
958
|
+
values,
|
|
959
|
+
poses: forwardKinematics(assembly, values)
|
|
960
|
+
});
|
|
961
|
+
}
|
|
962
|
+
return samples;
|
|
963
|
+
}
|
|
964
|
+
//#endregion
|
|
672
965
|
//#region src/operations/historyFns.ts
|
|
673
966
|
/** Create a new empty history. */
|
|
674
967
|
function createHistory() {
|
|
@@ -1044,6 +1337,18 @@ Object.defineProperty(exports, "gridPattern", {
|
|
|
1044
1337
|
return gridPattern;
|
|
1045
1338
|
}
|
|
1046
1339
|
});
|
|
1340
|
+
Object.defineProperty(exports, "inverseKinematics", {
|
|
1341
|
+
enumerable: true,
|
|
1342
|
+
get: function() {
|
|
1343
|
+
return inverseKinematics;
|
|
1344
|
+
}
|
|
1345
|
+
});
|
|
1346
|
+
Object.defineProperty(exports, "jointTrajectory", {
|
|
1347
|
+
enumerable: true,
|
|
1348
|
+
get: function() {
|
|
1349
|
+
return jointTrajectory;
|
|
1350
|
+
}
|
|
1351
|
+
});
|
|
1047
1352
|
Object.defineProperty(exports, "jointTransform", {
|
|
1048
1353
|
enumerable: true,
|
|
1049
1354
|
get: function() {
|