From c3500e4dba6f7a8316fd26e7464abef40858b7a2 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Mon, 15 Dec 2025 22:29:15 +0100 Subject: [PATCH] implement motion staging --- MotionCore/Arm.cpp | 66 +++++++++++++++++++++++++++++++ MotionCore/Arm.h | 16 +++++--- MotionCore/ArmSegment.cpp | 83 ++++++++++++++++++++++++++++++++++++++- MotionCore/ArmSegment.h | 15 ++++++- MotionCore/MainLoop.cpp | 54 ++++++++++++------------- MotionCore/MainLoop.h | 3 +- 6 files changed, 201 insertions(+), 36 deletions(-) diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index f087ed3..1f846f3 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -64,6 +64,72 @@ bool Arm::assumeHomePose() return true; } +void Arm::stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept +{ + j0.stageAngle(theta_j0); + j1.stageAngle(theta_j1); + j2.stageAngle(theta_j2); +} + +void Arm::commitPose() noexcept +{ + if (j0.isStaged()) { + j0.commitStaging(); + } + if (j1.isStaged()) { + j1.commitStaging(); + } + if (j2.isStaged()) { + j2.commitStaging(); + } +} + +void Arm::rollbackPose() noexcept +{ + if (j0.isStaged()) { + j0.rollbackStaging(); + } + if (j1.isStaged()) { + j1.rollbackStaging(); + } + if (j2.isStaged()) { + j2.rollbackStaging(); + } +} + +bool Arm::isEFPositionValid(std::span deadzones) const noexcept +{ + // Otherwise: check for endeffector position + const Vector3d ef = j2.getTargetedGlobalEndpoint(); + + // If any deadzone intersects with the EF pos, fail + for (const AxisAlignedBoundingBox& aabb : deadzones) { + if (aabb.doesIntersect(ef)) { + return false; + } + } + + return true; +} + +bool Arm::rollbackJointsBasedOnValidRange() noexcept +{ + bool allGood = true; + if (!j0.isPositionInRange(j0.getTargetPosition())) { + j0.rollbackStaging(); + allGood = false; + } + if (!j1.isPositionInRange(j0.getTargetPosition())) { + j2.rollbackStaging(); + allGood = false; + } + if (!j2.isPositionInRange(j0.getTargetPosition())) { + j2.rollbackStaging(); + allGood = false; + } + return allGood; +} + Arm::~Arm() { oSerialBus.close(); diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h index 895a5b4..fa79f16 100755 --- a/MotionCore/Arm.h +++ b/MotionCore/Arm.h @@ -1,6 +1,8 @@ #pragma once #include "ArmSegment.h" +#include "AxisAlignedBoundingBox.h" #include +#include class Arm { @@ -13,13 +15,17 @@ public: bool assumeHomePoseImmediate(); bool assumeHomePose(); - ArmSegment& getJ0() { return j0; }; + void stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept; + void commitPose() noexcept; + void rollbackPose() noexcept; + + // Will test if the end effectors current target (or staged) position is not within a collider + bool isEFPositionValid(std::span deadzones) const noexcept; + // Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made. + bool rollbackJointsBasedOnValidRange() noexcept; + const ArmSegment& getJ0() const { return j0; }; - - ArmSegment& getJ1() { return j1; }; const ArmSegment& getJ1() const { return j1; }; - - ArmSegment& getJ2() { return j2; }; const ArmSegment& getJ2() const { return j2; }; void update(double frametime); diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index 6c2470f..530718b 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -33,34 +33,58 @@ ArmSegment::ArmSegment( bool ArmSegment::goHome(const std::optional speed) noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveTo(homeAngle, speed); } bool ArmSegment::goHomeImmediate() noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveToImmediate(homeAngle); } bool ArmSegment::moveTo(const double theta, const std::optional speed) noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveTo(theta, speed); } bool ArmSegment::moveToImmediate(const double theta) noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveToImmediate(theta); } bool ArmSegment::moveBy(const double theta, const std::optional speed) noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveBy(theta, speed); } bool ArmSegment::moveByImmediate(const double theta) noexcept { + if (isAngleStaged) { + return false; + } + return motor.moveByImmediate(theta); } @@ -84,12 +108,43 @@ bool ArmSegment::assumeTargetPositionImmediately() noexcept return motor.assumeTargetPositionImmediately(); } +void ArmSegment::stageAngle(const double theta) noexcept +{ + stagedAngle = theta; + isAngleStaged = true; +} + +bool ArmSegment::commitStaging() noexcept +{ + if (!isAngleStaged) { + return false; + } + + isAngleStaged = false; + moveTo(stagedAngle, std::nullopt); + stagedAngle = 0; + + return true; +} + +bool ArmSegment::rollbackStaging() noexcept +{ + if (!isAngleStaged) { + return false; + } + + isAngleStaged = false; + stagedAngle = 0; + + return true; +} + void ArmSegment::update(const double frametime) { motor.update(frametime); } -Vector3d ArmSegment::getGlobalEndpoint() const +Vector3d ArmSegment::getCurrentGlobalEndpoint() const { // Collect the chain from this segment up to the root std::vector chain; @@ -113,6 +168,32 @@ Vector3d ArmSegment::getGlobalEndpoint() const return p; } +Vector3d ArmSegment::getTargetedGlobalEndpoint() const +{ + // Collect the chain from this segment up to the root + std::vector chain; + chain.reserve(3); + const ArmSegment* s = this; + while (s != nullptr) { + chain.push_back(s); + s = s->getParent(); + } + std::reverse(chain.begin(), chain.end()); // now from base to this segment + + // Forward kinematics: accumulate rotation and position + Matrix3x3 r = Matrix3x3::identity(); + Vector3d p(0.0, 0.0, 0.0); + + for (const ArmSegment* seg : chain) { + // Use the segments staged angle if it a position staged, else use its current target. + r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->isAngleStaged ? seg->stagedAngle : seg->motor.getTargetPosition()); // LOCAL axis + p += r * (seg->longitudinalAxis * seg->length); + } + + return p; +} + + Vector3d ArmSegment::getGlobalAngle() const { Matrix3x3 r = Matrix3x3::fromEulerXYZ(getLocalAngle()); diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h index 7934c37..afd72f5 100755 --- a/MotionCore/ArmSegment.h +++ b/MotionCore/ArmSegment.h @@ -41,9 +41,16 @@ public: void resumeMoving() noexcept; // Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term) bool assumeTargetPositionImmediately() noexcept; + void stageAngle(const double theta) noexcept; + // Returns false if not currently staged + bool commitStaging() noexcept; + // Returns false if not currently staged + bool rollbackStaging() noexcept; + bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); }; double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); }; - double getTargetPosition() const noexcept { return motor.getTargetPosition(); }; + double getTargetPosition() const noexcept { return isAngleStaged ? stagedAngle : motor.getTargetPosition(); }; + bool isStaged() const noexcept { return isAngleStaged; }; void update(const double frametime); @@ -55,7 +62,9 @@ public: ArmSegment* getParent() { return parent; }; ArmSegment const* getParent() const { return parent; }; // Will calculate the point in 3d space which is at the end of this arm segment, respecting the arm segments parent - Vector3d getGlobalEndpoint() const; + Vector3d getCurrentGlobalEndpoint() const; + // Will calculate the point in 3d space which is at the end of this arm segment, respecting the arm segments parent, if it was at its target angle + Vector3d getTargetedGlobalEndpoint() const; // Will calculate the arm segments global orientation as euler angles, respecting parents Vector3d getGlobalAngle() const; Vector3d getLocalAngle() const; @@ -66,5 +75,7 @@ private: Vector3d longitudinalAxis; double length; double homeAngle; + double stagedAngle; + bool isAngleStaged = false; ArmSegment* parent; }; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 81d55b0..6763086 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -8,10 +8,12 @@ MainLoop::MainLoop() noexcept : arm(L"COM4"), - electronicsBB( - Vector3d(-125, -45, -20), - Vector3d(65, 20, 120) - ), + deadzones({ + AxisAlignedBoundingBox( + Vector3d(-125, -125, -20), + Vector3d(65, 100, 120) + ) + }), leftVKB(L" VKBsim Gladiator EVO L ") { if (!arm.assumeHomePoseImmediate()) { @@ -43,12 +45,11 @@ void MainLoop::run() static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); - YZConsolePlotter plot(-200, 200, 20, 2); + //YZConsolePlotter plot(-200, 200, 20, 2); double j0Pos = 0; double j1Pos = 0; double j2Pos = 0; - double j2Off = 0; while (isRunning) { @@ -68,34 +69,33 @@ void MainLoop::run() // Calculate new joint positions j0Pos += -vkbi.roll * speed * frametime; - const double newJ1Pos = j1Pos + -vkbi.pitch * speed * frametime; - const double newJ2Off = j2Off + -vkbi.yaw * speed * frametime; - const double newJ2Pos = newJ2Off - newJ1Pos; + j1Pos += -vkbi.pitch * speed * frametime; + j2Pos += -vkbi.yaw * speed * frametime; - arm.getJ0().moveTo(j0Pos, std::nullopt); - // J1 and J2 are coupled. Only apply their position if they both are valid - if ( - arm.getJ1().isPositionInRange(newJ1Pos) && - arm.getJ2().isPositionInRange(newJ2Pos)) { - j1Pos = newJ1Pos; - j2Pos = newJ2Pos; - j2Off = newJ2Off; - arm.getJ1().moveTo(j1Pos, std::nullopt); - arm.getJ2().moveTo(j2Pos, std::nullopt); + // Stage the new pose and keep it if it is good + arm.stagePose(j0Pos, j1Pos, j2Pos); + if (arm.isEFPositionValid(deadzones)) { + // Roll back individual joints if their angles are out of range + arm.rollbackJointsBasedOnValidRange(); + + std::cout << "Pose good!!" << std::endl; + arm.commitPose(); + + j0Pos = arm.getJ0().getTargetPosition(); + j1Pos = arm.getJ1().getTargetPosition(); + j2Pos = arm.getJ2().getTargetPosition(); + } + else { + arm.rollbackPose(); + std::cout << "Hit deadzone!!" << std::endl; } - const Vector3d ef = arm.getJ2().getGlobalEndpoint(); + const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint(); //std::cout << ef << std::endl; - plot.draw(ef); + //plot.draw(ef); arm.update(frametime); - // Kill if endeffector enters electronics area - if (electronicsBB.doesIntersect(arm.getJ2().getGlobalEndpoint())) { - std::cout << "Terminating because of AABB violation..." << std::endl; - isRunning = false; - } - lastBegin = std::chrono::high_resolution_clock::now(); } } diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index b373188..be181e0 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -2,6 +2,7 @@ #include "Arm.h" #include "AxisAlignedBoundingBox.h" #include "vkb_controller.h" +#include // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime class MainLoop @@ -19,6 +20,6 @@ private: bool isRunning = true; Arm arm; - AxisAlignedBoundingBox electronicsBB; + std::array deadzones; VKBSimController leftVKB; };