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

feat: add j3,j4,j5

Browse Source
This commit is contained in:
Leonetienne
2026-02-10 20:54:09 +01:00
parent 0b7cc1ac50
commit 2bd1cc0bfd
5 changed files with 143 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/Arm.cpp
+105 -11
View File
@@ -31,12 +31,45 @@ Arm::Arm(const std::wstring& comPort) noexcept:
oSerialBus,
-Vector3d::X,
Vector3d::Up,
95.0, //mm
2,
-123,
57.0,
0.0,
97.0, //mm
2, // index
-90, // min
90.0, // max
0.0, // home
&j1
),
j3(
oSerialBus,
Vector3d::X,
Vector3d::Up,
114.0, //mm
3,
-90.0,
90.0,
90.0,
&j2
),
j4(
oSerialBus,
Vector3d::Y,
Vector3d::Forward,
0.0, //mm
4,
-90.0,
90.0,
90.0,
&j3
),
j5(
oSerialBus,
-Vector3d::X,
Vector3d::Up,
56.0, //mm
5,
-90.0,
90.0,
90.0,
&j4
)
{
if (!oSerialBus.isOpen()) {
@@ -56,15 +89,34 @@ bool Arm::assumeHomePose()
if (!j2.goHome(0.5)) {
return false;
}
if (!j3.goHome(0.5)) {
return false;
}
if (!j4.goHome(0.5)) {
return false;
}
if (!j5.goHome(0.5)) {
return false;
}
return true;
}
void Arm::stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept
void Arm::stagePose(
const double theta_j0,
const double theta_j1,
const double theta_j2,
const double theta_j3,
const double theta_j4,
const double theta_j5
) noexcept
{
j0.stageAngle(theta_j0);
j1.stageAngle(theta_j1);
j2.stageAngle(theta_j2);
j3.stageAngle(theta_j3);
j4.stageAngle(theta_j4);
j5.stageAngle(theta_j5);
}
void Arm::commitPose() noexcept
@@ -78,6 +130,15 @@ void Arm::commitPose() noexcept
if (j2.isStaged()) {
j2.commitStaging();
}
if (j3.isStaged()) {
j3.commitStaging();
}
if (j4.isStaged()) {
j4.commitStaging();
}
if (j5.isStaged()) {
j5.commitStaging();
}
}
void Arm::rollbackPose() noexcept
@@ -91,19 +152,28 @@ void Arm::rollbackPose() noexcept
if (j2.isStaged()) {
j2.rollbackStaging();
}
if (j3.isStaged()) {
j3.rollbackStaging();
}
if (j4.isStaged()) {
j4.rollbackStaging();
}
if (j5.isStaged()) {
j5.rollbackStaging();
}
}
bool Arm::isMoving() const noexcept
{
return j0.isMoving() || j1.isMoving() || j2.isMoving();
return j0.isMoving() || j1.isMoving() || j2.isMoving() || j3.isMoving() || j4.isMoving() || j5.isMoving();
}
bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
{
// Otherwise: check for endeffector position
const Vector3d ef = j2.getTargetedGlobalEndpoint();
const Vector3d ef = j5.getTargetedGlobalEndpoint();
// If any deadzone intersects with the EF pos, fail
// If any deadzone intersects with the EF pos, fai
for (const AxisAlignedBoundingBox& aabb : deadzones) {
if (aabb.doesIntersect(ef)) {
return false;
@@ -120,14 +190,26 @@ bool Arm::rollbackJointsBasedOnValidRange() noexcept
j0.rollbackStaging();
allGood = false;
}
if (!j1.isPositionInRange(j0.getTargetPosition())) {
if (!j1.isPositionInRange(j1.getTargetPosition())) {
j2.rollbackStaging();
allGood = false;
}
if (!j2.isPositionInRange(j0.getTargetPosition())) {
if (!j2.isPositionInRange(j2.getTargetPosition())) {
j2.rollbackStaging();
allGood = false;
}
if (!j3.isPositionInRange(j3.getTargetPosition())) {
j3.rollbackStaging();
allGood = false;
}
if (!j4.isPositionInRange(j4.getTargetPosition())) {
j4.rollbackStaging();
allGood = false;
}
if (!j5.isPositionInRange(j5.getTargetPosition())) {
j5.rollbackStaging();
allGood = false;
}
return allGood;
}
@@ -147,6 +229,15 @@ bool Arm::assumeHomePoseImmediate()
if (!j2.goHomeImmediate()) {
return false;
}
if (!j3.goHomeImmediate()) {
return false;
}
if (!j4.goHomeImmediate()) {
return false;
}
if (!j5.goHomeImmediate()) {
return false;
}
return true;
}
@@ -156,4 +247,7 @@ void Arm::update(double frametime)
j0.update(frametime);
j1.update(frametime);
j2.update(frametime);
j3.update(frametime);
j4.update(frametime);
j5.update(frametime);
}
MotionCore/Arm.h
+14 -1
View File
@@ -15,7 +15,14 @@ public:
bool assumeHomePoseImmediate();
bool assumeHomePose();
void stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept;
void stagePose(
const double theta_j0,
const double theta_j1,
const double theta_j2,
const double theta_j3,
const double theta_j4,
const double theta_j5
) noexcept;
void commitPose() noexcept;
void rollbackPose() noexcept;
bool isMoving() const noexcept;
@@ -28,6 +35,9 @@ public:
const ArmSegment& getJ0() const { return j0; };
const ArmSegment& getJ1() const { return j1; };
const ArmSegment& getJ2() const { return j2; };
const ArmSegment& getJ3() const { return j3; };
const ArmSegment& getJ4() const { return j4; };
const ArmSegment& getJ5() const { return j5; };
void update(double frametime);
@@ -36,5 +46,8 @@ private:
ArmSegment j0;
ArmSegment j1;
ArmSegment j2;
ArmSegment j3;
ArmSegment j4;
ArmSegment j5;
};
MotionCore/ArmSegment.cpp
+1 -1
View File
@@ -149,7 +149,7 @@ Vector3d ArmSegment::getCurrentGlobalEndpoint() const
{
// Collect the chain from this segment up to the root
std::vector<const ArmSegment*> chain;
chain.reserve(3);
chain.reserve(4);
const ArmSegment* s = this;
while (s != nullptr) {
chain.push_back(s);
MotionCore/MainLoop.cpp
+22 -8
View File
@@ -11,8 +11,12 @@ MainLoop::MainLoop() noexcept :
arm(L"COM4"),
deadzones({
AxisAlignedBoundingBox(
Vector3d(-125, -125, -20),
Vector3d(65, 100, 120)
Vector3d(-125, -125, -20),
Vector3d(65, 100, 120)
),
AxisAlignedBoundingBox(
Vector3d(0, -160, 0),
Vector3d(20000, 100, 20000)
)
}),
leftVKB(L" VKBsim Gladiator EVO L ")
@@ -50,11 +54,14 @@ void MainLoop::run()
static std::chrono::steady_clock::time_point lastBegin =
std::chrono::high_resolution_clock::now();
//YZConsolePlotter plot(-200, 200, 20, 2);
YZConsolePlotter plot(-200, 200, 20, 2);
double j0Pos = 0;
double j1Pos = 0;
double j2Pos = 0;
double j3Pos = 0;
double j4Pos = 0;
double j5Pos = 0;
while (isRunning)
{
@@ -79,28 +86,35 @@ void MainLoop::run()
j0Pos += -vkbi.roll * speed * frametime;
j1Pos += -vkbi.pitch * speed * frametime;
j2Pos += -vkbi.yaw * speed * frametime;
j3Pos = vkbi.wheel * (arm.getJ3().getMaxAngle() - arm.getJ3().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);
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;
//std::cout << "Pose good!!" << std::endl;
arm.commitPose();
j0Pos = arm.getJ0().getTargetPosition();
j1Pos = arm.getJ1().getTargetPosition();
j2Pos = arm.getJ2().getTargetPosition();
j3Pos = arm.getJ3().getTargetPosition();
j4Pos = arm.getJ4().getTargetPosition();
j5Pos = arm.getJ5().getTargetPosition();
}
else {
arm.rollbackPose();
std::cout << "Hit deadzone!!" << std::endl;
//std::cout << "Hit deadzone!!" << std::endl;
}
const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint();
const Vector3d ef = arm.getJ5().getCurrentGlobalEndpoint();
//std::cout << ef << std::endl;
//plot.draw(ef);
//std::cout << arm.getJ3().getCurrentPosition() << std::endl;
plot.draw(ef);
arm.update(frametime);
MotionCore/MainLoop.h
+1 -1
View File
@@ -20,6 +20,6 @@ private:
bool isRunning = true;
Arm arm;
std::array<AxisAlignedBoundingBox, 1> deadzones;
std::array<AxisAlignedBoundingBox, 2> deadzones;
VKBSimController leftVKB;
};