major-refactor, breaking: Create IK Solver class, rethink and reorgan…

…ize which parameters should be passed to which functions

src/hexapod/index.js

@@ -1,4 +1,4 @@
 import VirtualHexapod from "./VirtualHexapod"
-import solveInverseKinematics from "./solvers/ikSolver"
+import solveInverseKinematics from "./solvers/hexapodSolver"
 import getNewPlotParams from "./plotter"
 export { VirtualHexapod, getNewPlotParams, solveInverseKinematics }

src/hexapod/solvers/IKSolver.js

@@ -0,0 +1,204 @@
+import {
+    POSITION_NAMES_LIST,
+    NUMBER_OF_LEGS,
+    POSITION_NAME_TO_IS_LEFT_MAP,
+    POSITION_NAME_TO_AXIS_ANGLE_MAP,
+} from "../constants"
+
+import {
+    vectorFromTo,
+    projectedVectorOntoPlane,
+    getUnitVector,
+    scaleVector,
+    addVectors,
+    angleBetween,
+    vectorLength,
+    isCounterClockwise,
+} from "../geometry"
+import LinkageIKSolver from "./LinkageIKSolver"
+
+const hexapodNoSupport = legsNamesoffGround => {
+    if (legsNamesoffGround.length < 3) {
+        return [false, "Not enough information"]
+    }
+
+    if (legsNamesoffGround.length >= 4) {
+        return [true, "too many legs off the floor"]
+    }
+
+    // leg count is exactly 3
+    const legLeftOrRight = legsNamesoffGround.map(
+        legPosition => POSITION_NAME_TO_IS_LEFT_MAP[legPosition]
+    )
+
+    if (legLeftOrRight.every(isLeft => !isLeft)) {
+        return [true, "All right legs are off the floor"]
+    }
+
+    if (legLeftOrRight.every(isLeft => isLeft)) {
+        return [true, "All left legs are off the floor"]
+    }
+
+    return [false, "Not enough information"]
+}
+
+const computeAlpha = (coxiaVector, legXaxisAngle, xAxis, zAxis) => {
+    const sign = isCounterClockwise(coxiaVector, xAxis, zAxis) ? -1 : 1
+    const alphaWrtHexapod = sign * angleBetween(coxiaVector, xAxis)
+    const alpha = (alphaWrtHexapod - legXaxisAngle) % 360
+    if (alpha > 180) {
+        return alpha - 360
+    }
+    if (alpha < -180) {
+        return alpha + 360
+    }
+    return alpha
+}
+
+const computeInitialLegProperties = (
+    bodyContactPoint,
+    groundContactPoint,
+    zAxis,
+    coxia
+) => {
+    const bodyToFootVector = vectorFromTo(bodyContactPoint, groundContactPoint)
+    const coxiaDirectionVector = projectedVectorOntoPlane(bodyToFootVector, zAxis)
+    const coxiaUnitVector = getUnitVector(coxiaDirectionVector)
+    const coxiaVector = scaleVector(coxiaUnitVector, coxia)
+    const coxiaPoint = addVectors(bodyContactPoint, coxiaVector)
+    const rho = angleBetween(coxiaUnitVector, bodyToFootVector)
+    const summa = vectorLength(bodyToFootVector)
+
+    return {
+        coxiaUnitVector,
+        coxiaVector,
+        coxiaPoint,
+        rho,
+        summa,
+    }
+}
+
+class IKSolver {
+    params
+    partialPose
+    pose
+    foundSolution
+    legPositionsOffGround
+    message
+    constructor() {
+        this.message = "Has not solved for anything yet."
+        this.foundSolution = null
+        this.legPositionsOffGround = []
+        this.partialPose = {}
+    }
+
+    solve(legDimensions, bodyContactPoints, groundContactPoints, axes) {
+        // prettier-ignore
+        this.params = {
+            bodyContactPoints, groundContactPoints, axes, legDimensions
+        }
+
+        if (this._hasBadVertex(bodyContactPoints)) {
+            return this
+        }
+
+        const { coxia, femur, tibia } = legDimensions
+
+        for (let i = 0; i < NUMBER_OF_LEGS; i++) {
+            const legPosition = POSITION_NAMES_LIST[i]
+
+            // prettier-ignore
+            const known = computeInitialLegProperties(
+                bodyContactPoints[i], groundContactPoints[i], axes.zAxis
+            )
+
+            if (known.coxiaPoint.z < 0) {
+                this._handleBadPoint(known.coxiaPoint)
+                return this
+            }
+
+            const legXaxisAngle = POSITION_NAME_TO_AXIS_ANGLE_MAP[legPosition]
+
+            // prettier-ignore
+            const alpha = computeAlpha(
+                known.coxiaUnitVector, legXaxisAngle, axes.xAxis, axes.zAxis
+            )
+
+            // prettier-ignore
+            const solvedLegParams = new LinkageIKSolver(legPosition)
+                .solve(coxia, femur, tibia, known.summa, known.rho)
+
+            if (!solvedLegParams.obtainedSolution) {
+                this._finalizeFailure(solvedLegParams.message)
+                return this
+            }
+
+            if (!solvedLegParams.reachedTarget) {
+                if (this._hasNoMoreSupport(legPosition)) {
+                    return this
+                }
+            }
+
+            // prettier-ignore
+            this.partialPose[legPosition] = {
+                alpha, beta: solvedLegParams.beta, gamma: solvedLegParams.gamma
+            }
+        }
+
+        this._finalizeSuccess()
+        return this
+    }
+
+    get hasLegsOffGround() {
+        return this.legPositionsOffGround.length > 0 ? true : false
+    }
+
+    _hasNoMoreSupport(legPosition) {
+        this.legPositionsOffGround.push(legPosition)
+        const [noSupport, message] = hexapodNoSupport(this.legPositionsOffGround)
+        if (noSupport) {
+            this._finalizeFailure(message)
+            return true
+        }
+        return false
+    }
+
+    _handleBadPoint(point) {
+        this._finalizeFailure(
+            `Impossible! Atleast one point ${point} would be shoved on ground.`
+        )
+    }
+
+    _hasBadVertex(bodyContactPoints) {
+        for (let i = 0; i < NUMBER_OF_LEGS; i++) {
+            const vertex = bodyContactPoints[i]
+            if (vertex.z < 0) {
+                this._handleBadPoint(vertex)
+                return true
+            }
+        }
+        return false
+    }
+
+    _finalizeFailure(message) {
+        this.message = message
+        this.foundSolution = false
+    }
+
+    _finalizeSuccess() {
+        this.pose = this.partialPose
+        this.foundSolution = true
+
+        if (!this.hasLegsOffGround) {
+            this.message = "Success!"
+            return
+        }
+
+        this.message = this.legPositionOffGround.reduce(
+            (message, legPosition) => message + `${legPosition}\n\n`,
+            "Successful! These legs are off the ground: \n\n"
+        )
+    }
+}
+
+export default IKSolver

