implement experimental ccdik

MotionCore/MainLoop.cpp

@@ -6,6 +6,7 @@
 #include "StateMachine.h"
 #include "ArmSegment.h"
 #include "Scene_CCDIK.h"
+#include "Scene_JointSpaceControl.h"
 
 MainLoop::MainLoop() noexcept :
     arm(L"COM4")
@@ -33,7 +34,8 @@ void MainLoop::run()
     static std::chrono::steady_clock::time_point lastBegin =
         std::chrono::high_resolution_clock::now();
 
-    Scene_CCDIK sceneCCDIK(arm);
+    Scene_CCDIK scene(arm);
+    //Scene_JointSpaceControl scene(arm);
 
     while (isRunning)
     {
@@ -52,7 +54,7 @@ void MainLoop::run()
             case StateMachine::STATE::IDLING:
             case StateMachine::STATE::MOVING: {
                 // HERE SCENE UPDATE
-                sceneCCDIK.update(frametime);
+                scene.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

@@ -1,5 +1,9 @@
 #include "Scene_CCDIK.h"
 #include <iostream>
+#include <iomanip>
+
+#undef min
+#undef max
 
 Scene_CCDIK::Scene_CCDIK(Arm& arm) :
     arm{ arm },
@@ -14,14 +18,8 @@ Scene_CCDIK::Scene_CCDIK(Arm& arm) :
         )
     }),
     leftVKB(L" VKBsim Gladiator EVO  L  "),
-    cursor3d(0,0,0)
+    cursor3d(0, 300, 0)
 {
-
-    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);
@@ -32,20 +30,185 @@ void Scene_CCDIK::update(double frametime)
 {
     vkbi = leftVKB.get();
 
-    constexpr double speed = 5000;
+    update3dCursorSquare(frametime);
-    VkbInputs vkbi = leftVKB.get();
+    //update3dCursorByVKBI(frametime);
+    performCcdIK(cursor3d);
 
-    update3dCursor(frametime);
+    const double error = (cursor3d - arm.getEndEffectorJoint().getTargetedGlobalEndpoint()).magnitude();
+    //std::cout
+    //    << std::fixed
+    //    << std::setprecision(1)
+    //    << std::noshowpos
+    //    << "Cursor: "
+    //    //<< arm.getEndEffectorJoint().getTargetedGlobalEndpoint()
+    //    << cursor3d
+    //    << std::endl;
 
     arm.update(frametime);
 }
 
-void Scene_CCDIK::update3dCursor(double frametime)
+void Scene_CCDIK::update3dCursorByVKBI(double frametime)
 {
-    constexpr double cursorSpeed = 1.0;
+    constexpr double cursorSpeed = 50000.0;
     cursor3d += Vector3d(
         vkbi.roll * cursorSpeed * frametime,
         vkbi.yaw * cursorSpeed * frametime,
-        -vkbi.pitch * cursorSpeed * frametime
+        vkbi.pitch * cursorSpeed * frametime
     );
 }
