Skip to content

Commit dab2985

Browse files
henriksodhenriksod
authored
Merge pull request #23 from henriksod/development
Development
2 parents dfa1933 + 5d9bd50 commit dab2985

File tree

5 files changed

+167
-54
lines changed

5 files changed

+167
-54
lines changed

‎.github/workflows/test_runner.yml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ jobs:
3939
matrix:
4040
fqbn:
4141
- "arduino:avr:uno"
42-
- "arduino:samd:mkrzero"
42+
- "arduino:avr:mega"
43+
- "arduino:sam:arduino_due_x_dbg"
4344
steps:
4445
- uses: actions/checkout@v2
4546
- uses: arduino/compile-sketches@v1

‎.wordlist.txt

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,4 +38,7 @@ Todo
3838
cpu
3939
runtime
4040
destructor
41-
getTolerance
41+
getTolerance
42+
resetChain
43+
deleteChain
44+
Deallocates

‎src/FABRIK2D.cpp

Lines changed: 79 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -37,15 +37,7 @@ Fabrik2D::Fabrik2D(int numJoints, int lengths[], float tolerance) {
3737
}
3838

3939
Fabrik2D::~Fabrik2D() {
40-
if (_chain->joints) {
41-
delete[] _chain->joints;
42-
_chain->joints = NULL;
43-
}
44-
45-
if (_chain) {
46-
delete _chain;
47-
_chain = NULL;
48-
}
40+
_deleteChain();
4941
}
5042

5143
void Fabrik2D::begin(int numJoints, int lengths[], float tolerance) {
@@ -59,19 +51,35 @@ void Fabrik2D::_createChain(int lengths[]) {
5951
Chain* chain = new Chain();
6052
chain->joints = new Joint[this->_numJoints];
6153

62-
chain->joints[0].x = 0;
63-
chain->joints[0].y = 0;
64-
chain->joints[0].angle = 0;
54+
this->_chain = chain;
55+
56+
_resetChain(lengths);
57+
}
58+
59+
void Fabrik2D::_resetChain(int lengths[]) {
60+
this->_chain->joints[0].x = 0;
61+
this->_chain->joints[0].y = 0;
62+
this->_chain->joints[0].angle = 0;
6563

6664
int sumLengths = 0;
6765
for (int i = 1; i < this->_numJoints; i++) {
6866
sumLengths = sumLengths + lengths[i-1];
69-
chain->joints[i].x = 0;
70-
chain->joints[i].y = sumLengths;
71-
chain->joints[i].angle = 0;
67+
this->_chain->joints[i].x = 0;
68+
this->_chain->joints[i].y = sumLengths;
69+
this->_chain->joints[i].angle = 0;
7270
}
71+
}
7372

74-
this->_chain = chain;
73+
void Fabrik2D::_deleteChain() {
74+
if (this->_chain->joints != nullptr) {
75+
delete[] this->_chain->joints;
76+
this->_chain->joints = nullptr;
77+
}
78+
79+
if (this->_chain != nullptr) {
80+
delete this->_chain;
81+
this->_chain = nullptr;
82+
}
7583
}
7684

7785
uint8_t Fabrik2D::solve(float x, float y, int lengths[]) {
@@ -106,6 +114,7 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) {
106114
(1-lambda_i)*jy + lambda_i*y);
107115
}
108116

117+
_resetChain(lengths);
109118
return 0;
110119
} else {
111120
// 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[]) {
131140
tolerance *= 1.1;
132141
// If increased tolerance is higher than 2x the desired
133142
// tolerance report failed to converge
134-
if (tolerance > this->_tolerance*2)
143+
if (tolerance > this->_tolerance*2) {
144+
_resetChain(lengths);
135145
return 0;
146+
}
136147
}
137148

