diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 7b8c4e0..dbd7f87 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -76,15 +76,16 @@ bool Arm::isMoving() const noexcept return false; } -bool Arm::isEFPositionValid(std::span deadzones) const noexcept +bool Arm::checkJointCollisions(std::span deadzones) const noexcept { - // Otherwise: check for endeffector position - const Vector3d ef = joints.back().getTargetedGlobalEndpoint(); - + // TODO: Perform this check within the FK loop to prevent redundant operations // If any deadzone intersects with the EF pos, fai for (const AxisAlignedBoundingBox& aabb : deadzones) { - if (aabb.doesIntersect(ef)) { - return false; + + for (auto& joint : joints) { + if (aabb.doesIntersect(joint.getTargetedGlobalEndpoint())) { + return false; + } } } diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h index b55fab8..5d195b4 100755 --- a/MotionCore/Arm.h +++ b/MotionCore/Arm.h @@ -23,12 +23,13 @@ public: void rollbackPose() noexcept; bool isMoving() const noexcept; - // Will test if the end effectors current target (or staged) position is not within a collider - bool isEFPositionValid(std::span deadzones) const noexcept; + // Will test if the joints target position is not within a collider + bool checkJointCollisions(std::span deadzones) const noexcept; // Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made. bool rollbackJointsBasedOnValidRange() noexcept; const std::array& getJoints() const { return joints; }; + std::array& getJoints() { return joints; }; const ArmSegment& getEndEffectorJoint() const { return joints.back(); }; void update(double frametime); diff --git a/MotionCore/Scene_JointSpaceControl.cpp b/MotionCore/Scene_JointSpaceControl.cpp index bf07b92..8bea35a 100755 --- a/MotionCore/Scene_JointSpaceControl.cpp +++ b/MotionCore/Scene_JointSpaceControl.cpp @@ -43,7 +43,7 @@ void Scene_JointSpaceControl::update(double frametime) // Stage the new pose and keep it if it is good arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos }); - if (arm.isEFPositionValid(deadzones)) { + if (arm.checkJointCollisions(deadzones)) { // Roll back individual joints if their angles are out of range arm.rollbackJointsBasedOnValidRange();