Fabrice Tshilumba
nnama inverse kinematics
Diff: main.cpp
- Revision:
- 3:1a7bba50fb97
- Parent:
- 2:1d374991b3be
--- a/main.cpp Mon Oct 29 13:44:06 2018 +0000
+++ b/main.cpp Wed Oct 31 10:45:29 2018 +0000
@@ -1,125 +1,203 @@
#include "mbed.h"
+#include "MODSERIAL.h"
+#include <iostream>
+#include <string>
+
+
+
-template<std::size_t N, std::size_t M, std::size_t P>
-int mult(int A[N][M], int B[M][P]) {
- int C[N][P];
+//AnalogIn pot1(A0); // X-coord knob
+//AnalogIn pot2(A3); // Y-coord knob
+MODSERIAL pc(USBTX, USBRX);
+Ticker flipper;
+
+const double L1 = 0.328;
+const double L2 = 0.218;
- for (int n = 0; n < N; n++) {
- for (int p = 0; p < P; p++) {
- int num = 0;
- for (int m = 0; m < M; m++) {
- num += A[n][m] * B[m][p];
- }
- C[n][p] = num;
- std::cout << num << " ";
- }
- std::cout << std::endl;
- }
-
- return 0;
+const double PI = 3.14159265359;
+const double Ts = 1;
-}
-
-AnalogIn pot1(A0); // X-coord knob
-AnalogIn pot2(A3); // Y-coord knob
+ double qRef1 = 0*PI/180;
+ double qRef2 =(-qRef1 +0)*PI/180;
+
+ double tau_st1 =0;
+ double tau_st2 =0;
+
+ double pe0x =0;
+ double pe0y =0;
+
+ double Fx = 0;
+ double Fy = 0;
-int main()
-{
- const double L1 = 0.328;
- const double L2 = 0.218:
+
- double q1 = 0;
- double q2 =-q1 +0;
-
- double T1[3][3] = {
+ double T1[3][3]{
{0, -1, 0},
{1, 0, 0,},
{0, 0, 0,}};
- double T20[3][3] = {
+ double T20[3][3]{
{0, -1, 0},
{1, 0, -L1,},
{0, 0, 0,}};
- double H200[3][3] = {
+ double H200[3][3]{
{1, 0, L1+L2},
{0, 1, 0,},
{0, 0, 1,}};
- double Pe2 [3][1]={
+ double Pe2 [3][1]{
{0},
{0},
{1}};
- double q1 = q1*pi/180;
- double q2 = q2*pi/180;
+
- while (true) {
- double Pe_set[1][3] = { // defining setpoint location of end effector
- {pot1*0.546}, //setting xcoord to pot 1
- {pot2*0.4}, // setting ycoord to pot 2
+
+double C[5][5];
+
+template<std::size_t N, std::size_t M, std::size_t P>
+void mult(double A[N][M], double B[M][P]) {
+
+for( int n =0; n < 5; n++){
+ for(int p =0; p < 5; p++){
+ C[n][p] =0;
+ }
+ }
+ for (int n = 0; n < N; n++) {
+ for (int p = 0; p < P; p++) {
+ double num = 0;
+ for (int m = 0; m < M; m++) {
+ num += A[n][m] * B[m][p];
+
+ }
+
+ C[n][p] = num;
+
+ //std::cout << num << " ";
+ }
+ //std::cout << std::endl;
+ }
+
+}
+
+
+
+void flip () {
+ double potx = 0.218;//pot1.read()*0.546;
+ double poty = 0.328;//pot2.read()*0.4;
+
+ double Pe_set[3][1]{ // defining setpoint location of end effector
+ {potx}, //setting xcoord to pot 1
+ {poty}, // setting ycoord to pot 2
{1}};
//Calculating new H matrix
- double T1e=[3][3] = {
- {cos(q1), -sin(q1), 0},
- {sin(q1), cos(q1), 0},
+ double T1e[3][3]{
+ {cos(qRef1), -sin(qRef1), 0},
+ {sin(qRef1), cos(qRef1), 0},
{0, 0, 1}};
- double T20e[3][3] = {
- {cos(q2), -sin(q2), L1-L1*cos(q2)},
- {sin(q2), cos(q2), -L1*sin(q2)},
+ double T20e[3][3]{
+ {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
+ {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
{0, 0, 1}};
- double H20 = mult<3,3,3>(T1e,T20e); // matrix multiplication
- double Pe0 = mult<3,3,1>(H20,Pe2); // matrix multiplication
- double pe0x =Pe0[1]; // seperating coordinates of end effector location
- double pe0y = Pe[2];
+
+ mult<3,3,3>(T1e,T20e); // matrix multiplication
+ double H201[3][3]{
+ {C[0][0],C[0][1],C[0][2]},
+ {C[1][0],C[1][1],C[1][2]},
+ {C[2][0],C[2][1],C[2][2]}};
+
+ mult<3,3,3>(H201,H200); // matrix multiplication
+ double H20 [3][3]{
+ {C[0][0],C[0][1],C[0][2]},
+ {C[1][0],C[1][1],C[1][2]},
+ {C[2][0],C[2][1],C[2][2]}};
+
+ mult<3,3,1>(H20,Pe2); // matrix multiplication
+ double Pe0[3][1] {
+ {C[0][0]},
+ {C[1][0]},
+ {C[2][0]}};
+
+ double pe0x = Pe0[0][0]; // seperating coordinates of end effector location
+ double pe0y = Pe0[1][0];
// Determing the jacobian
- double T_1[3][1] = {
+ double T_1[3][1]{
{1},
- {T1[1][3]},
- {T1[2][3]};
+ {T1[0][2]},
+ {T1[1][2]}};
- double T_2[3][1] = {
+ double T_2[3][1]{
{1},
- {L1*sin(q1)},
- {-L1*cos(q1)}};
+ {L1*sin(qRef1)},
+ {-L1*cos(qRef1)}};
- double J[3][2] = {
- {T_1[1][1], T_2[1][1]},
- {T_1[2][1], T_2[2][1]},
- {T_1[3][1], T_2[3][1]}};
+ double J[3][2]{
+ {T_1[0][0], T_2[0][0]},
+ {T_1[1][0], T_2[1][0]},
+ {T_1[2][0], T_2[2][0]}};
//Determing 'Pulling" force to setpoint
double k= 1; //virtual stifness of the force
-
- double Fs[3][1] = { //force vector from end effector to setpoint
- {k*Pe_set[1][1] - k*Pe0[1][1]},
- {k*Pe_set[2][1] - k*Pe0[2][1]},
- {k*Pe_set[3][1] - k*Pe0[3][1]}};
-
- double Fx = Fs[1][1];
- double Fy = Fs[2][1];
- double W0t[3][1] = {
- {Pe0x*Fy - Pe0y*Fx},
+ double Fs[3][1]{ //force vector from end effector to setpoint
+ {k*Pe_set[0][0] - k*Pe0[0][0]},
+ {k*Pe_set[1][0] - k*Pe0[1][0]},
+ {k*Pe_set[2][0] - k*Pe0[2][0]}};
+ double Fx = k*potx - k*pe0x;
+ double Fy = k*poty - k*pe0y;
+ double W0t[3][1]{
+ {pe0x*Fy - pe0y*Fx},
{Fx},
{Fy}};
-
+
+ double Jt[2][3]{ // transposing jacobian matrix
+ {J[0][0], J[1][0], J[2][0]},
+ {T_2[0][0], T_2[1][0], T_2[2][0]}};
- double Jt[2][3] = { // transposing jacobian matrix
- {J[1][1], J[1][2], J[1][3]},
- {J[2][1], J[2][2], J[2][3]}};
-
- double tau_st = mult<2,3,1>(Jt,W0t);
+ mult<2,3,1>(Jt,W0t);
+ tau_st1 = C[0][0];
+ tau_st2 = C[1][0];
//Calculating joint behaviour
- double b =1; //joint friction coefficent
- double Ts = 1/1000; //Time step to reach the new angle
- double w_s = tau_st/b; // angular velocity
- double q1 = q1 +w_s*Ts; // calculating new angle of q1 in time step Ts
- double q2 = q2 + w_s*Ts; // calculating new angle of q2 in time step Ts
-
+ double b =1;
+ //joint friction coefficent
+ //double Ts = 1/1000; //Time step to reach the new angle
+ double w_s1 = tau_st1/b; // angular velocity
+ double w_s2 = tau_st2/b; // angular velocity
+ //checking angle boundaries
+ qRef1 = qRef1 +w_s1*Ts; // calculating new angle of qRef1 in time step Ts
+ if (qRef1 > 2*PI/3) {
+ qRef1 = 2*PI/3;
+ }
+ else if (qRef1 < PI/6) {
+ qRef1= PI/6;
+ }
+
+ qRef2 = qRef2 + w_s2*Ts; // calculating new angle of qRef2 in time step Ts
+ if (qRef2 > -PI/4) {
+ qRef2 = -PI/4;
+ }
+ else if (qRef2 < -PI/2) {
+ qRef2= -PI/2;
+ }
+
+}
+
+int main()
+{
+ pc.printf("start main function");
+ pc.baud(115200);
+ flipper.attach(&flip, Ts);
+ while (true){
+ pc.printf("qRef1 %f\n qRef2 %f\n, X %f\n Y %f \n",qRef1*180/PI,qRef2*180/PI,tau_st1,tau_st2);
+ wait (2.0);
+ }
+
}
+
+
- }
\ No newline at end of file
+
\ No newline at end of file