leonetienne/MotionCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

got forward kinematics working reliably

Browse Source
This commit is contained in:
Leonetienne
2025-12-14 22:13:52 +01:00
parent e87653ab47
commit 98cfa8b81c
7 changed files with 317 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/Arm.cpp
+6 -6
View File
@@ -12,8 +12,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
j0(
oSerialBus,
Vector3d::Y,
Vector3d::Up,
100.0, //mm
Vector3d::Forward,
0.0, //mm
0,
-135.0,
45.0,
@@ -22,8 +22,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
),
j1(
oSerialBus,
Vector3d::X,
Vector3d::Forward,
-Vector3d::X,
Vector3d::Up,
72.0, //mm
1,
-38.0,
@@ -33,8 +33,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
),
j2(
oSerialBus,
Vector3d::X,
Vector3d::Forward,
-Vector3d::X,
Vector3d::Up,
95.0, //mm
2,
-123,
MotionCore/ArmSegment.cpp
+2 -4
View File
@@ -106,10 +106,8 @@ Vector3d ArmSegment::getGlobalEndpoint() const
Vector3d p(0.0, 0.0, 0.0);
for (const ArmSegment* seg : chain) {
r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition());
Vector3d d = seg->longitudinalAxis * seg->length; // link offset in local frame
p = p + r * d;
r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition()); // LOCAL axis
p += r * (seg->longitudinalAxis * seg->length);
}
return p;
MotionCore/MainLoop.cpp
+12 -12
View File
@@ -1,18 +1,16 @@
#define WIN32_LEAN_AND_MEAN
#include "MainLoop.h"
#include <iostream>
#include <thread>
#include <chrono>
#include <Windows.h>
#include <algorithm>
#include <numbers>
#include "yz_plotter_win.h"
MainLoop::MainLoop() noexcept :
arm(L"COM4"),
electronicsBB(
Vector3d(100, 0, -20),
//Vector3d(65, 30, 75)
Vector3d(1000, 30, 1000)
Vector3d(-125, -45, -20),
Vector3d(65, 20, 120)
),
leftVKB(L" VKBsim Gladiator EVO L ")
{
@@ -25,8 +23,6 @@ MainLoop::MainLoop() noexcept :
std::cerr << "Unable to connect to left VKB controller!" << std::endl;
exit(-1);
}
SetProcessDPIAware();
}
MainLoop& MainLoop::getInstance()
@@ -46,7 +42,9 @@ void MainLoop::run()
{
static std::chrono::steady_clock::time_point lastBegin =
std::chrono::high_resolution_clock::now();
//YZConsolePlotter plot(-200, 200, 20, 2);
while (isRunning)
{
std::chrono::steady_clock::time_point begin =
@@ -71,14 +69,16 @@ void MainLoop::run()
arm.getJ1().moveBy(moveJ1By, std::nullopt);
arm.getJ2().moveBy(moveJ2By, std::nullopt);
//std::cout << "DJ0: " << moveJ0By << " DJ1: " << moveJ1By << " DJ2: " << moveJ2By << std::endl;
std::cout << arm.getJ2().getGlobalEndpoint() << std::endl;
const Vector3d ef = arm.getJ2().getGlobalEndpoint();
std::cout << ef << std::endl;
//plot.draw(ef);
arm.update(frametime);
// Kill if endeffector enters electronics area
if (electronicsBB.doesIntersect(arm.getJ2().getGlobalEndpoint())) {
//isRunning = false;
std::cout << "Terminating because of AABB violation..." << std::endl;
isRunning = false;
}
lastBegin = std::chrono::high_resolution_clock::now();
MotionCore/MotionCore.vcxproj
+1
View File
@@ -164,6 +164,7 @@
<ClInclude Include="OSerialBus.h" />
<ClInclude Include="Vector3d.h" />
<ClInclude Include="vkb_controller.h" />
<ClInclude Include="yz_plotter_win.h" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
MotionCore/MotionCore.vcxproj.filters
+3
View File
@@ -74,5 +74,8 @@
<ClInclude Include="vkb_controller.h">
<Filter>Headerdateien</Filter>
</ClInclude>
<ClInclude Include="yz_plotter_win.h">
<Filter>Headerdateien</Filter>
</ClInclude>
</ItemGroup>
</Project>
MotionCore/main.cpp
-1
View File
@@ -1,4 +1,3 @@
#include "MainLoop.h"
int main()
MotionCore/yz_plotter_win.h
Executable
+293
View File
@@ -0,0 +1,293 @@
// Externally sourced 2D plotter for data visualization
// yz_plotter_win.h
#pragma once
// Stop Windows from defining min/max macros (and nuke them if already defined)
#ifndef NOMINMAX
#define NOMINMAX
#endif
#include <windows.h>
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#include <string>
#include <vector>
#include <algorithm>
#include <cmath>
#include <sstream>
#include <iomanip>
/// Compute a step (units per pixel) that yields about `targetCells` pixels
/// across the given range [minVal..maxVal]. For [-100..100] span=200:
/// targetCells=21 -> step ~10, targetCells=11 -> step ~20, etc.
inline int stepForTargetCells(int minVal, int maxVal, int targetCells)
{
targetCells = std::max(2, targetCells);
const int span = maxVal - minVal;
const double raw = (double)span / (double)(targetCells - 1);
return std::max(1, (int)std::ceil(raw));
}
/// Flicker-free terminal plotter (Windows console):
/// - Vertical axis: Y (height)
/// - Horizontal axis: Z (forward/back)
/// - Point drawn as "**"
/// - Axes drawn as "||" and "--"
///
/// Range stays fixed (e.g. -100..100) while "pixel count" is controlled by `step`.
class YZConsolePlotter {
public:
// step = units per pixel cell (bigger step => fewer pixels)
// cellW = characters per pixel horizontally (2 recommended for Windows fonts)
explicit YZConsolePlotter(int minVal = -100, int maxVal = 100, int step = 10, int cellW = 2)
: minV_(minVal), maxV_(maxVal), step_(step), cellW_(cellW)
{
normalizeParams_();
recomputeGrid_();
hOut_ = GetStdHandle(STD_OUTPUT_HANDLE);
consoleOK_ = (hOut_ != INVALID_HANDLE_VALUE && hOut_ != nullptr);
if (consoleOK_) {
CONSOLE_SCREEN_BUFFER_INFO csbi{};
if (!GetConsoleScreenBufferInfo(hOut_, &csbi)) {
consoleOK_ = false;
}
else {
anchor_ = csbi.dwCursorPosition;
// Hide cursor (optional)
CONSOLE_CURSOR_INFO cci{};
if (GetConsoleCursorInfo(hOut_, &cci)) {
savedCursor_ = cci;
cci.bVisible = FALSE;
SetConsoleCursorInfo(hOut_, &cci);
cursorSaved_ = true;
}
}
}
// First draw to "claim" the region
draw(Vector3d{ 0, 0, 0 });
}
// Convenience: specify desired pixel count instead of step
static YZConsolePlotter withTargetCells(int minVal, int maxVal, int targetCells, int cellW = 2)
{
return YZConsolePlotter(minVal, maxVal, stepForTargetCells(minVal, maxVal, targetCells), cellW);
}
~YZConsolePlotter() {
if (consoleOK_ && cursorSaved_) {
SetConsoleCursorInfo(hOut_, &savedCursor_);
}
}
void setTitle(std::string t) { title_ = std::move(t); }
void setCompact(bool on) { compact_ = on; }
// If you want to relocate the plot to "here" (current cursor position)
void resetAnchorHere() {
if (!consoleOK_) return;
CONSOLE_SCREEN_BUFFER_INFO csbi{};
if (GetConsoleScreenBufferInfo(hOut_, &csbi)) {
anchor_ = csbi.dwCursorPosition;
}
}
// Main call: draw latest point
void draw(const Vector3d& v) {
const std::string frame = render_(v);
writeAtAnchor_(frame);
}
private:
int minV_{ -100 }, maxV_{ 100 }, step_{ 10 };
int cellW_{ 2 };
int span_{ 200 };
int cols_{ 21 }, rows_{ 21 };
int rowChars_{ 0 };
int originC_{ 0 }, originR_{ 0 };
HANDLE hOut_{ nullptr };
COORD anchor_{ 0, 0 };
bool consoleOK_{ false };
bool cursorSaved_{ false };
CONSOLE_CURSOR_INFO savedCursor_{};
bool compact_{ true };
std::string title_{ "YZ Plot (Y=height vertical, Z=forward horizontal)" };
// For clean overwrite: fixed line width
int labelW_{ 6 }; // " 100 " etc
int lineLen_{ 0 }; // labelW_ + rowChars_
private:
void normalizeParams_()
{
if (step_ <= 0) step_ = 10;
if (cellW_ <= 0) cellW_ = 2;
if (minV_ >= maxV_) { minV_ = -100; maxV_ = 100; }
}
void recomputeGrid_()
{
span_ = maxV_ - minV_;
cols_ = span_ / step_ + 1;
rows_ = span_ / step_ + 1;
rowChars_ = cols_ * cellW_;
originC_ = toCol_(0);
originR_ = toRow_(0);
lineLen_ = labelW_ + rowChars_;
}
int clampI_(double a) const {
int ai = (int)std::lround(a);
return std::max(minV_, std::min(maxV_, ai));
}
int toCol_(int zVal) const { return (zVal - minV_) / step_; }
int toRow_(int yVal) const { return (maxV_ - yVal) / step_; } // +Y upwards
void putCell2_(std::string& row, int c, char a, char b) const
{
const int i = c * cellW_;
if (i < 0 || i >= (int)row.size()) return;
row[i] = a;
if (cellW_ >= 2 && i + 1 < (int)row.size()) row[i + 1] = b;
}
void appendLinePadded_(std::ostringstream& out, const std::string& line) const
{
if ((int)line.size() >= lineLen_) {
out << line.substr(0, (size_t)lineLen_) << "\n";
}
else {
out << line;
out << std::string((size_t)(lineLen_ - (int)line.size()), ' ');
out << "\n";
}
}
void writeAtAnchor_(const std::string& s)
{
if (!consoleOK_) {
return;
}
SetConsoleCursorPosition(hOut_, anchor_);
DWORD written = 0;
WriteConsoleA(hOut_, s.c_str(), (DWORD)s.size(), &written, nullptr);
}
std::string render_(const Vector3d& v) const
{
const int yC = clampI_(v.y);
const int zC = clampI_(v.z);
const int starC = toCol_(zC);
const int starR = toRow_(yC);
std::vector<std::string> grid(rows_, std::string((size_t)rowChars_, ' '));
// axes
if (originR_ >= 0 && originR_ < rows_) {
for (int c = 0; c < cols_; ++c) putCell2_(grid[originR_], c, '-', '-');
}
if (originC_ >= 0 && originC_ < cols_) {
for (int r = 0; r < rows_; ++r) putCell2_(grid[r], originC_, '|', '|');
}
if (originR_ >= 0 && originR_ < rows_ && originC_ >= 0 && originC_ < cols_) {
putCell2_(grid[originR_], originC_, '+', '+');
}
// point
if (starR >= 0 && starR < rows_ && starC >= 0 && starC < cols_) {
putCell2_(grid[starR], starC, '*', '*');
}
std::ostringstream out;
// Header (kept short; padded to overwrite cleanly)
if (!compact_) {
appendLinePadded_(out, title_);
{
std::ostringstream h;
h << std::fixed << std::setprecision(1)
<< "raw x=" << std::setw(7) << v.x
<< " y=" << std::setw(7) << v.y
<< " z=" << std::setw(7) << v.z;
appendLinePadded_(out, h.str());
}
{
std::ostringstream h;
h << "range[" << minV_ << "," << maxV_ << "] step=" << step_
<< " grid=" << cols_ << "x" << rows_
<< " cellW=" << cellW_;
appendLinePadded_(out, h.str());
}
appendLinePadded_(out, ""); // blank
}
else {
std::ostringstream h;
h << std::fixed << std::setprecision(0)
<< "Y=" << std::setw(5) << v.y
<< " Z=" << std::setw(5) << v.z
<< " (clamp Y=" << std::setw(4) << yC
<< " Z=" << std::setw(4) << zC << ")";
appendLinePadded_(out, h.str());
}
// Grid with sparse Y labels
const int labelStride = step_ * (compact_ ? 20 : 10);
for (int r = 0; r < rows_; ++r) {
const int yVal = maxV_ - r * step_;
std::ostringstream line;
if (labelStride > 0 && (yVal % labelStride) == 0) {
line << std::setw(5) << yVal << " ";
}
else {
line << " ";
}
line << grid[r];
appendLinePadded_(out, line.str());
}
// Extra lines to obliterate previous longer output
appendLinePadded_(out, "");
appendLinePadded_(out, "");
return out.str();
}
};
/*
USAGE:
// Few pixels over same -100..100 space:
YZConsolePlotter plot(-100, 100, 20, 2); // 11x11 pixels
// Or pick pixels and auto-compute step:
auto plot = YZConsolePlotter::withTargetCells(-100, 100, 21, 2); // ~21x21 pixels
plot.setCompact(true);
while (...) {
Vector3d ef = arm.getGlobalEndpoint();
plot.draw(ef);
Sleep(16);
}
*/