diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 5d2eb18..7f9cbe8 100755 --- a/MotionCore/MainLoop.cpp +++ b/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()) { diff --git a/MotionCore/Scene_CCDIK.cpp b/MotionCore/Scene_CCDIK.cpp index 739034f..0d69816 100755 --- a/MotionCore/Scene_CCDIK.cpp +++ b/MotionCore/Scene_CCDIK.cpp @@ -1,5 +1,9 @@ #include "Scene_CCDIK.h" #include +#include + +#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; - VkbInputs vkbi = leftVKB.get(); + update3dCursorSquare(frametime); + //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(NUM_JOINTS) - 1; ji >= 0; --ji) + { + ArmSegment& j = joints[static_cast(ji)]; + + const Vector3d origin = jointOrigin(static_cast(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(); +} diff --git a/MotionCore/Scene_CCDIK.h b/MotionCore/Scene_CCDIK.h index db0b2f0..e22b271 100755 --- a/MotionCore/Scene_CCDIK.h +++ b/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 deadzones; diff --git a/MotionCore/Scene_JointSpaceControl.cpp b/MotionCore/Scene_JointSpaceControl.cpp index 8bea35a..a1ddd21 100755 --- a/MotionCore/Scene_JointSpaceControl.cpp +++ b/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); }