From 531eb6854669f586be290f86d5fe29166fca4675 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Thu, 5 Sep 2024 14:38:24 +0200 Subject: [PATCH 1/8] Add angular constraints functionality This reverts commit b5d14bd8b15d864ad841e0eb7a692d3027b4215a. --- .clang-format | 10 ++ src/FABRIK2D.cpp | 277 +++++++++++++++++++++++++------------------- src/FABRIK2D.h | 129 ++++++++++++--------- test/unit_tests.cpp | 205 ++++++++++++++------------------ 4 files changed, 327 insertions(+), 294 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5e14bdc --- /dev/null +++ b/.clang-format @@ -0,0 +1,10 @@ +BasedOnStyle: Google +AccessModifierOffset: -3 +AllowShortCaseLabelsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +ColumnLimit: 100 +IndentWidth: 4 +Language: Cpp +# Force pointers to the type for C++. +DerivePointerAlignment: false +PointerAlignment: Left \ No newline at end of file diff --git a/src/FABRIK2D.cpp b/src/FABRIK2D.cpp index b6ab829..065cdf1 100644 --- a/src/FABRIK2D.cpp +++ b/src/FABRIK2D.cpp @@ -24,36 +24,53 @@ * **********************************************************************************************/ +#include "FABRIK2D.h" #include "Arduino.h" -#include "FABRIK2D.h" // Defined epsilon value which is considered as 0 #define EPSILON_VALUE 0.001 - Fabrik2D::Fabrik2D(int numJoints, int lengths[], float tolerance) { - begin(numJoints, lengths, tolerance); + this->_numJoints = numJoints; + _createChain(lengths); + + this->_tolerance = tolerance; +} + +Fabrik2D::Fabrik2D(int numJoints, int lengths[], AngularConstraint angular_constraints[], + float tolerance) { + this->_numJoints = numJoints; + _createChain(lengths, angular_constraints); + + this->_tolerance = tolerance; } Fabrik2D::~Fabrik2D() { _deleteChain(); } -void Fabrik2D::begin(int numJoints, int lengths[], float tolerance) { - this->_numJoints = numJoints; - _createChain(lengths); +void Fabrik2D::_createChain(int* lengths) { + Chain* chain = new Chain(); + chain->joints = new Joint[this->_numJoints]; - this->_tolerance = tolerance; + this->_chain = chain; + + _resetChain(lengths); } -void Fabrik2D::_createChain(int lengths[]) { +void Fabrik2D::_createChain(int* lengths, AngularConstraint* angular_constraints) { Chain* chain = new Chain(); chain->joints = new Joint[this->_numJoints]; this->_chain = chain; _resetChain(lengths); + + for (int i = 1; i < this->_numJoints - 1; i++) { + this->_chain->joints[i].constraint.min_angle = angular_constraints[i].min_angle; + this->_chain->joints[i].constraint.max_angle = angular_constraints[i].max_angle; + } } void Fabrik2D::_resetChain(int lengths[]) { @@ -63,7 +80,7 @@ void Fabrik2D::_resetChain(int lengths[]) { int sumLengths = 0; for (int i = 1; i < this->_numJoints; i++) { - sumLengths = sumLengths + lengths[i-1]; + sumLengths = sumLengths + lengths[i - 1]; this->_chain->joints[i].x = 0; this->_chain->joints[i].y = sumLengths; this->_chain->joints[i].angle = 0; @@ -87,11 +104,11 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { _num_iterations = 0; // Used for debugging // Distance between root and target (root is always 0,0) - int origin_to_target = sqrt(x*x+y*y); + int origin_to_target = sqrt(x * x + y * y); // Total length of chain int totalLength = 0; - for (int i = 0; i < this->_numJoints-1; i++) { + for (int i = 0; i < this->_numJoints - 1; i++) { totalLength = totalLength + lengths[i]; } @@ -99,23 +116,23 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { if (origin_to_target > totalLength) { // The target is unreachable - for (int i = 0; i < this->_numJoints-1; i++) { + for (int i = 0; i < this->_numJoints - 1; i++) { // Find the distance r_i between the target (x,y) and the // joint i position (jx,jy) - float jx = this->_chain->joints[i].x; - float jy = this->_chain->joints[i].y; + Joint const& current_joint{this->_chain->joints[i]}; + float jx = current_joint.x; + float jy = current_joint.y; float r_i = _distance(jx, jy, x, y); - float lambda_i = static_cast(lengths[i])/r_i; + float lambda_i = static_cast(lengths[i]) / r_i; // Find the new joint positions - this->_chain->joints[i+1].x = static_cast( - (1-lambda_i)*jx + lambda_i*x); - this->_chain->joints[i+1].y = static_cast( - (1-lambda_i)*jy + lambda_i*y); + Joint& next_joint{this->_chain->joints[i + 1]}; + next_joint.x = static_cast((1 - lambda_i) * jx + lambda_i * x); + next_joint.y = static_cast((1 - lambda_i) * jy + lambda_i * y); } - _resetChain(lengths); - return 0; + _resetChain(lengths); + return 0; } else { // The target is reachable; this, set as (bx,by) the initial // position of the joint i @@ -124,8 +141,8 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { // Check whether the distance between the end effector // joint n (ex,ey) and the target is greater than a tolerance - float ex = this->_chain->joints[this->_numJoints-1].x; - float ey = this->_chain->joints[this->_numJoints-1].y; + float ex = this->_chain->joints[this->_numJoints - 1].x; + float ey = this->_chain->joints[this->_numJoints - 1].y; float dist = _distance(ex, ey, x, y); float prevDist = 0; @@ -140,7 +157,7 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { tolerance *= 1.1; // If increased tolerance is higher than 2x the desired // tolerance report failed to converge - if (tolerance > this->_tolerance*2) { + if (tolerance > this->_tolerance * 2) { _resetChain(lengths); return 0; } @@ -150,24 +167,22 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { // STAGE 1: FORWARD REACHING // Set the end effector as target - this->_chain->joints[this->_numJoints-1].x = x; - this->_chain->joints[this->_numJoints-1].y = y; + this->_chain->joints[this->_numJoints - 1].x = x; + this->_chain->joints[this->_numJoints - 1].y = y; - for (int i = this->_numJoints-2; i >= 0; i--) { + for (int i = this->_numJoints - 2; i >= 0; i--) { // Find the distance r_i between the new joint position // i+1 (nx,ny) and the joint i (jx,jy) float jx = this->_chain->joints[i].x; float jy = this->_chain->joints[i].y; - float nx = this->_chain->joints[i+1].x; - float ny = this->_chain->joints[i+1].y; + float nx = this->_chain->joints[i + 1].x; + float ny = this->_chain->joints[i + 1].y; float r_i = _distance(jx, jy, nx, ny); - float lambda_i = static_cast(lengths[i])/r_i; + float lambda_i = static_cast(lengths[i]) / r_i; // Find the new joint positions - this->_chain->joints[i].x = static_cast( - (1-lambda_i)*nx + lambda_i*jx); - this->_chain->joints[i].y = static_cast( - (1-lambda_i)*ny + lambda_i*jy); + this->_chain->joints[i].x = static_cast((1 - lambda_i) * nx + lambda_i * jx); + this->_chain->joints[i].y = static_cast((1 - lambda_i) * ny + lambda_i * jy); } // STAGE 2: BACKWARD REACHING @@ -175,44 +190,48 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { this->_chain->joints[0].x = bx; this->_chain->joints[0].y = by; - for (int i = 0; i < this->_numJoints-1; i++) { + for (int i = 0; i < this->_numJoints - 1; i++) { // Find the distance r_i between the new joint position // i (nx,ny) and the joint i+1 (jx,jy) - float jx = this->_chain->joints[i+1].x; - float jy = this->_chain->joints[i+1].y; + float jx = this->_chain->joints[i + 1].x; + float jy = this->_chain->joints[i + 1].y; float nx = this->_chain->joints[i].x; float ny = this->_chain->joints[i].y; float r_i = _distance(jx, jy, nx, ny); - float lambda_i = static_cast(lengths[i])/r_i; + float lambda_i = static_cast(lengths[i]) / r_i; // Find the new joint positions - this->_chain->joints[i+1].x = static_cast( - (1-lambda_i)*nx + lambda_i*jx); - this->_chain->joints[i+1].y = static_cast( - (1-lambda_i)*ny + lambda_i*jy); + this->_chain->joints[i + 1].x = + static_cast((1 - lambda_i) * nx + lambda_i * jx); + this->_chain->joints[i + 1].y = + static_cast((1 - lambda_i) * ny + lambda_i * jy); + + if (i > 0) { + _applyAngularConstraints(this->_chain->joints[i - 1], this->_chain->joints[i], + this->_chain->joints[i + 1]); + } } // Update distance between end effector and target - ex = this->_chain->joints[this->_numJoints-1].x; - ey = this->_chain->joints[this->_numJoints-1].y; + ex = this->_chain->joints[this->_numJoints - 1].x; + ey = this->_chain->joints[this->_numJoints - 1].y; dist = _distance(ex, ey, x, y); } } - this->_chain->joints[0].angle = atan2( - this->_chain->joints[1].y, this->_chain->joints[1].x); + this->_chain->joints[0].angle = atan2(this->_chain->joints[1].y, this->_chain->joints[1].x); float prevAngle = this->_chain->joints[0].angle; - for (int i = 2; i <= this->_numJoints-1; i++) { - float ax = this->_chain->joints[i-1].x; - float ay = this->_chain->joints[i-1].y; + for (int i = 2; i <= this->_numJoints - 1; i++) { + float ax = this->_chain->joints[i - 1].x; + float ay = this->_chain->joints[i - 1].y; float bx = this->_chain->joints[i].x; float by = this->_chain->joints[i].y; - float aAngle = atan2(by-ay, bx-ax); + float aAngle = atan2(by - ay, bx - ax); - this->_chain->joints[i-1].angle = aAngle - prevAngle; + this->_chain->joints[i - 1].angle = aAngle - prevAngle; prevAngle = aAngle; } @@ -220,12 +239,8 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { return result_status; } -uint8_t Fabrik2D::solve2( - float x, float y, float z, - float toolAngle, - float grippingOffset, - int lengths[] -) { +uint8_t Fabrik2D::solve2(float x, float y, float z, float toolAngle, float grippingOffset, + int lengths[]) { uint8_t result_status = 0; if (this->_numJoints >= 4) { @@ -234,15 +249,13 @@ uint8_t Fabrik2D::solve2( // Find wrist center by moving from the desired position with // tool angle and link length - float oc_r = r - ( - lengths[this->_numJoints-2]+grippingOffset)*cos(toolAngle); + float oc_r = r - (lengths[this->_numJoints - 2] + grippingOffset) * cos(toolAngle); - float oc_y = y - ( - lengths[this->_numJoints-2]+grippingOffset)*sin(toolAngle); + float oc_y = y - (lengths[this->_numJoints - 2] + grippingOffset) * sin(toolAngle); // We solve IK from first joint to wrist center int tmp = this->_numJoints; - this->_numJoints = this->_numJoints-1; + this->_numJoints = this->_numJoints - 1; result_status = solve(oc_r, oc_y, lengths); @@ -250,44 +263,42 @@ uint8_t Fabrik2D::solve2( if (result_status == 1) { // Update the end effector position to preserve tool angle - this->_chain->joints[this->_numJoints-1].x = - this->_chain->joints[this->_numJoints-2].x - + lengths[this->_numJoints-2]*cos(toolAngle); + this->_chain->joints[this->_numJoints - 1].x = + this->_chain->joints[this->_numJoints - 2].x + + lengths[this->_numJoints - 2] * cos(toolAngle); - this->_chain->joints[this->_numJoints-1].y = - this->_chain->joints[this->_numJoints-2].y - + lengths[this->_numJoints-2]*sin(toolAngle); + this->_chain->joints[this->_numJoints - 1].y = + this->_chain->joints[this->_numJoints - 2].y + + lengths[this->_numJoints - 2] * sin(toolAngle); // Update angle of last joint - this->_chain->joints[0].angle = atan2( - this->_chain->joints[1].y, - this->_chain->joints[1].x); + this->_chain->joints[0].angle = + atan2(this->_chain->joints[1].y, this->_chain->joints[1].x); float prevAngle = this->_chain->joints[0].angle; - for (int i = 2; i <= this->_numJoints-1; i++) { - float ax = this->_chain->joints[i-1].x; - float ay = this->_chain->joints[i-1].y; + for (int i = 2; i <= this->_numJoints - 1; i++) { + float ax = this->_chain->joints[i - 1].x; + float ay = this->_chain->joints[i - 1].y; float bx = this->_chain->joints[i].x; float by = this->_chain->joints[i].y; - float aAngle = atan2(by-ay, bx-ax); + float aAngle = atan2(by - ay, bx - ax); - this->_chain->joints[i-1].angle = aAngle - prevAngle; + this->_chain->joints[i - 1].angle = aAngle - prevAngle; prevAngle = aAngle; } // Save tool angle - this->_chain->joints[this->_numJoints-1].angle = toolAngle; + this->_chain->joints[this->_numJoints - 1].angle = toolAngle; // Save base angle this->_chain->z = z; this->_chain->angle = atan2(z, x); // Update joint X values based on base rotation - for (int i = 0; i <= this->_numJoints-1; i++) { - this->_chain->joints[i].x = - this->_chain->joints[i].x * cos(-this->_chain->angle); + for (int i = 0; i <= this->_numJoints - 1; i++) { + this->_chain->joints[i].x = this->_chain->joints[i].x * cos(-this->_chain->angle); } } } @@ -299,60 +310,50 @@ uint8_t Fabrik2D::solve(float x, float y, float toolAngle, int lengths[]) { return solve2(x, y, 0, toolAngle, 0, lengths); } -uint8_t Fabrik2D::solve( - float x, float y, - float toolAngle, - float grippingOffset, - int lengths[] -) { +uint8_t Fabrik2D::solve(float x, float y, float toolAngle, float grippingOffset, int lengths[]) { return solve2(x, y, 0, toolAngle, grippingOffset, lengths); } uint8_t Fabrik2D::solve2(float x, float y, float z, int lengths[]) { float r = _distance(0, 0, x, z); - uint8_t result_status = solve(r, y, lengths); + uint8_t result_status = solve(r, y, lengths); if (result_status == 1) { this->_chain->z = z; this->_chain->angle = atan2(z, x); // Update joint X values based on base rotation - for (int i = 0; i <= this->_numJoints-1; i++) { - this->_chain->joints[i].x = - this->_chain->joints[i].x * cos(-this->_chain->angle); + for (int i = 0; i <= this->_numJoints - 1; i++) { + this->_chain->joints[i].x = this->_chain->joints[i].x * cos(-this->_chain->angle); } } return result_status; } -uint8_t Fabrik2D::solve2( - float x, float y, float z, - float toolAngle, - int lengths[] -) { +uint8_t Fabrik2D::solve2(float x, float y, float z, float toolAngle, int lengths[]) { return solve2(x, y, z, toolAngle, 0, lengths); } float Fabrik2D::getX(int joint) { - if (joint >= 0 && joint < this->_numJoints) { - return this->_chain->joints[joint].x; - } - return 0; + if (joint >= 0 && joint < this->_numJoints) { + return this->_chain->joints[joint].x; + } + return 0; } float Fabrik2D::getY(int joint) { - if (joint >= 0 && joint < this->_numJoints) { - return this->_chain->joints[joint].y; - } - return 0; + if (joint >= 0 && joint < this->_numJoints) { + return this->_chain->joints[joint].y; + } + return 0; } float Fabrik2D::getAngle(int joint) { - if (joint >= 0 && joint < this->_numJoints) { - return this->_chain->joints[joint].angle; - } - return 0; + if (joint >= 0 && joint < this->_numJoints) { + return this->_chain->joints[joint].angle; + } + return 0; } float Fabrik2D::getZ(int joint) { @@ -369,12 +370,10 @@ void Fabrik2D::setBaseAngle(float baseAngle) { if (this->_numJoints >= 4) { // Update end effector Z value based on base rotation - this->_chain->z = - this->_chain->joints[this->_numJoints-1].x * sin(angle_diff); + this->_chain->z = this->_chain->joints[this->_numJoints - 1].x * sin(angle_diff); // Update joint X values based on base rotation - for (int i = 0; i <= this->_numJoints-1; i++) { - this->_chain->joints[i].x = - this->_chain->joints[i].x * cos(angle_diff); + for (int i = 0; i <= this->_numJoints - 1; i++) { + this->_chain->joints[i].x = this->_chain->joints[i].x * cos(angle_diff); } } } @@ -393,9 +392,9 @@ void Fabrik2D::setJoints(float angles[], int lengths[]) { float accY = 0; this->_chain->joints[0].angle = angles[0]; - for (int i = 1; i < this->_numJoints-1; i++) { - this->_chain->joints[i].x = accX + lengths[i-1]*cos(accAng); - this->_chain->joints[i].y = accY + lengths[i-1]*sin(accAng); + for (int i = 1; i < this->_numJoints - 1; i++) { + this->_chain->joints[i].x = accX + lengths[i - 1] * cos(accAng); + this->_chain->joints[i].y = accY + lengths[i - 1] * sin(accAng); this->_chain->joints[i].angle = angles[i]; accAng += angles[i]; accX = this->_chain->joints[i].x; @@ -403,11 +402,11 @@ void Fabrik2D::setJoints(float angles[], int lengths[]) { } // Update end effector x and y - this->_chain->joints[this->_numJoints-1].x = - accX + lengths[this->_numJoints-2]*cos(accAng); - this->_chain->joints[this->_numJoints-1].y = - accY + lengths[this->_numJoints-2]*sin(accAng); - this->_chain->joints[this->_numJoints-1].angle = 0; + this->_chain->joints[this->_numJoints - 1].x = + accX + lengths[this->_numJoints - 2] * cos(accAng); + this->_chain->joints[this->_numJoints - 1].y = + accY + lengths[this->_numJoints - 2] * sin(accAng); + this->_chain->joints[this->_numJoints - 1].angle = 0; } int Fabrik2D::numJoints() { @@ -421,5 +420,41 @@ Fabrik2D::Chain* Fabrik2D::getChain() { float Fabrik2D::_distance(float x1, float y1, float x2, float y2) { float xDiff = x2 - x1; float yDiff = y2 - y1; - return sqrt(xDiff*xDiff + yDiff*yDiff); + return sqrt(xDiff * xDiff + yDiff * yDiff); +} + +float Fabrik2D::_angleBetween(float x1, float y1, float x2, float y2) { + float dot = x1 * x2 + y1 * y2; + float mag1 = _distance(0.0, 0.0, x1, y1); + float mag2 = _distance(0.0, 0.0, x2, y2); + return acos(dot / (mag1 * mag2)); +} + +void Fabrik2D::_applyAngularConstraints(Joint const& parent_joint, Joint const& joint, + Joint& next_joint) { + float px = joint.x - parent_joint.x; + float py = joint.y - parent_joint.y; + float nx = next_joint.x - joint.x; + float ny = next_joint.y - joint.y; + + float current_angle = _angleBetween(px, py, nx, ny); + + // Clamp the angle between the joint's limits + float clamped_angle = + constrain(current_angle, joint.constraint.min_angle, joint.constraint.max_angle); + + // Adjust the joint position based on the clamped angle + if (current_angle != clamped_angle) { + float angle_diff = clamped_angle - current_angle; + + // Rotate the next joint around the current joint by the angle difference + float cos_angle = cos(angle_diff); + float sin_angle = sin(angle_diff); + + float new_x = nx.x * cos_angle - nx.y * sin_angle; + float new_y = nx.x * sin_angle + nx.y * cos_angle; + + next_joint.x = joint.x + new_x; + next_joint.y = joint.y + new_y; + } } diff --git a/src/FABRIK2D.h b/src/FABRIK2D.h index ae37dbe..3372bc1 100644 --- a/src/FABRIK2D.h +++ b/src/FABRIK2D.h @@ -24,7 +24,6 @@ * **********************************************************************************************/ - #ifndef SRC_FABRIK2D_H_ #define SRC_FABRIK2D_H_ @@ -32,51 +31,64 @@ class Fabrik2D { public: - // Joint struct - typedef struct { - float x = 0; // x position of joint relative to origin - float y = 0; // y position of joint relative to origin - float angle = 0; // angle of joint - } Joint; - - // Chain struct - typedef struct { - Joint* joints = nullptr; // list of joints - float z = 0; // z position defining the chain offset from the plane - float angle = 0; // base (plane) rotation - } Chain; - - /* Fabrik2D() - * - * Default constructor. Call begin(numJoints, lengths) to initialize - */ - Fabrik2D() {} + /// An angular constraint + struct AngularConstraint { + /// The minimum angle of in radians + float min_angle; + /// The maximum angle in radians + float max_angle; + }; + + /// Joint struct + struct Joint { + /// x position of joint relative to origin + float x = 0.0; + /// y position of joint relative to origin + float y = 0.0; + /// angle of joint relative to parent arm in radians + float angle = 0.0; + /// The angular constraint of the joint + AngularConstraint constraint = {-2.0 * M_PI, 2.0 * M_PI}; + }; + + /// Chain struct + struct Chain { + /// list of joints + Joint* joints = nullptr; + /// z position defining the chain offset from the plane + float z = 0.0; + /// base (plane) rotation in radians + float angle = 0.0; + }; /* Fabrik2D(numJoints, lengths, tolerance) * inputs: numJoints, lengths, (optional) tolerance * - * Calls begin(numJoints, lengths, tolerance) + * Initializes the library + * creates the chain to be used for the inverse kinematics solver * * tolerance is optional, will be set to 10 by default */ Fabrik2D(int numJoints, int lengths[], float tolerance = 10); - /* Fabrik2D destructor */ - ~Fabrik2D(); - - /* - * begin(numJoints, lengths, tolerance) - * inputs: numJoints, lengths, (optional) tolerance - * + /* Fabrik2D(numJoints, lengths, angular_constraints, tolerance) + * inputs: numJoints, lengths, angular_constraints, (optional) tolerance + * * Initializes the library * creates the chain to be used for the inverse kinematics solver - * + * + * Also takes in angular constraints for each joint + * * tolerance is optional, will be set to 10 by default */ - void begin(int numJoints, int lengths[], float tolerance = 10); + Fabrik2D(int numJoints, int lengths[], AngularConstraint angular_constraints[], + float tolerance = 10); + + /* Fabrik2D destructor */ + ~Fabrik2D(); /* solve(x, y, lengths) - * + * * Inputs: x and y positions of target, lengths between each joint * Returns: * 0 if FABRIK could not converge @@ -84,7 +96,7 @@ class Fabrik2D { * 2 if FABRIK converged with a higher tolerance value * * solves the inverse kinematics of the stored chain to reach the target - * + * * Returns: * 0 if FABRIK could not converge * 1 if FABRIK converged to the set threshold @@ -93,7 +105,7 @@ class Fabrik2D { uint8_t solve(float x, float y, int lengths[]); /* solve(x, y, angle, lengths) - * + * * Inputs: x and y positions of target, desired tool angle and lengths * between each joint * Returns: @@ -111,7 +123,7 @@ class Fabrik2D { uint8_t solve(float x, float y, float toolAngle, int lengths[]); /* solve(x, y, angle, offset, lengths) - * + * * Inputs: x and y positions of target, desired tool angle and lengths * between each joint * Returns: @@ -122,19 +134,15 @@ class Fabrik2D { * !!! tool angle is in radians !!! * * solves the inverse kinematics of the stored chain to reach the target - * with tool angle + * with tool angle * and gripping offset * * will only work for 3DOF */ - uint8_t solve( - float x, float y, - float toolAngle, - float grippingOffset, - int lengths[]); + uint8_t solve(float x, float y, float toolAngle, float grippingOffset, int lengths[]); /* solve2(x, y, z, lengths) - * + * * Inputs: x, y and z positions of target, desired tool angle and lengths * between each joint * Returns: @@ -157,7 +165,7 @@ class Fabrik2D { uint8_t solve2(float x, float y, float z, int lengths[]); /* solve2(x, y, z, toolAngle, lengths) - * + * * Inputs: x, y and z positions of target, desired tool angle and lengths * between each joint * Returns: @@ -177,12 +185,12 @@ class Fabrik2D { * from the plane * * will only work for 4DOF, i.e. 4 joints or more and a rotational base - * + * */ uint8_t solve2(float x, float y, float z, float toolAngle, int lengths[]); /* solve2(x, y, z, angle, offset, lengths) - * + * * Inputs: x, y and z positions of target, desired tool angle, gripping offset and lengths between each joint * Returns: @@ -192,7 +200,7 @@ class Fabrik2D { * * !!! tool angle is in radians !!! * - * solves the inverse kinematics of the stored chain to reach the target + * solves the inverse kinematics of the stored chain to reach the target * with tool angle and gripping offset introducing the z-axis, which * allows a rotational base of the manipulator * @@ -203,11 +211,7 @@ class Fabrik2D { * * will only work for 4DOF, i.e. 4 joints or more and a rotational base */ - uint8_t solve2( - float x, float y, float z, - float toolAngle, - float grippingOffset, - int lengths[]); + uint8_t solve2(float x, float y, float z, float toolAngle, float grippingOffset, int lengths[]); /* getX(joint) * inputs: joint number @@ -224,7 +228,7 @@ class Fabrik2D { /* getZ(joint) * inputs: (optional) joint number * outputs: z offset of the chain from the plane - * + * * Passing the joint argument will not do anything. * It is just there for consistency with getX and getY. */ @@ -253,7 +257,7 @@ class Fabrik2D { /* setTolerance(tolerance) * inputs: tolerance value - * + * * sets the tolerance of the distance between the end effector * and the target */ @@ -264,7 +268,7 @@ class Fabrik2D { * between each joint * * Angles must be equal to the number of joints - 1 - * + * * manually sets the joint angles and updates their position using * forward kinematics */ @@ -295,6 +299,16 @@ class Fabrik2D { */ void _createChain(int* lengths); + /* _createChain(lengths) + * inputs: lengths + * + * Creates a new chain and attaches it to the Fabrik2D object + * + * length size should always be one lesser than the number of joints + * angular_constraints size should always be one lesser than the number of joints + */ + void _createChain(int* lengths, AngularConstraint* angular_constraints); + /* _resetChain(lengths) * inputs: lengths * @@ -311,13 +325,20 @@ class Fabrik2D { */ void _deleteChain(); - /* distance(x1,y1,x2,y2) + /* _distance(x1,y1,x2,y2) * inputs: coordinates * outputs: distance between points * * Uses euclidean distance */ float _distance(float x1, float y1, float x2, float y2); + + /* _applyAngularConstraints(parent_joint, joint, next_joint) + * inputs: parent_joint, joint + * + * Applies angular constraints to a joint, based on the position of parent_joint + */ + void _applyAngularConstraints(Joint const& parent_joint, Joint const& joint, Joint& next_joint); }; #endif // SRC_FABRIK2D_H_ diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index d597103..f696e72 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -29,123 +29,93 @@ // assertNAN(arg); // isnan(a) // assertNotNAN(arg); // !isnan(a) - #include #include "Arduino.h" #include "FABRIK2D.h" - -unittest_setup() -{ -} - - -unittest_teardown() -{ +unittest_setup() {} + +unittest_teardown() {} + +unittest(test_constructor) { + int lengths_3_joints[] = {190, 200}; + int lengths_4_joints[] = {190, 200, 270}; + + // Test 4 joints init + fprintf(stderr, "Test 4 joints init\n"); + Fabrik2D fabrik2D_4(4, lengths_4_joints); + assertEqual(4, fabrik2D_4.numJoints()); + assertEqual(10, fabrik2D_4.getTolerance()); + assertEqual(0, fabrik2D_4.getChain()->angle); + assertEqual(0, fabrik2D_4.getChain()->z); + assertEqual(0, fabrik2D_4.getChain()->joints[0].x); + assertEqual(0, fabrik2D_4.getChain()->joints[0].y); + assertEqual(0, fabrik2D_4.getChain()->joints[0].angle); + assertEqual(0, fabrik2D_4.getChain()->joints[1].x); + assertEqual(190, fabrik2D_4.getChain()->joints[1].y); + assertEqual(0, fabrik2D_4.getChain()->joints[1].angle); + assertEqual(0, fabrik2D_4.getChain()->joints[2].x); + assertEqual(190 + 200, fabrik2D_4.getChain()->joints[2].y); + assertEqual(0, fabrik2D_4.getChain()->joints[2].angle); + assertEqual(0, fabrik2D_4.getChain()->joints[3].x); + assertEqual(190 + 200 + 270, fabrik2D_4.getChain()->joints[3].y); + assertEqual(0, fabrik2D_4.getChain()->joints[3].angle); + + // Test 3 joints init + fprintf(stderr, "Test 3 joints init\n"); + Fabrik2D fabrik2D_3(3, lengths_3_joints); + assertEqual(3, fabrik2D_3.numJoints()); + assertEqual(10, fabrik2D_3.getTolerance()); + assertEqual(0, fabrik2D_3.getChain()->angle); + assertEqual(0, fabrik2D_3.getChain()->z); + assertEqual(0, fabrik2D_3.getChain()->joints[0].x); + assertEqual(0, fabrik2D_3.getChain()->joints[0].y); + assertEqual(0, fabrik2D_3.getChain()->joints[0].angle); + assertEqual(0, fabrik2D_3.getChain()->joints[1].x); + assertEqual(190, fabrik2D_3.getChain()->joints[1].y); + assertEqual(0, fabrik2D_3.getChain()->joints[1].angle); + assertEqual(0, fabrik2D_3.getChain()->joints[2].x); + assertEqual(190 + 200, fabrik2D_3.getChain()->joints[2].y); + assertEqual(0, fabrik2D_3.getChain()->joints[2].angle); + + // Test different tolerance + fprintf(stderr, "Test different tolerance\n"); + Fabrik2D fabrik2D_4_tolerance_1(4, lengths_4_joints, 1); + assertEqual(1, fabrik2D_4_tolerance_1.getTolerance()); + fabrik2D_4_tolerance_1.setTolerance(20); + assertEqual(20, fabrik2D_4_tolerance_1.getTolerance()); } - -unittest(test_constructor) -{ - int lengths_3_joints[] = {190, 200}; - int lengths_4_joints[] = {190, 200, 270}; - - // Test 4 joints init - fprintf(stderr, "Test 4 joints init\n"); - Fabrik2D fabrik2D_4(4, lengths_4_joints); - assertEqual(4, fabrik2D_4.numJoints()); - assertEqual(10, fabrik2D_4.getTolerance()); - assertEqual(0, fabrik2D_4.getChain()->angle); - assertEqual(0, fabrik2D_4.getChain()->z); - assertEqual(0, fabrik2D_4.getChain()->joints[0].x); - assertEqual(0, fabrik2D_4.getChain()->joints[0].y); - assertEqual(0, fabrik2D_4.getChain()->joints[0].angle); - assertEqual(0, fabrik2D_4.getChain()->joints[1].x); - assertEqual(190, fabrik2D_4.getChain()->joints[1].y); - assertEqual(0, fabrik2D_4.getChain()->joints[1].angle); - assertEqual(0, fabrik2D_4.getChain()->joints[2].x); - assertEqual(190+200, fabrik2D_4.getChain()->joints[2].y); - assertEqual(0, fabrik2D_4.getChain()->joints[2].angle); - assertEqual(0, fabrik2D_4.getChain()->joints[3].x); - assertEqual(190+200+270, fabrik2D_4.getChain()->joints[3].y); - assertEqual(0, fabrik2D_4.getChain()->joints[3].angle); - - // Test 3 joints init - fprintf(stderr, "Test 3 joints init\n"); - Fabrik2D fabrik2D_3(3, lengths_3_joints); - assertEqual(3, fabrik2D_3.numJoints()); - assertEqual(10, fabrik2D_3.getTolerance()); - assertEqual(0, fabrik2D_3.getChain()->angle); - assertEqual(0, fabrik2D_3.getChain()->z); - assertEqual(0, fabrik2D_3.getChain()->joints[0].x); - assertEqual(0, fabrik2D_3.getChain()->joints[0].y); - assertEqual(0, fabrik2D_3.getChain()->joints[0].angle); - assertEqual(0, fabrik2D_3.getChain()->joints[1].x); - assertEqual(190, fabrik2D_3.getChain()->joints[1].y); - assertEqual(0, fabrik2D_3.getChain()->joints[1].angle); - assertEqual(0, fabrik2D_3.getChain()->joints[2].x); - assertEqual(190+200, fabrik2D_3.getChain()->joints[2].y); - assertEqual(0, fabrik2D_3.getChain()->joints[2].angle); - - // Test different tolerance - fprintf(stderr, "Test different tolerance\n"); - Fabrik2D fabrik2D_4_tolerance_1(4, lengths_4_joints, 1); - assertEqual(1, fabrik2D_4_tolerance_1.getTolerance()); - fabrik2D_4_tolerance_1.setTolerance(20); - assertEqual(20, fabrik2D_4_tolerance_1.getTolerance()); - - // Test begin - fprintf(stderr, "Test begin\n"); - Fabrik2D fabrik2D_3_begin_init; - fabrik2D_3_begin_init.begin(3, lengths_3_joints); - assertEqual(3, fabrik2D_3_begin_init.numJoints()); - assertEqual(10, fabrik2D_3_begin_init.getTolerance()); - assertEqual(0, fabrik2D_3_begin_init.getChain()->angle); - assertEqual(0, fabrik2D_3_begin_init.getChain()->z); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[0].x); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[0].y); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[0].angle); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[1].x); - assertEqual(190, fabrik2D_3_begin_init.getChain()->joints[1].y); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[1].angle); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[2].x); - assertEqual(190+200, fabrik2D_3_begin_init.getChain()->joints[2].y); - assertEqual(0, fabrik2D_3_begin_init.getChain()->joints[2].angle); -} - -unittest(test_solve) -{ +unittest(test_solve) { int success = 0; int lengths_3_joints[] = {200, 200}; int lengths_4_joints[] = {200, 200, 200}; - + // Solve 3 joints, 2DOF fprintf(stderr, "Solve 3 joints, 2DOF\n"); Fabrik2D fabrik2D_3_2DOF(3, lengths_3_joints, 1); success = fabrik2D_3_2DOF.solve(100, 100, lengths_3_joints); assertEqual(1, success); - - float dist = sqrt(100*100 + 100*100); - - float c_phi = ( - dist*dist - lengths_3_joints[0]*lengths_3_joints[0] - - lengths_3_joints[1]*lengths_3_joints[1] - ) / (2*lengths_3_joints[0]*lengths_3_joints[1]); - float s_phi = sqrt(1 - c_phi*c_phi); - + + float dist = sqrt(100 * 100 + 100 * 100); + + float c_phi = (dist * dist - lengths_3_joints[0] * lengths_3_joints[0] - + lengths_3_joints[1] * lengths_3_joints[1]) / + (2 * lengths_3_joints[0] * lengths_3_joints[1]); + float s_phi = sqrt(1 - c_phi * c_phi); + // Assign the angles for shoulder and elbow. - float a1 = atan2(100, 100) + atan2( - lengths_3_joints[1]*s_phi, - lengths_3_joints[0]+lengths_3_joints[1]*c_phi); + float a1 = atan2(100, 100) + atan2(lengths_3_joints[1] * s_phi, + lengths_3_joints[0] + lengths_3_joints[1] * c_phi); float a2 = -atan2(s_phi, c_phi); - + assertEqualFloat(a1, fabrik2D_3_2DOF.getAngle(0), fabrik2D_3_2DOF.getTolerance()); assertEqualFloat(a2, fabrik2D_3_2DOF.getAngle(1), fabrik2D_3_2DOF.getTolerance()); - + assertEqualFloat(100, fabrik2D_3_2DOF.getX(2), fabrik2D_3_2DOF.getTolerance()); assertEqualFloat(100, fabrik2D_3_2DOF.getY(2), fabrik2D_3_2DOF.getTolerance()); - + // Test Set Joints float angles[] = {a1, a2}; fprintf(stderr, "Test Set Joints\n"); @@ -169,65 +139,65 @@ unittest(test_solve) Fabrik2D fabrik2D_3_3DOF(4, lengths_4_joints, 10); success = fabrik2D_3_3DOF.solve(50, 50, -HALF_PI, lengths_4_joints); assertEqual(1, success); - + assertEqualFloat(50, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance()); assertEqualFloat(50, fabrik2D_3_3DOF.getY(3), fabrik2D_3_3DOF.getTolerance()); - + assertEqualFloat(50, fabrik2D_3_3DOF.getX(2), fabrik2D_3_3DOF.getTolerance()); assertEqualFloat(250, fabrik2D_3_3DOF.getY(2), fabrik2D_3_3DOF.getTolerance()); - + // Solve 4 joints, 3DOF (tool angle), Gripping offset fprintf(stderr, "Solve 4 joints, 3DOF, Gripping offset\n"); Fabrik2D fabrik2D_3_3DOF_GO(4, lengths_4_joints, 1); success = fabrik2D_3_3DOF_GO.solve(50, 50, -HALF_PI, 10, lengths_4_joints); assertEqual(1, success); - + assertEqualFloat(50, fabrik2D_3_3DOF_GO.getX(3), fabrik2D_3_3DOF_GO.getTolerance()); assertEqualFloat(60, fabrik2D_3_3DOF_GO.getY(3), fabrik2D_3_3DOF_GO.getTolerance()); - + assertEqualFloat(50, fabrik2D_3_3DOF_GO.getX(2), fabrik2D_3_3DOF_GO.getTolerance()); assertEqualFloat(260, fabrik2D_3_3DOF_GO.getY(2), fabrik2D_3_3DOF_GO.getTolerance()); - + // Solve 4 joints, 3DOF fprintf(stderr, "Solve 4 joints, 3DOF\n"); Fabrik2D fabrik2D_4_3DOF(4, lengths_4_joints, 1); success = fabrik2D_4_3DOF.solve2(100, 100, 100, lengths_4_joints); assertEqual(1, success); - + float base_angle = atan2(100, 100); - + assertEqualFloat(base_angle, fabrik2D_4_3DOF.getBaseAngle(), 1e-3); - + assertEqualFloat(100, fabrik2D_4_3DOF.getX(3), fabrik2D_4_3DOF.getTolerance()); assertEqualFloat(100, fabrik2D_4_3DOF.getY(3), fabrik2D_4_3DOF.getTolerance()); assertEqualFloat(100, fabrik2D_4_3DOF.getZ(3), fabrik2D_4_3DOF.getTolerance()); - + // Solve 4 joints, 4DOF fprintf(stderr, "Solve 4 joints, 4DOF\n"); Fabrik2D fabrik2D_4_4DOF(4, lengths_4_joints, 1); success = fabrik2D_4_4DOF.solve2(150, 50, 100, -HALF_PI, lengths_4_joints); assertEqual(1, success); - + assertEqualFloat(150, fabrik2D_4_4DOF.getX(3), fabrik2D_4_4DOF.getTolerance()); assertEqualFloat(50, fabrik2D_4_4DOF.getY(3), fabrik2D_4_4DOF.getTolerance()); assertEqualFloat(100, fabrik2D_4_4DOF.getZ(3), fabrik2D_4_4DOF.getTolerance()); - + assertEqualFloat(150, fabrik2D_4_4DOF.getX(2), fabrik2D_4_4DOF.getTolerance()); assertEqualFloat(250, fabrik2D_4_4DOF.getY(2), fabrik2D_4_4DOF.getTolerance()); - + // Solve 4 joints, 4DOF, Gripping offset fprintf(stderr, "Solve 4 joints, 4DOF, Gripping offset\n"); Fabrik2D fabrik2D_4_4DOF_GO(4, lengths_4_joints, 1); success = fabrik2D_4_4DOF_GO.solve2(150, 50, 0, -HALF_PI, 10, lengths_4_joints); assertEqual(1, success); - + assertEqualFloat(150, fabrik2D_4_4DOF_GO.getX(3), fabrik2D_4_4DOF_GO.getTolerance()); assertEqualFloat(60, fabrik2D_4_4DOF_GO.getY(3), fabrik2D_4_4DOF_GO.getTolerance()); assertEqualFloat(0, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance()); - + assertEqualFloat(150, fabrik2D_4_4DOF_GO.getX(2), fabrik2D_4_4DOF_GO.getTolerance()); assertEqualFloat(260, fabrik2D_4_4DOF_GO.getY(2), fabrik2D_4_4DOF_GO.getTolerance()); - + // Test Set Base Angle fprintf(stderr, "Test Set Base Angle\n"); fprintf(stderr, "Test Set Base Angle %f\n", HALF_PI - fabrik2D_4_4DOF_GO.getChain()->angle); @@ -235,20 +205,18 @@ unittest(test_solve) assertEqualFloat(0, fabrik2D_4_4DOF_GO.getX(3), fabrik2D_4_4DOF_GO.getTolerance()); assertEqualFloat(60, fabrik2D_4_4DOF_GO.getY(3), fabrik2D_4_4DOF_GO.getTolerance()); assertEqualFloat(150, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance()); - } -unittest(test_getters_setters) -{ +unittest(test_getters_setters) { int lengths[] = {200, 200}; Fabrik2D fabrik2D(3, lengths); fabrik2D.setTolerance(20); assertEqual(20, fabrik2D.getTolerance()); - + fabrik2D.setBaseAngle(HALF_PI); assertEqualFloat(HALF_PI, fabrik2D.getBaseAngle(), 1e-3); - + assertEqual(0, fabrik2D.getZ()); assertEqual(0, fabrik2D.getAngle(0)); assertEqual(0, fabrik2D.getAngle(1)); @@ -260,5 +228,4 @@ unittest(test_getters_setters) unittest_main() - -// -- END OF FILE -- \ No newline at end of file + // -- END OF FILE -- \ No newline at end of file From 7703b295698064b87443c641757c265096caa830 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Thu, 5 Sep 2024 21:03:41 +0200 Subject: [PATCH 2/8] Fix syntax issues MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Henrik Söderlund --- .github/workflows/test_runner.yml | 16 ++++++++++++---- src/FABRIK2D.cpp | 4 ++-- src/FABRIK2D.h | 6 ++++++ 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/.github/workflows/test_runner.yml b/.github/workflows/test_runner.yml index a3b50bd..954e517 100644 --- a/.github/workflows/test_runner.yml +++ b/.github/workflows/test_runner.yml @@ -16,13 +16,21 @@ jobs: compliance: strict recursive: true project-type: library - cpplint: + formatting-check: + name: Formatting Check runs-on: ubuntu-latest + strategy: + matrix: + path: + - 'src' steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - - run: pip install cpplint - - run: cpplint --recursive ./src + - name: Run clang-format style check for C/C++/Protobuf programs. + uses: jidicula/clang-format-action@v4.13.0 + with: + clang-format-version: '13' + check-path: ${{ matrix.path }} + fallback-style: 'Google' # optional unit: runs-on: ubuntu-latest steps: diff --git a/src/FABRIK2D.cpp b/src/FABRIK2D.cpp index 065cdf1..ab3fbd9 100644 --- a/src/FABRIK2D.cpp +++ b/src/FABRIK2D.cpp @@ -451,8 +451,8 @@ void Fabrik2D::_applyAngularConstraints(Joint const& parent_joint, Joint const& float cos_angle = cos(angle_diff); float sin_angle = sin(angle_diff); - float new_x = nx.x * cos_angle - nx.y * sin_angle; - float new_y = nx.x * sin_angle + nx.y * cos_angle; + float new_x = nx * cos_angle - nx * sin_angle; + float new_y = nx * sin_angle + nx * cos_angle; next_joint.x = joint.x + new_x; next_joint.y = joint.y + new_y; diff --git a/src/FABRIK2D.h b/src/FABRIK2D.h index 3372bc1..dffa7ec 100644 --- a/src/FABRIK2D.h +++ b/src/FABRIK2D.h @@ -333,6 +333,12 @@ class Fabrik2D { */ float _distance(float x1, float y1, float x2, float y2); + /* _angleBetween(x1,y1,x2,y2) + * inputs: two vectors + * outputs: angle between vectors in radians + */ + float _angleBetween(float x1, float y1, float x2, float y2); + /* _applyAngularConstraints(parent_joint, joint, next_joint) * inputs: parent_joint, joint * From f8cee507482ceb0f428674f85820aeaf7ee79c38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Thu, 5 Sep 2024 21:08:57 +0200 Subject: [PATCH 3/8] Fix spelling issues MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Henrik Söderlund --- .wordlist.txt | 4 +++- src/FABRIK2D.cpp | 5 +++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/.wordlist.txt b/.wordlist.txt index 9d00dff..9de96dc 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -41,4 +41,6 @@ destructor getTolerance resetChain deleteChain -Deallocates \ No newline at end of file +Deallocates +angleBetween +applyAngularConstraints \ No newline at end of file diff --git a/src/FABRIK2D.cpp b/src/FABRIK2D.cpp index ab3fbd9..0c49896 100644 --- a/src/FABRIK2D.cpp +++ b/src/FABRIK2D.cpp @@ -119,14 +119,14 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { for (int i = 0; i < this->_numJoints - 1; i++) { // Find the distance r_i between the target (x,y) and the // joint i position (jx,jy) - Joint const& current_joint{this->_chain->joints[i]}; + Joint const& current_joint = this->_chain->joints[i]; float jx = current_joint.x; float jy = current_joint.y; float r_i = _distance(jx, jy, x, y); float lambda_i = static_cast(lengths[i]) / r_i; // Find the new joint positions - Joint& next_joint{this->_chain->joints[i + 1]}; + Joint& next_joint = this->_chain->joints[i + 1]; next_joint.x = static_cast((1 - lambda_i) * jx + lambda_i * x); next_joint.y = static_cast((1 - lambda_i) * jy + lambda_i * y); } @@ -357,6 +357,7 @@ float Fabrik2D::getAngle(int joint) { } float Fabrik2D::getZ(int joint) { + static_cast(joint); return this->_chain->z; } From ff92fa0dd41cf4e96fde7b135f9f8dff60f26fc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Thu, 5 Sep 2024 21:41:15 +0200 Subject: [PATCH 4/8] Add angular constraints test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Henrik Söderlund --- src/FABRIK2D.cpp | 9 +++++++++ src/FABRIK2D.h | 4 ++-- test/unit_tests.cpp | 22 ++++++++++++++++++++++ 3 files changed, 33 insertions(+), 2 deletions(-) diff --git a/src/FABRIK2D.cpp b/src/FABRIK2D.cpp index 0c49896..9a22eed 100644 --- a/src/FABRIK2D.cpp +++ b/src/FABRIK2D.cpp @@ -207,8 +207,12 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) { static_cast((1 - lambda_i) * ny + lambda_i * jy); if (i > 0) { + Joint const& parent_joint = this->_chain->joints[i - 1]; _applyAngularConstraints(this->_chain->joints[i - 1], this->_chain->joints[i], this->_chain->joints[i + 1]); + } else { + _applyAngularConstraints(this->_chain->joints[i], this->_chain->joints[i], + this->_chain->joints[i + 1]); } } @@ -438,6 +442,11 @@ void Fabrik2D::_applyAngularConstraints(Joint const& parent_joint, Joint const& float nx = next_joint.x - joint.x; float ny = next_joint.y - joint.y; + // If previous vector results in zero vector, make it a vector pointing in positive x + if (_distance(0.0, 0.0, px, py) == 0.0) { + px = 1.0; + } + float current_angle = _angleBetween(px, py, nx, ny); // Clamp the angle between the joint's limits diff --git a/src/FABRIK2D.h b/src/FABRIK2D.h index dffa7ec..8b0f173 100644 --- a/src/FABRIK2D.h +++ b/src/FABRIK2D.h @@ -339,8 +339,8 @@ class Fabrik2D { */ float _angleBetween(float x1, float y1, float x2, float y2); - /* _applyAngularConstraints(parent_joint, joint, next_joint) - * inputs: parent_joint, joint + /* _applyAngularConstraints(parent_x, parent_y, joint, next_joint) + * inputs: joints * * Applies angular constraints to a joint, based on the position of parent_joint */ diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index f696e72..8d95a12 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -207,6 +207,28 @@ unittest(test_solve) { assertEqualFloat(150, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance()); } +unittest(test_solve_angular_constraint) { + int success = 0; + int lengths_4_joints[] = {200, 200, 200}; + Fabrik2D::AngularConstraint angular_constraints[] = + [ {-M_PI / 2.0, -M_PI / 2.0}, {-2.0 * M_PI, 2.0 * M_PI}, {-2.0 * M_PI, 2.0 * M_PI} ]; + + // Solve 4 joints, 3DOF, Angular Constraint + fprintf(stderr, "Solve 4 joints, 3DOF\n"); + Fabrik2D fabrik2D(4, lengths_4_joints, angular_constraints, 1); + success = fabrik2D.solve2(100, 100, 100, lengths_4_joints); + assertEqual(1, success); + + float base_angle = atan2(100, 100); + + assertEqualFloat(base_angle, fabrik2D_4_3DOF.getBaseAngle(), 1e-3); + + assertEqualFloat(100, fabrik2D_4_3DOF.getX(3), fabrik2D_4_3DOF.getTolerance()); + assertEqualFloat(100, fabrik2D_4_3DOF.getY(3), fabrik2D_4_3DOF.getTolerance()); + assertEqualFloat(100, fabrik2D_4_3DOF.getZ(3), fabrik2D_4_3DOF.getTolerance()); + assertEqualFloat(-M_PI / 2.0, fabrik2D_4_3DOF.getAngle(1), M_PI / 50.0); +} + unittest(test_getters_setters) { int lengths[] = {200, 200}; Fabrik2D fabrik2D(3, lengths); From 73c5bd1532fdd329a2da9ae5165e259671f2d583 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Thu, 5 Sep 2024 21:46:16 +0200 Subject: [PATCH 5/8] Add angular constraints test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Henrik Söderlund --- test/unit_tests.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index 8d95a12..46ffec6 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -210,8 +210,11 @@ unittest(test_solve) { unittest(test_solve_angular_constraint) { int success = 0; int lengths_4_joints[] = {200, 200, 200}; - Fabrik2D::AngularConstraint angular_constraints[] = - [ {-M_PI / 2.0, -M_PI / 2.0}, {-2.0 * M_PI, 2.0 * M_PI}, {-2.0 * M_PI, 2.0 * M_PI} ]; + Fabrik2D::AngularConstraint angular_constraints[] = [ + Fabrik2D::AngularConstraint{-M_PI / 2.0, -M_PI / 2.0}, + Fabrik2D::AngularConstraint{-2.0 * M_PI, 2.0 * M_PI}, + Fabrik2D::AngularConstraint{-2.0 * M_PI, 2.0 * M_PI} + ]; // Solve 4 joints, 3DOF, Angular Constraint fprintf(stderr, "Solve 4 joints, 3DOF\n"); From fec55e1d967dbeee50802b8b21c4635c968a41f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Wed, 11 Sep 2024 21:56:44 +0200 Subject: [PATCH 6/8] Fix test --- test/unit_tests.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index 46ffec6..d4b609d 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -210,11 +210,10 @@ unittest(test_solve) { unittest(test_solve_angular_constraint) { int success = 0; int lengths_4_joints[] = {200, 200, 200}; - Fabrik2D::AngularConstraint angular_constraints[] = [ + Fabrik2D::AngularConstraint angular_constraints[3] = { Fabrik2D::AngularConstraint{-M_PI / 2.0, -M_PI / 2.0}, Fabrik2D::AngularConstraint{-2.0 * M_PI, 2.0 * M_PI}, - Fabrik2D::AngularConstraint{-2.0 * M_PI, 2.0 * M_PI} - ]; + Fabrik2D::AngularConstraint{-2.0 * M_PI, 2.0 * M_PI}}; // Solve 4 joints, 3DOF, Angular Constraint fprintf(stderr, "Solve 4 joints, 3DOF\n"); From b4cad6586a709507384e8fd70ebed9e3d47ca8de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Wed, 11 Sep 2024 21:58:56 +0200 Subject: [PATCH 7/8] Fix test --- test/unit_tests.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index d4b609d..37395e4 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -223,12 +223,12 @@ unittest(test_solve_angular_constraint) { float base_angle = atan2(100, 100); - assertEqualFloat(base_angle, fabrik2D_4_3DOF.getBaseAngle(), 1e-3); + assertEqualFloat(base_angle, fabrik2D.getBaseAngle(), 1e-3); - assertEqualFloat(100, fabrik2D_4_3DOF.getX(3), fabrik2D_4_3DOF.getTolerance()); - assertEqualFloat(100, fabrik2D_4_3DOF.getY(3), fabrik2D_4_3DOF.getTolerance()); - assertEqualFloat(100, fabrik2D_4_3DOF.getZ(3), fabrik2D_4_3DOF.getTolerance()); - assertEqualFloat(-M_PI / 2.0, fabrik2D_4_3DOF.getAngle(1), M_PI / 50.0); + assertEqualFloat(100, fabrik2D.getX(3), fabrik2D.getTolerance()); + assertEqualFloat(100, fabrik2D.getY(3), fabrik2D.getTolerance()); + assertEqualFloat(100, fabrik2D.getZ(3), fabrik2D.getTolerance()); + assertEqualFloat(-M_PI / 2.0, fabrik2D.getAngle(1), M_PI / 50.0); } unittest(test_getters_setters) { From b2c69a391bd5edee12aef0853390bfdd9a572177 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henrik=20S=C3=B6derlund?= Date: Wed, 11 Sep 2024 22:22:29 +0200 Subject: [PATCH 8/8] Fix test --- test/unit_tests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp index 37395e4..62309ba 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -228,7 +228,7 @@ unittest(test_solve_angular_constraint) { assertEqualFloat(100, fabrik2D.getX(3), fabrik2D.getTolerance()); assertEqualFloat(100, fabrik2D.getY(3), fabrik2D.getTolerance()); assertEqualFloat(100, fabrik2D.getZ(3), fabrik2D.getTolerance()); - assertEqualFloat(-M_PI / 2.0, fabrik2D.getAngle(1), M_PI / 50.0); + assertEqualFloat(-M_PI / 2.0, fabrik2D.getAngle(1), M_PI / 25.0); } unittest(test_getters_setters) {