VirtualHexapod.js

import { POSITION_NAMES_LIST, POSITION_NAME_TO_ID_MAP } from "./constants"
import { matrixToAlignVectorAtoB, tRotZmatrix } from "./geometry"

import Vector from "./Vector"
import Hexagon from "./Hexagon"
import Linkage from "./Linkage"

import * as oSolverGeneral from "./solvers/orient/orientSolverGeneral"
import * as oSolverSpecific from "./solvers/orient/orientSolverSpecific"

import { simpleTwist, mightTwist, complexTwist } from "./solvers/twistSolver"

const DEFAULT_POSE = {
    leftFront: { alpha: 0, beta: 0, gamma: 0 },
    rightFront: { alpha: 0, beta: 0, gamma: 0 },
    leftMiddle: { alpha: 0, beta: 0, gamma: 0 },
    rightMiddle: { alpha: 0, beta: 0, gamma: 0 },
    leftBack: { alpha: 0, beta: 0, gamma: 0 },
    rightBack: { alpha: 0, beta: 0, gamma: 0 },
}

const DEFAULT_LOCAL_AXES = {
    xAxis: new Vector(1, 0, 0, "hexapodXaxis"),
    yAxis: new Vector(0, 1, 0, "hexapodYaxis"),
    zAxis: new Vector(0, 0, 1, "hexapodZaxis"),
}

const transformLocalAxes = (localAxes, twistMatrix) => ({
    xAxis: localAxes.xAxis.cloneTrot(twistMatrix),
    yAxis: localAxes.yAxis.cloneTrot(twistMatrix),
    zAxis: localAxes.zAxis.cloneTrot(twistMatrix),
})

/* * *
    build a list of six legs
    given dimensions and  the respective
    bodyContacts points and pose
 * * */
const buildLegsList = (bodyContactPoints, pose, legDimensions) =>
    POSITION_NAMES_LIST.map(
        (position, index) =>
            new Linkage(legDimensions, position, bodyContactPoints[index], pose[position])
    )

const hexapodErrorInfo = () => ({
    isAlert: true,
    subject: "Unstable position.",
    body: "error in solving for orientation ",
})

const hexapodSuccessInfo = () => ({
    isAlert: false,
    subject: "Success!",
    body: "Stable orientation found.",
})

/* * *

............................
    Virtual Hexapod properties
............................

Property types:
{}: hash map / object / dictionary
[]: array / list
##: number
"": string

{} this.dimensions: {front, side, middle, coxia, femur, tibia}

{} this.pose: A hash mapping the position name to a hash map of three angles
    which define the pose of the hexapod
    i.e.
    {
        rightMiddle: {alpha, beta, gamma },
        leftBack: { alpha, beta, gamma },
        ...
    }

[] this.body: A hexagon object
    which contains all the info of the 8 points defining the hexapod body
    (6 vertices, 1 head, 1 center of gravity)

[] this.legs: A list which has elements that point to six Linkage objects.
    The order goes counter clockwise starting from the first element
    which is the rightMiddle leg up until the last element which is rightBack leg.
    Each leg contains the points that define that leg
    as well as other properties pertaining it (see Linkage class)

[] this.legPositionsOnGround: A list of the leg positions (strings)
    that are known to be in contact with the ground

{} this.localAxes: A hash containing three vectors defining the local
    coordinate frame of the hexapod wrt the world coordinate frame
    i.e. {
        xAxis: {x, y, z, name="hexapodXaxis", id="no-id"},
        yAxis: {x, y, z, name="hexapodYaxis", id="no-id"},
        zAxis: {x, y, z, name="hexapodZaxis", id="no-id"},
    }

....................
(virtual hexapod derived properties)
....................

{} this.bodyDimensions: { front, side, middle }
{} this.legDimensions: { coxia, femur, tibia }

## this.distanceFromGround: A number which is the perpendicular distance
    from the hexapod's center of gravity to the ground plane

{} this.cogProjection: a point that represents the projection
    of the hexapod's center of gravity point to the ground plane
    i.e { x, y, z, name="centerOfGravityProjectionPoint", id="no-id"}

[] this.groundContactPoints: a list whose elements point to points
    from the leg which contacts the ground.
    This list can contain 6 or less elements.
    (It can have a length of 3, 4, 5 or 6)
    i.e. [
        { x, y, z, name="rightMiddle-femurPoint", id="0-2"},
        { x, y, z, name="leftBack-footTipPoint", id=4-3},
        ...
    ]

 * * */
