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

feat: abstractrize scene

Browse Source
This commit is contained in:
Leonetienne
2026-02-10 22:06:04 +01:00
parent 73263e5437
commit 6d076b3d3e
6 changed files with 111 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/MainLoop.cpp
+5 -70
View File
@@ -1,39 +1,17 @@
#include "MainLoop.h"
#include <iostream>
#include <thread>
#include <chrono>
#include <algorithm>
#include <numbers>
#include "StateMachine.h"
#include "ArmSegment.h"
#include "yz_plotter_win.h"
#include "Scene_JointSpaceControl.h"
MainLoop::MainLoop() noexcept :
arm(L"COM4"),
deadzones({
AxisAlignedBoundingBox(
Vector3d(-125, -125, -20),
Vector3d(65, 100, 120)
),
AxisAlignedBoundingBox(
Vector3d(0, -160, 0),
Vector3d(20000, 100, 20000)
)
}),
leftVKB(L" VKBsim Gladiator EVO L ")
arm(L"COM4")
{
StateMachine::getInstance();
if (!arm.assumeHomePoseImmediate()) {
std::cerr << "Unable to move arm to home!" << std::endl;
exit(-1);
}
if (!leftVKB.connect()) {
std::cerr << "Unable to connect to left VKB controller!" << std::endl;
exit(-1);
}
StateMachine::getInstance().setState(StateMachine::STATE::IDLING);
}
@@ -55,14 +33,7 @@ void MainLoop::run()
static std::chrono::steady_clock::time_point lastBegin =
std::chrono::high_resolution_clock::now();
YZConsolePlotter plot(-200, 200, 20, 2);
double j0Pos = 0;
double j1Pos = 0;
double j2Pos = 0;
double j3Pos = 0;
double j4Pos = 0;
double j5Pos = 0;
Scene_JointSpaceControl sceneJointSpaceControl(arm);
while (isRunning)
{
@@ -80,44 +51,8 @@ void MainLoop::run()
switch (StateMachine::getInstance().getState()) {
case StateMachine::STATE::IDLING:
case StateMachine::STATE::MOVING: {
constexpr double speed = 5000;
VkbInputs vkbi = leftVKB.get();
// Calculate new joint positions
j0Pos += -vkbi.roll * speed * frametime;
j1Pos += -vkbi.pitch * speed * frametime;
j2Pos += -vkbi.yaw * speed * frametime;
j3Pos = vkbi.wheel * (arm.getJoints()[3].getMaxAngle() - arm.getJoints()[3].getMinAngle());
j4Pos += vkbi.thumb_ver * speed * frametime;
j5Pos += vkbi.thumb_hor * speed * frametime;
// Stage the new pose and keep it if it is good
arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos });
if (arm.isEFPositionValid(deadzones)) {
// Roll back individual joints if their angles are out of range
arm.rollbackJointsBasedOnValidRange();
//std::cout << "Pose good!!" << std::endl;
arm.commitPose();
j0Pos = arm.getJoints()[0].getTargetPosition();
j1Pos = arm.getJoints()[1].getTargetPosition();
j2Pos = arm.getJoints()[2].getTargetPosition();
j3Pos = arm.getJoints()[3].getTargetPosition();
j4Pos = arm.getJoints()[4].getTargetPosition();
j5Pos = arm.getJoints()[5].getTargetPosition();
}
else {
arm.rollbackPose();
//std::cout << "Hit deadzone!!" << std::endl;
}
const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint();
//std::cout << ef << std::endl;
//std::cout << arm.getJ3().getCurrentPosition() << std::endl;
plot.draw(ef);
arm.update(frametime);
// HERE SCENE UPDATE
sceneJointSpaceControl.update(frametime);
// Unset MOVING to IDLING state if no arm is moving
if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) {
MotionCore/MainLoop.h
-5
View File
@@ -1,7 +1,5 @@
#pragma once
#include "Arm.h"
#include "AxisAlignedBoundingBox.h"
#include "vkb_controller.h"
#include <array>
// Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime
@@ -18,8 +16,5 @@ private:
MainLoop() noexcept;
bool isRunning = true;
Arm arm;
std::array<AxisAlignedBoundingBox, 2> deadzones;
VKBSimController leftVKB;
};
MotionCore/MotionCore.vcxproj
+2
View File
@@ -151,6 +151,7 @@
<ClCompile Include="Matrix3x3.cpp" />
<ClCompile Include="Motor.cpp" />
<ClCompile Include="OSerialBus.cpp" />
<ClCompile Include="Scene_JointSpaceControl.cpp" />
<ClCompile Include="StateMachine.cpp" />
<ClCompile Include="Vector3d.cpp" />
<ClCompile Include="vkb_controllerh.cpp" />
@@ -163,6 +164,7 @@
<ClInclude Include="Matrix3x3.h" />
<ClInclude Include="Motor.h" />
<ClInclude Include="OSerialBus.h" />
<ClInclude Include="Scene_JointSpaceControl.h" />
<ClInclude Include="StateMachine.h" />
<ClInclude Include="Vector3d.h" />
<ClInclude Include="vkb_controller.h" />
MotionCore/MotionCore.vcxproj.filters
+6
View File
@@ -48,6 +48,9 @@
<ClCompile Include="StateMachine.cpp">
<Filter>Quelldateien</Filter>
</ClCompile>
<ClCompile Include="Scene_JointSpaceControl.cpp">
<Filter>Quelldateien</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="MainLoop.h">
@@ -83,5 +86,8 @@
<ClInclude Include="StateMachine.h">
<Filter>Headerdateien</Filter>
</ClInclude>
<ClInclude Include="Scene_JointSpaceControl.h">
<Filter>Headerdateien</Filter>
</ClInclude>
</ItemGroup>
</Project>
MotionCore/Scene_JointSpaceControl.cpp
Executable
+71
View File
@@ -0,0 +1,71 @@
#include "Scene_JointSpaceControl.h"
#include <iostream>
Scene_JointSpaceControl::Scene_JointSpaceControl(Arm& arm):
arm{arm},
deadzones({
AxisAlignedBoundingBox(
Vector3d(-125, -125, -20),
Vector3d(65, 100, 120)
),
AxisAlignedBoundingBox(
Vector3d(0, -160, 0),
Vector3d(20000, 100, 20000)
)
}),
leftVKB(L" VKBsim Gladiator EVO L "),
plot(-200, 200, 20, 2)
{
if (!arm.assumeHomePoseImmediate()) {
std::cerr << "Unable to move arm to home!" << std::endl;
exit(-1);
}
if (!leftVKB.connect()) {
std::cerr << "Unable to connect to left VKB controller!" << std::endl;
exit(-1);
}
}
void Scene_JointSpaceControl::update(double frametime)
{
constexpr double speed = 5000;
VkbInputs vkbi = leftVKB.get();
// Calculate new joint positions
j0Pos += -vkbi.roll * speed * frametime;
j1Pos += -vkbi.pitch * speed * frametime;
j2Pos += -vkbi.yaw * speed * frametime;
j3Pos = vkbi.wheel * (arm.getJoints()[3].getMaxAngle() - arm.getJoints()[3].getMinAngle());
j4Pos += vkbi.thumb_ver * speed * frametime;
j5Pos += vkbi.thumb_hor * speed * frametime;
// Stage the new pose and keep it if it is good
arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos });
if (arm.isEFPositionValid(deadzones)) {
// Roll back individual joints if their angles are out of range
arm.rollbackJointsBasedOnValidRange();
//std::cout << "Pose good!!" << std::endl;
arm.commitPose();
j0Pos = arm.getJoints()[0].getTargetPosition();
j1Pos = arm.getJoints()[1].getTargetPosition();
j2Pos = arm.getJoints()[2].getTargetPosition();
j3Pos = arm.getJoints()[3].getTargetPosition();
j4Pos = arm.getJoints()[4].getTargetPosition();
j5Pos = arm.getJoints()[5].getTargetPosition();
}
else {
arm.rollbackPose();
//std::cout << "Hit deadzone!!" << std::endl;
}
const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint();
//std::cout << ef << std::endl;
//std::cout << arm.getJ3().getCurrentPosition() << std::endl;
plot.draw(ef);
arm.update(frametime);
}
MotionCore/Scene_JointSpaceControl.h
Executable
+27
View File
@@ -0,0 +1,27 @@
#pragma once
#include "Arm.h"
#include "AxisAlignedBoundingBox.h"
#include "vkb_controller.h"
#include "yz_plotter_win.h"
class Scene_JointSpaceControl
{
public:
Scene_JointSpaceControl(Arm& arm);
void update(double frametime);
private:
Arm& arm;
std::array<AxisAlignedBoundingBox, 2> deadzones;
VKBSimController leftVKB;
YZConsolePlotter plot;
double j0Pos = 0;
double j1Pos = 0;
double j2Pos = 0;
double j3Pos = 0;
double j4Pos = 0;
double j5Pos = 0;
};