Merge pull request #23 from henriksod/development

Development

Author: henriksod

.github/workflows/test_runner.yml

@@ -39,7 +39,8 @@ jobs:
       matrix:
         fqbn:
           - "arduino:avr:uno"
-          - "arduino:samd:mkrzero"
+          - "arduino:avr:mega"
+          - "arduino:sam:arduino_due_x_dbg"
     steps:
       - uses: actions/checkout@v2
       - uses: arduino/compile-sketches@v1

.wordlist.txt

@@ -38,4 +38,7 @@ Todo
 cpu
 runtime
 destructor
-getTolerance
+getTolerance
+resetChain
+deleteChain
+Deallocates

src/FABRIK2D.cpp

@@ -37,15 +37,7 @@ Fabrik2D::Fabrik2D(int numJoints, int lengths[], float tolerance) {
 }
 
 Fabrik2D::~Fabrik2D() {
-    if (_chain->joints) {
-        delete[] _chain->joints;
-        _chain->joints = NULL;
-    }
-
-    if (_chain) {
-        delete _chain;
-        _chain = NULL;
-    }
+    _deleteChain();
 }
 
 void Fabrik2D::begin(int numJoints, int lengths[], float tolerance) {
@@ -59,19 +51,35 @@ void Fabrik2D::_createChain(int lengths[]) {
     Chain* chain = new Chain();
     chain->joints = new Joint[this->_numJoints];
 
-    chain->joints[0].x = 0;
-    chain->joints[0].y = 0;
-    chain->joints[0].angle = 0;
+    this->_chain = chain;
+
+    _resetChain(lengths);
+}
+
+void Fabrik2D::_resetChain(int lengths[]) {
+    this->_chain->joints[0].x = 0;
+    this->_chain->joints[0].y = 0;
+    this->_chain->joints[0].angle = 0;
 
     int sumLengths = 0;
     for (int i = 1; i < this->_numJoints; i++) {
         sumLengths = sumLengths + lengths[i-1];
-        chain->joints[i].x = 0;
-        chain->joints[i].y = sumLengths;
-        chain->joints[i].angle = 0;
+        this->_chain->joints[i].x = 0;
+        this->_chain->joints[i].y = sumLengths;
+        this->_chain->joints[i].angle = 0;
     }
+}
 
-    this->_chain = chain;
+void Fabrik2D::_deleteChain() {
+    if (this->_chain->joints != nullptr) {
+        delete[] this->_chain->joints;
+        this->_chain->joints = nullptr;
+    }
+
+    if (this->_chain != nullptr) {
+        delete this->_chain;
+        this->_chain = nullptr;
+    }
 }
 
 uint8_t Fabrik2D::solve(float x, float y, int lengths[]) {
@@ -106,6 +114,7 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) {
                 (1-lambda_i)*jy + lambda_i*y);
         }
 
+       _resetChain(lengths);
        return 0;
     } else {
         // The target is reachable; this, set as (bx,by) the initial
@@ -131,8 +140,10 @@ 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;
+                }
             }
 
             prevDist = dist;
