Biorobotics / DemoMode

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

Fork of Robot-Software_jesse by Biorobotics

help_functions/kinematics.h@13:3482d315877c, 2018-10-29 (annotated)

Committer:
Peppypeppy
Date:
Mon Oct 29 12:55:38 2018 +0000
Revision:
13:3482d315877c
Parent:
6:8ff9566c91e2 
hello;

Who changed what in which revision?

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