+
+void Scene_CCDIK::update3dCursorSquare(double frametime)
+{
+    static double t = 0.0;
+    t += frametime;
+
+    const double centerX = 0.0;
+    const double centerY = 200.0;
+    const double zPlane = 100.0;
+
+    const double halfWidth = 150.0;  // X range: [-150, +150]
+    const double halfHeight = 100.0;  // Y range: [100, 300] SAFE
+
+    const double speed = 20.0;
+
+    double phase = std::fmod(t * speed, 1.0);
+
+    if (phase < 0.25)
+    {
+        double u = phase / 0.25;
+        cursor3d = Vector3d(
+            centerX - halfWidth + 2.0 * halfWidth * u,
+            centerY - halfHeight,
+            zPlane
+        );
+    }
+    else if (phase < 0.50)
+    {
+        double u = (phase - 0.25) / 0.25;
+        cursor3d = Vector3d(
+            centerX + halfWidth,
+            centerY - halfHeight + 2.0 * halfHeight * u,
+            zPlane
+        );
+    }
+    else if (phase < 0.75)
+    {
+        double u = (phase - 0.50) / 0.25;
+        cursor3d = Vector3d(
+            centerX + halfWidth - 2.0 * halfWidth * u,
+            centerY + halfHeight,
+            zPlane
+        );
+    }
+    else
+    {
+        double u = (phase - 0.75) / 0.25;
+        cursor3d = Vector3d(
+            centerX - halfWidth,
+            centerY + halfHeight - 2.0 * halfHeight * u,
+            zPlane
+        );
+    }
+}
+
+
+static inline double clampd(double v, double lo, double hi)
+{
+    return std::max(lo, std::min(v, hi));
+}
+
+// Proof of concept, this method not my code
+void Scene_CCDIK::performCcdIK(const Vector3d& target)
+{
+    const int    MAX_ITERS = 25;
+    const double POS_EPS = 0.002;
+    const double ANG_EPS = 1e-4;
+    const double MAX_STEP = 0.35;
+    const double GAIN = 0.85;
+
+    auto& joints = arm.getJoints();
+
+    auto jointOrigin = [&](std::size_t i) -> Vector3d
+        {
+            const ArmSegment* p = joints[i].getParent();
+            return p ? p->getTargetedGlobalEndpoint()
+                : Vector3d{ 0.0, 0.0, 0.0 };
+        };
+
+    auto endEffector = [&]() -> Vector3d
+        {
+            return joints.back().getTargetedGlobalEndpoint();
+        };
+
+    for (int iter = 0; iter < MAX_ITERS; ++iter)
+    {
+        if ((target - endEffector()).magnitude() <= POS_EPS)
+            break;
+
+        for (int ji = static_cast<int>(NUM_JOINTS) - 1; ji >= 0; --ji)
+        {
+            ArmSegment& j = joints[static_cast<std::size_t>(ji)];
+
+            const Vector3d origin = jointOrigin(static_cast<std::size_t>(ji));
+            const Vector3d eePos = endEffector();
+
+            Vector3d r1 = eePos - origin;
+            Vector3d r2 = target - origin;
+
+            double r1l = r1.magnitude();
+            double r2l = r2.magnitude();
+            if (r1l < 1e-9 || r2l < 1e-9)
+                continue;
+
+            r1 /= r1l;
+            r2 /= r2l;
+
+            const double theta0 = j.getTargetPosition();
+
+            j.stageAngle(theta0 + ANG_EPS);
+            const Vector3d eePlus = endEffector();
+            j.stageAngle(theta0);
+
+            Vector3d dEe = (eePlus - eePos) / ANG_EPS;
+
+            Vector3d rVec = eePos - origin;
+            Vector3d axis = rVec.cross(dEe);
+            double axisLen = axis.magnitude();
+            if (axisLen < 1e-12)
+                continue;
+            axis /= axisLen;
+
+            Vector3d p1 = r1 - axis * r1.dot(axis);
+            Vector3d p2 = r2 - axis * r2.dot(axis);
+
+            double p1l = p1.magnitude();
+            double p2l = p2.magnitude();
+            if (p1l < 1e-9 || p2l < 1e-9)
+                continue;
+
+            p1 /= p1l;
+            p2 /= p2l;
+
+            double c = clampd(p1.dot(p2), -1.0, 1.0);
+            double s = axis.dot(p1.cross(p2));
+            double dTheta = std::atan2(s, c);
+
+            dTheta *= GAIN;
+            dTheta = clampd(dTheta, -MAX_STEP, MAX_STEP);
+
+            double newTheta = theta0 + dTheta;
+            newTheta = clampd(newTheta, j.getMinAngle(), j.getMaxAngle());
+
+            j.stageAngle(newTheta);
+
+            arm.rollbackJointsBasedOnValidRange();
+            if (!arm.checkJointCollisions(deadzones))
+                arm.rollbackPose();
+
+            if ((target - endEffector()).magnitude() <= POS_EPS)
+                break;
+        }
+    }
+
+    arm.commitPose();
+}

MotionCore/Scene_CCDIK.h

@@ -12,7 +12,9 @@ public:
 	void update(double frametime);
 
 private:
-	void update3dCursor(double frametime);
+	void update3dCursorByVKBI(double frametime);
+	void update3dCursorSquare(double frametime);
+	void performCcdIK(const Vector3d& target);
 
 	Arm& arm;
 	std::array<AxisAlignedBoundingBox, 2> deadzones;

MotionCore/Scene_JointSpaceControl.cpp

@@ -14,7 +14,7 @@ Scene_JointSpaceControl::Scene_JointSpaceControl(Arm& arm):
         )
     }),
     leftVKB(L" VKBsim Gladiator EVO  L  "),
-    plot(-200, 200, 20, 2)
+    plot(-400, 400, 20, 2)
 {
 
     if (!arm.assumeHomePoseImmediate()) {
@@ -67,5 +67,29 @@ void Scene_JointSpaceControl::update(double frametime)
     //std::cout << arm.getJ3().getCurrentPosition() << std::endl;
     plot.draw(ef);
 
+    //std::cout
+    //    << std::fixed
+    //    << std::setprecision(1)
+    //    << std::noshowpos
+    //    << "j0: "
+    //    << j0Pos
+    //    << std::endl
+    //    << "j1: "
+    //    << j1Pos
+    //    << std::endl
+    //    << "j2: "
+    //    << j2Pos
+    //    << std::endl
+    //    << "j3: "
+    //    << j3Pos
+    //    << std::endl
+    //    << "j4: "
+    //    << j4Pos
+    //    << std::endl
+    //    << "j5: "
+    //    << j5Pos
+    //    << std::endl;
+    //system("cls");
+
     arm.update(frametime);
 }