A 3dof arm demo

carlo-demo.kts

import arrow.core.Either
import arrow.core.right
import com.google.common.collect.ImmutableList
import com.neuronrobotics.bowlercad.cadgenerator.DefaultCadGenerator
import com.neuronrobotics.bowlerkernel.hardware.Script
import com.neuronrobotics.bowlerkernel.kinematics.base.DefaultKinematicBase
import com.neuronrobotics.bowlerkernel.kinematics.base.KinematicBase
import com.neuronrobotics.bowlerkernel.kinematics.base.baseid.SimpleKinematicBaseId
import com.neuronrobotics.bowlerkernel.kinematics.closedloop.JointAngleController
import com.neuronrobotics.bowlerkernel.kinematics.closedloop.NoopBodyController
import com.neuronrobotics.bowlerkernel.kinematics.limb.DefaultLimb
import com.neuronrobotics.bowlerkernel.kinematics.limb.Limb
import com.neuronrobotics.bowlerkernel.kinematics.limb.limbid.SimpleLimbId
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.DefaultLink
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.DhParam
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.Link
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.LinkType
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.toFrameTransformation
import com.neuronrobotics.bowlerkernel.kinematics.motion.BasicMotionConstraints
import com.neuronrobotics.bowlerkernel.kinematics.motion.ForwardKinematicsSolver
import com.neuronrobotics.bowlerkernel.kinematics.motion.FrameTransformation
import com.neuronrobotics.bowlerkernel.kinematics.motion.InverseKinematicsSolver
import com.neuronrobotics.bowlerkernel.kinematics.motion.MotionConstraints
import com.neuronrobotics.bowlerkernel.kinematics.motion.NoopInertialStateEstimator
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.DefaultLimbMotionPlanFollower
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.LimbMotionPlan
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.LimbMotionPlanGenerator
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.LimbMotionPlanStep
import com.neuronrobotics.bowlerkernel.util.Limits
import org.octogonapus.ktguava.collections.immutableListOf
import org.octogonapus.ktguava.collections.immutableMapOf
import org.octogonapus.ktguava.collections.toImmutableList
import javax.inject.Inject
import kotlin.concurrent.thread
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.acos
import kotlin.math.asin
import kotlin.math.cos
import kotlin.math.pow
import kotlin.math.roundToInt
import kotlin.math.sin
import kotlin.math.sqrt