src/hexapod/solvers/hexapodSolver.js

@@ -0,0 +1,94 @@
+import { POSITION_NAMES_LIST } from "../constants"
+import { tRotXYZmatrix } from "../geometry"
+import Vector from "../Vector"
+import VirtualHexapod from "../VirtualHexapod"
+import IKSolver from "./IKSolver"
+
+const rawParamsToNumbers = rawParams =>
+    // Make sure all parameter values are numbers
+    Object.entries(rawParams).reduce(
+        (params, [key, val]) => ({ ...params, [key]: Number(val) }),
+        {}
+    )
+
+const convertFromPercentToTranslates = (middle, side, tibia, tx, ty, tz) => {
+    const shiftX = tx * middle
+    const shiftY = ty * side
+    const shiftZ = tz * tibia
+    return new Vector(shiftX, shiftY, shiftZ)
+}
+
+const makeStartingPose = (hipStance, legStance) => {
+    const betaAndGamma = { beta: legStance, gamma: -legStance }
+    const alphas = [0, -hipStance, hipStance, 0, -hipStance, hipStance]
+
+    const pose = POSITION_NAMES_LIST.reduce((pose, positionName, index) => {
+        pose[positionName] = { alpha: alphas[index], ...betaAndGamma }
+        return pose
+    }, {})
+
+    return pose
+}
+
+const convertIkParams = (dimensions, rawIKparams) => {
+    // Organize params to what the ikSolver can understand
+    const IKparams = rawParamsToNumbers(rawIKparams)
+
+    const { middle, side, tibia } = dimensions
+    const { tx, ty, tz } = IKparams
+
+    // prettier-ignore
+    const tVec = convertFromPercentToTranslates(
+        middle, side, tibia, tx, ty, tz
+    )
+
+    const { hipStance, legStance } = IKparams
+    const startPose = makeStartingPose(hipStance, legStance)
+
+    const { rx, ry, rz } = IKparams
+    const rotMatrix = tRotXYZmatrix(rx, ry, rz)
+
+    return { tVec, startPose, rotMatrix }
+}
+
+const solveInverseKinematics = (dimensions, rawIKparams) => {
+    const { tVec, rotMatrix, startPose } = convertIkParams(dimensions, rawIKparams)
+    const startHexapod = new VirtualHexapod(dimensions, startPose)
+
+    const targetGroundContactPoints = startHexapod.legs.map(
+        leg => leg.maybeGroundContactPoint
+    )
+    const targetBodyContactPoints = startHexapod.body
+        .cloneShift(tVec.x, tVec.y, tVec.z)
+        .cloneTrot(rotMatrix).verticesList
+
+    const targetAxes = {
+        xAxis: new Vector(1, 0, 0).cloneTrot(rotMatrix),
+        zAxis: new Vector(0, 0, 1).cloneTrot(rotMatrix),
+    }
+
+    const { coxia, femur, tibia } = dimensions
+    const legDimensions = { coxia, femur, tibia }
+    const solvedHexapodParams = new IKSolver().solve(
+        legDimensions,
+        targetBodyContactPoints,
+        targetGroundContactPoints,
+        targetAxes
+    )
+
+    if (solvedHexapodParams.foundSolution) {
+        return {
+            pose: solvedHexapodParams.pose,
+            obtainedSolution: true,
+            hasLegsOff: solvedHexapodParams.hasLegsOffGround,
+            message: solvedHexapodParams.message,
+        }
+    } else {
+        return {
+            pose: null,
+            message: solvedHexapodParams.message,
+        }
+    }
+}
+
+export default solveInverseKinematics