implement motion staging

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<const AxisAlignedBoundingBox> 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();

MotionCore/Arm.h

@@ -1,6 +1,8 @@
 #pragma once
 #include "ArmSegment.h"
+#include "AxisAlignedBoundingBox.h"
 #include <string>
+#include <span>
 
 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<const AxisAlignedBoundingBox> 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);

MotionCore/ArmSegment.cpp

@@ -33,34 +33,58 @@ ArmSegment::ArmSegment(
 
 bool ArmSegment::goHome(const std::optional<double> 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<double> 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<double> 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<const ArmSegment*> 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<const ArmSegment*> 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());

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;
 };

MotionCore/MainLoop.cpp

@@ -8,10 +8,12 @@
 
 MainLoop::MainLoop() noexcept :
     arm(L"COM4"),
-    electronicsBB(
+    deadzones({
-        Vector3d(-125, -45, -20),
+        AxisAlignedBoundingBox(
-        Vector3d(65, 20, 120)
+        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;
+        j1Pos += -vkbi.pitch * speed * frametime;
-        const double newJ2Off = j2Off + -vkbi.yaw * speed * frametime;
+        j2Pos += -vkbi.yaw * speed * frametime;
-        const double newJ2Pos = newJ2Off - newJ1Pos;
 
-        arm.getJ0().moveTo(j0Pos, std::nullopt);
+        // Stage the new pose and keep it if it is good
-        // J1 and J2 are coupled. Only apply their position if they both are valid
+        arm.stagePose(j0Pos, j1Pos, j2Pos);
-        if (
+        if (arm.isEFPositionValid(deadzones)) {
-           arm.getJ1().isPositionInRange(newJ1Pos) &&
+            // Roll back individual joints if their angles are out of range
-           arm.getJ2().isPositionInRange(newJ2Pos)) {
+            arm.rollbackJointsBasedOnValidRange();
-            j1Pos = newJ1Pos;
+
-            j2Pos = newJ2Pos;
+            std::cout << "Pose good!!" << std::endl;
-            j2Off = newJ2Off;
+            arm.commitPose();
-            arm.getJ1().moveTo(j1Pos, std::nullopt);
+
-            arm.getJ2().moveTo(j2Pos, std::nullopt);
+            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();
     }
 }

MotionCore/MainLoop.h

@@ -2,6 +2,7 @@
 #include "Arm.h"
 #include "AxisAlignedBoundingBox.h"
 #include "vkb_controller.h"
+#include <array>
 
 // 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<AxisAlignedBoundingBox, 1> deadzones;
 	VKBSimController leftVKB;
 };