Ja kno it
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 16 2022 06:32:39 by
1.7.2