Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
help_functions/kinematics.h@31:393a7ec1d396, 2018-10-29 (annotated)
- Committer:
- SvenD97
- Date:
- Mon Oct 29 20:07:54 2018 +0000
- Revision:
- 31:393a7ec1d396
- Parent:
- 29:d1e8eb135e6c
Made changes such that the code compiles
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MaikOvermars | 0:4cb1de41d049 | 1 | #include "mbed.h" |
| MaikOvermars | 0:4cb1de41d049 | 2 | |
| bjonkheer | 19:e1e18746d98d | 3 | double L_ua = 0.5; //upper arm |
| bjonkheer | 19:e1e18746d98d | 4 | double L_la = 0.7; //lower arm, attached to the end effector |
| bjonkheer | 19:e1e18746d98d | 5 | double x01 = 0.0; |
| bjonkheer | 19:e1e18746d98d | 6 | double y01 = 0.2; //height base joint |
| MaikOvermars | 0:4cb1de41d049 | 7 | |
| SvenD97 | 31:393a7ec1d396 | 8 | |
| SvenD97 | 14:4744cc6c90f4 | 9 | void forwardkinematics_function(double& q1, double& q2, double& x, double& y) { |
| MaikOvermars | 0:4cb1de41d049 | 10 | // input are joint angles, output are x and y position of end effector |
| bjonkheer | 22:31065a83d9e8 | 11 | |
| bjonkheer | 29:d1e8eb135e6c | 12 | x = x01 + L_ua * cos(q1) - L_la * cos(q2); |
| bjonkheer | 29:d1e8eb135e6c | 13 | y = y01 + L_ua * sin(q1) - L_la * sin(q2); |
| bjonkheer | 19:e1e18746d98d | 14 | |
| bjonkheer | 22:31065a83d9e8 | 15 | |
| MaikOvermars | 0:4cb1de41d049 | 16 | } |
| MaikOvermars | 0:4cb1de41d049 | 17 | |
| MaikOvermars | 17:1f93c83e211f | 18 | void inversekinematics_function(double &x, double &y, const double &T, double &qref1, double &qref2, double &q1, double &q2, double &des_vx, double &des_vy) { |
| MaikOvermars | 17:1f93c83e211f | 19 | // input is desired x and y velocity, output reference angles |
| MaikOvermars | 17:1f93c83e211f | 20 | // reference angle spees are calculated using the inverse of jacobian |
| MaikOvermars | 17:1f93c83e211f | 21 | // from the reference angle speeds the reference angles are computed |
| SvenD97 | 13:397b7c22475c | 22 | |
| SvenD97 | 8:bba05e863b68 | 23 | double q1_star_des; // desired joint velocity of q1_star |
| SvenD97 | 8:bba05e863b68 | 24 | double q2_star_des; // same as above but then for q2_star |
| SvenD97 | 13:397b7c22475c | 25 | |
| SvenD97 | 9:8e1112874c12 | 26 | // The calculation below assumes that the end effector position is calculated before this function is executed |
| SvenD97 | 13:397b7c22475c | 27 | // In our case the determinant will not equal zero, hence no problems with singularies I think. |
| SvenD97 | 31:393a7ec1d396 | 28 | q1_star_des = 1/(L_ua*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L_ua*cos(q1))*des_vx-x*des_vy); |
| SvenD97 | 31:393a7ec1d396 | 29 | q2_star_des = 1/(L_ua*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L_ua*sin(q1))*des_vx+1*(-y+y01)*des_vy); |
| SvenD97 | 7:b77f2201b156 | 30 | |
| SvenD97 | 13:397b7c22475c | 31 | qref1 = q1+T*q1_star_des; // Yet to adapt all these equations |
| SvenD97 | 13:397b7c22475c | 32 | qref2 = q2+T*(q2_star_des - q1_star_des); |
| MaikOvermars | 0:4cb1de41d049 | 33 | } |