testing using the CadEngine from a kotlin script

test_cad_engine.kts

/*
 * This file is part of BowlerBuilder.
 *
 * BowlerBuilder is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * BowlerBuilder is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with BowlerBuilder.  If not, see <https://www.gnu.org/licenses/>.
 */
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.util.Limits
import com.neuronrobotics.bowlerkernel.kinematics.base.DefaultKinematicBase
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.LinkType
import com.neuronrobotics.bowlerkernel.kinematics.motion.FrameTransformation
import com.neuronrobotics.bowlerkernel.kinematics.motion.MotionConstraints
import com.neuronrobotics.bowlerkernel.kinematics.motion.NoopForwardKinematicsSolver
import com.neuronrobotics.bowlerkernel.kinematics.motion.NoopInertialStateEstimator
import com.neuronrobotics.bowlerkernel.kinematics.motion.NoopInverseKinematicsSolver
import com.neuronrobotics.bowlerkernel.kinematics.motion.LengthBasedReachabilityCalculator
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.NoopLimbMotionPlanFollower
import com.neuronrobotics.bowlerkernel.kinematics.motion.plan.NoopLimbMotionPlanGenerator
import com.neuronrobotics.bowlerkernel.kinematics.closedloop.NoopJointAngleController
import org.octogonapus.ktguava.collections.immutableListOf
import org.octogonapus.ktguava.collections.toImmutableList
import org.octogonapus.ktguava.collections.toImmutableMap
import javax.inject.Inject
import com.google.common.collect.ImmutableSet
import com.google.common.collect.ImmutableSetMultimap
import com.neuronrobotics.bowlerkernel.kinematics.base.KinematicBase
import com.neuronrobotics.bowlerkernel.kinematics.limb.limbid.LimbId
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.Link
import com.neuronrobotics.bowlerkernel.kinematics.limb.link.toFrameTransformation
import org.octogonapus.ktguava.collections.emptyImmutableSet
import org.octogonapus.ktguava.collections.immutableListOf
import org.octogonapus.ktguava.collections.immutableSetOf
import org.octogonapus.ktguava.collections.plus
import org.octogonapus.ktguava.collections.toImmutableList
import org.octogonapus.ktguava.collections.toImmutableSet
import org.octogonapus.ktguava.collections.toImmutableSetMultimap

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

    private val cmmInputArmDhParams = immutableListOf(
        DhParam(13, -180, 32, -90),
        DhParam(25, -90, 93, 180),
        DhParam(11, 90, 24, 90),
        DhParam(128, -90, 0, 90),
        DhParam(0, 0, 0, -90),
        DhParam(25, 90, 0, 0)
    )
    
    private val hephaestusArmDhParams = immutableListOf(
        DhParam(135, 0, 0, -90),
        DhParam(0, 0, 175, 0),
        DhParam(0, 90, 169.28, 0)
    )
    
    private val epsonC3 = immutableListOf(
        DhParam(44.45, 0, 196.85, 0),
        DhParam(38.1, 0, 273.05, 0)
    )

    private val cadGen = DefaultCadGenerator()

    override fun runScript(args: ImmutableList<Any?>): Either<String, Any?> {
        val increasingController = object : JointAngleController {
            var angle = 0.0

            override fun getCurrentAngle(): Double {
                angle += 0.1
                return angle
            }

            override fun setTargetAngle(
                angle: Double,
                motionConstraints: MotionConstraints
            ) {
            }
        }

        val cmmArm = DefaultLimb(
            SimpleLimbId(""),
            cmmInputArmDhParams.map { dhParam ->
                DefaultLink(
                    LinkType.Rotary,
                    dhParam,
                    Limits(130, -130),
                    NoopInertialStateEstimator
                )
            }.toImmutableList(),
            NoopForwardKinematicsSolver,
            NoopInverseKinematicsSolver,
            LengthBasedReachabilityCalculator(),
            NoopLimbMotionPlanGenerator,
            NoopLimbMotionPlanFollower,
            immutableListOf(
                NoopJointAngleController,
                NoopJointAngleController,
                NoopJointAngleController,
                NoopJointAngleController,
                NoopJointAngleController,
                NoopJointAngleController
            ),
            NoopInertialStateEstimator
        ) as Limb

        val limbs = immutableListOf(cmmArm)

        val base = DefaultKinematicBase(
            SimpleKinematicBaseId(""),
            limbs,
            limbs.map { it.id to FrameTransformation.identity }.toImmutableMap(),
            NoopBodyController
        )

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

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

MyScript::class