Biorobotics / Robot-Software_jesse

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 by Biorobotics

help_functions/kinematics.h@9:8e1112874c12, 2018-10-23 (annotated)

Committer:
SvenD97
Date:
Tue Oct 23 06:04:49 2018 +0000
Revision:
9:8e1112874c12
Parent:
8:bba05e863b68
Child:
12:3c47c7b1d1d7
put a comment at line 70 saying: no state_changed action?

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 6:8ff9566c91e2 8 void forwardkinematics_function(double q1, double q2) {
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 9:8e1112874c12 15 double inversekinematics_function() {
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 7:b77f2201b156 18 // I assume here that reference is a vector (this should also be global I think)
SvenD97 8:bba05e863b68 19 // I also assume that the end effector position is global here and is denoted by x
SvenD97 8:bba05e863b68 20 // Assuming the ticker_rate is also set to be global
SvenD97 8:bba05e863b68 21
SvenD97 8:bba05e863b68 22 double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC)
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 8:bba05e863b68 25
SvenD97 9:8e1112874c12 26 // We have to give a des_twist. If we do EMG we give a x and y velocity, so do we have to use the reference x and y position?
SvenD97 9:8e1112874c12 27
SvenD97 9:8e1112874c12 28 // The calculation below assumes that the end effector position is calculated before this function is executed
SvenD97 8:bba05e863b68 29 q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]);
SvenD97 8:bba05e863b68 30 q2_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_twist[0]+1*(-y+y01))*des_twist[1]);
SvenD97 7:b77f2201b156 31
SvenD97 9:8e1112874c12 32 q_ref[1] = q_ref[1]+T*q1_star_des;
SvenD97 9:8e1112874c12 33 q_ref[2] = q_ref[2]+T*(q2_star_des - q1_star_des);
MaikOvermars 0:4cb1de41d049 34 }