Biorobotics / Robot-Software

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@14:4744cc6c90f4, 2018-10-25 (annotated)

Committer:
SvenD97
Date:
Thu Oct 25 08:14:51 2018 +0000
Branch:
bla
Revision:
14:4744cc6c90f4
Parent:
13:397b7c22475c
Child:
16:0280a604cf7e
Made some changes to measure_all and to kinematics.h. ; kinematics.h: added some more input variables.; measure_all: changed the forwardkinematics part and added the angle calculation using QEI (this last part is not fully working yet);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2
MaikOvermars 0:4cb1de41d049 3 double L1 = 0.5;
MaikOvermars 0:4cb1de41d049 4 double L2 = 0.7;
MaikOvermars 0:4cb1de41d049 5 double x01 = 0.0;
MaikOvermars 0:4cb1de41d049 6 double y01 = 0.2;
MaikOvermars 0:4cb1de41d049 7
SvenD97 14:4744cc6c90f4 8 void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
MaikOvermars 0:4cb1de41d049 9 // input are joint angles, output are x and y position of end effector
MaikOvermars 0:4cb1de41d049 10
SvenD97 7:b77f2201b156 11 x = x01 + L1*cos(q1)-L2*cos(q2);
SvenD97 7:b77f2201b156 12 y = y01 + L1 * sin(q1) - L2 * sin(q2);
MaikOvermars 0:4cb1de41d049 13 }
MaikOvermars 0:4cb1de41d049 14
SvenD97 13:397b7c22475c 15 double inversekinematics_function(double& x, double& y, const double& T, double& qref1, double& qref2, double& q1, double& q2, double& des_vx, double& des_vy) {
MaikOvermars 0:4cb1de41d049 16 // pseudo inverse jacobian to get joint speeds
MaikOvermars 0:4cb1de41d049 17 // input are desired vx and vy of end effector, output joint angle speeds
SvenD97 13:397b7c22475c 18
SvenD97 8:bba05e863b68 19 double q1_star_des; // desired joint velocity of q1_star
SvenD97 8:bba05e863b68 20 double q2_star_des; // same as above but then for q2_star
SvenD97 13:397b7c22475c 21
SvenD97 9:8e1112874c12 22 // The calculation below assumes that the end effector position is calculated before this function is executed
SvenD97 13:397b7c22475c 23 // In our case the determinant will not equal zero, hence no problems with singularies I think.
SvenD97 13:397b7c22475c 24 q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
SvenD97 13:397b7c22475c 25 q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01))*des_vy);
SvenD97 7:b77f2201b156 26
SvenD97 13:397b7c22475c 27 qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
SvenD97 13:397b7c22475c 28 qref2 = q2+T*(q2_star_des - q1_star_des);
MaikOvermars 0:4cb1de41d049 29 }