diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index 3692fa8..800e275 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -1,5 +1,7 @@ +#include #include "ArmSegment.h" #include "OSerialBus.h" +#include "Matrix3x3.h" ArmSegment::ArmSegment( OSerialBus& serialBus, @@ -104,6 +106,50 @@ void ArmSegment::update(const double frametime) motor.update(frametime); } +Vector3d ArmSegment::getGlobalEndpoint() 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) { + r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, mechanicalToVirtualAngle(seg->motor.getCurrentPosition())); + + Vector3d d = seg->longitudinalAxis * seg->length; // link offset in local frame + p = p + r * d; + } + + return p; +} + +Vector3d ArmSegment::getGlobalAngle() const +{ + Matrix3x3 r = Matrix3x3::fromEulerXYZ(getLocalAngle()); + + const ArmSegment* s = parent; + while (s != nullptr) { + r *= Matrix3x3::fromEulerXYZ(s->getLocalAngle()); + s = s->getParent(); + } + + return r.toEulerXYZ(); +} + +Vector3d ArmSegment::getLocalAngle() const +{ + return rotationAxis * mechanicalToVirtualAngle(motor.getCurrentPosition()); +} + double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) const noexcept { return virtualAngle + mechanicalToVirtualAngleOffset; diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h index 5a37949..f7c2be8 100755 --- a/MotionCore/ArmSegment.h +++ b/MotionCore/ArmSegment.h @@ -52,6 +52,11 @@ public: double getSpeed() const noexcept { return motor.getSpeed(); }; 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; + // Will calculate the arm segments global orientation as euler angles, respecting parents + Vector3d getGlobalAngle() const; + Vector3d getLocalAngle() const; private: double virtualToMechanicalAngle(const double virtualAngle) const noexcept; diff --git a/MotionCore/AxisAlignedBoundingBox.cpp b/MotionCore/AxisAlignedBoundingBox.cpp new file mode 100755 index 0000000..db3a5f4 --- /dev/null +++ b/MotionCore/AxisAlignedBoundingBox.cpp @@ -0,0 +1,38 @@ +#include "AxisAlignedBoundingBox.h" + +AxisAlignedBoundingBox::AxisAlignedBoundingBox(const Vector3d& center, const Vector3d& halfSize) noexcept: + center(center), + halfSize(halfSize) +{ +} + +bool AxisAlignedBoundingBox::doesIntersect(const Vector3d& point) const +{ + const double dx = std::abs(point.x - center.x); + const double dy = std::abs(point.y - center.y); + const double dz = std::abs(point.z - center.z); + + return + dx <= halfSize.x && + dy <= halfSize.y && + dz <= halfSize.z; +} + +bool AxisAlignedBoundingBox::doesIntersect(const AxisAlignedBoundingBox& other) const +{ + const double dx = std::abs(other.center.x - center.x); + const double dy = std::abs(other.center.y - center.y); + const double dz = std::abs(other.center.z - center.z); + + const double sumHalfX = halfSize.x + other.halfSize.x; + const double sumHalfY = halfSize.y + other.halfSize.y; + const double sumHalfZ = halfSize.z + other.halfSize.z; + + // If distance between centers along any axis + // is greater than sum of half sizes -> no intersection + if (dx > sumHalfX) return false; + if (dy > sumHalfY) return false; + if (dz > sumHalfZ) return false; + + return true; +} \ No newline at end of file diff --git a/MotionCore/AxisAlignedBoundingBox.h b/MotionCore/AxisAlignedBoundingBox.h new file mode 100755 index 0000000..f70dc7b --- /dev/null +++ b/MotionCore/AxisAlignedBoundingBox.h @@ -0,0 +1,17 @@ +#pragma once +#include "Vector3d.h" + +class AxisAlignedBoundingBox +{ +public: + AxisAlignedBoundingBox(const Vector3d& center, const Vector3d& halfSize) noexcept; + AxisAlignedBoundingBox(const AxisAlignedBoundingBox& other) noexcept = default; + AxisAlignedBoundingBox(AxisAlignedBoundingBox&& other) noexcept = default; + + bool doesIntersect(const Vector3d& point) const; + bool doesIntersect(const AxisAlignedBoundingBox& other) const; + +private: + Vector3d center; + Vector3d halfSize; +}; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 3fa9972..9b0e315 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -4,13 +4,19 @@ #include #include #include +#include // Bullshit macros from windows lib #undef max #undef min MainLoop::MainLoop() noexcept: - arm(L"COM4") + arm(L"COM4"), + electronicsBB( + Vector3d(100, 0, -20), + Vector3d(65, 30, 75) + //Vector3d(1000, 20, 1000) + ) { if (!arm.assumeHomePoseImmediate()) { std::cerr << "Unable to move arm to home!" << std::endl; @@ -68,18 +74,15 @@ void MainLoop::run() // Yaw (Y): rotation in XZ plane double yawRad = std::atan2(vx, vz); - double yawDeg = (yawRad * 180.0 / 3.14159) - 150; + double yawDeg = (yawRad * 180.0 / std::numbers::pi) - 150; // Pitch (X): rotation in vertical plane - double pitchRad = std::atan2(vy, horiz) * 3 - 1; - double pitchDegTotal = (pitchRad * 180.0 / 3.14159); + double pitchRad = std::atan2(vy, horiz) * 3.3 - 1.2; + double pitchDegTotal = (pitchRad * 180.0 / std::numbers::pi); double pitchDegj1 = 90 - (pitchDegTotal / 2.0) - 45; double pitchDegj2 = -(pitchDegTotal / 2.0) + 45; - std::cout << "cursor: " << cursor - << " yaw: " << yawDeg - << " pitch j1: " << pitchDegj1 << std::endl - << " pitch j2: " << pitchDegj2 << std::endl; + std::cout << arm.getJ2().getGlobalEndpoint() << std::endl; // Send yaw/pitch to your arm here... // arm.setYaw(yawRad); @@ -95,6 +98,9 @@ void MainLoop::run() arm.update(frametime); + if (electronicsBB.doesIntersect(arm.getJ2().getGlobalEndpoint())) { + isRunning = false; + } lastBegin = std::chrono::high_resolution_clock::now(); } diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index f67fac8..2147bff 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -1,5 +1,6 @@ #pragma once #include "Arm.h" +#include "AxisAlignedBoundingBox.h" // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime class MainLoop @@ -17,4 +18,5 @@ private: bool isRunning = true; Arm arm; + AxisAlignedBoundingBox electronicsBB; }; diff --git a/MotionCore/Matrix3x3.cpp b/MotionCore/Matrix3x3.cpp new file mode 100755 index 0000000..de5385d --- /dev/null +++ b/MotionCore/Matrix3x3.cpp @@ -0,0 +1,176 @@ +#include "Matrix3x3.h" +#include +#include +#include + +static constexpr double DEG2RAD = std::numbers::pi / 180.0; +static constexpr double RAD2DEG = 180 / std::numbers::pi; + +Matrix3x3::Matrix3x3() noexcept +{ + m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; + m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; + m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; +} + +Matrix3x3::Matrix3x3(const Vector3d& r0, + const Vector3d& r1, + const Vector3d& r2) noexcept +{ + m[0][0] = r0.x; m[0][1] = r0.y; m[0][2] = r0.z; + m[1][0] = r1.x; m[1][1] = r1.y; m[1][2] = r1.z; + m[2][0] = r2.x; m[2][1] = r2.y; m[2][2] = r2.z; +} + +Matrix3x3 Matrix3x3::identity() noexcept +{ + return Matrix3x3(); +} + +Matrix3x3 Matrix3x3::rotationX(double deg) noexcept +{ + const double r = deg * DEG2RAD; + const double c = std::cos(r); + const double s = std::sin(r); + + return Matrix3x3( + Vector3d(1.0, 0.0, 0.0), + Vector3d(0.0, c, -s), + Vector3d(0.0, s, c) + ); +} + +Matrix3x3 Matrix3x3::rotationY(double deg) noexcept +{ + const double r = deg * DEG2RAD; + const double c = std::cos(r); + const double s = std::sin(r); + + return Matrix3x3( + Vector3d(c, 0.0, s), + Vector3d(0.0, 1.0, 0.0), + Vector3d(-s, 0.0, c) + ); +} + +Matrix3x3 Matrix3x3::rotationZ(double deg) noexcept +{ + const double r = deg * DEG2RAD; + const double c = std::cos(r); + const double s = std::sin(r); + + return Matrix3x3( + Vector3d(c, -s, 0.0), + Vector3d(s, c, 0.0), + Vector3d(0.0, 0.0, 1.0) + ); +} + +Matrix3x3 Matrix3x3::fromEulerXYZ(const Vector3d& e) noexcept +{ + Matrix3x3 Rx = rotationX(e.x); + Matrix3x3 Ry = rotationY(e.y); + Matrix3x3 Rz = rotationZ(e.z); + + // Final order: Rz * Ry * Rx + return Rz * (Ry * Rx); +} + +Matrix3x3 Matrix3x3::fromAxisAngle(const Vector3d& axis, double angleDeg) noexcept +{ + const double r = angleDeg * DEG2RAD; + + const double c = std::cos(r); + const double s = std::sin(r); + const double t = 1.0 - c; + + Vector3d n = axis.normalized(); // MUST be a unit vector + const double x = n.x; + const double y = n.y; + const double z = n.z; + + // Rodrigues' rotation formula + return Matrix3x3( + Vector3d(t * x * x + c, t * x * y - s * z, t * x * z + s * y), + Vector3d(t * x * y + s * z, t * y * y + c, t * y * z - s * x), + Vector3d(t * x * z - s * y, t * y * z + s * x, t * z * z + c) + ); +} + +Vector3d Matrix3x3::toEulerXYZ() const noexcept +{ + // Based on: + // R = Rz * Ry * Rx + // + // Matrix layout: + // [ r00 r01 r02 ] + // [ r10 r11 r12 ] + // [ r20 r21 r22 ] + + // Clamp for numerical safety + const double sy = std::clamp(-m[2][0], -1.0, 1.0); + + const double ry = std::asin(sy); + const double cy = std::cos(ry); + + double rx; + double rz; + + // Check for gimbal lock + if (std::abs(cy) > 1e-8) + { + rx = std::atan2(m[2][1], m[2][2]); + rz = std::atan2(m[1][0], m[0][0]); + } + else + { + // Gimbal lock: Y is +-90° + // Z becomes ambiguous + rx = std::atan2(-m[0][1], m[1][1]); + rz = 0.0; + } + + return Vector3d(rx, ry, rz) * RAD2DEG; +} + +Vector3d Matrix3x3::operator*(const Vector3d& v) const noexcept +{ + return Vector3d( + m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, + m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z, + m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + ); +} + +Matrix3x3 Matrix3x3::operator*(const Matrix3x3& rhs) const noexcept +{ + Matrix3x3 r; + + for (int row = 0; row < 3; ++row) + { + for (int col = 0; col < 3; ++col) + { + r.m[row][col] = + m[row][0] * rhs.m[0][col] + + m[row][1] * rhs.m[1][col] + + m[row][2] * rhs.m[2][col]; + } + } + + return r; +} + +Matrix3x3 Matrix3x3::operator*=(const Matrix3x3& rhs) noexcept +{ + *this = *this * rhs; + return *this; +} + +Matrix3x3 Matrix3x3::transposed() const noexcept +{ + return Matrix3x3( + Vector3d(m[0][0], m[1][0], m[2][0]), + Vector3d(m[0][1], m[1][1], m[2][1]), + Vector3d(m[0][2], m[1][2], m[2][2]) + ); +} diff --git a/MotionCore/Matrix3x3.h b/MotionCore/Matrix3x3.h new file mode 100755 index 0000000..d3c52f5 --- /dev/null +++ b/MotionCore/Matrix3x3.h @@ -0,0 +1,41 @@ +#pragma once +#include "Vector3d.h" + +class Matrix3x3 +{ +public: + // Row-major: m[row][col] + double m[3][3]; + +public: + // Identity by default + Matrix3x3() noexcept; + Matrix3x3( + const Vector3d& row0, + const Vector3d& row1, + const Vector3d& row2 + ) noexcept; + + static Matrix3x3 identity() noexcept; + + // Axis rotations (radians) + static Matrix3x3 rotationX(double radians) noexcept; + static Matrix3x3 rotationY(double radians) noexcept; + static Matrix3x3 rotationZ(double radians) noexcept; + + // Euler constructor (radians) + // euler.x = X, euler.y = Y, euler.z = Z + static Matrix3x3 fromEulerXYZ(const Vector3d& eulerRadians) noexcept; + + static Matrix3x3 fromAxisAngle(const Vector3d& axis, double angleDeg) noexcept; + + // Extract Euler angles (XYZ order, radians) + Vector3d toEulerXYZ() const noexcept; + + // Multiplication + Vector3d operator*(const Vector3d& v) const noexcept; + Matrix3x3 operator*(const Matrix3x3& rhs) const noexcept; + Matrix3x3 operator*=(const Matrix3x3& rhs) noexcept; + + Matrix3x3 transposed() const noexcept; +}; diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index 3546016..8c50d41 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -143,18 +143,22 @@ + + + + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index 074d291..bbe2c0c 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -36,6 +36,12 @@ Quelldateien + + Quelldateien + + + Quelldateien + @@ -56,5 +62,11 @@ Headerdateien + + Headerdateien + + + Headerdateien + \ No newline at end of file