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

dynamically code arm joints

Browse Source
This commit is contained in:
Leonetienne
2026-02-10 21:48:40 +01:00
parent 2bd1cc0bfd
commit 73263e5437
5 changed files with 77 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/Arm.cpp
+46 -170
View File
@@ -2,176 +2,84 @@
#include <iostream> #include <iostream>
#include <numbers> #include <numbers>
#include <algorithm> #include <algorithm>
#include <ranges>
Arm::Arm(const std::wstring& comPort) noexcept: Arm::Arm(const std::wstring& comPort) noexcept:
oSerialBus(comPort), oSerialBus(comPort),
j0( joints{
oSerialBus, // bus axis ref len idx min max home parent
Vector3d::Y, ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 0, -135.0, 45.0, 0.0, nullptr),
Vector3d::Forward, ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 72.0, 1, -38.0, 85.0, 0.0, nullptr),
0.0, //mm ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 97.0, 2, -90.0, 90.0, 0.0, nullptr),
0, ArmSegment(oSerialBus, Vector3d::X, Vector3d::Up, 114.0, 3, -90.0, 90.0, 90.0, nullptr),
-135.0, ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 4, -90.0, 90.0, 90.0, nullptr),
45.0, ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 56.0, 5, -90.0, 90.0, 90.0, nullptr)
0.0, }
nullptr
),
j1(
oSerialBus,
-Vector3d::X,
Vector3d::Up,
72.0, //mm
1,
-38.0,
85.0,
0.0,
&j0
),
j2(
oSerialBus,
-Vector3d::X,
Vector3d::Up,
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()) { if (!oSerialBus.isOpen()) {
std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl;
exit(-1); exit(-1);
} }
// Assign parents
ArmSegment* parent = &joints[0];
for (auto& joint : joints | std::views::drop(1)) {
joint.setParent(parent);
parent = &joint;
}
} }
bool Arm::assumeHomePose() bool Arm::assumeHomePose()
{ {
if (!j0.goHome(0.5)) { for (auto& joint : joints) {
if (!joint.goHome(0.5)) {
return false; return false;
} }
if (!j1.goHome(0.5)) {
return false;
} }
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; return true;
} }
void Arm::stagePose( void Arm::stagePose(
const double theta_j0, const std::array<double, NUM_JOINTS>& pose
const double theta_j1,
const double theta_j2,
const double theta_j3,
const double theta_j4,
const double theta_j5
) noexcept ) noexcept
{ {
j0.stageAngle(theta_j0); for (std::size_t i = 0; i < NUM_JOINTS; i++) {
j1.stageAngle(theta_j1); joints[i].stageAngle(pose[i]);
j2.stageAngle(theta_j2); }
j3.stageAngle(theta_j3);
j4.stageAngle(theta_j4);
j5.stageAngle(theta_j5);
} }
void Arm::commitPose() noexcept void Arm::commitPose() noexcept
{ {
if (j0.isStaged()) { for (auto& joint : joints) {
j0.commitStaging(); if (joint.isStaged()) {
joint.commitStaging();
} }
if (j1.isStaged()) {
j1.commitStaging();
}
if (j2.isStaged()) {
j2.commitStaging();
}
if (j3.isStaged()) {
j3.commitStaging();
}
if (j4.isStaged()) {
j4.commitStaging();
}
if (j5.isStaged()) {
j5.commitStaging();
} }
} }
void Arm::rollbackPose() noexcept void Arm::rollbackPose() noexcept
{ {
if (j0.isStaged()) { for (auto& joint : joints) {
j0.rollbackStaging(); if (joint.isStaged()) {
joint.rollbackStaging();
} }
if (j1.isStaged()) {
j1.rollbackStaging();
}
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 bool Arm::isMoving() const noexcept
{ {
return j0.isMoving() || j1.isMoving() || j2.isMoving() || j3.isMoving() || j4.isMoving() || j5.isMoving(); for (auto& joint : joints) {
if (joint.isMoving()) {
return true;
}
}
return false;
} }
bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
{ {
// Otherwise: check for endeffector position // Otherwise: check for endeffector position
const Vector3d ef = j5.getTargetedGlobalEndpoint(); const Vector3d ef = joints.back().getTargetedGlobalEndpoint();
// If any deadzone intersects with the EF pos, fai // If any deadzone intersects with the EF pos, fai
for (const AxisAlignedBoundingBox& aabb : deadzones) { for (const AxisAlignedBoundingBox& aabb : deadzones) {
@@ -186,30 +94,14 @@ bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) c
bool Arm::rollbackJointsBasedOnValidRange() noexcept bool Arm::rollbackJointsBasedOnValidRange() noexcept
{ {
bool allGood = true; bool allGood = true;
if (!j0.isPositionInRange(j0.getTargetPosition())) {
j0.rollbackStaging(); for (auto& joint : joints) {
if (!joint.isPositionInRange(joint.getTargetPosition())) {
joint.rollbackStaging();
allGood = false; allGood = false;
} }
if (!j1.isPositionInRange(j1.getTargetPosition())) {
j2.rollbackStaging();
allGood = false;
}
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; return allGood;
} }
@@ -220,23 +112,10 @@ Arm::~Arm()
bool Arm::assumeHomePoseImmediate() bool Arm::assumeHomePoseImmediate()
{ {
if (!j0.goHomeImmediate()) { for (auto& joint : joints) {
if (!joint.goHomeImmediate()) {
return false; return false;
} }
if (!j1.goHomeImmediate()) {
return false;
}
if (!j2.goHomeImmediate()) {
return false;
}
if (!j3.goHomeImmediate()) {
return false;
}
if (!j4.goHomeImmediate()) {
return false;
}
if (!j5.goHomeImmediate()) {
return false;
} }
return true; return true;
@@ -244,10 +123,7 @@ bool Arm::assumeHomePoseImmediate()
void Arm::update(double frametime) void Arm::update(double frametime)
{ {
j0.update(frametime); for (auto& joint : joints) {
j1.update(frametime); joint.update(frametime);
j2.update(frametime); }
j3.update(frametime);
j4.update(frametime);
j5.update(frametime);
} }
MotionCore/Arm.h
+7 -21
View File
@@ -3,6 +3,9 @@
#include "AxisAlignedBoundingBox.h" #include "AxisAlignedBoundingBox.h"
#include <string> #include <string>
#include <span> #include <span>
#include <array>
constexpr std::size_t NUM_JOINTS = 6;
class Arm class Arm
{ {
@@ -15,14 +18,7 @@ public:
bool assumeHomePoseImmediate(); bool assumeHomePoseImmediate();
bool assumeHomePose(); bool assumeHomePose();
void stagePose( void stagePose(const std::array<double, NUM_JOINTS>& pose) noexcept;
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 commitPose() noexcept;
void rollbackPose() noexcept; void rollbackPose() noexcept;
bool isMoving() const noexcept; bool isMoving() const noexcept;
@@ -32,22 +28,12 @@ public:
// Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made. // Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made.
bool rollbackJointsBasedOnValidRange() noexcept; bool rollbackJointsBasedOnValidRange() noexcept;
const ArmSegment& getJ0() const { return j0; }; const std::array<ArmSegment, NUM_JOINTS>& getJoints() const { return joints; };
const ArmSegment& getJ1() const { return j1; }; const ArmSegment& getEndEffectorJoint() const { return joints.back(); };
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); void update(double frametime);
private: private:
OSerialBus oSerialBus; OSerialBus oSerialBus;
ArmSegment j0; std::array<ArmSegment, NUM_JOINTS> joints;
ArmSegment j1;
ArmSegment j2;
ArmSegment j3;
ArmSegment j4;
ArmSegment j5;
}; };
MotionCore/ArmSegment.cpp
+5
View File
@@ -140,6 +140,11 @@ bool ArmSegment::rollbackStaging() noexcept
return true; return true;
} }
void ArmSegment::setParent(ArmSegment* parent) noexcept
{
this->parent = parent;
}
void ArmSegment::update(const double frametime) void ArmSegment::update(const double frametime)
{ {
motor.update(frametime); motor.update(frametime);
MotionCore/ArmSegment.h
+1
View File
@@ -46,6 +46,7 @@ public:
bool commitStaging() noexcept; bool commitStaging() noexcept;
// Returns false if not currently staged // Returns false if not currently staged
bool rollbackStaging() noexcept; bool rollbackStaging() noexcept;
void setParent(ArmSegment* parent) noexcept;
bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); }; bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); };
double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); }; double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); };
MotionCore/MainLoop.cpp
+10 -9
View File
@@ -5,6 +5,7 @@
#include <algorithm> #include <algorithm>
#include <numbers> #include <numbers>
#include "StateMachine.h" #include "StateMachine.h"
#include "ArmSegment.h"
#include "yz_plotter_win.h" #include "yz_plotter_win.h"
MainLoop::MainLoop() noexcept : MainLoop::MainLoop() noexcept :
@@ -86,12 +87,12 @@ void MainLoop::run()
j0Pos += -vkbi.roll * speed * frametime; j0Pos += -vkbi.roll * speed * frametime;
j1Pos += -vkbi.pitch * speed * frametime; j1Pos += -vkbi.pitch * speed * frametime;
j2Pos += -vkbi.yaw * speed * frametime; j2Pos += -vkbi.yaw * speed * frametime;
j3Pos = vkbi.wheel * (arm.getJ3().getMaxAngle() - arm.getJ3().getMinAngle()); j3Pos = vkbi.wheel * (arm.getJoints()[3].getMaxAngle() - arm.getJoints()[3].getMinAngle());
j4Pos += vkbi.thumb_ver * speed * frametime; j4Pos += vkbi.thumb_ver * speed * frametime;
j5Pos += vkbi.thumb_hor * speed * frametime; j5Pos += vkbi.thumb_hor * speed * frametime;
// Stage the new pose and keep it if it is good // Stage the new pose and keep it if it is good
arm.stagePose(j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos); arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos });
if (arm.isEFPositionValid(deadzones)) { if (arm.isEFPositionValid(deadzones)) {
// Roll back individual joints if their angles are out of range // Roll back individual joints if their angles are out of range
arm.rollbackJointsBasedOnValidRange(); arm.rollbackJointsBasedOnValidRange();
@@ -99,19 +100,19 @@ void MainLoop::run()
//std::cout << "Pose good!!" << std::endl; //std::cout << "Pose good!!" << std::endl;
arm.commitPose(); arm.commitPose();
j0Pos = arm.getJ0().getTargetPosition(); j0Pos = arm.getJoints()[0].getTargetPosition();
j1Pos = arm.getJ1().getTargetPosition(); j1Pos = arm.getJoints()[1].getTargetPosition();
j2Pos = arm.getJ2().getTargetPosition(); j2Pos = arm.getJoints()[2].getTargetPosition();
j3Pos = arm.getJ3().getTargetPosition(); j3Pos = arm.getJoints()[3].getTargetPosition();
j4Pos = arm.getJ4().getTargetPosition(); j4Pos = arm.getJoints()[4].getTargetPosition();
j5Pos = arm.getJ5().getTargetPosition(); j5Pos = arm.getJoints()[5].getTargetPosition();
} }
else { else {
arm.rollbackPose(); arm.rollbackPose();
//std::cout << "Hit deadzone!!" << std::endl; //std::cout << "Hit deadzone!!" << std::endl;
} }
const Vector3d ef = arm.getJ5().getCurrentGlobalEndpoint(); const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint();
//std::cout << ef << std::endl; //std::cout << ef << std::endl;
//std::cout << arm.getJ3().getCurrentPosition() << std::endl; //std::cout << arm.getJ3().getCurrentPosition() << std::endl;
plot.draw(ef); plot.draw(ef);