есть два приложения где я сравниваю свою библиотек...
Creado el: 7 de agosto de 2026
Respondido usando GPT-5.6 Thinking por Chat01
Pregunta
есть два приложения где я сравниваю свою библиотеку оптимизации с решателем TRAC IK для робототехники и оба приложения собраны в релизе - в обоих используются одинаковые версии библиотек, одинаковые каталоги включаемых файлов, дополнительные каталоги библиотек и прочее, то есть оба приложения используют корректную релизную версию TRAC IK библиотеки и сопутствующих библиотек - проблема с замером времени и с его интерпретацией - в демо приложении когда я решаю задачу то вижу очень малое времени для TRAC IK на любой размерности - примерно 0.4 - 1.5 мс, тогда как в бенчмарке я вижу на графике наоборот время сильно больше примерно 35-42 мс, и такие отличия точно не могут быть объяснены разными задачами разной размерности или тем что в демо и бенчмарке у меня разные диапазон на углы - я пробовал ставить такой же диапазон в бенчмарке и время всё равно было сильно больше чем в демо - так что это не может являться убедительной причиной объясняющей такие сильные различия во времени, сейчас я скину тебе два кода демо и бенчмарк а ты должен будешь мне прислать полный код и того и того где мои настройки таймаута и режима решателя и ограничений на углы и прочие настройки TRAC IK не затронуты, единственное что - это то что я ещё новичок в использовании TRAC IK и возможно я неправильно использую эту библиотеку - например возможно я создаю решатель каждый раз тогда как его можно переиспользовать или возможно я неправильно замеряю время тогда как у TRAC IK могут быть встроенные способы замера времени решения - ты должен будешь мне прислать оба исправленных кода где исправлены только эти недостатки в неправильном понимании и неправильном использовании TRAC IK с неправильным замером и отображением времени - то есть и в демо приложении и в бенчмарке ты должен унифицировать способ замера времени чтобы и там и там способ замера времени был одинаков и корректен и корректно отображал время решения задачи TRAC IK в миллисекундах - при этом все остальные другие аспекты обоих приложений ты не должен затрагивать, ты должен будешь прислать мне две полностью исправленные версии целиком код полностью без пропусков кода и без комментариев и только с необходимыми для работы заголовками без лишнего, код демо который ты должен будешь переписать целиком: "#pragma once
#define WIN32_LEAN_AND_MEAN
#include <Windows.h>
#include <cmath>
#include <vector>
#include <algorithm>
#include <kdl/chain.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/frames.hpp>
#include <kdl/joint.hpp>
#include <kdl/segment.hpp>
#include <trac_ik/trac_ik.hpp>
using namespace System;
using namespace System::Drawing;
using namespace System::Windows::Forms;
using namespace System::Collections::Generic;
using namespace System::Drawing::Drawing2D;
using namespace System::Runtime::CompilerServices;
typedef void(__cdecl* P_MANIP)(int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, float**, float*, float*, float*, float*, size_t*, float*, int, const float*);
typedef void(__cdecl* P_FREE)(float*);
typedef void(__cdecl* P_START)(int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, int, const float*, const float*);
typedef void(__cdecl* P_BUILD_TRAJECTORY)(int, bool, float, const float*, const float*, int, float, bool, float, unsigned int, float, float, const float*, int, float**, int*, size_t*);
namespace TESTAGP
{
public enum class DemoMode { Positioning = 0, TrajectoryPlanning = 1 };
public enum class DragHandle { None = 0, Target = 1, Start = 2, AngleTarget = 3, AngleStart = 4 };
textpublic ref class PoseSnapshot sealed { public: PoseSnapshot(int n) { Angles = gcnew array<float>(n); Lengths = gcnew array<float>(n); EndX = EndY = EndA = BestF = 0.0f; Iterations = 0; AchievedEps = Micros = 0.0f; } array<float>^ Angles; array<float>^ Lengths; float EndX, EndY, EndA, BestF; int Iterations; float AchievedEps, Micros; }; public ref class MyForm sealed : public Form { public: MyForm(HMODULE hLib) : hLib(hLib) { this->SetStyle(ControlStyles::AllPaintingInWmPaint | ControlStyles::UserPaint | ControlStyles::OptimizedDoubleBuffer, true); this->Text = L"AGP Manipulator 2D"; this->ClientSize = System::Drawing::Size(1200, 800); this->Resize += gcnew EventHandler(this, &MyForm::OnResize); this->MouseDown += gcnew MouseEventHandler(this, &MyForm::OnMouseDownPoint); this->MouseMove += gcnew MouseEventHandler(this, &MyForm::OnMouseMovePoint); this->MouseUp += gcnew MouseEventHandler(this, &MyForm::OnMouseUpPoint); fManip = (P_MANIP)GetProcAddress(hLib, "AGP_Manip2D"); pFree = (P_FREE)GetProcAddress(hLib, "AGP_Free"); pStart = (P_START)GetProcAddress(hLib, "AgpStartManipND"); pBuildTrajectory = (P_BUILD_TRAJECTORY)GetProcAddress(hLib, "AGP_BuildTransitionTrajectory"); angles = gcnew List<float>(8); lengths = gcnew List<float>(8); obstacleX = gcnew List<float>(4); obstacleY = gcnew List<float>(4); obstacleHalf = gcnew List<float>(4); plannedPoses = gcnew List<PoseSnapshot^>(); animationFrames = gcnew List<PoseSnapshot^>(); trajectoryPathWorld = gcnew List<PointF>(); demoMode = DemoMode::Positioning; InitGraphicsResources(); InitAnimation(); InitUI(); ResetRandomConfig(); SetDefaultStartPoint(true); UpdateTrajectoryUiState(); } private: literal float ObstacleClearance = 0.05f, PI = 3.14159265358979323846f, TWO_PI = 6.28318530717958647692f; literal int AnimationFramesPerSegment = 10, AnimationIntervalMs = 16; literal float TransitionLengthEnergyWeight = 0.35f, TransitionPrefixEnergyWeight = 0.175f; ComboBox^ cbDemoMode; ComboBox^ cbBackend; CheckBox^ cbVarLen; CheckBox^ cbAdaptive; NumericUpDown^ nudMaxTheta; NumericUpDown^ nudBaseLength; NumericUpDown^ nudStretchFactor; NumericUpDown^ nudTargetX; NumericUpDown^ nudTargetY; NumericUpDown^ nudTargetAngle; NumericUpDown^ nudStartX; NumericUpDown^ nudStartY; NumericUpDown^ nudStartAngle; NumericUpDown^ nudMaxIter; NumericUpDown^ nudR; TextBox^ txtEps; float currentEps = 1e-9f; Button^ btnAdd; Button^ btnRem; Button^ btnOptimize; Button^ btnGenerateObstacles; Button^ btnClearObstacles; Label^ lblInfo; Label^ lblStartX; Label^ lblStartY; Label^ lblTargetAngle; Label^ lblStartAngle; P_BUILD_TRAJECTORY pBuildTrajectory; HMODULE hLib; P_MANIP fManip; P_FREE pFree; P_START pStart; Pen^ obstaclePen; Pen^ obstacleMarginPen; SolidBrush^ obstacleBrush; Pen^ wallPen; Pen^ dashedPen; Pen^ targetPen; Pen^ startPen; Pen^ pathPen; Pen^ penRod; SolidBrush^ jointBrush; SolidBrush^ waypointBrush; HatchBrush^ wallHatchBrush; Pen^ angleTargetPen; Pen^ angleStartPen; System::Drawing::Font^ uiFontBold11; System::Drawing::Font^ uiFontTextBox; System::Drawing::Font^ uiFontBold10; Timer^ animationTimer; DemoMode demoMode; int nSegments = 1; bool variableLengths = false; List<float>^ angles; List<float>^ lengths; List<float>^ obstacleX; List<float>^ obstacleY; List<float>^ obstacleHalf; List<PoseSnapshot^>^ plannedPoses; List<PoseSnapshot^>^ animationFrames; List<PointF>^ trajectoryPathWorld; UInt32 rngState = 0xA5C39E0Du; DragHandle activeDragHandle = DragHandle::None; bool updatingFromMouse = false; bool syncingDefaultStartPoint = false; bool startPointCustomized = false; int animationFrameIndex = 0; bool animationRunning = false; bool angleTargetDrag = false; bool angleStartDrag = false; static float WrapPi(float a) { while (a > PI) a -= TWO_PI; while (a < -PI) a += TWO_PI; return a; } static float WrappedDelta(float from, float to) { return WrapPi(to - from); } static float LerpWrappedAngle(float from, float to, float t) { return WrapPi(from + t * WrappedDelta(from, to)); } static float DistanceSquared(PointF a, PointF b) { float dx = a.X - b.X, dy = a.Y - b.Y; return dx * dx + dy * dy; } [MethodImpl(MethodImplOptions::AggressiveInlining)] PointF GetBasePoint() { int drawAreaTop = 180, drawAreaHeight = this->ClientSize.Height - 180, leftWallX = this->ClientSize.Width * 25 / 100; return PointF((float)leftWallX, (float)(drawAreaTop + drawAreaHeight / 2)); } void InitGraphicsResources() { uiFontBold11 = gcnew System::Drawing::Font("Yu Gothic UI", 11, FontStyle::Bold); uiFontBold10 = gcnew System::Drawing::Font("Yu Gothic UI", 10, FontStyle::Bold); uiFontTextBox = gcnew System::Drawing::Font("Yu Gothic UI", 11, FontStyle::Bold); wallPen = gcnew Pen(Color::Black, 2.0f); dashedPen = gcnew Pen(Color::Black, 2.0f); dashedPen->DashStyle = DashStyle::Dash; targetPen = gcnew Pen(Color::Green, 3.0f); targetPen->DashStyle = DashStyle::Dot; startPen = gcnew Pen(Color::FromArgb(255, 140, 0), 3.0f); startPen->DashStyle = DashStyle::Dot; pathPen = gcnew Pen(Color::FromArgb(40, 90, 180), 2.5f); pathPen->DashStyle = DashStyle::Dash; penRod = gcnew Pen(Color::Red, 6.0f); jointBrush = gcnew SolidBrush(Color::Blue); waypointBrush = gcnew SolidBrush(Color::FromArgb(40, 90, 180)); wallHatchBrush = gcnew HatchBrush(HatchStyle::BackwardDiagonal, Color::LightGray, Color::White); obstaclePen = gcnew Pen(Color::FromArgb(90, 30, 30), 2.0f); obstacleMarginPen = gcnew Pen(Color::FromArgb(215, 140, 0), 2.0f); obstacleMarginPen->DashStyle = DashStyle::Dash; obstacleBrush = gcnew SolidBrush(Color::FromArgb(180, 120, 120, 120)); angleTargetPen = gcnew Pen(Color::Red, 2.0f); angleTargetPen->DashStyle = DashStyle::Dash; angleStartPen = gcnew Pen(Color::DarkOrange, 2.0f); angleStartPen->DashStyle = DashStyle::Dash; } void InitAnimation() { animationTimer = gcnew Timer(); animationTimer->Interval = AnimationIntervalMs; animationTimer->Tick += gcnew EventHandler(this, &MyForm::OnAnimationTick); } void InitUI() { cbDemoMode = gcnew ComboBox(); cbDemoMode->Location = Point(920, 20); cbDemoMode->Width = 260; cbDemoMode->Height = 28; cbDemoMode->DropDownStyle = ComboBoxStyle::DropDownList; cbDemoMode->Font = uiFontBold11; cbDemoMode->BackColor = SystemColors::Info; cbDemoMode->FlatStyle = FlatStyle::Flat; cbDemoMode->Items->Add(L"Позиционирование"); cbDemoMode->Items->Add(L"Планирование траектории"); cbDemoMode->SelectedIndex = 0; cbDemoMode->SelectedIndexChanged += gcnew EventHandler(this, &MyForm::OnDemoModeChanged); this->Controls->Add(cbDemoMode); cbBackend = gcnew ComboBox(); cbBackend->Location = Point(920, 54); cbBackend->Width = 260; cbBackend->Height = 28; cbBackend->DropDownStyle = ComboBoxStyle::DropDownList; cbBackend->Font = uiFontBold11; cbBackend->BackColor = SystemColors::Info; cbBackend->FlatStyle = FlatStyle::Flat; cbBackend->Items->Add(L"AGP"); cbBackend->Items->Add(L"TRAC-IK"); cbBackend->SelectedIndex = 0; cbBackend->SelectedIndexChanged += gcnew EventHandler(this, &MyForm::OnBackendChanged); this->Controls->Add(cbBackend); Label^ L = gcnew Label(); L->Text = L"Макс. угол (рад.)"; L->Location = Point(20, 20); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudMaxTheta = gcnew NumericUpDown(); nudMaxTheta->Location = Point(20, 52); nudMaxTheta->Width = 200; nudMaxTheta->DecimalPlaces = 3; nudMaxTheta->Minimum = Decimal(0.01); nudMaxTheta->Maximum = Decimal(3.14159); nudMaxTheta->Value = Decimal(2.0); nudMaxTheta->Font = uiFontTextBox; nudMaxTheta->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudMaxTheta); L = gcnew Label(); L->Text = L"Базовая длина"; L->Location = Point(245, 20); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudBaseLength = gcnew NumericUpDown(); nudBaseLength->Location = Point(245, 52); nudBaseLength->Width = 200; nudBaseLength->DecimalPlaces = 2; nudBaseLength->Minimum = Decimal(0.5); nudBaseLength->Maximum = Decimal(2.0); nudBaseLength->Value = Decimal(1.0); nudBaseLength->Font = uiFontTextBox; nudBaseLength->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudBaseLength); L = gcnew Label(); L->Text = L"Макс. коэфф. растяжения/сжатия"; L->Location = Point(470, 20); L->Width = 300; L->Font = uiFontBold11; this->Controls->Add(L); nudStretchFactor = gcnew NumericUpDown(); nudStretchFactor->Location = Point(470, 52); nudStretchFactor->Width = 200; nudStretchFactor->DecimalPlaces = 2; nudStretchFactor->Minimum = Decimal(1.0); nudStretchFactor->Maximum = Decimal(1.5); nudStretchFactor->Increment = Decimal(0.01); nudStretchFactor->Value = Decimal(1.5); nudStretchFactor->Font = uiFontTextBox; nudStretchFactor->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudStretchFactor); cbVarLen = gcnew CheckBox(); cbVarLen->Text = L"Переменные длины"; cbVarLen->Location = Point(695, 52); cbVarLen->Width = 200; cbVarLen->Checked = false; cbVarLen->Font = uiFontBold11; cbVarLen->CheckedChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(cbVarLen); L = gcnew Label(); L->Text = L"Цель X"; L->Location = Point(20, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudTargetX = gcnew NumericUpDown(); nudTargetX->Location = Point(20, 139); nudTargetX->Width = 200; nudTargetX->DecimalPlaces = 2; nudTargetX->Minimum = Decimal(-10.0); nudTargetX->Maximum = Decimal(10.0); nudTargetX->Value = Decimal(2.5); nudTargetX->Font = uiFontTextBox; nudTargetX->ValueChanged += gcnew EventHandler(this, &MyForm::OnTargetChanged); this->Controls->Add(nudTargetX); L = gcnew Label(); L->Text = L"Цель Y"; L->Location = Point(245, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudTargetY = gcnew NumericUpDown(); nudTargetY->Location = Point(245, 139); nudTargetY->Width = 200; nudTargetY->DecimalPlaces = 2; nudTargetY->Minimum = Decimal(-10.0); nudTargetY->Maximum = Decimal(10.0); nudTargetY->Value = Decimal(-1.0); nudTargetY->Font = uiFontTextBox; nudTargetY->ValueChanged += gcnew EventHandler(this, &MyForm::OnTargetChanged); this->Controls->Add(nudTargetY); lblTargetAngle = gcnew Label(); lblTargetAngle->Text = L"Цель угол (рад.)"; lblTargetAngle->Location = Point(20, 194); lblTargetAngle->Width = 200; lblTargetAngle->Font = uiFontBold11; this->Controls->Add(lblTargetAngle); nudTargetAngle = gcnew NumericUpDown(); nudTargetAngle->Location = Point(20, 226); nudTargetAngle->Width = 200; nudTargetAngle->DecimalPlaces = 3; nudTargetAngle->Minimum = Decimal(-1000); nudTargetAngle->Maximum = Decimal(1000); nudTargetAngle->Increment = Decimal(0.1); nudTargetAngle->Value = Decimal(0); nudTargetAngle->Font = uiFontTextBox; nudTargetAngle->ValueChanged += gcnew EventHandler(this, &MyForm::OnAngleChanged); this->Controls->Add(nudTargetAngle); L = gcnew Label(); L->Text = L"Надежность (r)"; L->Location = Point(470, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudR = gcnew NumericUpDown(); nudR->Location = Point(470, 139); nudR->Width = 200; nudR->DecimalPlaces = 2; nudR->Minimum = Decimal(1.0); nudR->Maximum = Decimal(20.0); nudR->Value = Decimal(1.05); nudR->Font = uiFontTextBox; nudR->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudR); L = gcnew Label(); L->Text = L"Точность"; L->Location = Point(695, 107); L->Width = 100; L->Font = uiFontBold11; this->Controls->Add(L); txtEps = gcnew TextBox(); txtEps->Location = Point(695, 139); txtEps->Width = 80; txtEps->Font = uiFontTextBox; txtEps->Text = L"1E-09"; txtEps->TextChanged += gcnew EventHandler(this, &MyForm::OnEpsTextChanged); this->Controls->Add(txtEps); Button^ btnEpsUp = gcnew Button(); btnEpsUp->Text = L"×10"; btnEpsUp->Location = Point(780, 139); btnEpsUp->Width = 32; btnEpsUp->Height = 26; btnEpsUp->Font = uiFontBold11; btnEpsUp->TextAlign = ContentAlignment::TopRight; btnEpsUp->Padding = System::Windows::Forms::Padding(0, 0, 2, 3); btnEpsUp->Click += gcnew EventHandler(this, &MyForm::OnEpsOrderUp); this->Controls->Add(btnEpsUp); Button^ btnEpsDown = gcnew Button(); btnEpsDown->Text = L"÷10"; btnEpsDown->Location = Point(816, 139); btnEpsDown->Width = 32; btnEpsDown->Height = 26; btnEpsDown->Font = uiFontBold11; btnEpsDown->TextAlign = ContentAlignment::TopRight; btnEpsDown->Padding = System::Windows::Forms::Padding(0, 0, 2, 3); btnEpsDown->Click += gcnew EventHandler(this, &MyForm::OnEpsOrderDown); this->Controls->Add(btnEpsDown); L = gcnew Label(); L->Text = L"Макс. итераций"; L->Location = Point(860, 107); L->Width = 130; L->Font = uiFontBold11; this->Controls->Add(L); nudMaxIter = gcnew NumericUpDown(); nudMaxIter->Location = Point(860, 139); nudMaxIter->Width = 120; nudMaxIter->Minimum = 10; nudMaxIter->Maximum = 500000; nudMaxIter->Value = 1000; nudMaxIter->Font = uiFontTextBox; nudMaxIter->Increment = 100; nudMaxIter->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudMaxIter); cbAdaptive = gcnew CheckBox(); cbAdaptive->Text = L"Адаптивная схема"; cbAdaptive->Location = Point(995, 139); cbAdaptive->Width = 150; cbAdaptive->Checked = true; cbAdaptive->Font = uiFontBold11; cbAdaptive->CheckedChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(cbAdaptive); btnAdd = gcnew Button(); btnAdd->Text = L"+ Звено"; btnAdd->Location = Point(465, 211); btnAdd->Width = 90; btnAdd->Height = 35; btnAdd->BackColor = SystemColors::Info; btnAdd->Cursor = Cursors::Hand; btnAdd->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnAdd->FlatAppearance->BorderSize = 3; btnAdd->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnAdd->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnAdd->FlatStyle = FlatStyle::Flat; btnAdd->Font = uiFontBold11; btnAdd->ForeColor = SystemColors::ControlDarkDark; btnAdd->Click += gcnew EventHandler(this, &MyForm::OnAddClick); this->Controls->Add(btnAdd); btnRem = gcnew Button(); btnRem->Text = L"- Звено"; btnRem->Location = Point(560, 211); btnRem->Width = 90; btnRem->Height = 35; btnRem->BackColor = SystemColors::Info; btnRem->Cursor = Cursors::Hand; btnRem->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnRem->FlatAppearance->BorderSize = 3; btnRem->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnRem->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnRem->FlatStyle = FlatStyle::Flat; btnRem->Font = uiFontBold11; btnRem->ForeColor = SystemColors::ControlDarkDark; btnRem->Click += gcnew EventHandler(this, &MyForm::OnRemClick); this->Controls->Add(btnRem); btnOptimize = gcnew Button(); btnOptimize->Text = L"Оптимизировать"; btnOptimize->Location = Point(680, 211); btnOptimize->Width = 150; btnOptimize->Height = 35; btnOptimize->BackColor = SystemColors::Info; btnOptimize->Cursor = Cursors::Hand; btnOptimize->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnOptimize->FlatAppearance->BorderSize = 3; btnOptimize->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnOptimize->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnOptimize->FlatStyle = FlatStyle::Flat; btnOptimize->Font = uiFontBold11; btnOptimize->ForeColor = SystemColors::ControlDarkDark; btnOptimize->Click += gcnew EventHandler(this, &MyForm::OnOptimizeClick); this->Controls->Add(btnOptimize); btnGenerateObstacles = gcnew Button(); btnGenerateObstacles->Location = Point(465, 257); btnGenerateObstacles->Width = 365; btnGenerateObstacles->Height = 35; btnGenerateObstacles->BackColor = SystemColors::Info; btnGenerateObstacles->Cursor = Cursors::Hand; btnGenerateObstacles->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnGenerateObstacles->FlatAppearance->BorderSize = 3; btnGenerateObstacles->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnGenerateObstacles->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnGenerateObstacles->FlatStyle = FlatStyle::Flat; btnGenerateObstacles->Font = uiFontBold11; btnGenerateObstacles->ForeColor = SystemColors::ControlDarkDark; btnGenerateObstacles->Click += gcnew EventHandler(this, &MyForm::OnGenerateObstaclesClick); this->Controls->Add(btnGenerateObstacles); btnClearObstacles = gcnew Button(); btnClearObstacles->Location = Point(465, 304); btnClearObstacles->Width = 365; btnClearObstacles->Height = 35; btnClearObstacles->BackColor = SystemColors::Info; btnClearObstacles->Cursor = Cursors::Hand; btnClearObstacles->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnClearObstacles->FlatAppearance->BorderSize = 3; btnClearObstacles->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnClearObstacles->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnClearObstacles->FlatStyle = FlatStyle::Flat; btnClearObstacles->Font = uiFontBold11; btnClearObstacles->ForeColor = SystemColors::ControlDarkDark; btnClearObstacles->Text = L"Очистить"; btnClearObstacles->Click += gcnew EventHandler(this, &MyForm::OnClearObstaclesClick); this->Controls->Add(btnClearObstacles); lblInfo = gcnew Label(); lblInfo->Location = Point(835, 194); lblInfo->Size = System::Drawing::Size(275, 145); lblInfo->BorderStyle = BorderStyle::FixedSingle; lblInfo->Font = uiFontBold10; this->Controls->Add(lblInfo); lblStartX = gcnew Label(); lblStartX->Text = L"Начало X"; lblStartX->Location = Point(20, 272); lblStartX->Width = 200; lblStartX->Font = uiFontBold11; this->Controls->Add(lblStartX); nudStartX = gcnew NumericUpDown(); nudStartX->Location = Point(20, 304); nudStartX->Width = 200; nudStartX->DecimalPlaces = 2; nudStartX->Minimum = Decimal(-10.0); nudStartX->Maximum = Decimal(10.0); nudStartX->Value = Decimal(1.25); nudStartX->Font = uiFontTextBox; nudStartX->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartPointChanged); this->Controls->Add(nudStartX); lblStartY = gcnew Label(); lblStartY->Text = L"Начало Y"; lblStartY->Location = Point(245, 272); lblStartY->Width = 200; lblStartY->Font = uiFontBold11; this->Controls->Add(lblStartY); nudStartY = gcnew NumericUpDown(); nudStartY->Location = Point(245, 304); nudStartY->Width = 200; nudStartY->DecimalPlaces = 2; nudStartY->Minimum = Decimal(-10.0); nudStartY->Maximum = Decimal(10.0); nudStartY->Value = Decimal(-0.5); nudStartY->Font = uiFontTextBox; nudStartY->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartPointChanged); this->Controls->Add(nudStartY); lblStartAngle = gcnew Label(); lblStartAngle->Text = L"Нач. угол (рад.)"; lblStartAngle->Location = Point(20, 340); lblStartAngle->Width = 200; lblStartAngle->Font = uiFontBold11; this->Controls->Add(lblStartAngle); nudStartAngle = gcnew NumericUpDown(); nudStartAngle->Location = Point(20, 372); nudStartAngle->Width = 200; nudStartAngle->DecimalPlaces = 3; nudStartAngle->Minimum = Decimal(-1000); nudStartAngle->Maximum = Decimal(1000); nudStartAngle->Increment = Decimal(0.1); nudStartAngle->Value = Decimal(0); nudStartAngle->Font = uiFontTextBox; nudStartAngle->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartAngleChanged); this->Controls->Add(nudStartAngle); UpdateBackendUiState(); UpdateTrajectoryUiState(); } void UpdateBackendUiState() { bool tracIkSelected = (cbBackend->SelectedIndex == 1); bool obstaclesEnabled = !tracIkSelected; btnGenerateObstacles->Enabled = obstaclesEnabled; btnClearObstacles->Enabled = obstaclesEnabled; if (tracIkSelected) { btnGenerateObstacles->Text = L"Препятствия отключены"; btnGenerateObstacles->ForeColor = Color::Gold; btnGenerateObstacles->BackColor = SystemColors::Control; btnClearObstacles->BackColor = SystemColors::Control; if (obstacleX->Count > 0) { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); ClearTrajectoryCache(); this->Invalidate(); } } else { btnGenerateObstacles->Text = L"Сгенерировать препятствия"; btnGenerateObstacles->ForeColor = SystemColors::ControlDarkDark; btnGenerateObstacles->BackColor = SystemColors::Info; btnClearObstacles->BackColor = SystemColors::Info; } } void UpdateTrajectoryUiState() { bool trajectoryMode = IsTrajectoryMode(); lblStartX->Visible = trajectoryMode; lblStartY->Visible = trajectoryMode; nudStartX->Visible = trajectoryMode; nudStartY->Visible = trajectoryMode; lblStartX->Enabled = trajectoryMode; lblStartY->Enabled = trajectoryMode; nudStartX->Enabled = trajectoryMode; nudStartY->Enabled = trajectoryMode; if (trajectoryMode) { lblStartAngle->Location = Point(245, 194); lblStartAngle->Width = 200; nudStartAngle->Location = Point(245, 226); nudStartAngle->Width = 200; lblStartAngle->Visible = true; nudStartAngle->Visible = true; lblStartAngle->Enabled = true; nudStartAngle->Enabled = true; } else { lblStartAngle->Location = Point(20, 340); lblStartAngle->Width = 200; nudStartAngle->Location = Point(20, 372); nudStartAngle->Width = 200; lblStartAngle->Visible = false; nudStartAngle->Visible = false; lblStartAngle->Enabled = false; nudStartAngle->Enabled = false; } if (!trajectoryMode) { ClearTrajectoryCache(); nudStartAngle->Value = Decimal(0); } this->Invalidate(); } void ResetRandomConfig() { nSegments = 1; angles->Clear(); lengths->Clear(); obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); angles->Add(0.0f); lengths->Add((float)nudBaseLength->Value); variableLengths = false; ClearTrajectoryCache(); this->Invalidate(); } void ClearTrajectoryCache() { StopAnimation(); plannedPoses->Clear(); animationFrames->Clear(); trajectoryPathWorld->Clear(); } void StopAnimation() { if (animationTimer) animationTimer->Stop(); animationFrameIndex = 0; animationRunning = false; } void ClearObstacles() { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); ClearTrajectoryCache(); this->Invalidate(); } [MethodImpl(MethodImplOptions::AggressiveInlining)] float Rand01() { rngState ^= rngState << 13; rngState ^= rngState >> 17; rngState ^= rngState << 5; return (float)(rngState) * 2.3283064e-10f; } [MethodImpl(MethodImplOptions::AggressiveInlining)] float Lerp(float a, float b, float t) { return a + (b - a) * t; } [MethodImpl(MethodImplOptions::AggressiveInlining)] bool IsTrajectoryMode() { return demoMode == DemoMode::TrajectoryPlanning; } void SetDefaultStartPoint(bool forceResetCustomization) { if (forceResetCustomization) startPointCustomized = false; if (startPointCustomized) return; syncingDefaultStartPoint = true; nudStartX->Value = Decimal((double)((float)nudTargetX->Value * 0.5f)); nudStartY->Value = Decimal((double)((float)nudTargetY->Value * 0.5f)); syncingDefaultStartPoint = false; } array<float>^ BuildObstacleBuffer() { int count = obstacleX->Count; array<float>^ data = gcnew array<float>(count * 3); for (int i = 0; i < count; ++i) { data[3 * i] = obstacleX[i]; data[3 * i + 1] = obstacleY[i]; data[3 * i + 2] = obstacleHalf[i]; } return data; } void GenerateRandomObstacles() { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); bool varLen = cbVarLen->Checked; float baseLength = (float)nudBaseLength->Value, stretchFactor = (float)nudStretchFactor->Value, tx = (float)nudTargetX->Value, ty = (float)nudTargetY->Value; float dist = (float)Math::Sqrt(tx * tx + ty * ty); if (dist <= 1e-6f) { ClearTrajectoryCache(); this->Invalidate(); return; } float maxReach = nSegments * (varLen ? baseLength * stretchFactor : baseLength); float slack = maxReach - dist; float halfMin = Math::Max(0.14f * baseLength, 0.12f); float halfMax = Math::Min(0.26f * baseLength + 0.05f * (slack / (baseLength + 1e-6f)), 0.32f); if (halfMax < halfMin + 0.03f) halfMax = halfMin + 0.03f; float alongMargin = Math::Max(0.55f * baseLength, 1.5f * (halfMax + ObstacleClearance)); alongMargin = Math::Max(alongMargin, 0.35f); float usableLen = dist - 2.0f * alongMargin; int obstacleCountToCreate = 2 + (int)(Rand01() * 3.0f); if (nSegments == 2 && obstacleCountToCreate > 3) obstacleCountToCreate = 3; if (slack < 0.35f * baseLength && obstacleCountToCreate > 2) obstacleCountToCreate = 2; while (obstacleCountToCreate > 2) { float gapTest = usableLen / (obstacleCountToCreate + 1); if (gapTest >= 2.2f * halfMax) break; --obstacleCountToCreate; } float ux = tx / dist, uy = ty / dist, nx = -uy, ny = ux; float gap = usableLen / (obstacleCountToCreate + 1); float firstSide = (Rand01() < 0.5f) ? -1.0f : 1.0f; for (int i = 0; i < obstacleCountToCreate; ++i) { float nominalS = alongMargin + gap * (i + 1); float jitter = (Rand01() - 0.5f) * gap * 0.18f; float s = nominalS + jitter; float half = halfMin + (halfMax - halfMin) * Rand01(); float side = ((i & 1) == 0) ? firstSide : -firstSide; float offsetMag = half * (0.18f + 0.28f * Rand01()); obstacleX->Add(ux * s + nx * side * offsetMag); obstacleY->Add(uy * s + ny * side * offsetMag); obstacleHalf->Add(half); } ClearTrajectoryCache(); this->Invalidate(); } void UpdateEpsDisplay() { String^ value = currentEps.ToString("E3", System::Globalization::CultureInfo::InvariantCulture); if (txtEps->Text != value) txtEps->Text = value; } void OnEpsTextChanged(Object^ sender, EventArgs^ e) { float val; if (float::TryParse(txtEps->Text, System::Globalization::NumberStyles::Float, System::Globalization::CultureInfo::InvariantCulture, val)) { if (val < 1e-9f) val = 1e-9f; if (val > 1e-1f) val = 1e-1f; currentEps = val; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } else UpdateEpsDisplay(); } void OnEpsOrderUp(Object^ sender, EventArgs^ e) { float newVal = currentEps * 10.0f; if (newVal > 1e-1f) newVal = 1e-1f; currentEps = newVal; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } void OnEpsOrderDown(Object^ sender, EventArgs^ e) { float newVal = currentEps / 10.0f; if (newVal < 1e-9f) newVal = 1e-9f; currentEps = newVal; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } void OnAngleChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; float val = WrapPi((float)nudTargetAngle->Value); Decimal wrapped = Decimal((double)val); if (nudTargetAngle->Value != wrapped) nudTargetAngle->Value = wrapped; ClearTrajectoryCache(); this->Invalidate(); } void OnStartAngleChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; float val = WrapPi((float)nudStartAngle->Value); Decimal wrapped = Decimal((double)val); if (nudStartAngle->Value != wrapped) nudStartAngle->Value = wrapped; ClearTrajectoryCache(); this->Invalidate(); } void OnClearObstaclesClick(Object^ sender, EventArgs^ e) { ClearObstacles(); } void OnGenerateObstaclesClick(Object^ sender, EventArgs^ e) { GenerateRandomObstacles(); } void ApplyPoseToManipulator(PoseSnapshot^ pose) { angles->Clear(); lengths->Clear(); for (int i = 0; i < nSegments; ++i) { angles->Add(pose->Angles[i]); lengths->Add(pose->Lengths[i]); } } void ComputeEndEffector(array<float>^ poseAngles, array<float>^ poseLengths, float% outX, float% outY, float% outA) { float x = 0, y = 0, phi = 0; int count = Math::Min(poseAngles->Length, poseLengths->Length); for (int i = 0; i < count; ++i) { phi += poseAngles[i]; x += poseLengths[i] * (float)Math::Cos(phi); y += poseLengths[i] * (float)Math::Sin(phi); } outX = x; outY = y; outA = WrapPi(phi); } PoseSnapshot^ RunAgpCppAtPoint(float tx, float ty, float ta) { variableLengths = cbVarLen->Checked; float maxTheta = (float)nudMaxTheta->Value; int maxIter = (int)nudMaxIter->Value; bool adaptive = cbAdaptive->Checked; float r_param = (float)nudR->Value; float eps = currentEps; unsigned int seed = (unsigned int)GetTickCount(); float baseLength = (float)nudBaseLength->Value; float stretchFactor = (float)nudStretchFactor->Value; array<float>^ obstacleData = BuildObstacleBuffer(); pin_ptr<float> pinnedObstacles = nullptr; const float* pObstacleData = nullptr; if (obstacleData->Length > 0) { pinnedObstacles = &obstacleData[0]; pObstacleData = pinnedObstacles; } int obstacleCount = obstacleX->Count; pStart(nSegments, variableLengths, maxTheta, tx, ty, ta, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObstacleData, obstacleCount, 0, nullptr, nullptr); float* bestQ; float bestX, bestY, bestA, bestF; size_t actualIterations; float achievedEps; LARGE_INTEGER t0, t1, fq; QueryPerformanceCounter(&t0); fManip(nSegments, variableLengths, maxTheta, tx, ty, ta, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObstacleData, obstacleCount, &bestQ, &bestX, &bestY, &bestA, &bestF, &actualIterations, &achievedEps, 0, nullptr); QueryPerformanceCounter(&t1); QueryPerformanceFrequency(&fq); PoseSnapshot^ pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) pose->Angles[i] = bestQ[i]; if (variableLengths) for (int i = 0; i < nSegments; ++i) pose->Lengths[i] = bestQ[nSegments + i]; else for (int i = 0; i < nSegments; ++i) pose->Lengths[i] = baseLength; pFree(bestQ); pose->EndX = bestX; pose->EndY = bestY; pose->EndA = WrapPi(bestA); pose->BestF = bestF; pose->Iterations = (int)actualIterations; pose->AchievedEps = achievedEps; pose->Micros = (float)(1e6 * (double)(t1.QuadPart - t0.QuadPart) / (double)fq.QuadPart); return pose; } PoseSnapshot^ RunTracIkPositioning(float tx, float ty, float ta) { float baseLength = (float)nudBaseLength->Value, maxTheta = (float)nudMaxTheta->Value, eps = currentEps; KDL::Chain chain; for (int i = 0; i < nSegments; ++i) chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame(KDL::Vector(baseLength, 0.0, 0.0)))); int nJoints = chain.getNrOfJoints(); KDL::JntArray q_min(nJoints), q_max(nJoints); for (int i = 0; i < nJoints; ++i) { q_min(i) = -maxTheta; q_max(i) = maxTheta; } double max_time = 0.002; trac_ik::TRAC_IK ik_solver(chain, q_min, q_max, max_time, eps, trac_ik::SolveType::Speed); KDL::Frame target(KDL::Rotation::RotZ(ta), KDL::Vector(tx, ty, 0.0)); KDL::JntArray q_init(nJoints); for (int i = 0; i < nJoints; ++i) q_init(i) = 0.0; KDL::JntArray q_out(nJoints); KDL::Twist tolerances(KDL::Vector(eps, eps, 0.0), KDL::Vector(0.0, 0.0, eps)); LARGE_INTEGER t0, t1, fq; QueryPerformanceCounter(&t0); int result = ik_solver.CartToJnt(q_init, target, q_out, tolerances); QueryPerformanceCounter(&t1); QueryPerformanceFrequency(&fq); PoseSnapshot^ pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) { pose->Angles[i] = (float)q_out(i); pose->Lengths[i] = baseLength; } float x, y, a; ComputeEndEffector(pose->Angles, pose->Lengths, x, y, a); pose->EndX = x; pose->EndY = y; pose->EndA = a; float dx = x - tx, dy = y - ty, da = WrappedDelta(a, ta); pose->BestF = sqrtf(dx * dx + dy * dy + da * da); pose->Iterations = -1; pose->AchievedEps = -1.0f; pose->Micros = (float)(1e6 * (double)(t1.QuadPart - t0.QuadPart) / (double)fq.QuadPart); return pose; } void UpdatePositioningStats(PoseSnapshot^ pose, float tx, float ty, float ta, bool isTracIk) { float dx = pose->EndX - tx, dy = pose->EndY - ty; float distance = sqrtf(dx * dx + dy * dy); String^ iterationsStr = isTracIk ? "—" : pose->Iterations.ToString(); String^ epsStr = isTracIk ? "—" : pose->AchievedEps.ToString("E3"); lblInfo->Text = String::Format( L"Функционал: {0:F6}\nБлизость захвата: {1:F5}\nДостигнутая точка: ({2:F3}, {3:F3})\nДостигнутый угол: {4:F3} рад.\nВремя: {5:F2} мс\nЧисло шагов: {6}\nДостигнутая точность: {7}", pose->BestF, distance, pose->EndX, pose->EndY, pose->EndA, pose->Micros / 1000.0f, iterationsStr, epsStr); } void UpdateTrajectoryStats(List<PoseSnapshot^>^ poses, float totalMicros, size_t totalIterations, float finalAchievedEps, bool isTracIk) { if (poses->Count == 0) { lblInfo->Text = L"Траектория не построена"; return; } float totalEnergy = 0; for (int i = 1; i < poses->Count; ++i) totalEnergy += ComputeTransitionEnergy(poses[i - 1], poses[i]); PoseSnapshot^ lastPose = poses[poses->Count - 1]; float rawTargetX = (float)nudTargetX->Value, rawTargetY = (float)nudTargetY->Value; float finalDx = lastPose->EndX - rawTargetX, finalDy = lastPose->EndY - rawTargetY; float finalDistance = sqrtf(finalDx * finalDx + finalDy * finalDy); int actualIntermediateCount = Math::Max(0, poses->Count - 2); String^ iterationsStr = isTracIk ? "—" : totalIterations.ToString(); String^ epsStr = isTracIk ? "—" : finalAchievedEps.ToString("E3"); lblInfo->Text = String::Format( L"Промежуточных точек: {0}\nФункционал траектории: {1:F6}\nБлизость финиша: {2:F5}\nДостигнутый угол: {3:F3} рад.\nВремя: {4:F2} мс\nЧисло шагов: {5}\nДостигнутая точность: {6}", actualIntermediateCount, totalEnergy, finalDistance, lastPose->EndA, totalMicros / 1000.0f, iterationsStr, epsStr); } float ComputeTransitionEnergy(PoseSnapshot^ prevPose, PoseSnapshot^ nextPose) { float total = 0, prevPrefix = 0, nextPrefix = 0; for (int i = 0; i < prevPose->Angles->Length; ++i) { float d = WrappedDelta(prevPose->Angles[i], nextPose->Angles[i]); total += d * d; prevPrefix += prevPose->Angles[i]; nextPrefix += nextPose->Angles[i]; float dp = WrappedDelta(prevPrefix, nextPrefix); total += TransitionPrefixEnergyWeight * dp * dp; } if (variableLengths) for (int i = 0; i < prevPose->Lengths->Length; ++i) { float dl = nextPose->Lengths[i] - prevPose->Lengths[i]; total += TransitionLengthEnergyWeight * dl * dl; } return total; } void RunTrajectoryPlanningMode() { if (cbBackend->SelectedIndex == 1) RunTrajectoryWithTracIk(); else RunTrajectoryWithAGP(); } bool BuildPoseFromState(const float* state, bool varLen, float baseLength, PoseSnapshot^% pose) { pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) { pose->Angles[i] = state[i]; pose->Lengths[i] = varLen ? state[nSegments + i] : baseLength; } float x, y, a; ComputeEndEffector(pose->Angles, pose->Lengths, x, y, a); pose->EndX = x; pose->EndY = y; pose->EndA = a; return true; } void RunTrajectoryWithAGP() { ClearTrajectoryCache(); array<float>^ obsData = BuildObstacleBuffer(); pin_ptr<float> pinnedObs = nullptr; const float* pObs = nullptr; if (obsData->Length > 0) { pinnedObs = &obsData[0]; pObs = pinnedObs; } int obstacleCount = obstacleX->Count; float startX = (float)nudStartX->Value, startY = (float)nudStartY->Value, startA = (float)nudStartAngle->Value, targetX = (float)nudTargetX->Value, targetY = (float)nudTargetY->Value, targetA = (float)nudTargetAngle->Value, maxTheta = (float)nudMaxTheta->Value, baseLength = (float)nudBaseLength->Value, stretchFactor = (float)nudStretchFactor->Value, r_param = (float)nudR->Value, eps = currentEps; int maxIter = (int)nudMaxIter->Value; bool varLen = cbVarLen->Checked, adaptive = cbAdaptive->Checked; unsigned int seed = (unsigned int)GetTickCount(); int stateDim = nSegments << 1; pStart(nSegments, varLen, maxTheta, startX, startY, startA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 0, nullptr, nullptr); LARGE_INTEGER totalT0, totalT1, fq; QueryPerformanceCounter(&totalT0); float* startQ; float startXOut, startYOut, startAOut, startF; size_t startIterations; float startEps; fManip(nSegments, varLen, maxTheta, startX, startY, startA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &startQ, &startXOut, &startYOut, &startAOut, &startF, &startIterations, &startEps, 0, nullptr); PoseSnapshot^ startPose; BuildPoseFromState(startQ, varLen, baseLength, startPose); startPose->EndX = startXOut; startPose->EndY = startYOut; startPose->EndA = WrapPi(startAOut); startPose->BestF = startF; startPose->Iterations = (int)startIterations; startPose->AchievedEps = startEps; std::vector<float> startState(stateDim); for (int i = 0; i < nSegments; ++i) { startState[i] = startPose->Angles[i]; startState[nSegments + i] = startPose->Lengths[i]; } pFree(startQ); pStart(nSegments, varLen, maxTheta, targetX, targetY, targetA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 1, startState.data(), nullptr); float* finalQ; float finalXOut, finalYOut, finalAOut, finalF; size_t finalIterations; float finalEps; fManip(nSegments, varLen, maxTheta, targetX, targetY, targetA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &finalQ, &finalXOut, &finalYOut, &finalAOut, &finalF, &finalIterations, &finalEps, 1, startState.data()); PoseSnapshot^ finalPose; BuildPoseFromState(finalQ, varLen, baseLength, finalPose); finalPose->EndX = finalXOut; finalPose->EndY = finalYOut; finalPose->EndA = WrapPi(finalAOut); finalPose->BestF = finalF; finalPose->Iterations = (int)finalIterations; finalPose->AchievedEps = finalEps; std::vector<float> finalState(stateDim); for (int i = 0; i < nSegments; ++i) { finalState[i] = finalPose->Angles[i]; finalState[nSegments + i] = finalPose->Lengths[i]; } pFree(finalQ); pStart(nSegments, varLen, maxTheta, 0, 0, 0, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 2, startState.data(), finalState.data()); float* trajPoints; int pointCount; size_t trajectoryIterations; pBuildTrajectory(nSegments, varLen, maxTheta, startState.data(), finalState.data(), maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &trajPoints, &pointCount, &trajectoryIterations); QueryPerformanceCounter(&totalT1); QueryPerformanceFrequency(&fq); float micros = (float)(1e6 * (double)(totalT1.QuadPart - totalT0.QuadPart) / (double)fq.QuadPart); size_t totalIterations = trajectoryIterations + startIterations + finalIterations; plannedPoses->Clear(); for (int i = 0; i < pointCount; ++i) { const float* ptr = trajPoints + i * stateDim; PoseSnapshot^ pose; BuildPoseFromState(ptr, varLen, baseLength, pose); plannedPoses->Add(pose); } pFree(trajPoints); plannedPoses[0]->EndX = startPose->EndX; plannedPoses[0]->EndY = startPose->EndY; plannedPoses[0]->EndA = startPose->EndA; plannedPoses[plannedPoses->Count - 1]->EndX = finalPose->EndX; plannedPoses[plannedPoses->Count - 1]->EndY = finalPose->EndY; plannedPoses[plannedPoses->Count - 1]->EndA = finalPose->EndA; UpdateTrajectoryStats(plannedPoses, micros, totalIterations, finalEps, false); BuildAnimationFramesFromPlan(); ApplyPoseToManipulator(plannedPoses[0]); StartAnimationIfNeeded(); this->Invalidate(); } void RunTrajectoryWithTracIk() { ClearTrajectoryCache(); float startX = (float)nudStartX->Value, startY = (float)nudStartY->Value, startA = (float)nudStartAngle->Value; float targetX = (float)nudTargetX->Value, targetY = (float)nudTargetY->Value, targetA = (float)nudTargetAngle->Value; LARGE_INTEGER totalT0, totalT1, fq; QueryPerformanceCounter(&totalT0); PoseSnapshot^ startPose = RunTracIkPositioning(startX, startY, startA); PoseSnapshot^ endPose = RunTracIkPositioning(targetX, targetY, targetA); QueryPerformanceCounter(&totalT1); QueryPerformanceFrequency(&fq); float micros = (float)(1e6 * (double)(totalT1.QuadPart - totalT0.QuadPart) / (double)fq.QuadPart); plannedPoses->Clear(); plannedPoses->Add(startPose); plannedPoses->Add(endPose); UpdateTrajectoryStats(plannedPoses, micros, 0, -1.0f, true); BuildAnimationFramesFromPlan(); ApplyPoseToManipulator(plannedPoses[0]); StartAnimationIfNeeded(); this->Invalidate(); } void RunPositioningMode() { StopAnimation(); float tx = (float)nudTargetX->Value, ty = (float)nudTargetY->Value, ta = (float)nudTargetAngle->Value; bool isTracIk = (cbBackend->SelectedIndex == 1); PoseSnapshot^ pose = isTracIk ? RunTracIkPositioning(tx, ty, ta) : RunAgpCppAtPoint(tx, ty, ta); ClearTrajectoryCache(); ApplyPoseToManipulator(pose); UpdatePositioningStats(pose, tx, ty, ta, isTracIk); this->Invalidate(); this->Refresh(); } void DrawTrajectorySegmentClipped(Graphics^ g, PointF aWorld, PointF bWorld) { float prevX = aWorld.X, prevY = aWorld.Y; bool prevAllowed = IsDisplayPointAllowed(prevX, prevY); for (int s = 1; s <= 20; ++s) { float t = s / 20.0f; float currX = Lerp(aWorld.X, bWorld.X, t), currY = Lerp(aWorld.Y, bWorld.Y, t); bool currAllowed = IsDisplayPointAllowed(currX, currY); if (prevAllowed && currAllowed) { PointF pa = WorldToPixel(prevX, prevY), pb = WorldToPixel(currX, currY); g->DrawLine(pathPen, pa, pb); } prevX = currX; prevY = currY; prevAllowed = currAllowed; } } void DrawTrajectoryPath(Graphics^ g) { if (trajectoryPathWorld->Count < 2) return; for (int i = 1; i < trajectoryPathWorld->Count; ++i) DrawTrajectorySegmentClipped(g, trajectoryPathWorld[i - 1], trajectoryPathWorld[i]); if (plannedPoses->Count > 2) for (int i = 1; i < plannedPoses->Count - 1; ++i) { if (!IsDisplayPointAllowed(plannedPoses[i]->EndX, plannedPoses[i]->EndY)) continue; PointF wp = WorldToPixel(plannedPoses[i]->EndX, plannedPoses[i]->EndY); g->FillEllipse(waypointBrush, wp.X - 4.0f, wp.Y - 4.0f, 8.0f, 8.0f); } } void RebuildTrajectoryDisplayPathFromPlan() { trajectoryPathWorld->Clear(); if (animationFrames->Count > 0) { for (int i = 0; i < animationFrames->Count; ++i) { PointF p(animationFrames[i]->EndX, animationFrames[i]->EndY); if (trajectoryPathWorld->Count == 0 || DistanceSquared(trajectoryPathWorld[trajectoryPathWorld->Count - 1], p) > 1e-10f) trajectoryPathWorld->Add(p); } return; } for (int i = 0; i < plannedPoses->Count; ++i) { PointF p(plannedPoses[i]->EndX, plannedPoses[i]->EndY); if (trajectoryPathWorld->Count == 0 || DistanceSquared(trajectoryPathWorld[trajectoryPathWorld->Count - 1], p) > 1e-10f) trajectoryPathWorld->Add(p); } } void BuildAnimationFramesFromPlan() { animationFrames->Clear(); if (plannedPoses->Count == 0) { RebuildTrajectoryDisplayPathFromPlan(); return; } animationFrames->Add(plannedPoses[0]); for (int i = 0; i < plannedPoses->Count - 1; ++i) { PoseSnapshot^ firstPose = plannedPoses[i], ^ secondPose = plannedPoses[i + 1]; for (int frame = 1; frame <= AnimationFramesPerSegment; ++frame) { float t = frame / (float)AnimationFramesPerSegment; PoseSnapshot^ interp = gcnew PoseSnapshot(nSegments); for (int j = 0; j < nSegments; ++j) { interp->Angles[j] = LerpWrappedAngle(firstPose->Angles[j], secondPose->Angles[j], t); interp->Lengths[j] = variableLengths ? firstPose->Lengths[j] + t * (secondPose->Lengths[j] - firstPose->Lengths[j]) : (float)nudBaseLength->Value; } float x, y, angle; ComputeEndEffector(interp->Angles, interp->Lengths, x, y, angle); interp->EndX = x; interp->EndY = y; interp->EndA = angle; animationFrames->Add(interp); } } RebuildTrajectoryDisplayPathFromPlan(); } void StartAnimationIfNeeded() { if (animationFrames->Count <= 1) { animationRunning = false; this->Invalidate(); return; } animationFrameIndex = 0; animationRunning = true; animationTimer->Start(); } void OnAnimationTick(Object^ sender, EventArgs^ e) { if (!animationRunning || animationFrames->Count == 0) { StopAnimation(); return; } if (animationFrameIndex >= animationFrames->Count) { StopAnimation(); return; } ApplyPoseToManipulator(animationFrames[animationFrameIndex]); ++animationFrameIndex; this->Invalidate(); if (animationFrameIndex >= animationFrames->Count) StopAnimation(); } PointF WorldToPixel(float wx, float wy) { PointF basePoint = GetBasePoint(); return PointF(basePoint.X + wx * 160.0f, basePoint.Y - wy * 160.0f); } float GetCurrentMaxReach() { float baseLength = (float)nudBaseLength->Value, stretchFactor = (float)nudStretchFactor->Value; return nSegments * (cbVarLen->Checked ? baseLength * stretchFactor : baseLength); } bool IsDisplayPointAllowed(float x, float y) { float maxReach = GetCurrentMaxReach(); if (x * x + y * y > (maxReach + 0.05f) * (maxReach + 0.05f)) return false; for (int i = 0; i < obstacleX->Count; ++i) { float half = obstacleHalf[i] + ObstacleClearance; if (x >= obstacleX[i] - half && x <= obstacleX[i] + half && y >= obstacleY[i] - half && y <= obstacleY[i] + half) return false; } return true; } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseDownPoint(Object^ sender, MouseEventArgs^ e) { PointF basePoint = GetBasePoint(); float baseX = basePoint.X, baseY = basePoint.Y; float targetX = (float)nudTargetX->Value, targetY = (float)nudTargetY->Value, targetA = (float)nudTargetAngle->Value; float pixelTargetX = baseX + targetX * 160.0f, pixelTargetY = baseY - targetY * 160.0f, angleLineLen = 25.0f; float endX = pixelTargetX + angleLineLen * (float)Math::Cos(targetA), endY = pixelTargetY - angleLineLen * (float)Math::Sin(targetA); float dx = e->X - endX, dy = e->Y - endY; if (dx * dx + dy * dy < 225.0f) { StopAnimation(); activeDragHandle = DragHandle::AngleTarget; angleTargetDrag = true; updatingFromMouse = true; this->Capture = true; return; } float bestDistSquared = 100.0f; activeDragHandle = DragHandle::None; if (IsTrajectoryMode()) { float startX = (float)nudStartX->Value, startY = (float)nudStartY->Value, startA = (float)nudStartAngle->Value; float pixelStartX = baseX + startX * 160.0f, pixelStartY = baseY - startY * 160.0f; float sEndX = pixelStartX + angleLineLen * (float)Math::Cos(startA), sEndY = pixelStartY - angleLineLen * (float)Math::Sin(startA); dx = e->X - sEndX; dy = e->Y - sEndY; if (dx * dx + dy * dy < 225.0f) { StopAnimation(); activeDragHandle = DragHandle::AngleStart; angleStartDrag = true; updatingFromMouse = true; this->Capture = true; return; } float dsx = e->X - pixelStartX, dsy = e->Y - pixelStartY; float startDistSquared = dsx * dsx + dsy * dsy; if (startDistSquared < bestDistSquared) { bestDistSquared = startDistSquared; activeDragHandle = DragHandle::Start; } } float dtx = e->X - pixelTargetX, dty = e->Y - pixelTargetY; float targetDistSquared = dtx * dtx + dty * dty; if (targetDistSquared < bestDistSquared) activeDragHandle = DragHandle::Target; if (activeDragHandle != DragHandle::None) { StopAnimation(); updatingFromMouse = true; this->Capture = true; } } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseMovePoint(Object^ sender, MouseEventArgs^ e) { if (activeDragHandle == DragHandle::None && !angleTargetDrag && !angleStartDrag) return; PointF basePoint = GetBasePoint(); float baseX = basePoint.X, baseY = basePoint.Y; if (angleTargetDrag) { float targetX = (float)nudTargetX->Value, targetY = (float)nudTargetY->Value; float pixelTargetX = baseX + targetX * 160.0f, pixelTargetY = baseY - targetY * 160.0f; float dx = e->X - pixelTargetX, dy = e->Y - pixelTargetY; float angle = WrapPi(atan2f(-dy, dx)); nudTargetAngle->Value = Decimal((double)angle); ClearTrajectoryCache(); this->Invalidate(); return; } if (angleStartDrag && IsTrajectoryMode()) { float startX = (float)nudStartX->Value, startY = (float)nudStartY->Value; float pixelStartX = baseX + startX * 160.0f, pixelStartY = baseY - startY * 160.0f; float dx = e->X - pixelStartX, dy = e->Y - pixelStartY; float angle = WrapPi(atan2f(-dy, dx)); nudStartAngle->Value = Decimal((double)angle); ClearTrajectoryCache(); this->Invalidate(); return; } if (activeDragHandle == DragHandle::None) return; float minX = (float)nudTargetX->Minimum, maxX = (float)nudTargetX->Maximum, minY = (float)nudTargetY->Minimum, maxY = (float)nudTargetY->Maximum; float newX = (e->X - baseX) / 160.0f, newY = (baseY - e->Y) / 160.0f; newX = Math::Max(minX, Math::Min(maxX, newX)); newY = Math::Max(minY, Math::Min(maxY, newY)); if (activeDragHandle == DragHandle::Target) { nudTargetX->Value = Decimal((double)newX); nudTargetY->Value = Decimal((double)newY); } else if (activeDragHandle == DragHandle::Start) { startPointCustomized = true; nudStartX->Value = Decimal((double)newX); nudStartY->Value = Decimal((double)newY); } ClearTrajectoryCache(); this->Invalidate(); } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseUpPoint(Object^ sender, MouseEventArgs^ e) { if (activeDragHandle != DragHandle::None || angleTargetDrag || angleStartDrag) { activeDragHandle = DragHandle::None; angleTargetDrag = false; angleStartDrag = false; updatingFromMouse = false; this->Capture = false; } } void OnBackendChanged(Object^ sender, EventArgs^ e) { UpdateBackendUiState(); ClearTrajectoryCache(); this->Invalidate(); } void OnDemoModeChanged(Object^ sender, EventArgs^ e) { demoMode = (cbDemoMode->SelectedIndex == 0) ? DemoMode::Positioning : DemoMode::TrajectoryPlanning; if (demoMode == DemoMode::TrajectoryPlanning && !startPointCustomized) SetDefaultStartPoint(false); UpdateTrajectoryUiState(); } void OnResize(Object^ sender, EventArgs^ e) { this->Invalidate(); } void OnAnyChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; ClearTrajectoryCache(); this->Invalidate(); } void OnTargetChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; if (!startPointCustomized) SetDefaultStartPoint(false); ClearTrajectoryCache(); this->Invalidate(); } void OnStartPointChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; if (!syncingDefaultStartPoint) startPointCustomized = true; ClearTrajectoryCache(); this->Invalidate(); } void OnAddClick(Object^ sender, EventArgs^ e) { ++nSegments; angles->Add(0.0f); lengths->Add((float)nudBaseLength->Value); ClearTrajectoryCache(); this->Invalidate(); } void OnRemClick(Object^ sender, EventArgs^ e) { if (nSegments > 1) { --nSegments; angles->RemoveAt(angles->Count - 1); lengths->RemoveAt(lengths->Count - 1); ClearTrajectoryCache(); this->Invalidate(); } } void OnOptimizeClick(Object^ sender, EventArgs^ e) { if (IsTrajectoryMode()) RunTrajectoryPlanningMode(); else RunPositioningMode(); } protected: [MethodImpl(MethodImplOptions::AggressiveInlining)] virtual void OnPaint(PaintEventArgs^ e) override { Form::OnPaint(e); Graphics^ g = e->Graphics; g->SmoothingMode = SmoothingMode::HighQuality; g->Clear(this->BackColor); int drawHeight = Math::Max(0, this->ClientSize.Height - 180); System::Drawing::Rectangle drawArea(0, 180, this->ClientSize.Width, drawHeight); g->FillRectangle(Brushes::White, drawArea); float targetX = (float)nudTargetX->Value, targetY = (float)nudTargetY->Value, targetA = (float)nudTargetAngle->Value; PointF basePoint = GetBasePoint(); int baseX = (int)basePoint.X, baseY = (int)basePoint.Y; float pixelTargetX = baseX + targetX * 160.0f, pixelTargetY = baseY - targetY * 160.0f; g->DrawLine(wallPen, baseX - 25, baseY + 8, baseX + 25, baseY + 8); g->FillRectangle(wallHatchBrush, baseX - 25, baseY + 8, 50, 12); g->DrawLine(dashedPen, pixelTargetX - 25, pixelTargetY + 8, pixelTargetX + 25, pixelTargetY + 8); g->FillRectangle(wallHatchBrush, (int)pixelTargetX - 25, (int)pixelTargetY + 8, 50, 12); g->DrawEllipse(targetPen, pixelTargetX - 8.0f, pixelTargetY - 8.0f, 16.0f, 16.0f); float angleLen = 25.0f; float endX = pixelTargetX + angleLen * (float)Math::Cos(targetA), endY = pixelTargetY - angleLen * (float)Math::Sin(targetA); g->DrawLine(angleTargetPen, pixelTargetX, pixelTargetY, endX, endY); if (IsTrajectoryMode()) { float startX = (float)nudStartX->Value, startY = (float)nudStartY->Value, startA = (float)nudStartAngle->Value; float pixelStartX = baseX + startX * 160.0f, pixelStartY = baseY - startY * 160.0f; g->DrawLine(dashedPen, pixelStartX - 25, pixelStartY + 8, pixelStartX + 25, pixelStartY + 8); g->FillRectangle(wallHatchBrush, (int)pixelStartX - 25, (int)pixelStartY + 8, 50, 12); g->DrawEllipse(startPen, pixelStartX - 8.0f, pixelStartY - 8.0f, 16.0f, 16.0f); float sEndX = pixelStartX + angleLen * (float)Math::Cos(startA), sEndY = pixelStartY - angleLen * (float)Math::Sin(startA); g->DrawLine(angleStartPen, pixelStartX, pixelStartY, sEndX, sEndY); } if (trajectoryPathWorld->Count > 1 && IsTrajectoryMode()) DrawTrajectoryPath(g); if (obstacleX->Count > 0) { array<float>^ xs = obstacleX->ToArray(); array<float>^ ys = obstacleY->ToArray(); array<float>^ hs = obstacleHalf->ToArray(); int obstacleCount = Math::Min(xs->Length, Math::Min(ys->Length, hs->Length)); for (int obsIdx = 0; obsIdx < obstacleCount; ++obsIdx) { float cx = xs[obsIdx], cy = ys[obsIdx], half = hs[obsIdx]; float left = (float)baseX + (cx - half) * 160.0f, top = (float)baseY - (cy + half) * 160.0f, size = 2.0f * half * 160.0f; float marginLeft = (float)baseX + (cx - half - ObstacleClearance) * 160.0f, marginTop = (float)baseY - (cy + half + ObstacleClearance) * 160.0f, marginSize = 2.0f * (half + ObstacleClearance) * 160.0f; g->DrawRectangle(obstacleMarginPen, marginLeft, marginTop, marginSize, marginSize); g->FillRectangle(obstacleBrush, left, top, size, size); g->DrawRectangle(obstaclePen, left, top, size, size); } } int drawableSegments = Math::Min(nSegments, Math::Min(angles->Count, lengths->Count)); array<PointF>^ pts = gcnew array<PointF>(drawableSegments + 1); pts[0] = PointF((float)baseX, (float)baseY); float x = 0.0f, y = 0.0f, phi = 0.0f; array<float>^ localAngles = angles->ToArray(); array<float>^ localLengths = lengths->ToArray(); for (int i = 0; i < drawableSegments; ++i) { phi += localAngles[i]; x += localLengths[i] * (float)Math::Cos(phi); y += localLengths[i] * (float)Math::Sin(phi); pts[i + 1] = PointF((float)baseX + x * 160.0f, (float)baseY - y * 160.0f); } for (int i = 0; i < drawableSegments; ++i) g->DrawLine(penRod, pts[i], pts[i + 1]); for (int i = 0; i <= drawableSegments; ++i) g->FillEllipse(jointBrush, pts[i].X - 8.0f, pts[i].Y - 8.0f, 16.0f, 16.0f); } };
}", код бенчмарка который ты должен будешь переписать целиком: "#pragma once
#define WIN32_LEAN_AND_MEAN
#include <Windows.h>
#include <cfloat>
#include <cmath>
#include <algorithm>
#include <memory>
#include <limits>
#include <kdl/chain.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/frames.hpp>
#include <kdl/joint.hpp>
#include <kdl/segment.hpp>
#include <trac_ik/trac_ik.hpp>
using namespace System;
using namespace System::Drawing;
using namespace System::Windows::Forms;
using namespace System::Collections::Generic;
using namespace System::Text;
using namespace System::Windows::Forms::DataVisualization::Charting;
typedef void(__cdecl* P_MANIP_INDEX)(
int,
bool,
float,
float,
float,
float,
int,
float,
bool,
float,
unsigned int,
float,
float,
const float*,
int,
float**,
float*,
float*,
float*,
float*,
size_t*,
float*,
int,
const float*
);
typedef void(__cdecl* P_FREE)(
float*
);
typedef void(__cdecl* P_START_INDEX)(
int,
bool,
float,
float,
float,
float,
int,
float,
bool,
float,
unsigned int,
float,
float,
const float*,
int,
int,
const float*,
const float*
);
class TracIkRunner final
{
private:
int nSegments_;
unsigned int nJoints_;
double eps_;
double baseLength_;
textKDL::Chain chain_; KDL::JntArray qMin_; KDL::JntArray qMax_; KDL::JntArray qInit_; KDL::JntArray qOut_; std::unique_ptr<trac_ik::TRAC_IK> solver_; LARGE_INTEGER frequency_;
public:
TracIkRunner(
int nSegments,
double baseLength,
double maxTime,
double eps)
:
nSegments_(nSegments),
nJoints_(0),
eps_(eps),
baseLength_(baseLength)
{
for (int i = 0; i < nSegments_; ++i) {
chain_.addSegment(
KDL::Segment(
KDL::Joint(KDL::Joint::RotZ),
KDL::Frame(
KDL::Vector(
baseLength_,
0.0,
0.0
)
)
)
);
}
textnJoints_ = chain_.getNrOfJoints(); qMin_.resize(nJoints_); qMax_.resize(nJoints_); qInit_.resize(nJoints_); qOut_.resize(nJoints_); const double pi = 3.1415926535897932384626433832795; for (unsigned int i = 0; i < nJoints_; ++i) { qMin_(i) = -pi; qMax_(i) = pi; qInit_(i) = 0.0; qOut_(i) = 0.0; } solver_ = std::make_unique<trac_ik::TRAC_IK>( chain_, qMin_, qMax_, maxTime, eps_, trac_ik::SolveType::Speed ); QueryPerformanceFrequency(&frequency_); } TracIkRunner(const TracIkRunner&) = delete; TracIkRunner& operator=(const TracIkRunner&) = delete; bool Solve( float targetX, float targetY, float targetAngle, double& outBestF, double& outBestX, double& outBestY, double& outMillis) { for (unsigned int i = 0; i < nJoints_; ++i) { qInit_(i) = 0.0; qOut_(i) = 0.0; } KDL::Frame target( KDL::Rotation::RotZ( static_cast<double>(targetAngle) ), KDL::Vector( static_cast<double>(targetX), static_cast<double>(targetY), 0.0 ) ); KDL::Twist tolerances( KDL::Vector( eps_, eps_, 0.0 ), KDL::Vector( 0.0, 0.0, eps_ ) ); LARGE_INTEGER t0; LARGE_INTEGER t1; QueryPerformanceCounter(&t0); const int result = solver_->CartToJnt( qInit_, target, qOut_, tolerances ); QueryPerformanceCounter(&t1); outMillis = 1.0e3 * static_cast<double>(t1.QuadPart - t0.QuadPart) / static_cast<double>(frequency_.QuadPart); if (result < 0) { outBestF = std::numeric_limits<double>::quiet_NaN(); outBestX = std::numeric_limits<double>::quiet_NaN(); outBestY = std::numeric_limits<double>::quiet_NaN(); return false; } const double pi = 3.1415926535897932384626433832795; double x = 0.0; double y = 0.0; double phi = 0.0; for (unsigned int i = 0; i < nJoints_; ++i) { phi += qOut_(i); x += baseLength_ * std::cos(phi); y += baseLength_ * std::sin(phi); } const double dx = x - static_cast<double>(targetX); const double dy = y - static_cast<double>(targetY); double da = phi - static_cast<double>(targetAngle); da = std::fmod( da + 3.0 * pi, 2.0 * pi ) - pi; const double f = std::sqrt( dx * dx + dy * dy + da * da ); if (!std::isfinite(f) || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(outMillis)) { return false; } outBestF = f; outBestX = x; outBestY = y; return true; }
};
namespace TESTAGP {
textpublic ref struct BenchmarkCase sealed { public: float TargetX; float TargetY; float TargetAngle; double TracF; double TracX; double TracY; double TracMillis; }; public ref class MyForm sealed : public Form { public: MyForm(HMODULE hLib) : hLib(hLib), experimentsStarted(false) { this->Text = L"AGP vs TRAC-IK - позиционирование с ориентацией"; this->ClientSize = System::Drawing::Size( 1350, 860 ); this->SetStyle( ControlStyles::AllPaintingInWmPaint | ControlStyles::UserPaint | ControlStyles::OptimizedDoubleBuffer, true ); fManipIndex = reinterpret_cast<P_MANIP_INDEX>( GetProcAddress( hLib, "AGP_Manip2D" ) ); pFree = reinterpret_cast<P_FREE>( GetProcAddress( hLib, "AGP_Free" ) ); pStartIndex = reinterpret_cast<P_START_INDEX>( GetProcAddress( hLib, "AgpStartManipND" ) ); if (!fManipIndex || !pFree || !pStartIndex) { MessageBox::Show( L"Не удалось получить адреса AGP_Manip2D / AGP_Free / AgpStartManipND из DLL", L"Ошибка", MessageBoxButtons::OK, MessageBoxIcon::Error ); this->Close(); return; } maxTheta = static_cast<float>( Math::PI ); rParam = 1.05f; epsParam = 1.0e-9f; baseLength = 1.0f; stretchFactor = 1.0f; adaptiveAgp = true; randomEngine = gcnew Random( 123456 ); uiFontBold11 = gcnew System::Drawing::Font( "Yu Gothic UI", 11, FontStyle::Bold ); uiFont10 = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Regular ); InitUI(); this->Shown += gcnew EventHandler( this, &MyForm::OnFormShown ); this->Resize += gcnew EventHandler( this, &MyForm::OnResizeInternal ); } private: initonly HMODULE hLib; initonly P_MANIP_INDEX fManipIndex; initonly P_FREE pFree; initonly P_START_INDEX pStartIndex; System::Drawing::Font^ uiFontBold11; System::Drawing::Font^ uiFont10; Random^ randomEngine; Chart^ chartTime; Chart^ chartF; TextBox^ tbStats; bool experimentsStarted; float maxTheta; float rParam; float epsParam; float baseLength; float stretchFactor; bool adaptiveAgp; double NextUniform( double a, double b) { return a + (b - a) * randomEngine->NextDouble(); } unsigned int NextSeedU32() { const unsigned int hi = static_cast<unsigned int>( randomEngine->Next( 1, Int32::MaxValue ) ); const unsigned int lo = static_cast<unsigned int>( randomEngine->Next( 0, Int32::MaxValue ) ); return (hi << 1) ^ lo ^ 0x9E3779B9u; } static void ComputeMeanStd( List<double>^ values, double% mean, double% stdDev) { const int n = values->Count; if (n <= 0) { mean = Double::NaN; stdDev = Double::NaN; return; } double sum = 0.0; for each(double v in values) { if (Double::IsNaN(v) || Double::IsInfinity(v)) { continue; } sum += v; } int validCount = 0; for each(double v in values) { if (!Double::IsNaN(v) && !Double::IsInfinity(v)) { ++validCount; } } if (validCount <= 0) { mean = Double::NaN; stdDev = Double::NaN; return; } mean = sum / static_cast<double>( validCount ); if (validCount == 1) { stdDev = 0.0; return; } double varianceSum = 0.0; for each(double v in values) { if (Double::IsNaN(v) || Double::IsInfinity(v)) { continue; } const double d = v - mean; varianceSum += d * d; } stdDev = Math::Sqrt( varianceSum / static_cast<double>( validCount - 1 ) ); } static bool IsFinite( double value) { return !Double::IsNaN(value) && !Double::IsInfinity(value); } static bool IsFinitePair( double x, double y) { return IsFinite(x) && IsFinite(y); } BenchmarkCase^ GenerateRandomCase( int nSegments) { BenchmarkCase^ task = gcnew BenchmarkCase(); const double maxReach = Math::Max( 1.0, static_cast<double>( nSegments ) * static_cast<double>( baseLength ) ); const double targetRadiusMin = 0.30 * maxReach; const double targetRadiusMax = 0.90 * maxReach; const double angle = NextUniform( 0.0, 2.0 * Math::PI ); const double radius = NextUniform( targetRadiusMin, targetRadiusMax ); const double targetAngle = NextUniform( -Math::PI, Math::PI ); task->TargetX = static_cast<float>( radius * Math::Cos(angle) ); task->TargetY = static_cast<float>( radius * Math::Sin(angle) ); task->TargetAngle = static_cast<float>( targetAngle ); task->TracF = Double::NaN; task->TracX = Double::NaN; task->TracY = Double::NaN; task->TracMillis = Double::NaN; return task; } void InitUI() { chartTime = gcnew Chart(); chartF = gcnew Chart(); tbStats = gcnew TextBox(); chartTime->Parent = this; chartF->Parent = this; tbStats->Parent = this; chartTime->BorderlineDashStyle = ChartDashStyle::Solid; chartTime->BorderlineWidth = 1; chartTime->BorderlineColor = Color::Black; chartF->BorderlineDashStyle = ChartDashStyle::Solid; chartF->BorderlineWidth = 1; chartF->BorderlineColor = Color::Black; ChartArea^ areaTime = gcnew ChartArea( "TimeArea" ); areaTime->AxisX->Title = "Порог maxIter"; areaTime->AxisY->Title = "Время, мс"; areaTime->AxisX->Minimum = 100.0; areaTime->AxisX->Maximum = 1000.0; areaTime->AxisY->Minimum = 0.0; chartTime->ChartAreas->Add( areaTime ); Legend^ legendTime = gcnew Legend( "LegendTime" ); legendTime->Docking = Docking::Top; legendTime->Font = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Bold ); chartTime->Legends->Add( legendTime ); CreateSeriesWithStyle( chartTime, "AGP", "TimeArea", Color::Red, 3 ); CreateSeriesWithStyle( chartTime, "TRAC IK", "TimeArea", Color::Blue, 3 ); ChartArea^ areaF = gcnew ChartArea( "FArea" ); areaF->AxisX->Title = "Порог maxIter"; areaF->AxisY->Title = "Значение целевой функции f"; areaF->AxisX->Minimum = 100.0; areaF->AxisX->Maximum = 1000.0; areaF->AxisY->Minimum = 0.0; chartF->ChartAreas->Add( areaF ); Legend^ legendF = gcnew Legend( "LegendF" ); legendF->Docking = Docking::Top; legendF->Font = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Bold ); chartF->Legends->Add( legendF ); CreateSeriesWithStyle( chartF, "AGP", "FArea", Color::Red, 3 ); CreateSeriesWithStyle( chartF, "TRAC IK", "FArea", Color::Blue, 3 ); tbStats->Multiline = true; tbStats->ReadOnly = true; tbStats->ScrollBars = ScrollBars::Vertical; tbStats->Font = uiFont10; tbStats->Text = L"Подготовка benchmark: генерация хороших задач...\r\n"; OnResizeInternal( nullptr, nullptr ); } void CreateSeriesWithStyle( Chart^ chart, String^ name, String^ areaName, Color color, int borderWidth) { Series^ s = gcnew Series( name ); s->ChartType = SeriesChartType::Line; s->ChartArea = areaName; s->Color = color; s->BorderWidth = borderWidth; s->MarkerStyle = MarkerStyle::Circle; s->MarkerSize = 7; s->MarkerColor = color; chart->Series->Add( s ); } void OnResizeInternal( Object^, EventArgs^) { const int margin = 10; const int statsWidth = 320; int totalWidth = this->ClientSize.Width; int totalHeight = this->ClientSize.Height; if (totalWidth < 500) { totalWidth = 500; } if (totalHeight < 320) { totalHeight = 320; } int chartsWidth = totalWidth - statsWidth - 3 * margin; if (chartsWidth < 250) { chartsWidth = 250; } const int chartHeight = (totalHeight - 3 * margin) / 2; chartTime->Location = Point( margin, margin ); chartTime->Size = System::Drawing::Size( chartsWidth, chartHeight ); chartF->Location = Point( margin, 2 * margin + chartHeight ); chartF->Size = System::Drawing::Size( chartsWidth, chartHeight ); tbStats->Location = Point( 2 * margin + chartsWidth, margin ); tbStats->Size = System::Drawing::Size( statsWidth, totalHeight - 2 * margin ); } void OnFormShown( Object^, EventArgs^) { if (experimentsStarted) { return; } experimentsStarted = true; try { RunAllExperiments(); } catch (Exception^ ex) { tbStats->Text = L"Ошибка при выполнении benchmark:\r\n" + ex->ToString(); } } bool RunAgpIndexSingle( int nSegments, BenchmarkCase^ task, int maxIter, unsigned int seed, double% outBestF, double% outBestX, double% outBestY, std::size_t% outIterations, double% outAchievedEps, double% outMillis) { float* bestQ = nullptr; float bestXf = 0.0f; float bestYf = 0.0f; float bestAf = 0.0f; float bestFf = FLT_MAX; std::size_t actualIterations = 0; float achievedEps = 0.0f; const float* obsRaw = nullptr; const int obstacleCount = 0; const float ta = task->TargetAngle; pStartIndex( nSegments, false, maxTheta, task->TargetX, task->TargetY, ta, maxIter, rParam, adaptiveAgp, epsParam, seed, baseLength, stretchFactor, obsRaw, obstacleCount, 0, nullptr, nullptr ); LARGE_INTEGER t0; LARGE_INTEGER t1; LARGE_INTEGER fq; QueryPerformanceFrequency( &fq ); QueryPerformanceCounter( &t0 ); fManipIndex( nSegments, false, maxTheta, task->TargetX, task->TargetY, ta, maxIter, rParam, adaptiveAgp, epsParam, seed, baseLength, stretchFactor, obsRaw, obstacleCount, &bestQ, &bestXf, &bestYf, &bestAf, &bestFf, &actualIterations, &achievedEps, 0, nullptr ); QueryPerformanceCounter( &t1 ); outBestF = static_cast<double>( bestFf ); outBestX = static_cast<double>( bestXf ); outBestY = static_cast<double>( bestYf ); outIterations = actualIterations; outAchievedEps = static_cast<double>( achievedEps ); outMillis = 1.0e3 * static_cast<double>( t1.QuadPart - t0.QuadPart ) / static_cast<double>( fq.QuadPart ); if (bestQ != nullptr) { pFree( bestQ ); bestQ = nullptr; } if (!IsFinite(outBestF) || !IsFinite(outBestX) || !IsFinite(outBestY) || !IsFinite(outAchievedEps) || !IsFinite(outMillis)) { return false; } return true; } bool RunTracIkSingle( TracIkRunner& runner, BenchmarkCase^ task, double% outBestF, double% outBestX, double% outBestY, double% outMillis) { double bestF = Double::NaN; double bestX = Double::NaN; double bestY = Double::NaN; double millis = Double::NaN; const bool result = runner.Solve( task->TargetX, task->TargetY, task->TargetAngle, bestF, bestX, bestY, millis ); outBestF = bestF; outBestX = bestX; outBestY = bestY; outMillis = millis; return result && IsFinitePair( outBestF, outMillis ); } void RunAllExperiments() { array<int>^ dimensions = gcnew array<int> { 3, 4, 5, 6, 7, 8 }; array<int>^ thresholds = gcnew array<int> { 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000 }; const int steps = thresholds->Length; const int dimensionCount = dimensions->Length; const int goodTasksPerDimension = 10; const int repeatsStochastic = 100; const double tracMaxTime = 0.002; array<List<BenchmarkCase^>^>^ goodTasksByDimension = gcnew array<List<BenchmarkCase^>^>( dimensionCount ); for (int dimIndex = 0; dimIndex < dimensionCount; ++dimIndex) { const int nSegments = dimensions[dimIndex]; tbStats->AppendText( String::Format( L"Генерация хороших задач для размерности {0}...\r\n", nSegments ) ); Application::DoEvents(); List<BenchmarkCase^>^ goodForDim = gcnew List<BenchmarkCase^>(); TracIkRunner tracRunner( nSegments, static_cast<double>( baseLength ), tracMaxTime, static_cast<double>( epsParam ) ); int attempts = 0; while (goodForDim->Count < goodTasksPerDimension) { ++attempts; BenchmarkCase^ task = GenerateRandomCase( nSegments ); double f; double x; double y; double ms; const bool ok = RunTracIkSingle( tracRunner, task, f, x, y, ms ); if (ok && f <= 0.05) { task->TracF = f; task->TracX = x; task->TracY = y; task->TracMillis = ms; goodForDim->Add( task ); tbStats->AppendText( String::Format( L" Найдена хорошая задача {0}, ошибка = {1:F6}, время = {2:F4} мс\r\n", goodForDim->Count, f, ms ) ); } if ((attempts % 32) == 0) { Application::DoEvents(); } } goodTasksByDimension[dimIndex] = goodForDim; Application::DoEvents(); } const int totalGoodTasks = dimensionCount * goodTasksPerDimension; const int totalSeedCount = totalGoodTasks * repeatsStochastic; array<unsigned int>^ fixedSeeds = gcnew array<unsigned int>( totalSeedCount ); for (int i = 0; i < totalSeedCount; ++i) { fixedSeeds[i] = NextSeedU32(); } array<double>^ agpTimeMean = gcnew array<double>( steps ); array<double>^ agpTimeStd = gcnew array<double>( steps ); array<double>^ agpFMean = gcnew array<double>( steps ); array<double>^ agpFStd = gcnew array<double>( steps ); array<double>^ tracTimeMean = gcnew array<double>( steps ); array<double>^ tracTimeStd = gcnew array<double>( steps ); array<double>^ tracFMean = gcnew array<double>( steps ); array<double>^ tracFStd = gcnew array<double>( steps ); for (int i = 0; i < steps; ++i) { agpTimeMean[i] = Double::NaN; agpTimeStd[i] = Double::NaN; agpFMean[i] = Double::NaN; agpFStd[i] = Double::NaN; tracTimeMean[i] = Double::NaN; tracTimeStd[i] = Double::NaN; tracFMean[i] = Double::NaN; tracFStd[i] = Double::NaN; } array<List<double>^>^ agpTimeForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ agpFForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ tracTimeForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ tracFForThreshold = gcnew array<List<double>^>( steps ); for (int i = 0; i < steps; ++i) { agpTimeForThreshold[i] = gcnew List<double>(); agpFForThreshold[i] = gcnew List<double>(); tracTimeForThreshold[i] = gcnew List<double>(); tracFForThreshold[i] = gcnew List<double>(); } List<double>^ agpTimesAll = gcnew List<double>(); List<double>^ agpFAll = gcnew List<double>(); List<double>^ tracTimesAll = gcnew List<double>(); List<double>^ tracFAll = gcnew List<double>(); for (int dimIndex = 0; dimIndex < dimensionCount; ++dimIndex) { const int nSegments = dimensions[dimIndex]; List<BenchmarkCase^>^ tasks = goodTasksByDimension[dimIndex]; tbStats->AppendText( String::Format( L"\r\nBenchmark для размерности {0}...\r\n", nSegments ) ); Application::DoEvents(); for (int caseIndex = 0; caseIndex < goodTasksPerDimension; ++caseIndex) { BenchmarkCase^ task = tasks[caseIndex]; const int globalTaskIndex = dimIndex * goodTasksPerDimension + caseIndex; if (IsFinite(task->TracF) && IsFinite(task->TracMillis)) { tracFAll->Add( task->TracF ); tracTimesAll->Add( task->TracMillis ); } for (int thresholdIndex = 0; thresholdIndex < steps; ++thresholdIndex) { const int maxIter = thresholds[thresholdIndex]; if (IsFinite(task->TracF) && IsFinite(task->TracMillis)) { tracFForThreshold[thresholdIndex]->Add( task->TracF ); tracTimeForThreshold[thresholdIndex]->Add( task->TracMillis ); } for (int r = 0; r < repeatsStochastic; ++r) { const int seedIndex = globalTaskIndex * repeatsStochastic + r; if (seedIndex < 0 || seedIndex >= fixedSeeds->Length) { throw gcnew IndexOutOfRangeException( String::Format( L"Некорректный индекс seed: {0}, размер массива: {1}", seedIndex, fixedSeeds->Length ) ); } const unsigned int seed = fixedSeeds[seedIndex]; double bF; double bX; double bY; double achievedEps; double tMs; std::size_t iterations = 0; const bool ok = RunAgpIndexSingle( nSegments, task, maxIter, seed, bF, bX, bY, iterations, achievedEps, tMs ); if (ok) { agpFForThreshold[thresholdIndex]->Add( bF ); agpTimeForThreshold[thresholdIndex]->Add( tMs ); agpFAll->Add( bF ); agpTimesAll->Add( tMs ); } } Application::DoEvents(); } tbStats->AppendText( String::Format( L" Задача {0}/{1} завершена.\r\n", caseIndex + 1, goodTasksPerDimension ) ); Application::DoEvents(); } } for (int thresholdIndex = 0; thresholdIndex < steps; ++thresholdIndex) { if (agpTimeForThreshold[thresholdIndex]->Count > 0) { ComputeMeanStd( agpTimeForThreshold[thresholdIndex], agpTimeMean[thresholdIndex], agpTimeStd[thresholdIndex] ); ComputeMeanStd( agpFForThreshold[thresholdIndex], agpFMean[thresholdIndex], agpFStd[thresholdIndex] ); } if (tracTimeForThreshold[thresholdIndex]->Count > 0) { ComputeMeanStd( tracTimeForThreshold[thresholdIndex], tracTimeMean[thresholdIndex], tracTimeStd[thresholdIndex] ); ComputeMeanStd( tracFForThreshold[thresholdIndex], tracFMean[thresholdIndex], tracFStd[thresholdIndex] ); } UpdateCharts( thresholds, steps, thresholdIndex, agpTimeMean, agpFMean, tracTimeMean, tracFMean ); tbStats->AppendText( String::Format( L"Порог {0} обработан: AGP runs = {1}, TRAC-IK tasks = {2}\r\n", thresholds[thresholdIndex], agpTimeForThreshold[thresholdIndex]->Count, tracTimeForThreshold[thresholdIndex]->Count ) ); Application::DoEvents(); } double agpTimeMu; double agpTimeSigma; double agpFMu; double agpFSigma; double tracTimeMu; double tracTimeSigma; double tracFMu; double tracFSigma; ComputeMeanStd( agpTimesAll, agpTimeMu, agpTimeSigma ); ComputeMeanStd( agpFAll, agpFMu, agpFSigma ); ComputeMeanStd( tracTimesAll, tracTimeMu, tracTimeSigma ); ComputeMeanStd( tracFAll, tracFMu, tracFSigma ); StringBuilder^ sb = gcnew StringBuilder(); sb->AppendLine(); sb->AppendLine( L"==============================================" ); sb->AppendLine( L"СВОДНАЯ СТАТИСТИКА" ); sb->AppendLine( L"==============================================" ); sb->AppendLine( L"Размерности: 3..8" ); sb->AppendLine( L"Каждая размерность: 10 задач, успешно решённых TRAC-IK (f <= 0.05)" ); sb->AppendLine( L"Каждая задача: случайная цель + угол, без препятствий" ); sb->AppendFormat( L"Stochastic-повторы AGP: {0} разных seed на задачу\r\n", repeatsStochastic ); sb->AppendFormat( L"Порогов maxIter: {0}\r\n", steps ); sb->AppendLine( L"TRAC-IK запускается один раз для принятой задачи" ); sb->AppendLine(); sb->AppendLine( L"Общие результаты:" ); sb->AppendFormat( L" AGP runs: {0}\r\n", agpTimesAll->Count ); sb->AppendFormat( L" TRAC-IK runs: {0}\r\n", tracTimesAll->Count ); sb->AppendLine(); sb->AppendLine( L"Средние значения по всем фактическим запускам:" ); sb->AppendFormat( L" AGP: время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n", agpTimeMu, agpTimeSigma, agpFMu, agpFSigma ); sb->AppendFormat( L" TRAC-IK: время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n", tracTimeMu, tracTimeSigma, tracFMu, tracFSigma ); sb->AppendLine(); sb->AppendLine( L"Количество задач:" ); sb->AppendFormat( L" Всего задач: {0}\r\n", totalGoodTasks ); sb->AppendFormat( L" AGP запусков на задачу: {0}\r\n", steps * repeatsStochastic ); sb->AppendFormat( L" Ожидаемое число AGP запусков: {0}\r\n", totalGoodTasks * steps * repeatsStochastic ); sb->AppendLine(); sb->AppendLine( L"Параметры:" ); sb->AppendFormat( L" maxTheta = {0:G9}\r\n", maxTheta ); sb->AppendFormat( L" r = {0:G9}\r\n", rParam ); sb->AppendFormat( L" eps = {0:G9}\r\n", epsParam ); sb->AppendFormat( L" baseLength = {0:G9}\r\n", baseLength ); sb->AppendFormat( L" stretchFactor = {0:G9}\r\n", stretchFactor ); sb->AppendFormat( L" adaptiveAgp = {0}\r\n", adaptiveAgp ); sb->AppendFormat( L" TRAC-IK timeout = {0:F3} мс\r\n", tracMaxTime * 1000.0 ); tbStats->AppendText( sb->ToString() ); } void UpdateCharts( array<int>^ thresholds, int steps, int filledUpTo, array<double>^ agpTimeMean, array<double>^ agpFMean, array<double>^ tracTimeMean, array<double>^ tracFMean) { Series^ sAgpTime = chartTime->Series["AGP"]; Series^ sTracTime = chartTime->Series["TRAC IK"]; Series^ sAgpF = chartF->Series["AGP"]; Series^ sTracF = chartF->Series["TRAC IK"]; sAgpTime->Points->Clear(); sTracTime->Points->Clear(); sAgpF->Points->Clear(); sTracF->Points->Clear(); for (int i = 0; i <= filledUpTo && i < steps; ++i) { const double x = static_cast<double>( thresholds[i] ); if (!Double::IsNaN( agpTimeMean[i] ) && !Double::IsInfinity( agpTimeMean[i] )) { sAgpTime->Points->AddXY( x, agpTimeMean[i] ); } if (!Double::IsNaN( tracTimeMean[i] ) && !Double::IsInfinity( tracTimeMean[i] )) { sTracTime->Points->AddXY( x, tracTimeMean[i] ); } if (!Double::IsNaN( agpFMean[i] ) && !Double::IsInfinity( agpFMean[i] )) { sAgpF->Points->AddXY( x, agpFMean[i] ); } if (!Double::IsNaN( tracFMean[i] ) && !Double::IsInfinity( tracFMean[i] )) { sTracF->Points->AddXY( x, tracFMean[i] ); } } chartTime->Invalidate(); chartF->Invalidate(); this->Refresh(); } };
}"
Pensamiento
Respuesta
Да. Важные исправления здесь два: один и тот же TRAC_IK теперь переиспользуется между последовательными решениями при неизменной конфигурации, что соответствует официальному примеру, а время в обоих приложениях измеряется одной и той же границей — строго вокруг CartToJnt. Сам CartToJnt перед каждым вызовом сбрасывает внутренние решатели, очищает решения и запускает два рабочих потока, так что последовательное переиспользование экземпляра предусмотрено самой реализацией. (GitHub)
Также в benchmark я убрал использование времени, полученного на этапе отбора задач: теперь после отбора каждая принятая задача отдельно один раз запускается через TRAC-IK уже для benchmark-измерения, а это измерение используется в графике. timeout = 0.002, SolveType::Speed, начальное состояние, tolerances и ограничения суставов не изменены. Официальный benchmark TRAC-IK также измеряет внешнее elapsed-время вокруг одного CartToJnt, а не использует какой-либо внутренний таймер API. (GitHub)
Демо
cpp#pragma once #define WIN32_LEAN_AND_MEAN #include <Windows.h> #include <cmath> #include <vector> #include <memory> #include <kdl/chain.hpp> #include <kdl/jntarray.hpp> #include <kdl/frames.hpp> #include <kdl/joint.hpp> #include <kdl/segment.hpp> #include <trac_ik/trac_ik.hpp> using namespace System; using namespace System::Drawing; using namespace System::Windows::Forms; using namespace System::Collections::Generic; using namespace System::Drawing::Drawing2D; using namespace System::Runtime::CompilerServices; typedef void(__cdecl* P_MANIP)(int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, float**, float*, float*, float*, float*, size_t*, float*, int, const float*); typedef void(__cdecl* P_FREE)(float*); typedef void(__cdecl* P_START)(int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, int, const float*, const float*); typedef void(__cdecl* P_BUILD_TRAJECTORY)(int, bool, float, const float*, const float*, int, float, bool, float, unsigned int, float, float, const float*, int, float**, int*, size_t*); class TracIkRunner final { private: int nSegments_; unsigned int nJoints_; double baseLength_; double maxTheta_; double maxTime_; double eps_; KDL::Chain chain_; KDL::JntArray qMin_; KDL::JntArray qMax_; KDL::JntArray qInit_; KDL::JntArray qOut_; std::unique_ptr<trac_ik::TRAC_IK> solver_; LARGE_INTEGER frequency_; static double WrapPi(double a) { const double pi = 3.1415926535897932384626433832795; const double twoPi = 2.0 * pi; while (a > pi) a -= twoPi; while (a < -pi) a += twoPi; return a; } public: TracIkRunner( int nSegments, double baseLength, double maxTheta, double maxTime, double eps) : nSegments_(nSegments), nJoints_(0), baseLength_(baseLength), maxTheta_(maxTheta), maxTime_(maxTime), eps_(eps) { for (int i = 0; i < nSegments_; ++i) { chain_.addSegment( KDL::Segment( KDL::Joint(KDL::Joint::RotZ), KDL::Frame( KDL::Vector( baseLength_, 0.0, 0.0 ) ) ) ); } nJoints_ = chain_.getNrOfJoints(); qMin_.resize(nJoints_); qMax_.resize(nJoints_); qInit_.resize(nJoints_); qOut_.resize(nJoints_); for (unsigned int i = 0; i < nJoints_; ++i) { qMin_(i) = -maxTheta_; qMax_(i) = maxTheta_; qInit_(i) = 0.0; qOut_(i) = 0.0; } solver_ = std::make_unique<trac_ik::TRAC_IK>( chain_, qMin_, qMax_, maxTime_, eps_, trac_ik::SolveType::Speed ); QueryPerformanceFrequency(&frequency_); } TracIkRunner(const TracIkRunner&) = delete; TracIkRunner& operator=(const TracIkRunner&) = delete; bool Matches( int nSegments, double baseLength, double maxTheta, double maxTime, double eps) const { return nSegments_ == nSegments && baseLength_ == baseLength && maxTheta_ == maxTheta && maxTime_ == maxTime && eps_ == eps; } bool Solve( float targetX, float targetY, float targetAngle, double& outBestF, double& outBestX, double& outBestY, double& outBestA, double& outMillis) { for (unsigned int i = 0; i < nJoints_; ++i) { qInit_(i) = 0.0; qOut_(i) = 0.0; } KDL::Frame target( KDL::Rotation::RotZ( static_cast<double>(targetAngle) ), KDL::Vector( static_cast<double>(targetX), static_cast<double>(targetY), 0.0 ) ); KDL::Twist tolerances( KDL::Vector( eps_, eps_, 0.0 ), KDL::Vector( 0.0, 0.0, eps_ ) ); LARGE_INTEGER t0; LARGE_INTEGER t1; QueryPerformanceCounter(&t0); const int result = solver_->CartToJnt( qInit_, target, qOut_, tolerances ); QueryPerformanceCounter(&t1); outMillis = 1.0e3 * static_cast<double>( t1.QuadPart - t0.QuadPart ) / static_cast<double>( frequency_.QuadPart ); double x = 0.0; double y = 0.0; double phi = 0.0; for (unsigned int i = 0; i < nJoints_; ++i) { phi += qOut_(i); x += baseLength_ * std::cos(phi); y += baseLength_ * std::sin(phi); } const double dx = x - static_cast<double>(targetX); const double dy = y - static_cast<double>(targetY); const double da = WrapPi( phi - static_cast<double>(targetAngle) ); outBestF = std::sqrt( dx * dx + dy * dy + da * da ); outBestX = x; outBestY = y; outBestA = WrapPi(phi); return result >= 0; } const KDL::JntArray& GetSolution() const { return qOut_; } }; namespace TESTAGP { public enum class DemoMode { Positioning = 0, TrajectoryPlanning = 1 }; public enum class DragHandle { None = 0, Target = 1, Start = 2, AngleTarget = 3, AngleStart = 4 }; public ref class PoseSnapshot sealed { public: PoseSnapshot(int n) { Angles = gcnew array<float>(n); Lengths = gcnew array<float>(n); EndX = EndY = EndA = BestF = 0.0f; Iterations = 0; AchievedEps = Millis = 0.0f; } array<float>^ Angles; array<float>^ Lengths; float EndX, EndY, EndA, BestF; int Iterations; float AchievedEps, Millis; }; public ref class MyForm sealed : public Form { public: MyForm(HMODULE hLib) : hLib(hLib), tracIkRunner(nullptr) { this->SetStyle(ControlStyles::AllPaintingInWmPaint | ControlStyles::UserPaint | ControlStyles::OptimizedDoubleBuffer, true); this->Text = L"AGP Manipulator 2D"; this->ClientSize = System::Drawing::Size(1200, 800); this->Resize += gcnew EventHandler(this, &MyForm::OnResize); this->MouseDown += gcnew MouseEventHandler(this, &MyForm::OnMouseDownPoint); this->MouseMove += gcnew MouseEventHandler(this, &MyForm::OnMouseMovePoint); this->MouseUp += gcnew MouseEventHandler(this, &MyForm::OnMouseUpPoint); fManip = (P_MANIP)GetProcAddress(hLib, "AGP_Manip2D"); pFree = (P_FREE)GetProcAddress(hLib, "AGP_Free"); pStart = (P_START)GetProcAddress(hLib, "AgpStartManipND"); pBuildTrajectory = (P_BUILD_TRAJECTORY)GetProcAddress(hLib, "AGP_BuildTransitionTrajectory"); angles = gcnew List<float>(8); lengths = gcnew List<float>(8); obstacleX = gcnew List<float>(4); obstacleY = gcnew List<float>(4); obstacleHalf = gcnew List<float>(4); plannedPoses = gcnew List<PoseSnapshot^>(); animationFrames = gcnew List<PoseSnapshot^>(); trajectoryPathWorld = gcnew List<PointF>(); demoMode = DemoMode::Positioning; InitGraphicsResources(); InitAnimation(); InitUI(); ResetRandomConfig(); SetDefaultStartPoint(true); UpdateTrajectoryUiState(); } ~MyForm() { this->!MyForm(); } !MyForm() { if (tracIkRunner != nullptr) { delete tracIkRunner; tracIkRunner = nullptr; } } private: literal float ObstacleClearance = 0.05f; literal float PI = 3.14159265358979323846f; literal float TWO_PI = 6.28318530717958647692f; literal int AnimationFramesPerSegment = 10; literal int AnimationIntervalMs = 16; literal float TransitionLengthEnergyWeight = 0.35f; literal float TransitionPrefixEnergyWeight = 0.175f; ComboBox^ cbDemoMode; ComboBox^ cbBackend; CheckBox^ cbVarLen; CheckBox^ cbAdaptive; NumericUpDown^ nudMaxTheta; NumericUpDown^ nudBaseLength; NumericUpDown^ nudStretchFactor; NumericUpDown^ nudTargetX; NumericUpDown^ nudTargetY; NumericUpDown^ nudTargetAngle; NumericUpDown^ nudStartX; NumericUpDown^ nudStartY; NumericUpDown^ nudStartAngle; NumericUpDown^ nudMaxIter; NumericUpDown^ nudR; TextBox^ txtEps; float currentEps = 1e-9f; Button^ btnAdd; Button^ btnRem; Button^ btnOptimize; Button^ btnGenerateObstacles; Button^ btnClearObstacles; Label^ lblInfo; Label^ lblStartX; Label^ lblStartY; Label^ lblTargetAngle; Label^ lblStartAngle; P_BUILD_TRAJECTORY pBuildTrajectory; HMODULE hLib; P_MANIP fManip; P_FREE pFree; P_START pStart; TracIkRunner* tracIkRunner; Pen^ obstaclePen; Pen^ obstacleMarginPen; SolidBrush^ obstacleBrush; Pen^ wallPen; Pen^ dashedPen; Pen^ targetPen; Pen^ startPen; Pen^ pathPen; Pen^ penRod; SolidBrush^ jointBrush; SolidBrush^ waypointBrush; HatchBrush^ wallHatchBrush; Pen^ angleTargetPen; Pen^ angleStartPen; System::Drawing::Font^ uiFontBold11; System::Drawing::Font^ uiFontTextBox; System::Drawing::Font^ uiFontBold10; Timer^ animationTimer; DemoMode demoMode; int nSegments = 1; bool variableLengths = false; List<float>^ angles; List<float>^ lengths; List<float>^ obstacleX; List<float>^ obstacleY; List<float>^ obstacleHalf; List<PoseSnapshot^>^ plannedPoses; List<PoseSnapshot^>^ animationFrames; List<PointF>^ trajectoryPathWorld; UInt32 rngState = 0xA5C39E0Du; DragHandle activeDragHandle = DragHandle::None; bool updatingFromMouse = false; bool syncingDefaultStartPoint = false; bool startPointCustomized = false; int animationFrameIndex = 0; bool animationRunning = false; bool angleTargetDrag = false; bool angleStartDrag = false; static float WrapPi(float a) { while (a > PI) a -= TWO_PI; while (a < -PI) a += TWO_PI; return a; } static float WrappedDelta(float from, float to) { return WrapPi(to - from); } static float LerpWrappedAngle(float from, float to, float t) { return WrapPi(from + t * WrappedDelta(from, to)); } static float DistanceSquared(PointF a, PointF b) { float dx = a.X - b.X; float dy = a.Y - b.Y; return dx * dx + dy * dy; } [MethodImpl(MethodImplOptions::AggressiveInlining)] PointF GetBasePoint() { int drawAreaTop = 180; int drawAreaHeight = this->ClientSize.Height - 180; int leftWallX = this->ClientSize.Width * 25 / 100; return PointF((float)leftWallX, (float)(drawAreaTop + drawAreaHeight / 2)); } TracIkRunner* GetTracIkRunner() { const double maxTime = 0.002; const double baseLengthValue = static_cast<double>((float)nudBaseLength->Value); const double maxThetaValue = static_cast<double>((float)nudMaxTheta->Value); const double epsValue = static_cast<double>(currentEps); if (tracIkRunner == nullptr || !tracIkRunner->Matches( nSegments, baseLengthValue, maxThetaValue, maxTime, epsValue)) { if (tracIkRunner != nullptr) { delete tracIkRunner; tracIkRunner = nullptr; } tracIkRunner = new TracIkRunner( nSegments, baseLengthValue, maxThetaValue, maxTime, epsValue ); } return tracIkRunner; } void InitGraphicsResources() { uiFontBold11 = gcnew System::Drawing::Font("Yu Gothic UI", 11, FontStyle::Bold); uiFontBold10 = gcnew System::Drawing::Font("Yu Gothic UI", 10, FontStyle::Bold); uiFontTextBox = gcnew System::Drawing::Font("Yu Gothic UI", 11, FontStyle::Bold); wallPen = gcnew Pen(Color::Black, 2.0f); dashedPen = gcnew Pen(Color::Black, 2.0f); dashedPen->DashStyle = DashStyle::Dash; targetPen = gcnew Pen(Color::Green, 3.0f); targetPen->DashStyle = DashStyle::Dot; startPen = gcnew Pen(Color::FromArgb(255, 140, 0), 3.0f); startPen->DashStyle = DashStyle::Dot; pathPen = gcnew Pen(Color::FromArgb(40, 90, 180), 2.5f); pathPen->DashStyle = DashStyle::Dash; penRod = gcnew Pen(Color::Red, 6.0f); jointBrush = gcnew SolidBrush(Color::Blue); waypointBrush = gcnew SolidBrush(Color::FromArgb(40, 90, 180)); wallHatchBrush = gcnew HatchBrush(HatchStyle::BackwardDiagonal, Color::LightGray, Color::White); obstaclePen = gcnew Pen(Color::FromArgb(90, 30, 30), 2.0f); obstacleMarginPen = gcnew Pen(Color::FromArgb(215, 140, 0), 2.0f); obstacleMarginPen->DashStyle = DashStyle::Dash; obstacleBrush = gcnew SolidBrush(Color::FromArgb(180, 120, 120, 120)); angleTargetPen = gcnew Pen(Color::Red, 2.0f); angleTargetPen->DashStyle = DashStyle::Dash; angleStartPen = gcnew Pen(Color::DarkOrange, 2.0f); angleStartPen->DashStyle = DashStyle::Dash; } void InitAnimation() { animationTimer = gcnew Timer(); animationTimer->Interval = AnimationIntervalMs; animationTimer->Tick += gcnew EventHandler(this, &MyForm::OnAnimationTick); } void InitUI() { cbDemoMode = gcnew ComboBox(); cbDemoMode->Location = Point(920, 20); cbDemoMode->Width = 260; cbDemoMode->Height = 28; cbDemoMode->DropDownStyle = ComboBoxStyle::DropDownList; cbDemoMode->Font = uiFontBold11; cbDemoMode->BackColor = SystemColors::Info; cbDemoMode->FlatStyle = FlatStyle::Flat; cbDemoMode->Items->Add(L"Позиционирование"); cbDemoMode->Items->Add(L"Планирование траектории"); cbDemoMode->SelectedIndex = 0; cbDemoMode->SelectedIndexChanged += gcnew EventHandler(this, &MyForm::OnDemoModeChanged); this->Controls->Add(cbDemoMode); cbBackend = gcnew ComboBox(); cbBackend->Location = Point(920, 54); cbBackend->Width = 260; cbBackend->Height = 28; cbBackend->DropDownStyle = ComboBoxStyle::DropDownList; cbBackend->Font = uiFontBold11; cbBackend->BackColor = SystemColors::Info; cbBackend->FlatStyle = FlatStyle::Flat; cbBackend->Items->Add(L"AGP"); cbBackend->Items->Add(L"TRAC-IK"); cbBackend->SelectedIndex = 0; cbBackend->SelectedIndexChanged += gcnew EventHandler(this, &MyForm::OnBackendChanged); this->Controls->Add(cbBackend); Label^ L = gcnew Label(); L->Text = L"Макс. угол (рад.)"; L->Location = Point(20, 20); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudMaxTheta = gcnew NumericUpDown(); nudMaxTheta->Location = Point(20, 52); nudMaxTheta->Width = 200; nudMaxTheta->DecimalPlaces = 3; nudMaxTheta->Minimum = Decimal(0.01); nudMaxTheta->Maximum = Decimal(3.14159); nudMaxTheta->Value = Decimal(2.0); nudMaxTheta->Font = uiFontTextBox; nudMaxTheta->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudMaxTheta); L = gcnew Label(); L->Text = L"Базовая длина"; L->Location = Point(245, 20); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudBaseLength = gcnew NumericUpDown(); nudBaseLength->Location = Point(245, 52); nudBaseLength->Width = 200; nudBaseLength->DecimalPlaces = 2; nudBaseLength->Minimum = Decimal(0.5); nudBaseLength->Maximum = Decimal(2.0); nudBaseLength->Value = Decimal(1.0); nudBaseLength->Font = uiFontTextBox; nudBaseLength->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudBaseLength); L = gcnew Label(); L->Text = L"Макс. коэфф. растяжения/сжатия"; L->Location = Point(470, 20); L->Width = 300; L->Font = uiFontBold11; this->Controls->Add(L); nudStretchFactor = gcnew NumericUpDown(); nudStretchFactor->Location = Point(470, 52); nudStretchFactor->Width = 200; nudStretchFactor->DecimalPlaces = 2; nudStretchFactor->Minimum = Decimal(1.0); nudStretchFactor->Maximum = Decimal(1.5); nudStretchFactor->Increment = Decimal(0.01); nudStretchFactor->Value = Decimal(1.5); nudStretchFactor->Font = uiFontTextBox; nudStretchFactor->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudStretchFactor); cbVarLen = gcnew CheckBox(); cbVarLen->Text = L"Переменные длины"; cbVarLen->Location = Point(695, 52); cbVarLen->Width = 200; cbVarLen->Checked = false; cbVarLen->Font = uiFontBold11; cbVarLen->CheckedChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(cbVarLen); L = gcnew Label(); L->Text = L"Цель X"; L->Location = Point(20, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudTargetX = gcnew NumericUpDown(); nudTargetX->Location = Point(20, 139); nudTargetX->Width = 200; nudTargetX->DecimalPlaces = 2; nudTargetX->Minimum = Decimal(-10.0); nudTargetX->Maximum = Decimal(10.0); nudTargetX->Value = Decimal(2.5); nudTargetX->Font = uiFontTextBox; nudTargetX->ValueChanged += gcnew EventHandler(this, &MyForm::OnTargetChanged); this->Controls->Add(nudTargetX); L = gcnew Label(); L->Text = L"Цель Y"; L->Location = Point(245, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudTargetY = gcnew NumericUpDown(); nudTargetY->Location = Point(245, 139); nudTargetY->Width = 200; nudTargetY->DecimalPlaces = 2; nudTargetY->Minimum = Decimal(-10.0); nudTargetY->Maximum = Decimal(10.0); nudTargetY->Value = Decimal(-1.0); nudTargetY->Font = uiFontTextBox; nudTargetY->ValueChanged += gcnew EventHandler(this, &MyForm::OnTargetChanged); this->Controls->Add(nudTargetY); lblTargetAngle = gcnew Label(); lblTargetAngle->Text = L"Цель угол (рад.)"; lblTargetAngle->Location = Point(20, 194); lblTargetAngle->Width = 200; lblTargetAngle->Font = uiFontBold11; this->Controls->Add(lblTargetAngle); nudTargetAngle = gcnew NumericUpDown(); nudTargetAngle->Location = Point(20, 226); nudTargetAngle->Width = 200; nudTargetAngle->DecimalPlaces = 3; nudTargetAngle->Minimum = Decimal(-1000); nudTargetAngle->Maximum = Decimal(1000); nudTargetAngle->Increment = Decimal(0.1); nudTargetAngle->Value = Decimal(0); nudTargetAngle->Font = uiFontTextBox; nudTargetAngle->ValueChanged += gcnew EventHandler(this, &MyForm::OnAngleChanged); this->Controls->Add(nudTargetAngle); L = gcnew Label(); L->Text = L"Надежность (r)"; L->Location = Point(470, 107); L->Width = 200; L->Font = uiFontBold11; this->Controls->Add(L); nudR = gcnew NumericUpDown(); nudR->Location = Point(470, 139); nudR->Width = 200; nudR->DecimalPlaces = 2; nudR->Minimum = Decimal(1.0); nudR->Maximum = Decimal(20.0); nudR->Value = Decimal(1.05); nudR->Font = uiFontTextBox; nudR->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudR); L = gcnew Label(); L->Text = L"Точность"; L->Location = Point(695, 107); L->Width = 100; L->Font = uiFontBold11; this->Controls->Add(L); txtEps = gcnew TextBox(); txtEps->Location = Point(695, 139); txtEps->Width = 80; txtEps->Font = uiFontTextBox; txtEps->Text = L"1E-09"; txtEps->TextChanged += gcnew EventHandler(this, &MyForm::OnEpsTextChanged); this->Controls->Add(txtEps); Button^ btnEpsUp = gcnew Button(); btnEpsUp->Text = L"×10"; btnEpsUp->Location = Point(780, 139); btnEpsUp->Width = 32; btnEpsUp->Height = 26; btnEpsUp->Font = uiFontBold11; btnEpsUp->TextAlign = ContentAlignment::TopRight; btnEpsUp->Padding = System::Windows::Forms::Padding(0, 0, 2, 3); btnEpsUp->Click += gcnew EventHandler(this, &MyForm::OnEpsOrderUp); this->Controls->Add(btnEpsUp); Button^ btnEpsDown = gcnew Button(); btnEpsDown->Text = L"÷10"; btnEpsDown->Location = Point(816, 139); btnEpsDown->Width = 32; btnEpsDown->Height = 26; btnEpsDown->Font = uiFontBold11; btnEpsDown->TextAlign = ContentAlignment::TopRight; btnEpsDown->Padding = System::Windows::Forms::Padding(0, 0, 2, 3); btnEpsDown->Click += gcnew EventHandler(this, &MyForm::OnEpsOrderDown); this->Controls->Add(btnEpsDown); L = gcnew Label(); L->Text = L"Макс. итераций"; L->Location = Point(860, 107); L->Width = 130; L->Font = uiFontBold11; this->Controls->Add(L); nudMaxIter = gcnew NumericUpDown(); nudMaxIter->Location = Point(860, 139); nudMaxIter->Width = 120; nudMaxIter->Minimum = 10; nudMaxIter->Maximum = 500000; nudMaxIter->Value = 1000; nudMaxIter->Font = uiFontTextBox; nudMaxIter->Increment = 100; nudMaxIter->ValueChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(nudMaxIter); cbAdaptive = gcnew CheckBox(); cbAdaptive->Text = L"Адаптивная схема"; cbAdaptive->Location = Point(995, 139); cbAdaptive->Width = 150; cbAdaptive->Checked = true; cbAdaptive->Font = uiFontBold11; cbAdaptive->CheckedChanged += gcnew EventHandler(this, &MyForm::OnAnyChanged); this->Controls->Add(cbAdaptive); btnAdd = gcnew Button(); btnAdd->Text = L"+ Звено"; btnAdd->Location = Point(465, 211); btnAdd->Width = 90; btnAdd->Height = 35; btnAdd->BackColor = SystemColors::Info; btnAdd->Cursor = Cursors::Hand; btnAdd->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnAdd->FlatAppearance->BorderSize = 3; btnAdd->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnAdd->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnAdd->FlatStyle = FlatStyle::Flat; btnAdd->Font = uiFontBold11; btnAdd->ForeColor = SystemColors::ControlDarkDark; btnAdd->Click += gcnew EventHandler(this, &MyForm::OnAddClick); this->Controls->Add(btnAdd); btnRem = gcnew Button(); btnRem->Text = L"- Звено"; btnRem->Location = Point(560, 211); btnRem->Width = 90; btnRem->Height = 35; btnRem->BackColor = SystemColors::Info; btnRem->Cursor = Cursors::Hand; btnRem->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnRem->FlatAppearance->BorderSize = 3; btnRem->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnRem->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnRem->FlatStyle = FlatStyle::Flat; btnRem->Font = uiFontBold11; btnRem->ForeColor = SystemColors::ControlDarkDark; btnRem->Click += gcnew EventHandler(this, &MyForm::OnRemClick); this->Controls->Add(btnRem); btnOptimize = gcnew Button(); btnOptimize->Text = L"Оптимизировать"; btnOptimize->Location = Point(680, 211); btnOptimize->Width = 150; btnOptimize->Height = 35; btnOptimize->BackColor = SystemColors::Info; btnOptimize->Cursor = Cursors::Hand; btnOptimize->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnOptimize->FlatAppearance->BorderSize = 3; btnOptimize->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnOptimize->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnOptimize->FlatStyle = FlatStyle::Flat; btnOptimize->Font = uiFontBold11; btnOptimize->ForeColor = SystemColors::ControlDarkDark; btnOptimize->Click += gcnew EventHandler(this, &MyForm::OnOptimizeClick); this->Controls->Add(btnOptimize); btnGenerateObstacles = gcnew Button(); btnGenerateObstacles->Location = Point(465, 257); btnGenerateObstacles->Width = 365; btnGenerateObstacles->Height = 35; btnGenerateObstacles->BackColor = SystemColors::Info; btnGenerateObstacles->Cursor = Cursors::Hand; btnGenerateObstacles->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnGenerateObstacles->FlatAppearance->BorderSize = 3; btnGenerateObstacles->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnGenerateObstacles->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnGenerateObstacles->FlatStyle = FlatStyle::Flat; btnGenerateObstacles->Font = uiFontBold11; btnGenerateObstacles->ForeColor = SystemColors::ControlDarkDark; btnGenerateObstacles->Click += gcnew EventHandler(this, &MyForm::OnGenerateObstaclesClick); this->Controls->Add(btnGenerateObstacles); btnClearObstacles = gcnew Button(); btnClearObstacles->Location = Point(465, 304); btnClearObstacles->Width = 365; btnClearObstacles->Height = 35; btnClearObstacles->BackColor = SystemColors::Info; btnClearObstacles->Cursor = Cursors::Hand; btnClearObstacles->FlatAppearance->BorderColor = Color::FromArgb(64, 64, 64); btnClearObstacles->FlatAppearance->BorderSize = 3; btnClearObstacles->FlatAppearance->MouseDownBackColor = Color::FromArgb(128, 128, 255); btnClearObstacles->FlatAppearance->MouseOverBackColor = Color::FromArgb(192, 192, 255); btnClearObstacles->FlatStyle = FlatStyle::Flat; btnClearObstacles->Font = uiFontBold11; btnClearObstacles->ForeColor = SystemColors::ControlDarkDark; btnClearObstacles->Text = L"Очистить"; btnClearObstacles->Click += gcnew EventHandler(this, &MyForm::OnClearObstaclesClick); this->Controls->Add(btnClearObstacles); lblInfo = gcnew Label(); lblInfo->Location = Point(835, 194); lblInfo->Size = System::Drawing::Size(275, 145); lblInfo->BorderStyle = BorderStyle::FixedSingle; lblInfo->Font = uiFontBold10; this->Controls->Add(lblInfo); lblStartX = gcnew Label(); lblStartX->Text = L"Начало X"; lblStartX->Location = Point(20, 272); lblStartX->Width = 200; lblStartX->Font = uiFontBold11; this->Controls->Add(lblStartX); nudStartX = gcnew NumericUpDown(); nudStartX->Location = Point(20, 304); nudStartX->Width = 200; nudStartX->DecimalPlaces = 2; nudStartX->Minimum = Decimal(-10.0); nudStartX->Maximum = Decimal(10.0); nudStartX->Value = Decimal(1.25); nudStartX->Font = uiFontTextBox; nudStartX->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartPointChanged); this->Controls->Add(nudStartX); lblStartY = gcnew Label(); lblStartY->Text = L"Начало Y"; lblStartY->Location = Point(245, 272); lblStartY->Width = 200; lblStartY->Font = uiFontBold11; this->Controls->Add(lblStartY); nudStartY = gcnew NumericUpDown(); nudStartY->Location = Point(245, 304); nudStartY->Width = 200; nudStartY->DecimalPlaces = 2; nudStartY->Minimum = Decimal(-10.0); nudStartY->Maximum = Decimal(10.0); nudStartY->Value = Decimal(-0.5); nudStartY->Font = uiFontTextBox; nudStartY->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartPointChanged); this->Controls->Add(nudStartY); lblStartAngle = gcnew Label(); lblStartAngle->Text = L"Нач. угол (рад.)"; lblStartAngle->Location = Point(20, 340); lblStartAngle->Width = 200; lblStartAngle->Font = uiFontBold11; this->Controls->Add(lblStartAngle); nudStartAngle = gcnew NumericUpDown(); nudStartAngle->Location = Point(20, 372); nudStartAngle->Width = 200; nudStartAngle->DecimalPlaces = 3; nudStartAngle->Minimum = Decimal(-1000); nudStartAngle->Maximum = Decimal(1000); nudStartAngle->Increment = Decimal(0.1); nudStartAngle->Value = Decimal(0); nudStartAngle->Font = uiFontTextBox; nudStartAngle->ValueChanged += gcnew EventHandler(this, &MyForm::OnStartAngleChanged); this->Controls->Add(nudStartAngle); UpdateBackendUiState(); UpdateTrajectoryUiState(); } void UpdateBackendUiState() { bool tracIkSelected = (cbBackend->SelectedIndex == 1); bool obstaclesEnabled = !tracIkSelected; btnGenerateObstacles->Enabled = obstaclesEnabled; btnClearObstacles->Enabled = obstaclesEnabled; if (tracIkSelected) { btnGenerateObstacles->Text = L"Препятствия отключены"; btnGenerateObstacles->ForeColor = Color::Gold; btnGenerateObstacles->BackColor = SystemColors::Control; btnClearObstacles->BackColor = SystemColors::Control; if (obstacleX->Count > 0) { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); ClearTrajectoryCache(); this->Invalidate(); } } else { btnGenerateObstacles->Text = L"Сгенерировать препятствия"; btnGenerateObstacles->ForeColor = SystemColors::ControlDarkDark; btnGenerateObstacles->BackColor = SystemColors::Info; btnClearObstacles->BackColor = SystemColors::Info; } } void UpdateTrajectoryUiState() { bool trajectoryMode = IsTrajectoryMode(); lblStartX->Visible = trajectoryMode; lblStartY->Visible = trajectoryMode; nudStartX->Visible = trajectoryMode; nudStartY->Visible = trajectoryMode; lblStartX->Enabled = trajectoryMode; lblStartY->Enabled = trajectoryMode; nudStartX->Enabled = trajectoryMode; nudStartY->Enabled = trajectoryMode; if (trajectoryMode) { lblStartAngle->Location = Point(245, 194); lblStartAngle->Width = 200; nudStartAngle->Location = Point(245, 226); nudStartAngle->Width = 200; lblStartAngle->Visible = true; nudStartAngle->Visible = true; lblStartAngle->Enabled = true; nudStartAngle->Enabled = true; } else { lblStartAngle->Location = Point(20, 340); lblStartAngle->Width = 200; nudStartAngle->Location = Point(20, 372); nudStartAngle->Width = 200; lblStartAngle->Visible = false; nudStartAngle->Visible = false; lblStartAngle->Enabled = false; nudStartAngle->Enabled = false; } if (!trajectoryMode) { ClearTrajectoryCache(); nudStartAngle->Value = Decimal(0); } this->Invalidate(); } void ResetRandomConfig() { nSegments = 1; angles->Clear(); lengths->Clear(); obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); angles->Add(0.0f); lengths->Add((float)nudBaseLength->Value); variableLengths = false; ClearTrajectoryCache(); this->Invalidate(); } void ClearTrajectoryCache() { StopAnimation(); plannedPoses->Clear(); animationFrames->Clear(); trajectoryPathWorld->Clear(); } void StopAnimation() { if (animationTimer) animationTimer->Stop(); animationFrameIndex = 0; animationRunning = false; } void ClearObstacles() { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); ClearTrajectoryCache(); this->Invalidate(); } [MethodImpl(MethodImplOptions::AggressiveInlining)] float Rand01() { rngState ^= rngState << 13; rngState ^= rngState >> 17; rngState ^= rngState << 5; return (float)(rngState) * 2.3283064e-10f; } [MethodImpl(MethodImplOptions::AggressiveInlining)] float Lerp(float a, float b, float t) { return a + (b - a) * t; } [MethodImpl(MethodImplOptions::AggressiveInlining)] bool IsTrajectoryMode() { return demoMode == DemoMode::TrajectoryPlanning; } void SetDefaultStartPoint(bool forceResetCustomization) { if (forceResetCustomization) startPointCustomized = false; if (startPointCustomized) return; syncingDefaultStartPoint = true; nudStartX->Value = Decimal((double)((float)nudTargetX->Value * 0.5f)); nudStartY->Value = Decimal((double)((float)nudTargetY->Value * 0.5f)); syncingDefaultStartPoint = false; } array<float>^ BuildObstacleBuffer() { int count = obstacleX->Count; array<float>^ data = gcnew array<float>(count * 3); for (int i = 0; i < count; ++i) { data[3 * i] = obstacleX[i]; data[3 * i + 1] = obstacleY[i]; data[3 * i + 2] = obstacleHalf[i]; } return data; } void GenerateRandomObstacles() { obstacleX->Clear(); obstacleY->Clear(); obstacleHalf->Clear(); bool varLen = cbVarLen->Checked; float baseLength = (float)nudBaseLength->Value; float stretchFactor = (float)nudStretchFactor->Value; float tx = (float)nudTargetX->Value; float ty = (float)nudTargetY->Value; float dist = (float)Math::Sqrt(tx * tx + ty * ty); if (dist <= 1e-6f) { ClearTrajectoryCache(); this->Invalidate(); return; } float maxReach = nSegments * (varLen ? baseLength * stretchFactor : baseLength); float slack = maxReach - dist; float halfMin = Math::Max(0.14f * baseLength, 0.12f); float halfMax = Math::Min(0.26f * baseLength + 0.05f * (slack / (baseLength + 1e-6f)), 0.32f); if (halfMax < halfMin + 0.03f) halfMax = halfMin + 0.03f; float alongMargin = Math::Max(0.55f * baseLength, 1.5f * (halfMax + ObstacleClearance)); alongMargin = Math::Max(alongMargin, 0.35f); float usableLen = dist - 2.0f * alongMargin; int obstacleCountToCreate = 2 + (int)(Rand01() * 3.0f); if (nSegments == 2 && obstacleCountToCreate > 3) obstacleCountToCreate = 3; if (slack < 0.35f * baseLength && obstacleCountToCreate > 2) obstacleCountToCreate = 2; while (obstacleCountToCreate > 2) { float gapTest = usableLen / (obstacleCountToCreate + 1); if (gapTest >= 2.2f * halfMax) break; --obstacleCountToCreate; } float ux = tx / dist; float uy = ty / dist; float nx = -uy; float ny = ux; float gap = usableLen / (obstacleCountToCreate + 1); float firstSide = (Rand01() < 0.5f) ? -1.0f : 1.0f; for (int i = 0; i < obstacleCountToCreate; ++i) { float nominalS = alongMargin + gap * (i + 1); float jitter = (Rand01() - 0.5f) * gap * 0.18f; float s = nominalS + jitter; float half = halfMin + (halfMax - halfMin) * Rand01(); float side = ((i & 1) == 0) ? firstSide : -firstSide; float offsetMag = half * (0.18f + 0.28f * Rand01()); obstacleX->Add(ux * s + nx * side * offsetMag); obstacleY->Add(uy * s + ny * side * offsetMag); obstacleHalf->Add(half); } ClearTrajectoryCache(); this->Invalidate(); } void UpdateEpsDisplay() { String^ value = currentEps.ToString("E3", System::Globalization::CultureInfo::InvariantCulture); if (txtEps->Text != value) txtEps->Text = value; } void OnEpsTextChanged(Object^ sender, EventArgs^ e) { float val; if (float::TryParse(txtEps->Text, System::Globalization::NumberStyles::Float, System::Globalization::CultureInfo::InvariantCulture, val)) { if (val < 1e-9f) val = 1e-9f; if (val > 1e-1f) val = 1e-1f; currentEps = val; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } else { UpdateEpsDisplay(); } } void OnEpsOrderUp(Object^ sender, EventArgs^ e) { float newVal = currentEps * 10.0f; if (newVal > 1e-1f) newVal = 1e-1f; currentEps = newVal; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } void OnEpsOrderDown(Object^ sender, EventArgs^ e) { float newVal = currentEps / 10.0f; if (newVal < 1e-9f) newVal = 1e-9f; currentEps = newVal; UpdateEpsDisplay(); OnAnyChanged(nullptr, nullptr); } void OnAngleChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; float val = WrapPi((float)nudTargetAngle->Value); Decimal wrapped = Decimal((double)val); if (nudTargetAngle->Value != wrapped) nudTargetAngle->Value = wrapped; ClearTrajectoryCache(); this->Invalidate(); } void OnStartAngleChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; float val = WrapPi((float)nudStartAngle->Value); Decimal wrapped = Decimal((double)val); if (nudStartAngle->Value != wrapped) nudStartAngle->Value = wrapped; ClearTrajectoryCache(); this->Invalidate(); } void OnClearObstaclesClick(Object^ sender, EventArgs^ e) { ClearObstacles(); } void OnGenerateObstaclesClick(Object^ sender, EventArgs^ e) { GenerateRandomObstacles(); } void ApplyPoseToManipulator(PoseSnapshot^ pose) { angles->Clear(); lengths->Clear(); for (int i = 0; i < nSegments; ++i) { angles->Add(pose->Angles[i]); lengths->Add(pose->Lengths[i]); } } void ComputeEndEffector(array<float>^ poseAngles, array<float>^ poseLengths, float% outX, float% outY, float% outA) { float x = 0; float y = 0; float phi = 0; int count = Math::Min(poseAngles->Length, poseLengths->Length); for (int i = 0; i < count; ++i) { phi += poseAngles[i]; x += poseLengths[i] * (float)Math::Cos(phi); y += poseLengths[i] * (float)Math::Sin(phi); } outX = x; outY = y; outA = WrapPi(phi); } PoseSnapshot^ RunAgpCppAtPoint(float tx, float ty, float ta) { variableLengths = cbVarLen->Checked; float maxTheta = (float)nudMaxTheta->Value; int maxIter = (int)nudMaxIter->Value; bool adaptive = cbAdaptive->Checked; float r_param = (float)nudR->Value; float eps = currentEps; unsigned int seed = (unsigned int)GetTickCount(); float baseLength = (float)nudBaseLength->Value; float stretchFactor = (float)nudStretchFactor->Value; array<float>^ obstacleData = BuildObstacleBuffer(); pin_ptr<float> pinnedObstacles = nullptr; const float* pObstacleData = nullptr; if (obstacleData->Length > 0) { pinnedObstacles = &obstacleData[0]; pObstacleData = pinnedObstacles; } int obstacleCount = obstacleX->Count; pStart( nSegments, variableLengths, maxTheta, tx, ty, ta, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObstacleData, obstacleCount, 0, nullptr, nullptr ); float* bestQ; float bestX; float bestY; float bestA; float bestF; size_t actualIterations; float achievedEps; LARGE_INTEGER t0; LARGE_INTEGER t1; LARGE_INTEGER fq; QueryPerformanceFrequency(&fq); QueryPerformanceCounter(&t0); fManip( nSegments, variableLengths, maxTheta, tx, ty, ta, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObstacleData, obstacleCount, &bestQ, &bestX, &bestY, &bestA, &bestF, &actualIterations, &achievedEps, 0, nullptr ); QueryPerformanceCounter(&t1); PoseSnapshot^ pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) pose->Angles[i] = bestQ[i]; if (variableLengths) { for (int i = 0; i < nSegments; ++i) pose->Lengths[i] = bestQ[nSegments + i]; } else { for (int i = 0; i < nSegments; ++i) pose->Lengths[i] = baseLength; } pFree(bestQ); pose->EndX = bestX; pose->EndY = bestY; pose->EndA = WrapPi(bestA); pose->BestF = bestF; pose->Iterations = (int)actualIterations; pose->AchievedEps = achievedEps; pose->Millis = (float)( 1.0e3 * (double)(t1.QuadPart - t0.QuadPart) / (double)fq.QuadPart ); return pose; } PoseSnapshot^ RunTracIkPositioning(float tx, float ty, float ta) { float baseLength = (float)nudBaseLength->Value; TracIkRunner* runner = GetTracIkRunner(); double bestF = 0.0; double bestX = 0.0; double bestY = 0.0; double bestA = 0.0; double millis = 0.0; runner->Solve( tx, ty, ta, bestF, bestX, bestY, bestA, millis ); const KDL::JntArray& qOut = runner->GetSolution(); PoseSnapshot^ pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) { pose->Angles[i] = (float)qOut(i); pose->Lengths[i] = baseLength; } pose->EndX = (float)bestX; pose->EndY = (float)bestY; pose->EndA = (float)bestA; pose->BestF = (float)bestF; pose->Iterations = -1; pose->AchievedEps = -1.0f; pose->Millis = (float)millis; return pose; } void UpdatePositioningStats(PoseSnapshot^ pose, float tx, float ty, float ta, bool isTracIk) { float dx = pose->EndX - tx; float dy = pose->EndY - ty; float distance = sqrtf(dx * dx + dy * dy); String^ iterationsStr = isTracIk ? "—" : pose->Iterations.ToString(); String^ epsStr = isTracIk ? "—" : pose->AchievedEps.ToString("E3"); lblInfo->Text = String::Format( L"Функционал: {0:F6}\nБлизость захвата: {1:F5}\nДостигнутая точка: ({2:F3}, {3:F3})\nДостигнутый угол: {4:F3} рад.\nВремя: {5:F3} мс\nЧисло шагов: {6}\nДостигнутая точность: {7}", pose->BestF, distance, pose->EndX, pose->EndY, pose->EndA, pose->Millis, iterationsStr, epsStr ); } void UpdateTrajectoryStats(List<PoseSnapshot^>^ poses, float totalMillis, size_t totalIterations, float finalAchievedEps, bool isTracIk) { if (poses->Count == 0) { lblInfo->Text = L"Траектория не построена"; return; } float totalEnergy = 0; for (int i = 1; i < poses->Count; ++i) totalEnergy += ComputeTransitionEnergy(poses[i - 1], poses[i]); PoseSnapshot^ lastPose = poses[poses->Count - 1]; float rawTargetX = (float)nudTargetX->Value; float rawTargetY = (float)nudTargetY->Value; float finalDx = lastPose->EndX - rawTargetX; float finalDy = lastPose->EndY - rawTargetY; float finalDistance = sqrtf(finalDx * finalDx + finalDy * finalDy); int actualIntermediateCount = Math::Max(0, poses->Count - 2); String^ iterationsStr = isTracIk ? "—" : totalIterations.ToString(); String^ epsStr = isTracIk ? "—" : finalAchievedEps.ToString("E3"); lblInfo->Text = String::Format( L"Промежуточных точек: {0}\nФункционал траектории: {1:F6}\nБлизость финиша: {2:F5}\nДостигнутый угол: {3:F3} рад.\nВремя: {4:F3} мс\nЧисло шагов: {5}\nДостигнутая точность: {6}", actualIntermediateCount, totalEnergy, finalDistance, lastPose->EndA, totalMillis, iterationsStr, epsStr ); } float ComputeTransitionEnergy(PoseSnapshot^ prevPose, PoseSnapshot^ nextPose) { float total = 0; float prevPrefix = 0; float nextPrefix = 0; for (int i = 0; i < prevPose->Angles->Length; ++i) { float d = WrappedDelta(prevPose->Angles[i], nextPose->Angles[i]); total += d * d; prevPrefix += prevPose->Angles[i]; nextPrefix += nextPose->Angles[i]; float dp = WrappedDelta(prevPrefix, nextPrefix); total += TransitionPrefixEnergyWeight * dp * dp; } if (variableLengths) { for (int i = 0; i < prevPose->Lengths->Length; ++i) { float dl = nextPose->Lengths[i] - prevPose->Lengths[i]; total += TransitionLengthEnergyWeight * dl * dl; } } return total; } void RunTrajectoryPlanningMode() { if (cbBackend->SelectedIndex == 1) RunTrajectoryWithTracIk(); else RunTrajectoryWithAGP(); } bool BuildPoseFromState(const float* state, bool varLen, float baseLength, PoseSnapshot^% pose) { pose = gcnew PoseSnapshot(nSegments); for (int i = 0; i < nSegments; ++i) { pose->Angles[i] = state[i]; pose->Lengths[i] = varLen ? state[nSegments + i] : baseLength; } float x; float y; float a; ComputeEndEffector(pose->Angles, pose->Lengths, x, y, a); pose->EndX = x; pose->EndY = y; pose->EndA = a; return true; } void RunTrajectoryWithAGP() { ClearTrajectoryCache(); array<float>^ obsData = BuildObstacleBuffer(); pin_ptr<float> pinnedObs = nullptr; const float* pObs = nullptr; if (obsData->Length > 0) { pinnedObs = &obsData[0]; pObs = pinnedObs; } int obstacleCount = obstacleX->Count; float startX = (float)nudStartX->Value; float startY = (float)nudStartY->Value; float startA = (float)nudStartAngle->Value; float targetX = (float)nudTargetX->Value; float targetY = (float)nudTargetY->Value; float targetA = (float)nudTargetAngle->Value; float maxTheta = (float)nudMaxTheta->Value; float baseLength = (float)nudBaseLength->Value; float stretchFactor = (float)nudStretchFactor->Value; float r_param = (float)nudR->Value; float eps = currentEps; int maxIter = (int)nudMaxIter->Value; bool varLen = cbVarLen->Checked; bool adaptive = cbAdaptive->Checked; unsigned int seed = (unsigned int)GetTickCount(); int stateDim = nSegments << 1; pStart( nSegments, varLen, maxTheta, startX, startY, startA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 0, nullptr, nullptr ); LARGE_INTEGER totalT0; LARGE_INTEGER totalT1; LARGE_INTEGER fq; QueryPerformanceFrequency(&fq); QueryPerformanceCounter(&totalT0); float* startQ; float startXOut; float startYOut; float startAOut; float startF; size_t startIterations; float startEps; fManip( nSegments, varLen, maxTheta, startX, startY, startA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &startQ, &startXOut, &startYOut, &startAOut, &startF, &startIterations, &startEps, 0, nullptr ); PoseSnapshot^ startPose; BuildPoseFromState(startQ, varLen, baseLength, startPose); startPose->EndX = startXOut; startPose->EndY = startYOut; startPose->EndA = WrapPi(startAOut); startPose->BestF = startF; startPose->Iterations = (int)startIterations; startPose->AchievedEps = startEps; std::vector<float> startState(stateDim); for (int i = 0; i < nSegments; ++i) { startState[i] = startPose->Angles[i]; startState[nSegments + i] = startPose->Lengths[i]; } pFree(startQ); pStart( nSegments, varLen, maxTheta, targetX, targetY, targetA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 1, startState.data(), nullptr ); float* finalQ; float finalXOut; float finalYOut; float finalAOut; float finalF; size_t finalIterations; float finalEps; fManip( nSegments, varLen, maxTheta, targetX, targetY, targetA, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &finalQ, &finalXOut, &finalYOut, &finalAOut, &finalF, &finalIterations, &finalEps, 1, startState.data() ); PoseSnapshot^ finalPose; BuildPoseFromState(finalQ, varLen, baseLength, finalPose); finalPose->EndX = finalXOut; finalPose->EndY = finalYOut; finalPose->EndA = WrapPi(finalAOut); finalPose->BestF = finalF; finalPose->Iterations = (int)finalIterations; finalPose->AchievedEps = finalEps; std::vector<float> finalState(stateDim); for (int i = 0; i < nSegments; ++i) { finalState[i] = finalPose->Angles[i]; finalState[nSegments + i] = finalPose->Lengths[i]; } pFree(finalQ); pStart( nSegments, varLen, maxTheta, 0, 0, 0, maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, 2, startState.data(), finalState.data() ); float* trajPoints; int pointCount; size_t trajectoryIterations; pBuildTrajectory( nSegments, varLen, maxTheta, startState.data(), finalState.data(), maxIter, r_param, adaptive, eps, seed, baseLength, stretchFactor, pObs, obstacleCount, &trajPoints, &pointCount, &trajectoryIterations ); QueryPerformanceCounter(&totalT1); float millis = (float)( 1.0e3 * (double)(totalT1.QuadPart - totalT0.QuadPart) / (double)fq.QuadPart ); size_t totalIterations = trajectoryIterations + startIterations + finalIterations; plannedPoses->Clear(); for (int i = 0; i < pointCount; ++i) { const float* ptr = trajPoints + i * stateDim; PoseSnapshot^ pose; BuildPoseFromState(ptr, varLen, baseLength, pose); plannedPoses->Add(pose); } pFree(trajPoints); plannedPoses[0]->EndX = startPose->EndX; plannedPoses[0]->EndY = startPose->EndY; plannedPoses[0]->EndA = startPose->EndA; plannedPoses[plannedPoses->Count - 1]->EndX = finalPose->EndX; plannedPoses[plannedPoses->Count - 1]->EndY = finalPose->EndY; plannedPoses[plannedPoses->Count - 1]->EndA = finalPose->EndA; UpdateTrajectoryStats( plannedPoses, millis, totalIterations, finalEps, false ); BuildAnimationFramesFromPlan(); ApplyPoseToManipulator(plannedPoses[0]); StartAnimationIfNeeded(); this->Invalidate(); } void RunTrajectoryWithTracIk() { ClearTrajectoryCache(); float startX = (float)nudStartX->Value; float startY = (float)nudStartY->Value; float startA = (float)nudStartAngle->Value; float targetX = (float)nudTargetX->Value; float targetY = (float)nudTargetY->Value; float targetA = (float)nudTargetAngle->Value; PoseSnapshot^ startPose = RunTracIkPositioning( startX, startY, startA ); PoseSnapshot^ endPose = RunTracIkPositioning( targetX, targetY, targetA ); float totalMillis = startPose->Millis + endPose->Millis; plannedPoses->Clear(); plannedPoses->Add(startPose); plannedPoses->Add(endPose); UpdateTrajectoryStats( plannedPoses, totalMillis, 0, -1.0f, true ); BuildAnimationFramesFromPlan(); ApplyPoseToManipulator(plannedPoses[0]); StartAnimationIfNeeded(); this->Invalidate(); } void RunPositioningMode() { StopAnimation(); float tx = (float)nudTargetX->Value; float ty = (float)nudTargetY->Value; float ta = (float)nudTargetAngle->Value; bool isTracIk = (cbBackend->SelectedIndex == 1); PoseSnapshot^ pose = isTracIk ? RunTracIkPositioning(tx, ty, ta) : RunAgpCppAtPoint(tx, ty, ta); ClearTrajectoryCache(); ApplyPoseToManipulator(pose); UpdatePositioningStats(pose, tx, ty, ta, isTracIk); this->Invalidate(); this->Refresh(); } void DrawTrajectorySegmentClipped(Graphics^ g, PointF aWorld, PointF bWorld) { float prevX = aWorld.X; float prevY = aWorld.Y; bool prevAllowed = IsDisplayPointAllowed(prevX, prevY); for (int s = 1; s <= 20; ++s) { float t = s / 20.0f; float currX = Lerp(aWorld.X, bWorld.X, t); float currY = Lerp(aWorld.Y, bWorld.Y, t); bool currAllowed = IsDisplayPointAllowed(currX, currY); if (prevAllowed && currAllowed) { PointF pa = WorldToPixel(prevX, prevY); PointF pb = WorldToPixel(currX, currY); g->DrawLine(pathPen, pa, pb); } prevX = currX; prevY = currY; prevAllowed = currAllowed; } } void DrawTrajectoryPath(Graphics^ g) { if (trajectoryPathWorld->Count < 2) return; for (int i = 1; i < trajectoryPathWorld->Count; ++i) DrawTrajectorySegmentClipped(g, trajectoryPathWorld[i - 1], trajectoryPathWorld[i]); if (plannedPoses->Count > 2) { for (int i = 1; i < plannedPoses->Count - 1; ++i) { if (!IsDisplayPointAllowed(plannedPoses[i]->EndX, plannedPoses[i]->EndY)) continue; PointF wp = WorldToPixel(plannedPoses[i]->EndX, plannedPoses[i]->EndY); g->FillEllipse(waypointBrush, wp.X - 4.0f, wp.Y - 4.0f, 8.0f, 8.0f); } } } void RebuildTrajectoryDisplayPathFromPlan() { trajectoryPathWorld->Clear(); if (animationFrames->Count > 0) { for (int i = 0; i < animationFrames->Count; ++i) { PointF p(animationFrames[i]->EndX, animationFrames[i]->EndY); if (trajectoryPathWorld->Count == 0 || DistanceSquared(trajectoryPathWorld[trajectoryPathWorld->Count - 1], p) > 1e-10f) { trajectoryPathWorld->Add(p); } } return; } for (int i = 0; i < plannedPoses->Count; ++i) { PointF p(plannedPoses[i]->EndX, plannedPoses[i]->EndY); if (trajectoryPathWorld->Count == 0 || DistanceSquared(trajectoryPathWorld[trajectoryPathWorld->Count - 1], p) > 1e-10f) { trajectoryPathWorld->Add(p); } } } void BuildAnimationFramesFromPlan() { animationFrames->Clear(); if (plannedPoses->Count == 0) { RebuildTrajectoryDisplayPathFromPlan(); return; } animationFrames->Add(plannedPoses[0]); for (int i = 0; i < plannedPoses->Count - 1; ++i) { PoseSnapshot^ firstPose = plannedPoses[i]; PoseSnapshot^ secondPose = plannedPoses[i + 1]; for (int frame = 1; frame <= AnimationFramesPerSegment; ++frame) { float t = frame / (float)AnimationFramesPerSegment; PoseSnapshot^ interp = gcnew PoseSnapshot(nSegments); for (int j = 0; j < nSegments; ++j) { interp->Angles[j] = LerpWrappedAngle( firstPose->Angles[j], secondPose->Angles[j], t ); interp->Lengths[j] = variableLengths ? firstPose->Lengths[j] + t * (secondPose->Lengths[j] - firstPose->Lengths[j]) : (float)nudBaseLength->Value; } float x; float y; float angle; ComputeEndEffector( interp->Angles, interp->Lengths, x, y, angle ); interp->EndX = x; interp->EndY = y; interp->EndA = angle; animationFrames->Add(interp); } } RebuildTrajectoryDisplayPathFromPlan(); } void StartAnimationIfNeeded() { if (animationFrames->Count <= 1) { animationRunning = false; this->Invalidate(); return; } animationFrameIndex = 0; animationRunning = true; animationTimer->Start(); } void OnAnimationTick(Object^ sender, EventArgs^ e) { if (!animationRunning || animationFrames->Count == 0) { StopAnimation(); return; } if (animationFrameIndex >= animationFrames->Count) { StopAnimation(); return; } ApplyPoseToManipulator(animationFrames[animationFrameIndex]); ++animationFrameIndex; this->Invalidate(); if (animationFrameIndex >= animationFrames->Count) StopAnimation(); } PointF WorldToPixel(float wx, float wy) { PointF basePoint = GetBasePoint(); return PointF(basePoint.X + wx * 160.0f, basePoint.Y - wy * 160.0f); } float GetCurrentMaxReach() { float baseLength = (float)nudBaseLength->Value; float stretchFactor = (float)nudStretchFactor->Value; return nSegments * (cbVarLen->Checked ? baseLength * stretchFactor : baseLength); } bool IsDisplayPointAllowed(float x, float y) { float maxReach = GetCurrentMaxReach(); if (x * x + y * y > (maxReach + 0.05f) * (maxReach + 0.05f)) { return false; } for (int i = 0; i < obstacleX->Count; ++i) { float half = obstacleHalf[i] + ObstacleClearance; if (x >= obstacleX[i] - half && x <= obstacleX[i] + half && y >= obstacleY[i] - half && y <= obstacleY[i] + half) { return false; } } return true; } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseDownPoint(Object^ sender, MouseEventArgs^ e) { PointF basePoint = GetBasePoint(); float baseX = basePoint.X; float baseY = basePoint.Y; float targetX = (float)nudTargetX->Value; float targetY = (float)nudTargetY->Value; float targetA = (float)nudTargetAngle->Value; float pixelTargetX = baseX + targetX * 160.0f; float pixelTargetY = baseY - targetY * 160.0f; float angleLineLen = 25.0f; float endX = pixelTargetX + angleLineLen * (float)Math::Cos(targetA); float endY = pixelTargetY - angleLineLen * (float)Math::Sin(targetA); float dx = e->X - endX; float dy = e->Y - endY; if (dx * dx + dy * dy < 225.0f) { StopAnimation(); activeDragHandle = DragHandle::AngleTarget; angleTargetDrag = true; updatingFromMouse = true; this->Capture = true; return; } float bestDistSquared = 100.0f; activeDragHandle = DragHandle::None; if (IsTrajectoryMode()) { float startX = (float)nudStartX->Value; float startY = (float)nudStartY->Value; float startA = (float)nudStartAngle->Value; float pixelStartX = baseX + startX * 160.0f; float pixelStartY = baseY - startY * 160.0f; float sEndX = pixelStartX + angleLineLen * (float)Math::Cos(startA); float sEndY = pixelStartY - angleLineLen * (float)Math::Sin(startA); dx = e->X - sEndX; dy = e->Y - sEndY; if (dx * dx + dy * dy < 225.0f) { StopAnimation(); activeDragHandle = DragHandle::AngleStart; angleStartDrag = true; updatingFromMouse = true; this->Capture = true; return; } float dsx = e->X - pixelStartX; float dsy = e->Y - pixelStartY; float startDistSquared = dsx * dsx + dsy * dsy; if (startDistSquared < bestDistSquared) { bestDistSquared = startDistSquared; activeDragHandle = DragHandle::Start; } } float dtx = e->X - pixelTargetX; float dty = e->Y - pixelTargetY; float targetDistSquared = dtx * dtx + dty * dty; if (targetDistSquared < bestDistSquared) activeDragHandle = DragHandle::Target; if (activeDragHandle != DragHandle::None) { StopAnimation(); updatingFromMouse = true; this->Capture = true; } } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseMovePoint(Object^ sender, MouseEventArgs^ e) { if (activeDragHandle == DragHandle::None && !angleTargetDrag && !angleStartDrag) { return; } PointF basePoint = GetBasePoint(); float baseX = basePoint.X; float baseY = basePoint.Y; if (angleTargetDrag) { float targetX = (float)nudTargetX->Value; float targetY = (float)nudTargetY->Value; float pixelTargetX = baseX + targetX * 160.0f; float pixelTargetY = baseY - targetY * 160.0f; float dx = e->X - pixelTargetX; float dy = e->Y - pixelTargetY; float angle = WrapPi(atan2f(-dy, dx)); nudTargetAngle->Value = Decimal((double)angle); ClearTrajectoryCache(); this->Invalidate(); return; } if (angleStartDrag && IsTrajectoryMode()) { float startX = (float)nudStartX->Value; float startY = (float)nudStartY->Value; float pixelStartX = baseX + startX * 160.0f; float pixelStartY = baseY - startY * 160.0f; float dx = e->X - pixelStartX; float dy = e->Y - pixelStartY; float angle = WrapPi(atan2f(-dy, dx)); nudStartAngle->Value = Decimal((double)angle); ClearTrajectoryCache(); this->Invalidate(); return; } if (activeDragHandle == DragHandle::None) return; float minX = (float)nudTargetX->Minimum; float maxX = (float)nudTargetX->Maximum; float minY = (float)nudTargetY->Minimum; float maxY = (float)nudTargetY->Maximum; float newX = (e->X - baseX) / 160.0f; float newY = (baseY - e->Y) / 160.0f; newX = Math::Max(minX, Math::Min(maxX, newX)); newY = Math::Max(minY, Math::Min(maxY, newY)); if (activeDragHandle == DragHandle::Target) { nudTargetX->Value = Decimal((double)newX); nudTargetY->Value = Decimal((double)newY); } else if (activeDragHandle == DragHandle::Start) { startPointCustomized = true; nudStartX->Value = Decimal((double)newX); nudStartY->Value = Decimal((double)newY); } ClearTrajectoryCache(); this->Invalidate(); } [MethodImpl(MethodImplOptions::AggressiveInlining)] void OnMouseUpPoint(Object^ sender, MouseEventArgs^ e) { if (activeDragHandle != DragHandle::None || angleTargetDrag || angleStartDrag) { activeDragHandle = DragHandle::None; angleTargetDrag = false; angleStartDrag = false; updatingFromMouse = false; this->Capture = false; } } void OnBackendChanged(Object^ sender, EventArgs^ e) { UpdateBackendUiState(); ClearTrajectoryCache(); this->Invalidate(); } void OnDemoModeChanged(Object^ sender, EventArgs^ e) { demoMode = (cbDemoMode->SelectedIndex == 0) ? DemoMode::Positioning : DemoMode::TrajectoryPlanning; if (demoMode == DemoMode::TrajectoryPlanning && !startPointCustomized) { SetDefaultStartPoint(false); } UpdateTrajectoryUiState(); } void OnResize(Object^ sender, EventArgs^ e) { this->Invalidate(); } void OnAnyChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; ClearTrajectoryCache(); this->Invalidate(); } void OnTargetChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; if (!startPointCustomized) SetDefaultStartPoint(false); ClearTrajectoryCache(); this->Invalidate(); } void OnStartPointChanged(Object^ sender, EventArgs^ e) { if (updatingFromMouse) return; if (!syncingDefaultStartPoint) startPointCustomized = true; ClearTrajectoryCache(); this->Invalidate(); } void OnAddClick(Object^ sender, EventArgs^ e) { ++nSegments; angles->Add(0.0f); lengths->Add((float)nudBaseLength->Value); ClearTrajectoryCache(); this->Invalidate(); } void OnRemClick(Object^ sender, EventArgs^ e) { if (nSegments > 1) { --nSegments; angles->RemoveAt(angles->Count - 1); lengths->RemoveAt(lengths->Count - 1); ClearTrajectoryCache(); this->Invalidate(); } } void OnOptimizeClick(Object^ sender, EventArgs^ e) { if (IsTrajectoryMode()) RunTrajectoryPlanningMode(); else RunPositioningMode(); } protected: [MethodImpl(MethodImplOptions::AggressiveInlining)] virtual void OnPaint(PaintEventArgs^ e) override { Form::OnPaint(e); Graphics^ g = e->Graphics; g->SmoothingMode = SmoothingMode::HighQuality; g->Clear(this->BackColor); int drawHeight = Math::Max(0, this->ClientSize.Height - 180); System::Drawing::Rectangle drawArea(0, 180, this->ClientSize.Width, drawHeight); g->FillRectangle(Brushes::White, drawArea); float targetX = (float)nudTargetX->Value; float targetY = (float)nudTargetY->Value; float targetA = (float)nudTargetAngle->Value; PointF basePoint = GetBasePoint(); int baseX = (int)basePoint.X; int baseY = (int)basePoint.Y; float pixelTargetX = baseX + targetX * 160.0f; float pixelTargetY = baseY - targetY * 160.0f; g->DrawLine(wallPen, baseX - 25, baseY + 8, baseX + 25, baseY + 8); g->FillRectangle(wallHatchBrush, baseX - 25, baseY + 8, 50, 12); g->DrawLine(dashedPen, pixelTargetX - 25, pixelTargetY + 8, pixelTargetX + 25, pixelTargetY + 8); g->FillRectangle(wallHatchBrush, (int)pixelTargetX - 25, (int)pixelTargetY + 8, 50, 12); g->DrawEllipse(targetPen, pixelTargetX - 8.0f, pixelTargetY - 8.0f, 16.0f, 16.0f); float angleLen = 25.0f; float endX = pixelTargetX + angleLen * (float)Math::Cos(targetA); float endY = pixelTargetY - angleLen * (float)Math::Sin(targetA); g->DrawLine(angleTargetPen, pixelTargetX, pixelTargetY, endX, endY); if (IsTrajectoryMode()) { float startX = (float)nudStartX->Value; float startY = (float)nudStartY->Value; float startA = (float)nudStartAngle->Value; float pixelStartX = baseX + startX * 160.0f; float pixelStartY = baseY - startY * 160.0f; g->DrawLine(dashedPen, pixelStartX - 25, pixelStartY + 8, pixelStartX + 25, pixelStartY + 8); g->FillRectangle(wallHatchBrush, (int)pixelStartX - 25, (int)pixelStartY + 8, 50, 12); g->DrawEllipse(startPen, pixelStartX - 8.0f, pixelStartY - 8.0f, 16.0f, 16.0f); float sEndX = pixelStartX + angleLen * (float)Math::Cos(startA); float sEndY = pixelStartY - angleLen * (float)Math::Sin(startA); g->DrawLine(angleStartPen, pixelStartX, pixelStartY, sEndX, sEndY); } if (trajectoryPathWorld->Count > 1 && IsTrajectoryMode()) DrawTrajectoryPath(g); if (obstacleX->Count > 0) { array<float>^ xs = obstacleX->ToArray(); array<float>^ ys = obstacleY->ToArray(); array<float>^ hs = obstacleHalf->ToArray(); int obstacleCount = Math::Min( xs->Length, Math::Min( ys->Length, hs->Length ) ); for (int obsIdx = 0; obsIdx < obstacleCount; ++obsIdx) { float cx = xs[obsIdx]; float cy = ys[obsIdx]; float half = hs[obsIdx]; float left = (float)baseX + (cx - half) * 160.0f; float top = (float)baseY - (cy + half) * 160.0f; float size = 2.0f * half * 160.0f; float marginLeft = (float)baseX + (cx - half - ObstacleClearance) * 160.0f; float marginTop = (float)baseY - (cy + half + ObstacleClearance) * 160.0f; float marginSize = 2.0f * (half + ObstacleClearance) * 160.0f; g->DrawRectangle(obstacleMarginPen, marginLeft, marginTop, marginSize, marginSize); g->FillRectangle(obstacleBrush, left, top, size, size); g->DrawRectangle(obstaclePen, left, top, size, size); } } int drawableSegments = Math::Min( nSegments, Math::Min( angles->Count, lengths->Count ) ); array<PointF>^ pts = gcnew array<PointF>( drawableSegments + 1 ); pts[0] = PointF((float)baseX, (float)baseY); float x = 0.0f; float y = 0.0f; float phi = 0.0f; array<float>^ localAngles = angles->ToArray(); array<float>^ localLengths = lengths->ToArray(); for (int i = 0; i < drawableSegments; ++i) { phi += localAngles[i]; x += localLengths[i] * (float)Math::Cos(phi); y += localLengths[i] * (float)Math::Sin(phi); pts[i + 1] = PointF( (float)baseX + x * 160.0f, (float)baseY - y * 160.0f ); } for (int i = 0; i < drawableSegments; ++i) g->DrawLine(penRod, pts[i], pts[i + 1]); for (int i = 0; i <= drawableSegments; ++i) g->FillEllipse(jointBrush, pts[i].X - 8.0f, pts[i].Y - 8.0f, 16.0f, 16.0f); } }; }
Benchmark
cpp#pragma once #define WIN32_LEAN_AND_MEAN #include <Windows.h> #include <cfloat> #include <cmath> #include <memory> #include <limits> #include <cstddef> #include <kdl/chain.hpp> #include <kdl/jntarray.hpp> #include <kdl/frames.hpp> #include <kdl/joint.hpp> #include <kdl/segment.hpp> #include <trac_ik/trac_ik.hpp> using namespace System; using namespace System::Drawing; using namespace System::Windows::Forms; using namespace System::Collections::Generic; using namespace System::Text; using namespace System::Windows::Forms::DataVisualization::Charting; typedef void(__cdecl* P_MANIP_INDEX)( int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, float**, float*, float*, float*, float*, size_t*, float*, int, const float* ); typedef void(__cdecl* P_FREE)( float* ); typedef void(__cdecl* P_START_INDEX)( int, bool, float, float, float, float, int, float, bool, float, unsigned int, float, float, const float*, int, int, const float*, const float* ); class TracIkRunner final { private: int nSegments_; unsigned int nJoints_; double baseLength_; double maxTheta_; double maxTime_; double eps_; KDL::Chain chain_; KDL::JntArray qMin_; KDL::JntArray qMax_; KDL::JntArray qInit_; KDL::JntArray qOut_; std::unique_ptr<trac_ik::TRAC_IK> solver_; LARGE_INTEGER frequency_; static double WrapPi(double a) { const double pi = 3.1415926535897932384626433832795; const double twoPi = 2.0 * pi; while (a > pi) a -= twoPi; while (a < -pi) a += twoPi; return a; } public: TracIkRunner( int nSegments, double baseLength, double maxTheta, double maxTime, double eps) : nSegments_(nSegments), nJoints_(0), baseLength_(baseLength), maxTheta_(maxTheta), maxTime_(maxTime), eps_(eps) { for (int i = 0; i < nSegments_; ++i) { chain_.addSegment( KDL::Segment( KDL::Joint(KDL::Joint::RotZ), KDL::Frame( KDL::Vector( baseLength_, 0.0, 0.0 ) ) ) ); } nJoints_ = chain_.getNrOfJoints(); qMin_.resize(nJoints_); qMax_.resize(nJoints_); qInit_.resize(nJoints_); qOut_.resize(nJoints_); for (unsigned int i = 0; i < nJoints_; ++i) { qMin_(i) = -maxTheta_; qMax_(i) = maxTheta_; qInit_(i) = 0.0; qOut_(i) = 0.0; } solver_ = std::make_unique<trac_ik::TRAC_IK>( chain_, qMin_, qMax_, maxTime_, eps_, trac_ik::SolveType::Speed ); QueryPerformanceFrequency(&frequency_); } TracIkRunner(const TracIkRunner&) = delete; TracIkRunner& operator=(const TracIkRunner&) = delete; bool Solve( float targetX, float targetY, float targetAngle, double& outBestF, double& outBestX, double& outBestY, double& outBestA, double& outMillis) { for (unsigned int i = 0; i < nJoints_; ++i) { qInit_(i) = 0.0; qOut_(i) = 0.0; } KDL::Frame target( KDL::Rotation::RotZ( static_cast<double>(targetAngle) ), KDL::Vector( static_cast<double>(targetX), static_cast<double>(targetY), 0.0 ) ); KDL::Twist tolerances( KDL::Vector( eps_, eps_, 0.0 ), KDL::Vector( 0.0, 0.0, eps_ ) ); LARGE_INTEGER t0; LARGE_INTEGER t1; QueryPerformanceCounter(&t0); const int result = solver_->CartToJnt( qInit_, target, qOut_, tolerances ); QueryPerformanceCounter(&t1); outMillis = 1.0e3 * static_cast<double>( t1.QuadPart - t0.QuadPart ) / static_cast<double>( frequency_.QuadPart ); if (result < 0) { outBestF = std::numeric_limits<double>::quiet_NaN(); outBestX = std::numeric_limits<double>::quiet_NaN(); outBestY = std::numeric_limits<double>::quiet_NaN(); outBestA = std::numeric_limits<double>::quiet_NaN(); return false; } double x = 0.0; double y = 0.0; double phi = 0.0; for (unsigned int i = 0; i < nJoints_; ++i) { phi += qOut_(i); x += baseLength_ * std::cos(phi); y += baseLength_ * std::sin(phi); } const double dx = x - static_cast<double>(targetX); const double dy = y - static_cast<double>(targetY); const double da = WrapPi( phi - static_cast<double>( targetAngle ) ); const double f = std::sqrt( dx * dx + dy * dy + da * da ); if (!std::isfinite(f) || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(phi) || !std::isfinite(outMillis)) { return false; } outBestF = f; outBestX = x; outBestY = y; outBestA = WrapPi(phi); return true; } }; namespace TESTAGP { public ref struct BenchmarkCase sealed { public: float TargetX; float TargetY; float TargetAngle; double TracF; double TracX; double TracY; double TracMillis; }; public ref class MyForm sealed : public Form { public: MyForm(HMODULE hLib) : hLib(hLib), experimentsStarted(false) { this->Text = L"AGP vs TRAC-IK - позиционирование с ориентацией"; this->ClientSize = System::Drawing::Size( 1350, 860 ); this->SetStyle( ControlStyles::AllPaintingInWmPaint | ControlStyles::UserPaint | ControlStyles::OptimizedDoubleBuffer, true ); fManipIndex = reinterpret_cast<P_MANIP_INDEX>( GetProcAddress( hLib, "AGP_Manip2D" ) ); pFree = reinterpret_cast<P_FREE>( GetProcAddress( hLib, "AGP_Free" ) ); pStartIndex = reinterpret_cast<P_START_INDEX>( GetProcAddress( hLib, "AgpStartManipND" ) ); if (!fManipIndex || !pFree || !pStartIndex) { MessageBox::Show( L"Не удалось получить адреса AGP_Manip2D / AGP_Free / AgpStartManipND из DLL", L"Ошибка", MessageBoxButtons::OK, MessageBoxIcon::Error ); this->Close(); return; } maxTheta = static_cast<float>( Math::PI ); rParam = 1.05f; epsParam = 1.0e-9f; baseLength = 1.0f; stretchFactor = 1.0f; adaptiveAgp = true; randomEngine = gcnew Random( 123456 ); uiFontBold11 = gcnew System::Drawing::Font( "Yu Gothic UI", 11, FontStyle::Bold ); uiFont10 = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Regular ); InitUI(); this->Shown += gcnew EventHandler( this, &MyForm::OnFormShown ); this->Resize += gcnew EventHandler( this, &MyForm::OnResizeInternal ); } private: initonly HMODULE hLib; initonly P_MANIP_INDEX fManipIndex; initonly P_FREE pFree; initonly P_START_INDEX pStartIndex; System::Drawing::Font^ uiFontBold11; System::Drawing::Font^ uiFont10; Random^ randomEngine; Chart^ chartTime; Chart^ chartF; TextBox^ tbStats; bool experimentsStarted; float maxTheta; float rParam; float epsParam; float baseLength; float stretchFactor; bool adaptiveAgp; double NextUniform( double a, double b) { return a + (b - a) * randomEngine->NextDouble(); } unsigned int NextSeedU32() { const unsigned int hi = static_cast<unsigned int>( randomEngine->Next( 1, Int32::MaxValue ) ); const unsigned int lo = static_cast<unsigned int>( randomEngine->Next( 0, Int32::MaxValue ) ); return (hi << 1) ^ lo ^ 0x9E3779B9u; } static void ComputeMeanStd( List<double>^ values, double% mean, double% stdDev) { const int n = values->Count; if (n <= 0) { mean = Double::NaN; stdDev = Double::NaN; return; } double sum = 0.0; for each(double v in values) { if (Double::IsNaN(v) || Double::IsInfinity(v)) { continue; } sum += v; } int validCount = 0; for each(double v in values) { if (!Double::IsNaN(v) && !Double::IsInfinity(v)) { ++validCount; } } if (validCount <= 0) { mean = Double::NaN; stdDev = Double::NaN; return; } mean = sum / static_cast<double>( validCount ); if (validCount == 1) { stdDev = 0.0; return; } double varianceSum = 0.0; for each(double v in values) { if (Double::IsNaN(v) || Double::IsInfinity(v)) { continue; } const double d = v - mean; varianceSum += d * d; } stdDev = Math::Sqrt( varianceSum / static_cast<double>( validCount - 1 ) ); } static bool IsFinite( double value) { return !Double::IsNaN(value) && !Double::IsInfinity(value); } static bool IsFinitePair( double x, double y) { return IsFinite(x) && IsFinite(y); } BenchmarkCase^ GenerateRandomCase( int nSegments) { BenchmarkCase^ task = gcnew BenchmarkCase(); const double maxReach = Math::Max( 1.0, static_cast<double>( nSegments ) * static_cast<double>( baseLength ) ); const double targetRadiusMin = 0.30 * maxReach; const double targetRadiusMax = 0.90 * maxReach; const double angle = NextUniform( 0.0, 2.0 * Math::PI ); const double radius = NextUniform( targetRadiusMin, targetRadiusMax ); const double targetAngle = NextUniform( -Math::PI, Math::PI ); task->TargetX = static_cast<float>( radius * Math::Cos(angle) ); task->TargetY = static_cast<float>( radius * Math::Sin(angle) ); task->TargetAngle = static_cast<float>( targetAngle ); task->TracF = Double::NaN; task->TracX = Double::NaN; task->TracY = Double::NaN; task->TracMillis = Double::NaN; return task; } void InitUI() { chartTime = gcnew Chart(); chartF = gcnew Chart(); tbStats = gcnew TextBox(); chartTime->Parent = this; chartF->Parent = this; tbStats->Parent = this; chartTime->BorderlineDashStyle = ChartDashStyle::Solid; chartTime->BorderlineWidth = 1; chartTime->BorderlineColor = Color::Black; chartF->BorderlineDashStyle = ChartDashStyle::Solid; chartF->BorderlineWidth = 1; chartF->BorderlineColor = Color::Black; ChartArea^ areaTime = gcnew ChartArea( "TimeArea" ); areaTime->AxisX->Title = "Порог maxIter"; areaTime->AxisY->Title = "Время, мс"; areaTime->AxisX->Minimum = 100.0; areaTime->AxisX->Maximum = 1000.0; areaTime->AxisY->Minimum = 0.0; chartTime->ChartAreas->Add( areaTime ); Legend^ legendTime = gcnew Legend( "LegendTime" ); legendTime->Docking = Docking::Top; legendTime->Font = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Bold ); chartTime->Legends->Add( legendTime ); CreateSeriesWithStyle( chartTime, "AGP", "TimeArea", Color::Red, 3 ); CreateSeriesWithStyle( chartTime, "TRAC IK", "TimeArea", Color::Blue, 3 ); ChartArea^ areaF = gcnew ChartArea( "FArea" ); areaF->AxisX->Title = "Порог maxIter"; areaF->AxisY->Title = "Значение целевой функции f"; areaF->AxisX->Minimum = 100.0; areaF->AxisX->Maximum = 1000.0; areaF->AxisY->Minimum = 0.0; chartF->ChartAreas->Add( areaF ); Legend^ legendF = gcnew Legend( "LegendF" ); legendF->Docking = Docking::Top; legendF->Font = gcnew System::Drawing::Font( "Yu Gothic UI", 10, FontStyle::Bold ); chartF->Legends->Add( legendF ); CreateSeriesWithStyle( chartF, "AGP", "FArea", Color::Red, 3 ); CreateSeriesWithStyle( chartF, "TRAC IK", "FArea", Color::Blue, 3 ); tbStats->Multiline = true; tbStats->ReadOnly = true; tbStats->ScrollBars = ScrollBars::Vertical; tbStats->Font = uiFont10; tbStats->Text = L"Подготовка benchmark: генерация хороших задач...\r\n"; OnResizeInternal( nullptr, nullptr ); } void CreateSeriesWithStyle( Chart^ chart, String^ name, String^ areaName, Color color, int borderWidth) { Series^ s = gcnew Series( name ); s->ChartType = SeriesChartType::Line; s->ChartArea = areaName; s->Color = color; s->BorderWidth = borderWidth; s->MarkerStyle = MarkerStyle::Circle; s->MarkerSize = 7; s->MarkerColor = color; chart->Series->Add( s ); } void OnResizeInternal( Object^, EventArgs^) { const int margin = 10; const int statsWidth = 320; int totalWidth = this->ClientSize.Width; int totalHeight = this->ClientSize.Height; if (totalWidth < 500) totalWidth = 500; if (totalHeight < 320) totalHeight = 320; int chartsWidth = totalWidth - statsWidth - 3 * margin; if (chartsWidth < 250) chartsWidth = 250; const int chartHeight = (totalHeight - 3 * margin) / 2; chartTime->Location = Point( margin, margin ); chartTime->Size = System::Drawing::Size( chartsWidth, chartHeight ); chartF->Location = Point( margin, 2 * margin + chartHeight ); chartF->Size = System::Drawing::Size( chartsWidth, chartHeight ); tbStats->Location = Point( 2 * margin + chartsWidth, margin ); tbStats->Size = System::Drawing::Size( statsWidth, totalHeight - 2 * margin ); } void OnFormShown( Object^, EventArgs^) { if (experimentsStarted) return; experimentsStarted = true; try { RunAllExperiments(); } catch (Exception^ ex) { tbStats->Text = L"Ошибка при выполнении benchmark:\r\n" + ex->ToString(); } } bool RunAgpIndexSingle( int nSegments, BenchmarkCase^ task, int maxIter, unsigned int seed, double% outBestF, double% outBestX, double% outBestY, std::size_t% outIterations, double% outAchievedEps, double% outMillis) { float* bestQ = nullptr; float bestXf = 0.0f; float bestYf = 0.0f; float bestAf = 0.0f; float bestFf = FLT_MAX; std::size_t actualIterations = 0; float achievedEps = 0.0f; const float* obsRaw = nullptr; const int obstacleCount = 0; const float ta = task->TargetAngle; pStartIndex( nSegments, false, maxTheta, task->TargetX, task->TargetY, ta, maxIter, rParam, adaptiveAgp, epsParam, seed, baseLength, stretchFactor, obsRaw, obstacleCount, 0, nullptr, nullptr ); LARGE_INTEGER t0; LARGE_INTEGER t1; LARGE_INTEGER fq; QueryPerformanceFrequency( &fq ); QueryPerformanceCounter( &t0 ); fManipIndex( nSegments, false, maxTheta, task->TargetX, task->TargetY, ta, maxIter, rParam, adaptiveAgp, epsParam, seed, baseLength, stretchFactor, obsRaw, obstacleCount, &bestQ, &bestXf, &bestYf, &bestAf, &bestFf, &actualIterations, &achievedEps, 0, nullptr ); QueryPerformanceCounter( &t1 ); outBestF = static_cast<double>( bestFf ); outBestX = static_cast<double>( bestXf ); outBestY = static_cast<double>( bestYf ); outIterations = actualIterations; outAchievedEps = static_cast<double>( achievedEps ); outMillis = 1.0e3 * static_cast<double>( t1.QuadPart - t0.QuadPart ) / static_cast<double>( fq.QuadPart ); if (bestQ != nullptr) { pFree( bestQ ); bestQ = nullptr; } if (!IsFinite(outBestF) || !IsFinite(outBestX) || !IsFinite(outBestY) || !IsFinite(outAchievedEps) || !IsFinite(outMillis)) { return false; } return true; } bool RunTracIkSingle( TracIkRunner& runner, BenchmarkCase^ task, double% outBestF, double% outBestX, double% outBestY, double% outMillis) { double bestF = Double::NaN; double bestX = Double::NaN; double bestY = Double::NaN; double bestA = Double::NaN; double millis = Double::NaN; const bool result = runner.Solve( task->TargetX, task->TargetY, task->TargetAngle, bestF, bestX, bestY, bestA, millis ); outBestF = bestF; outBestX = bestX; outBestY = bestY; outMillis = millis; return result && IsFinitePair( outBestF, outMillis ); } void RunAllExperiments() { array<int>^ dimensions = gcnew array<int> { 3, 4, 5, 6, 7, 8 }; array<int>^ thresholds = gcnew array<int> { 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000 }; const int steps = thresholds->Length; const int dimensionCount = dimensions->Length; const int goodTasksPerDimension = 10; const int repeatsStochastic = 100; const double tracMaxTime = 0.002; array<List<BenchmarkCase^>^>^ goodTasksByDimension = gcnew array<List<BenchmarkCase^>^>( dimensionCount ); for (int dimIndex = 0; dimIndex < dimensionCount; ++dimIndex) { const int nSegments = dimensions[dimIndex]; tbStats->AppendText( String::Format( L"Генерация хороших задач для размерности {0}...\r\n", nSegments ) ); Application::DoEvents(); List<BenchmarkCase^>^ goodForDim = gcnew List<BenchmarkCase^>(); TracIkRunner tracRunner( nSegments, static_cast<double>( baseLength ), static_cast<double>( maxTheta ), tracMaxTime, static_cast<double>( epsParam ) ); int attempts = 0; while (goodForDim->Count < goodTasksPerDimension) { ++attempts; BenchmarkCase^ task = GenerateRandomCase( nSegments ); double f; double x; double y; double ms; const bool ok = RunTracIkSingle( tracRunner, task, f, x, y, ms ); if (ok && f <= 0.05) { goodForDim->Add( task ); tbStats->AppendText( String::Format( L" Найдена хорошая задача {0}, ошибка = {1:F6}\r\n", goodForDim->Count, f ) ); } if ((attempts % 32) == 0) Application::DoEvents(); } goodTasksByDimension[dimIndex] = goodForDim; Application::DoEvents(); } const int totalGoodTasks = dimensionCount * goodTasksPerDimension; const int totalSeedCount = totalGoodTasks * repeatsStochastic; array<unsigned int>^ fixedSeeds = gcnew array<unsigned int>( totalSeedCount ); for (int i = 0; i < totalSeedCount; ++i) { fixedSeeds[i] = NextSeedU32(); } array<double>^ agpTimeMean = gcnew array<double>( steps ); array<double>^ agpTimeStd = gcnew array<double>( steps ); array<double>^ agpFMean = gcnew array<double>( steps ); array<double>^ agpFStd = gcnew array<double>( steps ); array<double>^ tracTimeMean = gcnew array<double>( steps ); array<double>^ tracTimeStd = gcnew array<double>( steps ); array<double>^ tracFMean = gcnew array<double>( steps ); array<double>^ tracFStd = gcnew array<double>( steps ); for (int i = 0; i < steps; ++i) { agpTimeMean[i] = Double::NaN; agpTimeStd[i] = Double::NaN; agpFMean[i] = Double::NaN; agpFStd[i] = Double::NaN; tracTimeMean[i] = Double::NaN; tracTimeStd[i] = Double::NaN; tracFMean[i] = Double::NaN; tracFStd[i] = Double::NaN; } array<List<double>^>^ agpTimeForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ agpFForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ tracTimeForThreshold = gcnew array<List<double>^>( steps ); array<List<double>^>^ tracFForThreshold = gcnew array<List<double>^>( steps ); for (int i = 0; i < steps; ++i) { agpTimeForThreshold[i] = gcnew List<double>(); agpFForThreshold[i] = gcnew List<double>(); tracTimeForThreshold[i] = gcnew List<double>(); tracFForThreshold[i] = gcnew List<double>(); } List<double>^ agpTimesAll = gcnew List<double>(); List<double>^ agpFAll = gcnew List<double>(); List<double>^ tracTimesAll = gcnew List<double>(); List<double>^ tracFAll = gcnew List<double>(); for (int dimIndex = 0; dimIndex < dimensionCount; ++dimIndex) { const int nSegments = dimensions[dimIndex]; List<BenchmarkCase^>^ tasks = goodTasksByDimension[dimIndex]; TracIkRunner tracRunner( nSegments, static_cast<double>( baseLength ), static_cast<double>( maxTheta ), tracMaxTime, static_cast<double>( epsParam ) ); tbStats->AppendText( String::Format( L"\r\nBenchmark для размерности {0}...\r\n", nSegments ) ); Application::DoEvents(); for (int caseIndex = 0; caseIndex < goodTasksPerDimension; ++caseIndex) { BenchmarkCase^ task = tasks[caseIndex]; const int globalTaskIndex = dimIndex * goodTasksPerDimension + caseIndex; double tracF; double tracX; double tracY; double tracMs; const bool tracOk = RunTracIkSingle( tracRunner, task, tracF, tracX, tracY, tracMs ); if (tracOk) { task->TracF = tracF; task->TracX = tracX; task->TracY = tracY; task->TracMillis = tracMs; tracFAll->Add( tracF ); tracTimesAll->Add( tracMs ); } else { task->TracF = Double::NaN; task->TracX = Double::NaN; task->TracY = Double::NaN; task->TracMillis = Double::NaN; } for (int thresholdIndex = 0; thresholdIndex < steps; ++thresholdIndex) { const int maxIter = thresholds[thresholdIndex]; if (IsFinite(task->TracF) && IsFinite(task->TracMillis)) { tracFForThreshold[thresholdIndex]->Add( task->TracF ); tracTimeForThreshold[thresholdIndex]->Add( task->TracMillis ); } for (int r = 0; r < repeatsStochastic; ++r) { const int seedIndex = globalTaskIndex * repeatsStochastic + r; if (seedIndex < 0 || seedIndex >= fixedSeeds->Length) { throw gcnew IndexOutOfRangeException( String::Format( L"Некорректный индекс seed: {0}, размер массива: {1}", seedIndex, fixedSeeds->Length ) ); } const unsigned int seed = fixedSeeds[seedIndex]; double bF; double bX; double bY; double achievedEps; double tMs; std::size_t iterations = 0; const bool ok = RunAgpIndexSingle( nSegments, task, maxIter, seed, bF, bX, bY, iterations, achievedEps, tMs ); if (ok) { agpFForThreshold[thresholdIndex]->Add( bF ); agpTimeForThreshold[thresholdIndex]->Add( tMs ); agpFAll->Add( bF ); agpTimesAll->Add( tMs ); } } Application::DoEvents(); } tbStats->AppendText( String::Format( L" Задача {0}/{1} завершена. TRAC-IK = {2:F4} мс\r\n", caseIndex + 1, goodTasksPerDimension, task->TracMillis ) ); Application::DoEvents(); } } for (int thresholdIndex = 0; thresholdIndex < steps; ++thresholdIndex) { if (agpTimeForThreshold[thresholdIndex]->Count > 0) { ComputeMeanStd( agpTimeForThreshold[thresholdIndex], agpTimeMean[thresholdIndex], agpTimeStd[thresholdIndex] ); ComputeMeanStd( agpFForThreshold[thresholdIndex], agpFMean[thresholdIndex], agpFStd[thresholdIndex] ); } if (tracTimeForThreshold[thresholdIndex]->Count > 0) { ComputeMeanStd( tracTimeForThreshold[thresholdIndex], tracTimeMean[thresholdIndex], tracTimeStd[thresholdIndex] ); ComputeMeanStd( tracFForThreshold[thresholdIndex], tracFMean[thresholdIndex], tracFStd[thresholdIndex] ); } UpdateCharts( thresholds, steps, thresholdIndex, agpTimeMean, agpFMean, tracTimeMean, tracFMean ); tbStats->AppendText( String::Format( L"Порог {0} обработан: AGP runs = {1}, TRAC-IK tasks = {2}\r\n", thresholds[thresholdIndex], agpTimeForThreshold[thresholdIndex]->Count, tracTimeForThreshold[thresholdIndex]->Count ) ); Application::DoEvents(); } double agpTimeMu; double agpTimeSigma; double agpFMu; double agpFSigma; double tracTimeMu; double tracTimeSigma; double tracFMu; double tracFSigma; ComputeMeanStd( agpTimesAll, agpTimeMu, agpTimeSigma ); ComputeMeanStd( agpFAll, agpFMu, agpFSigma ); ComputeMeanStd( tracTimesAll, tracTimeMu, tracTimeSigma ); ComputeMeanStd( tracFAll, tracFMu, tracFSigma ); StringBuilder^ sb = gcnew StringBuilder(); sb->AppendLine(); sb->AppendLine( L"==============================================" ); sb->AppendLine( L"СВОДНАЯ СТАТИСТИКА" ); sb->AppendLine( L"==============================================" ); sb->AppendLine( L"Размерности: 3..8" ); sb->AppendLine( L"Каждая размерность: 10 задач, успешно решённых TRAC-IK (f <= 0.05)" ); sb->AppendLine( L"Каждая задача: случайная цель + угол, без препятствий" ); sb->AppendFormat( L"Stochastic-повторы AGP: {0} разных seed на задачу\r\n", repeatsStochastic ); sb->AppendFormat( L"Порогов maxIter: {0}\r\n", steps ); sb->AppendLine( L"TRAC-IK запускается один раз для принятой задачи в benchmark-фазе" ); sb->AppendLine(); sb->AppendLine( L"Общие результаты:" ); sb->AppendFormat( L" AGP runs: {0}\r\n", agpTimesAll->Count ); sb->AppendFormat( L" TRAC-IK runs: {0}\r\n", tracTimesAll->Count ); sb->AppendLine(); sb->AppendLine( L"Средние значения по всем фактическим запускам:" ); sb->AppendFormat( L" AGP: время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n", agpTimeMu, agpTimeSigma, agpFMu, agpFSigma ); sb->AppendFormat( L" TRAC-IK: время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n", tracTimeMu, tracTimeSigma, tracFMu, tracFSigma ); sb->AppendLine(); sb->AppendLine( L"Количество задач:" ); sb->AppendFormat( L" Всего задач: {0}\r\n", totalGoodTasks ); sb->AppendFormat( L" AGP запусков на задачу: {0}\r\n", steps * repeatsStochastic ); sb->AppendFormat( L" Ожидаемое число AGP запусков: {0}\r\n", totalGoodTasks * steps * repeatsStochastic ); sb->AppendLine(); sb->AppendLine( L"Параметры:" ); sb->AppendFormat( L" maxTheta = {0:G9}\r\n", maxTheta ); sb->AppendFormat( L" r = {0:G9}\r\n", rParam ); sb->AppendFormat( L" eps = {0:G9}\r\n", epsParam ); sb->AppendFormat( L" baseLength = {0:G9}\r\n", baseLength ); sb->AppendFormat( L" stretchFactor = {0:G9}\r\n", stretchFactor ); sb->AppendFormat( L" adaptiveAgp = {0}\r\n", adaptiveAgp ); sb->AppendFormat( L" TRAC-IK timeout = {0:F3} мс\r\n", tracMaxTime * 1000.0 ); tbStats->AppendText( sb->ToString() ); } void UpdateCharts( array<int>^ thresholds, int steps, int filledUpTo, array<double>^ agpTimeMean, array<double>^ agpFMean, array<double>^ tracTimeMean, array<double>^ tracFMean) { Series^ sAgpTime = chartTime->Series["AGP"]; Series^ sTracTime = chartTime->Series["TRAC IK"]; Series^ sAgpF = chartF->Series["AGP"]; Series^ sTracF = chartF->Series["TRAC IK"]; sAgpTime->Points->Clear(); sTracTime->Points->Clear(); sAgpF->Points->Clear(); sTracF->Points->Clear(); for (int i = 0; i <= filledUpTo && i < steps; ++i) { const double x = static_cast<double>( thresholds[i] ); if (!Double::IsNaN( agpTimeMean[i] ) && !Double::IsInfinity( agpTimeMean[i] )) { sAgpTime->Points->AddXY( x, agpTimeMean[i] ); } if (!Double::IsNaN( tracTimeMean[i] ) && !Double::IsInfinity( tracTimeMean[i] )) { sTracTime->Points->AddXY( x, tracTimeMean[i] ); } if (!Double::IsNaN( agpFMean[i] ) && !Double::IsInfinity( agpFMean[i] )) { sAgpF->Points->AddXY( x, agpFMean[i] ); } if (!Double::IsNaN( tracFMean[i] ) && !Double::IsInfinity( tracFMean[i] )) { sTracF->Points->AddXY( x, tracFMean[i] ); } } chartTime->Invalidate(); chartF->Invalidate(); this->Refresh(); } }; }
В этой версии TRAC-IK timeout = 2 мс по-прежнему является параметром самого решателя, но это не гарантирует, что внешнее wall-clock время CartToJnt всегда будет ≤ 2 мс: CartToJnt создаёт два std::thread и затем делает join() обоих, а внутренние решатели проверяют оставшееся время между своими вычислительными участками. Поэтому превышение timeout в измеренном wall-clock возможно; теперь, однако, и demo, и benchmark измеряют именно одно и то же — полный фактический wall-clock вызова CartToJnt. (GitHub)