File content as of revision 13:3482d315877c:

 
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
    
    currentx = x01 + L1*cos(q1)-L2*cos(q2);
    currenty = y01 + L1 * sin(q1) - L2 * sin(q2);    
}
 
double inversekinematics_function(double& x, double& y, const double& T, double& qref1, double& qref2, double& q1, double& q2, double& des_vx, double& des_vy) {
    // x, y: positions of end effector | T: period | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
    
    // pseudo inverse jacobian to get joint speeds
    // input are desired vx and vy of end effector, output joint angle speeds
 
    double q1_star_des; // desired joint velocity of q1_star
    double q2_star_des; // same as above but then for q2_star
 
    // The calculation below assumes that the end effector position is calculated before this function is executed
    // In our case the determinant will not equal zero, hence no problems with singularies I think.
    q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
    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);
    
    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
    qref2 = q2+T*(q2_star_des - q1_star_des);    
    
}