File content as of revision 0:537f81d7b756:

#include "inverseKinematics.h"
#include "mbed.h"
#include "Matrix.h"
#include "MatrixMath.h"

// Member function definitions
inverseKinematics::inverseKinematics(float _L1, float _L2, float _dt):L1(_L1), L2(_L2), dt(_dt){

    }

// computeAngles computes the 
Matrix inverseKinematics :: computeAngles(float Vx, float Vy, float angle1, float angle2){
    
    Matrix T1(3,1);
    T1 << 1 << 0 << 0;
    
    Matrix T2(3,1);
    T2 << 1 << L1*cos(angle1) << L1*sin(angle1);
    
    Matrix J = T2;
    J.AddCol(J,T1,1);
//    J.print();

    
    Matrix H_f0(3,3);
    H_f0 << 1  << 0  << -L1*sin(angle1)-L2*sin(angle1+angle2)
         << 0  << 1  << L1*cos(angle1)+L2*cos(angle1+angle2)
         << 0  << 0  << 1;

    Matrix H_0f = MatrixMath::Inv( H_f0 );

    // Computing adjoint matrix    
    Matrix Ad_H_0f(3,3);
    Ad_H_0f << 1 << 0 << 0
            << -1*(L1*cos(angle1)+L2*cos(angle1+angle2)) << 1 << 0
            << -L1*sin(angle1)-L2*sin(angle1+angle2) << 0 << 1;
    
    Matrix J2 = Ad_H_0f*J;
    
    
    J2.DeleteRow(J2,1);
    //J2.print();
    
    Matrix J2_T = MatrixMath::Transpose(J2);
    Matrix J_fancy = MatrixMath::Inv(J2_T*J2) * J2_T;
    //J_fancy.print();
    
    Matrix v_des(2,1);
    v_des << Vx << Vy;
    
    Matrix q_dot = J_fancy*v_des;

    //angle1 = angle1 + dt*q_dot(1,1);
//    angle2 = angle2 + dt*q_dot(2,1);
    
    return q_dot;    
    }