implement very basic motion validation via end-effector deadzone
This commit is contained in:
Leonetienne
@@ -1,5 +1,7 @@
|
||||
#include <vector>
|
||||
#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<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) {
|
||||
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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
Executable
+38
@@ -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;
|
||||
}
|
||||
Executable
+17
@@ -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;
|
||||
};
|
||||
+14
-8
@@ -4,13 +4,19 @@
|
||||
#include <chrono>
|
||||
#include <Windows.h>
|
||||
#include <algorithm>
|
||||
#include <numbers>
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
Executable
+176
@@ -0,0 +1,176 @@
|
||||
#include "Matrix3x3.h"
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <numbers>
|
||||
|
||||
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])
|
||||
);
|
||||
}
|
||||
Executable
+41
@@ -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;
|
||||
};
|
||||
@@ -143,18 +143,22 @@
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="AxisAlignedBoundingBox.cpp" />
|
||||
<ClCompile Include="Arm.cpp" />
|
||||
<ClCompile Include="ArmSegment.cpp" />
|
||||
<ClCompile Include="main.cpp" />
|
||||
<ClCompile Include="MainLoop.cpp" />
|
||||
<ClCompile Include="Matrix3x3.cpp" />
|
||||
<ClCompile Include="Motor.cpp" />
|
||||
<ClCompile Include="OSerialBus.cpp" />
|
||||
<ClCompile Include="Vector3d.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="AxisAlignedBoundingBox.h" />
|
||||
<ClInclude Include="Arm.h" />
|
||||
<ClInclude Include="ArmSegment.h" />
|
||||
<ClInclude Include="MainLoop.h" />
|
||||
<ClInclude Include="Matrix3x3.h" />
|
||||
<ClInclude Include="Motor.h" />
|
||||
<ClInclude Include="OSerialBus.h" />
|
||||
<ClInclude Include="Vector3d.h" />
|
||||
|
||||
@@ -36,6 +36,12 @@
|
||||
<ClCompile Include="Arm.cpp">
|
||||
<Filter>Quelldateien</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="AxisAlignedBoundingBox.cpp">
|
||||
<Filter>Quelldateien</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="Matrix3x3.cpp">
|
||||
<Filter>Quelldateien</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="MainLoop.h">
|
||||
@@ -56,5 +62,11 @@
|
||||
<ClInclude Include="Arm.h">
|
||||
<Filter>Headerdateien</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="AxisAlignedBoundingBox.h">
|
||||
<Filter>Headerdateien</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="Matrix3x3.h">
|
||||
<Filter>Headerdateien</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
Reference in New Issue
Block a user