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
+54 -178
View File
@@ -2,176 +2,84 @@
#include <iostream>
#include <numbers>
#include <algorithm>
#include <ranges>
Arm::Arm(const std::wstring& comPort) noexcept:
oSerialBus(comPort),
j0(
oSerialBus,
Vector3d::Y,
Vector3d::Forward,
0.0, //mm
0,
-135.0,
45.0,
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
)
joints{
// bus axis ref len idx min max home parent
ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 0, -135.0, 45.0, 0.0, nullptr),
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 72.0, 1, -38.0, 85.0, 0.0, nullptr),
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 97.0, 2, -90.0, 90.0, 0.0, nullptr),
ArmSegment(oSerialBus, Vector3d::X, Vector3d::Up, 114.0, 3, -90.0, 90.0, 90.0, nullptr),
ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 4, -90.0, 90.0, 90.0, nullptr),
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 56.0, 5, -90.0, 90.0, 90.0, nullptr)
}
{
if (!oSerialBus.isOpen()) {
std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl;
exit(-1);
}
// Assign parents
ArmSegment* parent = &joints[0];
for (auto& joint : joints | std::views::drop(1)) {
joint.setParent(parent);
parent = &joint;
}
}
bool Arm::assumeHomePose()
{
if (!j0.goHome(0.5)) {
return false;
for (auto& joint : joints) {
if (!joint.goHome(0.5)) {
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;
}
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
const std::array<double, NUM_JOINTS>& pose
) 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);
for (std::size_t i = 0; i < NUM_JOINTS; i++) {
joints[i].stageAngle(pose[i]);
}
}
void Arm::commitPose() noexcept
{
if (j0.isStaged()) {
j0.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();
for (auto& joint : joints) {
if (joint.isStaged()) {
joint.commitStaging();
}
}
}
void Arm::rollbackPose() noexcept
{
if (j0.isStaged()) {
j0.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();
for (auto& joint : joints) {
if (joint.isStaged()) {
joint.rollbackStaging();
}
}
}
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
{
// 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
for (const AxisAlignedBoundingBox& aabb : deadzones) {
@@ -186,30 +94,14 @@ bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) c
bool Arm::rollbackJointsBasedOnValidRange() noexcept
{
bool allGood = true;
if (!j0.isPositionInRange(j0.getTargetPosition())) {
j0.rollbackStaging();
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;
for (auto& joint : joints) {
if (!joint.isPositionInRange(joint.getTargetPosition())) {
joint.rollbackStaging();
allGood = false;
}
}
return allGood;
}
@@ -220,23 +112,10 @@ Arm::~Arm()
bool Arm::assumeHomePoseImmediate()
{
if (!j0.goHomeImmediate()) {
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;
for (auto& joint : joints) {
if (!joint.goHomeImmediate()) {
return false;
}
}
return true;
@@ -244,10 +123,7 @@ bool Arm::assumeHomePoseImmediate()
void Arm::update(double frametime)
{
j0.update(frametime);
j1.update(frametime);
j2.update(frametime);
j3.update(frametime);
j4.update(frametime);
j5.update(frametime);
for (auto& joint : joints) {
joint.update(frametime);
}
}
MotionCore/Arm.h
+7 -21
View File
@@ -3,6 +3,9 @@
#include "AxisAlignedBoundingBox.h"
#include <string>
#include <span>
#include <array>
constexpr std::size_t NUM_JOINTS = 6;
class Arm
{
@@ -15,14 +18,7 @@ public:
bool assumeHomePoseImmediate();
bool assumeHomePose();
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 stagePose(const std::array<double, NUM_JOINTS>& pose) noexcept;
void commitPose() noexcept;
void rollbackPose() 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.
bool rollbackJointsBasedOnValidRange() noexcept;
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; };
const std::array<ArmSegment, NUM_JOINTS>& getJoints() const { return joints; };
const ArmSegment& getEndEffectorJoint() const { return joints.back(); };
void update(double frametime);
private:
OSerialBus oSerialBus;
ArmSegment j0;
ArmSegment j1;
ArmSegment j2;
ArmSegment j3;
ArmSegment j4;
ArmSegment j5;
std::array<ArmSegment, NUM_JOINTS> joints;
};
MotionCore/ArmSegment.cpp
+5
View File
@@ -140,6 +140,11 @@ bool ArmSegment::rollbackStaging() noexcept
return true;
}
void ArmSegment::setParent(ArmSegment* parent) noexcept
{
this->parent = parent;
}
void ArmSegment::update(const double frametime)
{
motor.update(frametime);
MotionCore/ArmSegment.h
+1
View File
@@ -46,6 +46,7 @@ public:
bool commitStaging() noexcept;
// Returns false if not currently staged
bool rollbackStaging() noexcept;
void setParent(ArmSegment* parent) noexcept;
bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); };
double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); };
MotionCore/MainLoop.cpp
+10 -9
View File
@@ -5,6 +5,7 @@
#include <algorithm>
#include <numbers>
#include "StateMachine.h"
#include "ArmSegment.h"
#include "yz_plotter_win.h"
MainLoop::MainLoop() noexcept :
@@ -86,12 +87,12 @@ 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());
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);
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();
@@ -99,19 +100,19 @@ void MainLoop::run()
//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();
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.getJ5().getCurrentGlobalEndpoint();
const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint();
//std::cout << ef << std::endl;
//std::cout << arm.getJ3().getCurrentPosition() << std::endl;
plot.draw(ef);