Diff: main.cpp

Revision:

33:daa6ec305441

Parent:

32:76c4d7bb2022

Child:

34:d6ec7c634763

--- a/main.cpp	Thu Oct 22 10:57:50 2015 +0000
+++ b/main.cpp	Thu Oct 22 11:03:51 2015 +0000
@@ -902,8 +902,8 @@
     
     //inverse kinematics (error in position to error in angles)
     dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+    dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
     /*
-    dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*l2^2*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
     dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*cos(theta1 + theta2)^2*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
     dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
  */