Revision 0:92c31f1d4274, committed 2017-10-30

Comitter:

BramS23

Date:

Mon Oct 30 13:51:13 2017 +0000

Commit message:

Added inverse Jacobian;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BrocketJacobian.cpp	Mon Oct 30 13:51:13 2017 +0000
@@ -0,0 +1,25 @@
+#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);
+    y = L2*sin(q1+q2) + L1*sin(q1);
+}
+
+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));
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BrocketJacobian.h	Mon Oct 30 13:51:13 2017 +0000
@@ -0,0 +1,32 @@
+#include "mbed.h"
+
+/** Brocket function
+    Used to determine the end effector position depending on the joint angles
+    @param q1 current position of joint 1 in radians
+    @param q2 current position of joint 2 in radians
+    @param x Output variable for the x position of the end effector
+    @param y Output variable for the y position of the end effector
+*/
+void Brocket(float q1,float q2,float &x,float &y);
+
+/** Jacobian Tansposed function
+    Used to determine the joint torques depending on the Forces applied to the end effector
+    @param q1 current position of joint 1 in radians
+    @param q2 current position of joint 2 in radians
+    @param Fsprx Force applied to the end effector in x direction
+    @param Fspry Force applied to the end effector in y direction
+    @param tau1 Output variable for the Moment in joint1
+    @param tau2 Output variable for the Moment in joint2
+*/
+void TransposeJacobian(float q1,float q2,float Fsprx,float Fspry,float &tau1,float &tau2);
+
+/** Jacobian Tansposed function
+    Used to determine the joint velocities depending on the end effector velocities
+    @param q1 current position of joint 1 in radians
+    @param q2 current position of joint 2 in radians
+    @param vx Velocity of the end effector in x direction
+    @param vy Velocity of the end effector in y direciton
+    @param tau1 Output variable for the Moment in joint1
+    @param tau2 Output variable for the Moment in joint2
+*/
+void InverseJacobian(float q1 , float q2, float vx, float vy, float &q1dot, float &q2dot);
\ No newline at end of file