Ja kno it

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers BrocketJacobian.cpp Source File

BrocketJacobian.cpp

00001 #include "BrocketJacobian.h"
00002 
00003 void Brocket(float q1,float q2,float &x,float &y){
00004     float L1=27.5f;
00005     float L2=32.0f;
00006     
00007     x = L2*cos(q1+q2) + L1*cos(q1);
00008     y = L2*sin(q1+q2) + L1*sin(q1);
00009 }
00010 
00011 void TransposeJacobian(float q1,float q2,float Fsprx,float Fspry,float &tau1,float &tau2){
00012     float L1=27.5f;
00013     float L2=32.0f;
00014     tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1));
00015     tau2 = Fspry*L2*cos(q1+q2)              - Fsprx*L2*sin(q1+q2);
00016 }
00017 
00018 void InverseJacobian(float q1 , float q2, float vx, float vy, float &q1dot, float &q2dot){
00019     float L1=27.5f;
00020     float L2=32.0f;
00021     q1dot = ( vx*cos(q1+q2)         + vy*sin(q1+q2) )
00022             /(L1*sin(q2));
00023     q2dot = -(L1*vy*sin(q1) + L2*vx*cos(q1+q2) + L2*vy*sin(q1+q2) + L1*vx*cos(q1))
00024             /(L1*L2*sin(q2));
00025 }