138149
prevDist = dist;
@@ -218,9 +229,12 @@ uint8_t Fabrik2D::solve2(
218229
uint8_t result_status = 0;
219230

220231
if (this->_numJoints >= 4) {
232+
// Solve in 2D plane
233+
float r = _distance(0, 0, x, z);
234+
221235
// Find wrist center by moving from the desired position with
222236
// tool angle and link length
223-
float oc_x = x - (
237+
float oc_r = r - (
224238
lengths[this->_numJoints-2]+grippingOffset)*cos(toolAngle);
225239

226240
float oc_y = y - (
@@ -230,7 +244,7 @@ uint8_t Fabrik2D::solve2(
230244
int tmp = this->_numJoints;
231245
this->_numJoints = this->_numJoints-1;
232246

233-
result_status = solve(oc_x, oc_y, lengths);
247+
result_status = solve(oc_r, oc_y, lengths);
234248

235249
this->_numJoints = tmp;
236250

@@ -266,10 +280,14 @@ uint8_t Fabrik2D::solve2(
266280
// Save tool angle
267281
this->_chain->joints[this->_numJoints-1].angle = toolAngle;
268282

269-
// Save base angle (if z different from zero)
270-
if (z != 0) {
271-
this->_chain->z = z;
272-
this->_chain->angle = atan2(z, x);
283+
// Save base angle
284+
this->_chain->z = z;
285+
this->_chain->angle = atan2(z, x);
286+
287+
// Update joint X values based on base rotation
288+
for (int i = 0; i <= this->_numJoints-1; i++) {
289+
this->_chain->joints[i].x =
290+
this->_chain->joints[i].x * cos(-this->_chain->angle);
273291
}
274292
}
275293
}
@@ -297,6 +315,12 @@ uint8_t Fabrik2D::solve2(float x, float y, float z, int lengths[]) {
297315
if (result_status == 1) {
298316
this->_chain->z = z;
299317
this->_chain->angle = atan2(z, x);
318+
319+
// Update joint X values based on base rotation
320+
for (int i = 0; i <= this->_numJoints-1; i++) {
321+
this->_chain->joints[i].x =
322+
this->_chain->joints[i].x * cos(-this->_chain->angle);
323+
}
300324
}
301325

302326
return result_status;
@@ -340,7 +364,19 @@ float Fabrik2D::getBaseAngle() {
340364
}
341365

342366
void Fabrik2D::setBaseAngle(float baseAngle) {
367+
float angle_diff = baseAngle - this->_chain->angle;
343368
this->_chain->angle = baseAngle;
369+
370+
if (this->_numJoints >= 4) {
371+
// Update end effector Z value based on base rotation
372+
this->_chain->z =
373+
this->_chain->joints[this->_numJoints-1].x * sin(angle_diff);
374+
// Update joint X values based on base rotation
375+
for (int i = 0; i <= this->_numJoints-1; i++) {
376+
this->_chain->joints[i].x =
377+
this->_chain->joints[i].x * cos(angle_diff);
378+
}
379+
}
344380
}
345381

346382
float Fabrik2D::getTolerance() {
@@ -352,19 +388,26 @@ void Fabrik2D::setTolerance(float tolerance) {
352388
}
353389

354390
void Fabrik2D::setJoints(float angles[], int lengths[]) {
355-
float accAng = angles[0];
356-
float accX = 0;
357-
float accY = 0;
358-
this->_chain->joints[0].angle = angles[0];
359-
360-
for (int i = 1; i < this->_numJoints; i++) {
361-
accAng += angles[i];
362-
this->_chain->joints[i].x = accX + lengths[i-1]*cos(accAng);
363-
this->_chain->joints[i].y = accY + lengths[i-1]*sin(accAng);
364-
this->_chain->joints[i].angle = angles[i];
365-
accX = this->_chain->joints[i].x;
366-
accY = this->_chain->joints[i].y;
367-
}
391+
float accAng = angles[0];
392+
float accX = 0;
393+
float accY = 0;
394+
this->_chain->joints[0].angle = angles[0];
395+
396+
for (int i = 1; i < this->_numJoints-1; i++) {
397+
this->_chain->joints[i].x = accX + lengths[i-1]*cos(accAng);
398+
this->_chain->joints[i].y = accY + lengths[i-1]*sin(accAng);
399+
this->_chain->joints[i].angle = angles[i];
400+
accAng += angles[i];
401+
accX = this->_chain->joints[i].x;
402+
accY = this->_chain->joints[i].y;
403+
}
404+
405+
// Update end effector x and y
406+
this->_chain->joints[this->_numJoints-1].x =
407+
accX + lengths[this->_numJoints-2]*cos(accAng);
408+
this->_chain->joints[this->_numJoints-1].y =
409+
accY + lengths[this->_numJoints-2]*sin(accAng);
410+
this->_chain->joints[this->_numJoints-1].angle = 0;
368411
}
369412

370413
int Fabrik2D::numJoints() {

‎src/FABRIK2D.h

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ class Fabrik2D {
4141

4242
// Chain struct
4343
typedef struct {
44-
Joint* joints = NULL; // list of joints
44+
Joint* joints = nullptr; // list of joints
4545
float z = 0; // z position defining the chain offset from the plane
4646
float angle = 0; // base (plane) rotation
4747
} Chain;
@@ -260,8 +260,10 @@ class Fabrik2D {
260260
void setTolerance(float tolerance);
261261

262262
/* setJoints(angles, lengths)
263-
* inputs: New joint angles(in radians) and list of lengths
263+
* inputs: New joint angles (in radians) and list of lengths
264264
* between each joint
265+
*
266+
* Angles must be equal to the number of joints - 1
265267
*
266268
* manually sets the joint angles and updates their position using
267269
* forward kinematics
@@ -280,17 +282,35 @@ class Fabrik2D {
280282
// Tolerance of distance between end effector and target
281283
float _tolerance;
282284
// The chain containing joints
283-
Chain* _chain = NULL;
285+
Chain* _chain = nullptr;
284286
// Number of iterations to converge for last run (debugging only)
285287
int _num_iterations = 0;
286288

287289
/* _createChain(lengths)
288290
* inputs: lengths
289291
*
292+
* Creates a new chain and attaches it to the Fabrik2D object
293+
*
290294
* length size should always be one lesser than the number of joints
291295
*/
292296
void _createChain(int* lengths);
293297

298+
/* _resetChain(lengths)
299+
* inputs: lengths
300+
*
301+
* Resets the chain to initial configuration
302+
*
303+
* length size should always be one lesser than the number of joints
304+
*/
305+
void _resetChain(int lengths[]);
306+
307+
/* _deleteChain()
308+
*
309+
* Deallocates _chain from memory
310+
*
311+
*/
312+
void _deleteChain();
313+
294314
/* distance(x1,y1,x2,y2)
295315
* inputs: coordinates
296316
* outputs: distance between points

‎test/unit_tests.cpp

Lines changed: 59 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@ unittest(test_constructor)
116116

117117
unittest(test_solve)
118118
{
119-
uint8_t success = 0;
119+
int success = 0;
120120
int lengths_3_joints[] = {200, 200};
121121
int lengths_4_joints[] = {200, 200, 200};
122122

@@ -146,28 +146,46 @@ unittest(test_solve)
146146
assertEqualFloat(100, fabrik2D_3_2DOF.getX(2), fabrik2D_3_2DOF.getTolerance());
147147
assertEqualFloat(100, fabrik2D_3_2DOF.getY(2), fabrik2D_3_2DOF.getTolerance());
148148

149+
// Test Set Joints
150+
float angles[] = {a1, a2};
151+
fprintf(stderr, "Test Set Joints\n");
152+
fabrik2D_3_2DOF.setJoints(angles, lengths_3_joints);
153+
assertEqualFloat(100, fabrik2D_3_2DOF.getX(2), 1e-3);
154+
assertEqualFloat(100, fabrik2D_3_2DOF.getY(2), 1e-3);
155+
156+
// Test Solve Fail
157+
fprintf(stderr, "Test Solve Fail\n");
158+
success = fabrik2D_3_2DOF.solve(1000, 1000, lengths_3_joints);
159+
assertEqual(0, success);
160+
161+
// Test Solve tolerance too low
162+
fprintf(stderr, "Test Solve tolerance too low\n");
163+
fabrik2D_3_2DOF.setTolerance(0.001);
164+
success = fabrik2D_3_2DOF.solve(100, 100, lengths_3_joints);
165+
assertEqual(2, success);
166+
149167
// Solve 4 joints, 3DOF
150-
fprintf(stderr, "Solve 4 joints, 3DOF\n");
151-
Fabrik2D fabrik2D_3_3DOF(3, lengths_4_joints, 10);
152-
success = fabrik2D_3_3DOF.solve(150, 50, -HALF_PI, lengths_4_joints);
168+
fprintf(stderr, "Solve 4 joints, 3DOF (tool angle)\n");
169+
Fabrik2D fabrik2D_3_3DOF(4, lengths_4_joints, 10);
170+
success = fabrik2D_3_3DOF.solve(50, 50, -HALF_PI, lengths_4_joints);
153171
assertEqual(1, success);
154172

155-
assertEqualFloat(150, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance());
173+
assertEqualFloat(50, fabrik2D_3_3DOF.getX(3), fabrik2D_3_3DOF.getTolerance());
156174
assertEqualFloat(50, fabrik2D_3_3DOF.getY(3), fabrik2D_3_3DOF.getTolerance());
157175

158-
assertEqualFloat(150, fabrik2D_3_3DOF.getX(2), fabrik2D_3_3DOF.getTolerance());
176+
assertEqualFloat(50, fabrik2D_3_3DOF.getX(2), fabrik2D_3_3DOF.getTolerance());
159177
assertEqualFloat(250, fabrik2D_3_3DOF.getY(2), fabrik2D_3_3DOF.getTolerance());
160178

161-
// Solve 4 joints, 3DOF, Gripping offset
179+
// Solve 4 joints, 3DOF (tool angle), Gripping offset
162180
fprintf(stderr, "Solve 4 joints, 3DOF, Gripping offset\n");
163-
Fabrik2D fabrik2D_3_3DOF_GO(3, lengths_4_joints, 1);
164-
success = fabrik2D_3_3DOF_GO.solve(150, 50, -HALF_PI, 10, lengths_4_joints);
181+
Fabrik2D fabrik2D_3_3DOF_GO(4, lengths_4_joints, 1);
182+
success = fabrik2D_3_3DOF_GO.solve(50, 50, -HALF_PI, 10, lengths_4_joints);
165183
assertEqual(1, success);
166184

167-
assertEqualFloat(150, fabrik2D_3_3DOF_GO.getX(3), fabrik2D_3_3DOF_GO.getTolerance());
185+
assertEqualFloat(50, fabrik2D_3_3DOF_GO.getX(3), fabrik2D_3_3DOF_GO.getTolerance());
168186
assertEqualFloat(60, fabrik2D_3_3DOF_GO.getY(3), fabrik2D_3_3DOF_GO.getTolerance());
169187

170-
assertEqualFloat(150, fabrik2D_3_3DOF_GO.getX(2), fabrik2D_3_3DOF_GO.getTolerance());
188+
assertEqualFloat(50, fabrik2D_3_3DOF_GO.getX(2), fabrik2D_3_3DOF_GO.getTolerance());
171189
assertEqualFloat(260, fabrik2D_3_3DOF_GO.getY(2), fabrik2D_3_3DOF_GO.getTolerance());
172190

173191
// Solve 4 joints, 3DOF
@@ -200,17 +218,45 @@ unittest(test_solve)
200218
// Solve 4 joints, 4DOF, Gripping offset
201219
fprintf(stderr, "Solve 4 joints, 4DOF, Gripping offset\n");
202220
Fabrik2D fabrik2D_4_4DOF_GO(4, lengths_4_joints, 1);
203-
success = fabrik2D_4_4DOF_GO.solve(150, 50, -HALF_PI, 10, lengths_4_joints);
221+
success = fabrik2D_4_4DOF_GO.solve2(150, 50, 0, -HALF_PI, 10, lengths_4_joints);
204222
assertEqual(1, success);
205223

206224
assertEqualFloat(150, fabrik2D_4_4DOF_GO.getX(3), fabrik2D_4_4DOF_GO.getTolerance());
207225
assertEqualFloat(60, fabrik2D_4_4DOF_GO.getY(3), fabrik2D_4_4DOF_GO.getTolerance());
208-
assertEqualFloat(100, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance());
226+
assertEqualFloat(0, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance());
209227

210228
assertEqualFloat(150, fabrik2D_4_4DOF_GO.getX(2), fabrik2D_4_4DOF_GO.getTolerance());
211229
assertEqualFloat(260, fabrik2D_4_4DOF_GO.getY(2), fabrik2D_4_4DOF_GO.getTolerance());
230+
231+
// Test Set Base Angle
232+
fprintf(stderr, "Test Set Base Angle\n");
233+
fprintf(stderr, "Test Set Base Angle %f\n", HALF_PI - fabrik2D_4_4DOF_GO.getChain()->angle);
234+
fabrik2D_4_4DOF_GO.setBaseAngle(HALF_PI);
235+
assertEqualFloat(0, fabrik2D_4_4DOF_GO.getX(3), fabrik2D_4_4DOF_GO.getTolerance());
236+
assertEqualFloat(60, fabrik2D_4_4DOF_GO.getY(3), fabrik2D_4_4DOF_GO.getTolerance());
237+
assertEqualFloat(150, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance());
238+
212239
}
213240

241+
unittest(test_getters_setters)
242+
{
243+
int lengths[] = {200, 200};
244+
Fabrik2D fabrik2D(3, lengths);
245+
246+
fabrik2D.setTolerance(20);
247+
assertEqual(20, fabrik2D.getTolerance());
248+
249+
fabrik2D.setBaseAngle(HALF_PI);
250+
assertEqualFloat(HALF_PI, fabrik2D.getBaseAngle(), 1e-3);
251+
252+
assertEqual(0, fabrik2D.getZ());
253+
assertEqual(0, fabrik2D.getAngle(0));
254+
assertEqual(0, fabrik2D.getAngle(1));
255+
assertEqual(200, fabrik2D.getY(1));
256+
assertEqual(400, fabrik2D.getY(2));
257+
assertEqual(0, fabrik2D.getX(0));
258+
assertEqual(0, fabrik2D.getX(1));
259+
}
214260

215261
unittest_main()
216262

0 commit comments

Comments
 (0)