class CarloScript
@Inject constructor() : Script() {

    class CarloFK : ForwardKinematicsSolver {

        override fun solveChain(
            links: ImmutableList<Link>,
            currentJointAngles: ImmutableList<Double>
        ): FrameTransformation =
            links.mapIndexed { index, link ->
                link.dhParam.copy(theta = currentJointAngles[index] + link.dhParam.theta)
            }.toFrameTransformation()
    }

    class CarloIK : InverseKinematicsSolver {

        override fun solveChain(
            links: ImmutableList<Link>,
            currentJointAngles: ImmutableList<Double>,
            targetFrameTransform: FrameTransformation
        ): ImmutableList<Double> {
            require(links.size >= 3) {
                "Must have at least 3 links, given ${links.size}"
            }

            require(links.size == currentJointAngles.size) {
                """
            Links and joint angles must have equal length:
            Number of links: ${links.size}
            Number of joint angles: ${currentJointAngles.size}
            """.trimIndent()
            }

            val params = links.map { it.dhParam }

            val lengthXYPlaneVec = sqrt(
                targetFrameTransform.translationX.pow(2) +
                    targetFrameTransform.translationY.pow(2)
            )

            val angleXYPlaneVec = asin(targetFrameTransform.translationY / lengthXYPlaneVec)

            val d = params[1].d - params[2].d
            val r = params[0].r
            val angleRectangleAdjustedXY = asin(d / lengthXYPlaneVec)
            val lengthRectangleAdjustedXY = lengthXYPlaneVec * cos(angleRectangleAdjustedXY) - r

            val orientation = (angleXYPlaneVec - angleRectangleAdjustedXY).let {
                if (abs(Math.toDegrees(it)) < 0.01) 0.0 else it
            }

            val ySet = lengthRectangleAdjustedXY * sin(orientation)
            val xSet = lengthRectangleAdjustedXY * cos(orientation)
            val zSet = targetFrameTransform.translationZ - params[0].d
            val vec = sqrt(xSet.pow(2) + ySet.pow(2) + zSet.pow(2))
            val l1 = params[1].r
            val l2 = params[2].r

            if (vec > l1 + l2 || vec < 0 || lengthRectangleAdjustedXY < 0) {
                throw IllegalStateException(
                    """
                Hypot too long:
                vect: $vec
                l1: $l1
                l2: $l2
                lengthRectangleAdjustedXY: $lengthRectangleAdjustedXY
                """.trimIndent()
                )
            }

            val inv = DoubleArray(links.size)
            inv[0] = Math.toDegrees(orientation)

            val elevation = asin(zSet / vec)
            val (A, C) = lawOfTriangles(l2, l1, vec)
            inv[1] = -Math.toDegrees((A + elevation + Math.toRadians(params[1].theta)))

            inv[2] = when (params[1].alpha.roundToInt()) {
                180 -> Math.toDegrees(C) - 180 - params[2].theta
                0 -> -Math.toDegrees(C) + params[2].theta
                else -> currentJointAngles[2]
            }

            return inv.toImmutableList()
        }

        private fun lawOfTriangles(
            l2: Double,
            l1: Double,
            vec: Double
        ): Pair<Double, Double> {
            val a = l2
            val b = l1
            val c = vec
            val A = acos((b.pow(2) + c.pow(2) - a.pow(2)) / (2 * b * c))
            val B = acos((c.pow(2) + a.pow(2) - b.pow(2)) / (2 * a * c))
            val C = PI - A - B
            return A to C
        }
    }

    class CarloLimbMotionPlanGenerator(
        private val ikSolver: InverseKinematicsSolver,
        private val numStepsPerPlan: Int = 5
    ) : LimbMotionPlanGenerator {

        override fun generatePlanForTaskSpaceTransform(
            limb: Limb,
            currentTaskSpaceTransform: FrameTransformation,
            targetTaskSpaceTransform: FrameTransformation,
            motionConstraints: MotionConstraints
        ): LimbMotionPlan {
            val currentAngles = limb.getCurrentJointAngles()

            val targetAngles = ikSolver.solveChain(
                limb.links,
                limb.getCurrentJointAngles(),
                targetTaskSpaceTransform
            )

            val steps = (1..numStepsPerPlan).map { stepNum ->
                val fraction = stepNum / numStepsPerPlan.toDouble()

                val angles = targetAngles.mapIndexed { index, elem ->
                    currentAngles[index] + (elem - currentAngles[index]) * fraction
                }.toImmutableList()

                LimbMotionPlanStep(
                    angles,
                    BasicMotionConstraints(
                        motionConstraints.motionDuration / numStepsPerPlan,
                        motionConstraints.maximumVelocity,
                        motionConstraints.maximumAcceleration,
                        motionConstraints.maximumJerk
                    )
                )
            }.toImmutableList()

            return LimbMotionPlan(steps)
        }
    }

    class SimulatedJointAngleController(
        private val name: String
    ) : JointAngleController {

        private var angle = 0.0

        override fun getCurrentAngle() = angle.also {
            // println("SJAC $name: get $angle")
        }

        override fun setTargetAngle(angle: Double, motionConstraints: MotionConstraints) {
            this.angle = angle
            // println("SJAC $name: set $angle")
        }
    }

    private fun KinematicBase.waitToStopMoving() {
        while (limbs.map { it.isMovingToTaskSpaceTransform() }.reduce { acc, elem -> acc && elem }) {
            Thread.sleep(1)
        }
    }

    private val seaArmLinks: ImmutableList<Link> = immutableListOf(
        DefaultLink(
            LinkType.Rotary,
            DhParam(135, 0, 0, -90),
            Limits(180, -180),
            NoopInertialStateEstimator
        ),
        DefaultLink(
            LinkType.Rotary,
            DhParam(0, 0, 175, 0),
            Limits(180, -180),
            NoopInertialStateEstimator
        ),
        DefaultLink(
            LinkType.Rotary,
            DhParam(0, 90, 169.28, 0),
            Limits(180, -180),
            NoopInertialStateEstimator
        )
    )

    private val cadGen = DefaultCadGenerator()

    private val limb1Id = SimpleLimbId("carlo-arm-limb1")

    private val ikEngine = CarloIK()

    override fun runScript(args: ImmutableList<Any?>): Either<String, Any?> {
        val limb1 = DefaultLimb(
            limb1Id,
            seaArmLinks,
            CarloFK(),
            ikEngine,
            CarloLimbMotionPlanGenerator(ikEngine, 50),
            DefaultLimbMotionPlanFollower(),
            seaArmLinks.mapIndexed { index, _ ->
                SimulatedJointAngleController("$index")
            }.toImmutableList(),
            NoopInertialStateEstimator
        )

        val base = DefaultKinematicBase(
            SimpleKinematicBaseId("carlo-arm-base"),
            immutableListOf(limb1),
            immutableMapOf(limb1Id to FrameTransformation.identity),
            NoopBodyController
        )

        thread {
            // val home = limb1.links.map { it.dhParam }.toFrameTransformation()
            limb1.setDesiredTaskSpaceTransform(
                FrameTransformation.fromTranslation(100, 0, 0),
                BasicMotionConstraints(1500, 10, 100, 100)
            )
            base.waitToStopMoving()
            Thread.sleep(250)

            limb1.setDesiredTaskSpaceTransform(
                FrameTransformation.fromTranslation(200, 0, 0),
                BasicMotionConstraints(1500, 10, 100, 100)
            )
            base.waitToStopMoving()
            Thread.sleep(250)

            limb1.setDesiredTaskSpaceTransform(
                FrameTransformation.fromTranslation(200, 100, 0),
                BasicMotionConstraints(1500, 10, 100, 100)
            )
            base.waitToStopMoving()
            Thread.sleep(250)

            limb1.setDesiredTaskSpaceTransform(
                FrameTransformation.fromTranslation(100, 100, 0),
                BasicMotionConstraints(1500, 10, 100, 100)
            )
            base.waitToStopMoving()
            Thread.sleep(250)

            limb1.setDesiredTaskSpaceTransform(
                FrameTransformation.fromTranslation(100, 0, 0),
                BasicMotionConstraints(1500, 10, 100, 100)
            )
            base.waitToStopMoving()
        }

        return listOf(base, cadGen).right()
    }

    override fun stopScript() {
        cadGen.stopThreads()
    }
}

CarloScript::class