create and hook up ccdik scene

MotionCore/MainLoop.cpp

@@ -5,7 +5,7 @@
 #include <numbers>
 #include "StateMachine.h"
 #include "ArmSegment.h"
-#include "Scene_JointSpaceControl.h"
+#include "Scene_CCDIK.h"
 
 MainLoop::MainLoop() noexcept :
     arm(L"COM4")
@@ -33,7 +33,7 @@ void MainLoop::run()
     static std::chrono::steady_clock::time_point lastBegin =
         std::chrono::high_resolution_clock::now();
 
-    Scene_JointSpaceControl sceneJointSpaceControl(arm);
+    Scene_CCDIK sceneCCDIK(arm);
 
     while (isRunning)
     {
@@ -52,7 +52,7 @@ void MainLoop::run()
             case StateMachine::STATE::IDLING:
             case StateMachine::STATE::MOVING: {
                 // HERE SCENE UPDATE
-                sceneJointSpaceControl.update(frametime);
+                sceneCCDIK.update(frametime);
 
                 // Unset MOVING to IDLING state if no arm is moving
                 if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) {

MotionCore/Scene_CCDIK.cpp

@@ -0,0 +1,36 @@
+#include "Scene_CCDIK.h"
+#include <iostream>
+
+Scene_CCDIK::Scene_CCDIK(Arm& arm) :
+    arm{ arm },
+    deadzones({
+        AxisAlignedBoundingBox(
+            Vector3d(-125, -125, -20),
+            Vector3d(65, 100, 120)
+        ),
+        AxisAlignedBoundingBox(
+            Vector3d(0, -160, 0),
+            Vector3d(20000, 100, 20000)
+        )
+        }),
+    leftVKB(L" VKBsim Gladiator EVO  L  ")
+{
+
+    if (!arm.assumeHomePoseImmediate()) {
+        std::cerr << "Unable to move arm to home!" << std::endl;
+        exit(-1);
+    }
+
+    if (!leftVKB.connect()) {
+        std::cerr << "Unable to connect to left VKB controller!" << std::endl;
+        exit(-1);
+    }
+}
+
+void Scene_CCDIK::update(double frametime)
+{
+    constexpr double speed = 5000;
+    VkbInputs vkbi = leftVKB.get();
+
+    arm.update(frametime);
+}

MotionCore/Scene_CCDIK.h

@@ -0,0 +1,26 @@
+#pragma once
+#include "Arm.h"
+#include "AxisAlignedBoundingBox.h"
+#include "vkb_controller.h"
+
+class Scene_CCDIK
+{
+public:
+	Scene_CCDIK(Arm& arm);
+
+	void update(double frametime);
+
+private:
+	Arm& arm;
+	std::array<AxisAlignedBoundingBox, 2> deadzones;
+	VKBSimController leftVKB;
+
+
+	double j0Pos = 0;
+	double j1Pos = 0;
+	double j2Pos = 0;
+	double j3Pos = 0;
+	double j4Pos = 0;
+	double j5Pos = 0;
+};
+