jordy morsinkhof / BrocketJacobian

Code werkend!

Fork of BrocketJacobian by Bram S

BrocketJacobian.cpp@0:92c31f1d4274, 2017-10-30 (annotated)

Committer:
BramS23
Date:
Mon Oct 30 13:51:13 2017 +0000
Revision:
0:92c31f1d4274
Child:
1:f58264bf73f9
Added inverse Jacobian;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:92c31f1d4274 1 #include "BrocketJacobian.h"
BramS23 0:92c31f1d4274 2
BramS23 0:92c31f1d4274 3 void Brocket(float q1,float q2,float &x,float &y){
BramS23 0:92c31f1d4274 4 float L1=27.5f;
BramS23 0:92c31f1d4274 5 float L2=32.0f;
BramS23 0:92c31f1d4274 6
BramS23 0:92c31f1d4274 7 x = L2*cos(q1+q2) + L1*cos(q1);
BramS23 0:92c31f1d4274 8 y = L2*sin(q1+q2) + L1*sin(q1);
BramS23 0:92c31f1d4274 9 }
BramS23 0:92c31f1d4274 10
BramS23 0:92c31f1d4274 11 void TransposeJacobian(float q1,float q2,float Fsprx,float Fspry,float &tau1,float &tau2){
BramS23 0:92c31f1d4274 12 float L1=27.5f;
BramS23 0:92c31f1d4274 13 float L2=32.0f;
BramS23 0:92c31f1d4274 14 tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1));
BramS23 0:92c31f1d4274 15 tau2 = Fspry*L2*cos(q1+q2) - Fsprx*L2*sin(q1+q2);
BramS23 0:92c31f1d4274 16 }
BramS23 0:92c31f1d4274 17
BramS23 0:92c31f1d4274 18 void InverseJacobian(float q1 , float q2, float vx, float vy, float &q1dot, float &q2dot){
BramS23 0:92c31f1d4274 19 float L1=27.5f;
BramS23 0:92c31f1d4274 20 float L2=32.0f;
BramS23 0:92c31f1d4274 21 q1dot = ( vx*cos(q1+q2) + vy*sin(q1+q2) )
BramS23 0:92c31f1d4274 22 /(L1*sin(q2));
BramS23 0:92c31f1d4274 23 q2dot = -(L1*vy*sin(q1) + L2*vx*cos(q1+q2) + L2*vy*sin(q1+q2) + L1*vx*cos(q1))
BramS23 0:92c31f1d4274 24 /(L1*L2*sin(q2));
BramS23 0:92c31f1d4274 25 }