class VirtualHexapod {
    dimensions
    pose
    body
    legs
    legPositionsOnGround
    localAxes
    foundSolution
    constructor(
        dimensions,
        pose,
        flags = { hasNoPoints: false, assumeKnownGroundPoints: false, wontRotate: false }
    ) {
        Object.assign(this, { dimensions, pose })

        if (flags.hasNoPoints) {
            return
        }

        // .................
        // STEP 1: Build a flatHexagon and 'dangling' linkages
        // then find  properties we can derive from this
        // .................

        const flatHexagon = new Hexagon(this.bodyDimensions)

        // legsNoGravity are linkages have the correct pose but
        // are not necessarily correctly oriented wrt the world
        // prettier-ignore
        const legsNoGravity = buildLegsList(
            flatHexagon.verticesList, this.pose, this.legDimensions
        )

        // `solved` has:
        // - new orientation of the body (nAxis)
        // - which legs are on the ground (groundLegsNoGravity)
        // - distance of center of gravity to the ground (height)

        const solved = flags.assumeKnownGroundPoints
            ? oSolverSpecific.computeOrientationProperties(legsNoGravity)
            : oSolverGeneral.computeOrientationProperties(legsNoGravity)

        if (solved === null) {
            this.foundSolution = false
            return
        }

        this.foundSolution = true
        this.legPositionsOnGround = solved.groundLegsNoGravity.map(leg => leg.position)

        // .................
        // STEP 2: Rotate and shift legs and body given what we've solved
        // .................

        // prettier-ignore
        const transformMatrix = matrixToAlignVectorAtoB(
            solved.nAxis, DEFAULT_LOCAL_AXES.zAxis
        )

        this.legs = legsNoGravity.map(leg =>
            leg.cloneTrotShift(transformMatrix, 0, 0, solved.height)
        )
        this.body = flatHexagon.cloneTrotShift(transformMatrix, 0, 0, solved.height)
        this.localAxes = transformLocalAxes(DEFAULT_LOCAL_AXES, transformMatrix)

        // .................
        // STEP 3: Twist around the zAxis if you have to
        // .................
        if (flags.wontRotate) {
            return
        }

        // case 1: hexapod will not twist about z axis
        if (this.legs.every(leg => leg.pose.alpha === 0)) {
            return
        }

        // case 2: When all alpha angles are the same for all legs
        const twistAngle = simpleTwist(solved.groundLegsNoGravity)
        if (this.maybeTwistAngle !== 0) {
            this._twist(twistAngle)
            return
        }

        // case 3: All other cases
        if (mightTwist(solved.groundLegsNoGravity)) {
            this._handleComplexTwist(flatHexagon.verticesList)
        }
    }

    get distanceFromGround() {
        return this.body.cog.z
    }

    get cogProjection() {
        return new Vector(
            this.body.cog.x,
            this.body.cog.y,
            0,
            "centerOfGravityProjectionPoint"
        )
    }

    get info() {
        return this.foundSolution ? hexapodSuccessInfo() : hexapodErrorInfo()
    }

    get bodyDimensions() {
        const { front, middle, side } = this.dimensions
        return { front, middle, side }
    }

    get legDimensions() {
        const { coxia, femur, tibia } = this.dimensions
        return { coxia, femur, tibia }
    }

    get groundContactPoints() {
        return this.legPositionsOnGround.map(position => {
            const index = POSITION_NAME_TO_ID_MAP[position]
            return this.legs[index].maybeGroundContactPoint
        })
    }

    cloneTrot(transformMatrix) {
        // Note: transform matrix passed should be purely rotational
        const body = this.body.cloneTrot(transformMatrix)
        const legs = this.legs.map(leg => leg.cloneTrot(transformMatrix))
        const localAxes = transformLocalAxes(this.localAxes, transformMatrix)
        return this._buildClone(body, legs, localAxes)
    }

    cloneShift(tx, ty, tz) {
        const body = this.body.cloneShift(tx, ty, tz)
        const legs = this.legs.map(leg => leg.cloneShift(tx, ty, tz))
        return this._buildClone(body, legs, this.localAxes)
    }

    _buildClone(body, legs, localAxes) {
        // FIXME:
        // After shifting and/or rotating the hexapod
        // We can no longer guarrantee that the legPositionsOnGround
        // is the same as before
        // must handle this soon!!
        let clone = new VirtualHexapod(this.dimensions, this.pose, { hasNoPoints: true })
        Object.assign(clone, {
            body,
            legs,
            localAxes,
            legPositionsOnGround: this.legPositionsOnGround,
            foundSolution: this.foundSolution,
        })
        return clone
    }

    _handleComplexTwist(verticesList) {
        // prettier-ignore
        const defaultLegs = buildLegsList(
            verticesList, DEFAULT_POSE, this.legDimensions
        )

        // DefaultLegs: The list of legs when a hexapod
        // of these dimensions is at the default pose
        // (ie all angles are zero)
        // DefaultPoints: the corresponding ground contact
        // points of defaultLegs
        const defaultPoints = defaultLegs.map(
            leg => leg.cloneShift(0, 0, this.dimensions.tibia).maybeGroundContactPoint
        )

        // currentPoints: Where the ground contact points are currently
        // given all the transformations we have done so far
        const currentPoints = this.groundContactPoints
        const twistAngle = complexTwist(currentPoints, defaultPoints)

        if (twistAngle !== 0) {
            this._twist()
        }
    }

    _twist(twistAngle) {
        const twistMatrix = tRotZmatrix(twistAngle)
        this.body = this.body.cloneTrot(twistMatrix)
        this.legs = this.legs.map(leg => leg.cloneTrot(twistMatrix))
        this.localAxes = transformLocalAxes(this.localAxes, twistMatrix)
    }
}

export default VirtualHexapod