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
- Committer:
- SvenD97
- Date:
- 2018-10-23
- Revision:
- 9:8e1112874c12
- Parent:
- 8:bba05e863b68
- Child:
- 12:3c47c7b1d1d7
File content as of revision 9:8e1112874c12:
#include "mbed.h" double L1 = 0.5; double L2 = 0.7; double x01 = 0.0; double y01 = 0.2; void forwardkinematics_function(double q1, double q2) { // input are joint angles, output are x and y position of end effector x = x01 + L1*cos(q1)-L2*cos(q2); y = y01 + L1 * sin(q1) - L2 * sin(q2); } double inversekinematics_function() { // pseudo inverse jacobian to get joint speeds // input are desired vx and vy of end effector, output joint angle speeds // I assume here that reference is a vector (this should also be global I think) // I also assume that the end effector position is global here and is denoted by x // Assuming the ticker_rate is also set to be global double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC) double q1_star_des; // desired joint velocity of q1_star double q2_star_des; // same as above but then for q2_star // 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? // The calculation below assumes that the end effector position is calculated before this function is executed q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]); 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]); q_ref[1] = q_ref[1]+T*q1_star_des; q_ref[2] = q_ref[2]+T*(q2_star_des - q1_star_des); }