jordy morsinkhof / BrocketJacobian

Code werkend!

Fork of BrocketJacobian by Bram S

BrocketJacobian.cpp

Committer:
jordymorsinkhof
Date:
2017-11-01
Revision:
2:380a27f546ea
Parent:
1:f58264bf73f9

File content as of revision 2:380a27f546ea:

#include "BrocketJacobian.h"

void Brocket(float q1,float q2,float &x,float &y){
    float L1=27.5f;
    float L2=32.0f;
    
    x = L2*cos(q1+q2) + L1*cos(q1) - 14.3f;
    y = L2*sin(q1+q2) + L1*sin(q1) + 11.9f;
}

void TransposeJacobian(float q1,float q2,float Fsprx,float Fspry,float &tau1,float &tau2){
    float L1=27.5f;
    float L2=32.0f;
    tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1));
    tau2 = Fspry*L2*cos(q1+q2)              - Fsprx*L2*sin(q1+q2);
}

void InverseJacobian(float q1 , float q2, float vx, float vy, float &q1dot, float &q2dot){
    float L1=27.5f;
    float L2=32.0f;
    q1dot = ( vx*cos(q1+q2)         + vy*sin(q1+q2) )
            /(L1*sin(q2));
    q2dot = -(L1*vy*sin(q1) + L2*vx*cos(q1+q2) + L2*vy*sin(q1+q2) + L1*vx*cos(q1))
            /(L1*L2*sin(q2));
}