diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 5e14bdc..0000000 --- a/.clang-format +++ /dev/null @@ -1,10 +0,0 @@ -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 065cdf1..b6ab829 100644 --- a/src/FABRIK2D.cpp +++ b/src/FABRIK2D.cpp @@ -24,53 +24,36 @@ * **********************************************************************************************/ -#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) { - 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(int numJoints, int lengths[], float tolerance) { + begin(numJoints, lengths, tolerance); } Fabrik2D::~Fabrik2D() { _deleteChain(); } -void Fabrik2D::_createChain(int* lengths) { - Chain* chain = new Chain(); - chain->joints = new Joint[this->_numJoints]; - - this->_chain = chain; +void Fabrik2D::begin(int numJoints, int lengths[], float tolerance) { + this->_numJoints = numJoints; + _createChain(lengths); - _resetChain(lengths); + this->_tolerance = tolerance; } -void Fabrik2D::_createChain(int* lengths, AngularConstraint* angular_constraints) { +void Fabrik2D::_createChain(int lengths[]) { 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[]) { @@ -80,7 +63,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; @@ -104,11 +87,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]; } @@ -116,23 +99,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) - Joint const& current_joint{this->_chain->joints[i]}; - float jx = current_joint.x; - float jy = current_joint.y; + float jx = this->_chain->joints[i].x; + float jy = this->_chain->joints[i].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 - 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); + 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); } - _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 @@ -141,8 +124,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; @@ -157,7 +140,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; } @@ -167,22 +150,24 @@ 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 @@ -190,48 +175,44 @@ 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); - - if (i > 0) { - _applyAngularConstraints(this->_chain->joints[i - 1], this->_chain->joints[i], - this->_chain->joints[i + 1]); - } + 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); } // 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; } @@ -239,8 +220,12 @@ 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) { @@ -249,13 +234,15 @@ uint8_t Fabrik2D::solve2(float x, float y, float z, float toolAngle, float gripp // 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); @@ -263,42 +250,44 @@ uint8_t Fabrik2D::solve2(float x, float y, float z, float toolAngle, float gripp 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); } } } @@ -310,50 +299,60 @@ 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) { @@ -370,10 +369,12 @@ 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); } } } @@ -392,9 +393,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; @@ -402,11 +403,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() { @@ -420,41 +421,5 @@ 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); -} - -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; - } + return sqrt(xDiff*xDiff + yDiff*yDiff); } diff --git a/src/FABRIK2D.h b/src/FABRIK2D.h index 3372bc1..ae37dbe 100644 --- a/src/FABRIK2D.h +++ b/src/FABRIK2D.h @@ -24,6 +24,7 @@ * **********************************************************************************************/ + #ifndef SRC_FABRIK2D_H_ #define SRC_FABRIK2D_H_ @@ -31,64 +32,51 @@ class Fabrik2D { public: - /// 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; - }; + // 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() {} /* Fabrik2D(numJoints, lengths, tolerance) * inputs: numJoints, lengths, (optional) tolerance * - * Initializes the library - * creates the chain to be used for the inverse kinematics solver + * Calls begin(numJoints, lengths, tolerance) * * tolerance is optional, will be set to 10 by default */ Fabrik2D(int numJoints, int lengths[], float tolerance = 10); - /* Fabrik2D(numJoints, lengths, angular_constraints, tolerance) - * inputs: numJoints, lengths, angular_constraints, (optional) tolerance - * + /* Fabrik2D destructor */ + ~Fabrik2D(); + + /* + * begin(numJoints, lengths, tolerance) + * inputs: numJoints, lengths, (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 */ - Fabrik2D(int numJoints, int lengths[], AngularConstraint angular_constraints[], - float tolerance = 10); - - /* Fabrik2D destructor */ - ~Fabrik2D(); + void begin(int numJoints, int lengths[], float tolerance = 10); /* solve(x, y, lengths) - * + * * Inputs: x and y positions of target, lengths between each joint * Returns: * 0 if FABRIK could not converge @@ -96,7 +84,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 @@ -105,7 +93,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: @@ -123,7 +111,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: @@ -134,15 +122,19 @@ 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: @@ -165,7 +157,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: @@ -185,12 +177,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: @@ -200,7 +192,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 * @@ -211,7 +203,11 @@ 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 @@ -228,7 +224,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. */ @@ -257,7 +253,7 @@ class Fabrik2D { /* setTolerance(tolerance) * inputs: tolerance value - * + * * sets the tolerance of the distance between the end effector * and the target */ @@ -268,7 +264,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 */ @@ -299,16 +295,6 @@ 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 * @@ -325,20 +311,13 @@ 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 f696e72..d597103 100644 --- a/test/unit_tests.cpp +++ b/test/unit_tests.cpp @@ -29,93 +29,123 @@ // assertNAN(arg); // isnan(a) // assertNotNAN(arg); // !isnan(a) + #include #include "Arduino.h" #include "FABRIK2D.h" -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_setup() +{ +} + + +unittest_teardown() +{ } -unittest(test_solve) { + +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) +{ 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"); @@ -139,65 +169,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); @@ -205,18 +235,20 @@ 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)); @@ -228,4 +260,5 @@ unittest(test_getters_setters) { unittest_main() - // -- END OF FILE -- \ No newline at end of file + +// -- END OF FILE -- \ No newline at end of file