@@ -218,9 +229,12 @@ uint8_t Fabrik2D::solve2(
     uint8_t result_status = 0;
 
     if (this->_numJoints >= 4) {
+        // Solve in 2D plane
+        float r = _distance(0, 0, x, z);
+
         // Find wrist center by moving from the desired position with
         // tool angle and link length
-        float oc_x = x - (
+        float oc_r = r - (
             lengths[this->_numJoints-2]+grippingOffset)*cos(toolAngle);
 
         float oc_y = y - (
@@ -230,7 +244,7 @@ uint8_t Fabrik2D::solve2(
         int tmp = this->_numJoints;
         this->_numJoints = this->_numJoints-1;
 
-        result_status = solve(oc_x, oc_y, lengths);
+        result_status = solve(oc_r, oc_y, lengths);
 
         this->_numJoints = tmp;
 
@@ -266,10 +280,14 @@ uint8_t Fabrik2D::solve2(
             // Save tool angle
             this->_chain->joints[this->_numJoints-1].angle = toolAngle;
 
-            // Save base angle (if z different from zero)
-            if (z != 0) {
-                this->_chain->z = z;
-                this->_chain->angle = atan2(z, x);
+            // 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);
             }
         }
     }
@@ -297,6 +315,12 @@ uint8_t Fabrik2D::solve2(float x, float y, float z, int 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);
+        }
     }
 
     return result_status;
@@ -340,7 +364,19 @@ float Fabrik2D::getBaseAngle() {
 }
 
 void Fabrik2D::setBaseAngle(float baseAngle) {
+    float angle_diff = baseAngle - this->_chain->angle;
     this->_chain->angle = 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);
+        // 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);
+        }
+    }
 }
 
 float Fabrik2D::getTolerance() {
@@ -352,19 +388,26 @@ void Fabrik2D::setTolerance(float tolerance) {
 }
 
 void Fabrik2D::setJoints(float angles[], int lengths[]) {
-     float accAng = angles[0];
-     float accX = 0;
-     float accY = 0;
-     this->_chain->joints[0].angle = angles[0];
-
-     for (int i = 1; i < this->_numJoints; i++) {
-         accAng += angles[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];
-         accX = this->_chain->joints[i].x;
-         accY = this->_chain->joints[i].y;
-     }
+    float accAng = angles[0];
+    float accX = 0;
+    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);
+        this->_chain->joints[i].angle = angles[i];
+        accAng += angles[i];
+        accX = this->_chain->joints[i].x;
+        accY = this->_chain->joints[i].y;
+    }
+
+    // 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;
 }
 
 int Fabrik2D::numJoints() {

src/FABRIK2D.h

@@ -41,7 +41,7 @@ class Fabrik2D {
 
     // Chain struct
     typedef struct {
-      Joint* joints = NULL;  // list of joints
+      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;
@@ -260,8 +260,10 @@ class Fabrik2D {
     void setTolerance(float tolerance);
 
     /* setJoints(angles, lengths)
-     * inputs: New joint angles(in radians) and list of lengths
+     * inputs: New joint angles (in radians) and list of lengths
      *         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
@@ -280,17 +282,35 @@ class Fabrik2D {
     // Tolerance of distance between end effector and target
     float _tolerance;
     // The chain containing joints
-    Chain* _chain = NULL;
+    Chain* _chain = nullptr;
     // Number of iterations to converge for last run (debugging only)
     int _num_iterations = 0;
 
     /* _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
      */
     void _createChain(int* lengths);
 
+    /* _resetChain(lengths)
+     * inputs: lengths
+     *
+     * Resets the chain to initial configuration
+     *
+     * length size should always be one lesser than the number of joints
+     */
+    void _resetChain(int lengths[]);
+
+    /* _deleteChain()
+     *
+     * Deallocates _chain from memory
+     *
+     */
+    void _deleteChain();
+
     /* distance(x1,y1,x2,y2)
      * inputs: coordinates
      * outputs: distance between points

test/unit_tests.cpp

@@ -116,7 +116,7 @@ unittest(test_constructor)
 
 unittest(test_solve)
 {
-    uint8_t success = 0;
+    int success = 0;
     int lengths_3_joints[] = {200, 200};
     int lengths_4_joints[] = {200, 200, 200};
 
@@ -146,28 +146,46 @@ unittest(test_solve)
     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");
+    fabrik2D_3_2DOF.setJoints(angles, lengths_3_joints);
+    assertEqualFloat(100, fabrik2D_3_2DOF.getX(2), 1e-3);
+    assertEqualFloat(100, fabrik2D_3_2DOF.getY(2), 1e-3);
+
+    // Test Solve Fail
+    fprintf(stderr, "Test Solve Fail\n");
+    success = fabrik2D_3_2DOF.solve(1000, 1000, lengths_3_joints);
+    assertEqual(0, success);
+
+    // Test Solve tolerance too low
+    fprintf(stderr, "Test Solve tolerance too low\n");
+    fabrik2D_3_2DOF.setTolerance(0.001);
+    success = fabrik2D_3_2DOF.solve(100, 100, lengths_3_joints);
+    assertEqual(2, success);
+
     // Solve 4 joints, 3DOF
-    fprintf(stderr, "Solve 4 joints, 3DOF\n");
-    Fabrik2D fabrik2D_3_3DOF(3, lengths_4_joints, 10);
-    success = fabrik2D_3_3DOF.solve(150, 50, -HALF_PI, lengths_4_joints);
+    fprintf(stderr, "Solve 4 joints, 3DOF (tool angle)\n");
+    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(150, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance());
+    assertEqualFloat(50, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance());
     assertEqualFloat(50, fabrik2D_3_3DOF.getY(3), fabrik2D_3_3DOF.getTolerance());
 
-    assertEqualFloat(150, fabrik2D_3_3DOF.getX(2), 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, Gripping offset
+    // Solve 4 joints, 3DOF (tool angle), Gripping offset
     fprintf(stderr, "Solve 4 joints, 3DOF, Gripping offset\n");
-    Fabrik2D fabrik2D_3_3DOF_GO(3, lengths_4_joints, 1);
-    success = fabrik2D_3_3DOF_GO.solve(150, 50, -HALF_PI, 10, lengths_4_joints);
+    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(150, fabrik2D_3_3DOF_GO.getX(3), fabrik2D_3_3DOF_GO.getTolerance());
+    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(150, fabrik2D_3_3DOF_GO.getX(2), 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
@@ -200,17 +218,45 @@ unittest(test_solve)
     // 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.solve(150, 50, -HALF_PI, 10, lengths_4_joints);
+    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(100, fabrik2D_4_4DOF_GO.getZ(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);
+    fabrik2D_4_4DOF_GO.setBaseAngle(HALF_PI);
+    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)
+{
+    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));
+    assertEqual(200, fabrik2D.getY(1));
+    assertEqual(400, fabrik2D.getY(2));
+    assertEqual(0, fabrik2D.getX(0));
+    assertEqual(0, fabrik2D.getX(1));
+}
 
 unittest_main()