PRISMA Lab / Mbed OS Hyfliers_arm_2

prima prova bracctio

Dependencies:   Eigen MX64_senzaCorrente

main.cpp@0:82a5f03b7eb4, 2019-11-18 (annotated)

Committer:
anfontanelli
Date:
Mon Nov 18 17:50:32 2019 +0000
Revision:
0:82a5f03b7eb4
arm created

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anfontanelli 0:82a5f03b7eb4 1 #include "mbed.h"
anfontanelli 0:82a5f03b7eb4 2 #include "ARAP180.h"
anfontanelli 0:82a5f03b7eb4 3 #include "platform/mbed_thread.h"
anfontanelli 0:82a5f03b7eb4 4 ARAP180 arm;
anfontanelli 0:82a5f03b7eb4 5
anfontanelli 0:82a5f03b7eb4 6
anfontanelli 0:82a5f03b7eb4 7
anfontanelli 0:82a5f03b7eb4 8 int main() {
anfontanelli 0:82a5f03b7eb4 9 arm.initArmMotors();
anfontanelli 0:82a5f03b7eb4 10
anfontanelli 0:82a5f03b7eb4 11 Vector6f q = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 12 Vector6f q_out = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 13 Vector6f q_out_sat = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 14
anfontanelli 0:82a5f03b7eb4 15 Matrix4f T = Matrix4f::Zero();
anfontanelli 0:82a5f03b7eb4 16 Matrix6f J = Matrix6f::Zero();
anfontanelli 0:82a5f03b7eb4 17 Matrix4f Td = Matrix4f::Identity();
anfontanelli 0:82a5f03b7eb4 18
anfontanelli 0:82a5f03b7eb4 19
anfontanelli 0:82a5f03b7eb4 20 Matrix4f T_e_b_0 = Matrix4f::Zero();
anfontanelli 0:82a5f03b7eb4 21 /*T_e_b_0(0,2) = 1.0;
anfontanelli 0:82a5f03b7eb4 22 T_e_b_0(1,0) = -1.0;
anfontanelli 0:82a5f03b7eb4 23 T_e_b_0(2,1) = -1.0;
anfontanelli 0:82a5f03b7eb4 24 T_e_b_0(0,3) = 0.142;
anfontanelli 0:82a5f03b7eb4 25 T_e_b_0(2,3) = -0.213;
anfontanelli 0:82a5f03b7eb4 26 T_e_b_0(3,3) = 1.0;*/
anfontanelli 0:82a5f03b7eb4 27
anfontanelli 0:82a5f03b7eb4 28
anfontanelli 0:82a5f03b7eb4 29
anfontanelli 0:82a5f03b7eb4 30 Matrix4f T_e_b = Matrix4f::Identity();
anfontanelli 0:82a5f03b7eb4 31 Matrix4f RotoTraslation = Matrix4f::Identity();
anfontanelli 0:82a5f03b7eb4 32
anfontanelli 0:82a5f03b7eb4 33 Matrix4f T_r_e = Matrix4f::Identity();
anfontanelli 0:82a5f03b7eb4 34
anfontanelli 0:82a5f03b7eb4 35 T_r_e(1,3) = -0.218;
anfontanelli 0:82a5f03b7eb4 36
anfontanelli 0:82a5f03b7eb4 37 double t = 0.0;
anfontanelli 0:82a5f03b7eb4 38
anfontanelli 0:82a5f03b7eb4 39 Vector6f q0 = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 40 Vector6f q_offsetted = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 41 Vector6f q_act = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 42 //q0 = arm.getJointPos();
anfontanelli 0:82a5f03b7eb4 43 q0(0) = 0.0;
anfontanelli 0:82a5f03b7eb4 44 q0(1) = M_PI/4;
anfontanelli 0:82a5f03b7eb4 45 q0(2) = M_PI/3;
anfontanelli 0:82a5f03b7eb4 46 q0(3) = -M_PI/3 + M_PI/4;
anfontanelli 0:82a5f03b7eb4 47 q0(4) = M_PI/2;
anfontanelli 0:82a5f03b7eb4 48 q0(5) = 0.0;
anfontanelli 0:82a5f03b7eb4 49 T_e_b_0 = arm.forwardKinematics(q0);
anfontanelli 0:82a5f03b7eb4 50
anfontanelli 0:82a5f03b7eb4 51
anfontanelli 0:82a5f03b7eb4 52 q_out = q0;
anfontanelli 0:82a5f03b7eb4 53
anfontanelli 0:82a5f03b7eb4 54 float traslation_x;
anfontanelli 0:82a5f03b7eb4 55 float traslation_y;
anfontanelli 0:82a5f03b7eb4 56 float traslation_z;
anfontanelli 0:82a5f03b7eb4 57
anfontanelli 0:82a5f03b7eb4 58 arm.setJointPos(q0);
anfontanelli 0:82a5f03b7eb4 59
anfontanelli 0:82a5f03b7eb4 60 wait_us(5000000);
anfontanelli 0:82a5f03b7eb4 61
anfontanelli 0:82a5f03b7eb4 62 arm.setMaxVel(100);
anfontanelli 0:82a5f03b7eb4 63
anfontanelli 0:82a5f03b7eb4 64 arm.setMaxAcc(100);
anfontanelli 0:82a5f03b7eb4 65
anfontanelli 0:82a5f03b7eb4 66 while (true){
anfontanelli 0:82a5f03b7eb4 67
anfontanelli 0:82a5f03b7eb4 68
anfontanelli 0:82a5f03b7eb4 69 traslation_x = 0.0;//0.3*sin(0.2*2*M_PI*t);//+0.15*sin(0.2*2*M_PI*t);
anfontanelli 0:82a5f03b7eb4 70 traslation_y = 0.0;//-0.4*sin(0.2*2*M_PI*t);
anfontanelli 0:82a5f03b7eb4 71 traslation_z = 0.25*sin(0.2*2*M_PI*t);//-0.15*sin(0.2*2*M_PI*t);//0.3+0.1*sin(0.2*2*M_PI*t);
anfontanelli 0:82a5f03b7eb4 72
anfontanelli 0:82a5f03b7eb4 73 RotoTraslation.block(0,0,3,3) = utilities::rotx(0.0);//utilities::rotx((M_PI/3)*sin(0.2*2*M_PI*t));
anfontanelli 0:82a5f03b7eb4 74
anfontanelli 0:82a5f03b7eb4 75 RotoTraslation(0,3) = traslation_x;
anfontanelli 0:82a5f03b7eb4 76 RotoTraslation(1,3) = traslation_y;
anfontanelli 0:82a5f03b7eb4 77 RotoTraslation(2,3) = traslation_z;
anfontanelli 0:82a5f03b7eb4 78
anfontanelli 0:82a5f03b7eb4 79 T_e_b=T_e_b_0*T_r_e*RotoTraslation*T_r_e.inverse();
anfontanelli 0:82a5f03b7eb4 80
anfontanelli 0:82a5f03b7eb4 81 //T_e_b=T_e_b_0*RotoTraslation;
anfontanelli 0:82a5f03b7eb4 82
anfontanelli 0:82a5f03b7eb4 83 T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization(T_e_b.block(0,0,3,3));
anfontanelli 0:82a5f03b7eb4 84
anfontanelli 0:82a5f03b7eb4 85 //T_e_b(0,3) = traslation_x;
anfontanelli 0:82a5f03b7eb4 86 //T_e_b(1,3) = traslation_y;
anfontanelli 0:82a5f03b7eb4 87 //T_e_b(2,3) = traslation_z;
anfontanelli 0:82a5f03b7eb4 88
anfontanelli 0:82a5f03b7eb4 89 /*printf("T_e_b=\n");
anfontanelli 0:82a5f03b7eb4 90 for(int i = 0; i<4; i++){
anfontanelli 0:82a5f03b7eb4 91 for(int j = 0; j<4; j++){
anfontanelli 0:82a5f03b7eb4 92 printf("%4.3f", T_e_b(i,j));
anfontanelli 0:82a5f03b7eb4 93 }
anfontanelli 0:82a5f03b7eb4 94 printf("\r\n\n");
anfontanelli 0:82a5f03b7eb4 95 } */
anfontanelli 0:82a5f03b7eb4 96
anfontanelli 0:82a5f03b7eb4 97 q_out = arm.backwardKinematics(T_e_b, q_out, 0.005, 1, 0.001, 0.001 ,150, 50);
anfontanelli 0:82a5f03b7eb4 98
anfontanelli 0:82a5f03b7eb4 99 q_out_sat = arm.jointSaturation(q_out);
anfontanelli 0:82a5f03b7eb4 100 //q_out = Vector6f::Zero();
anfontanelli 0:82a5f03b7eb4 101
anfontanelli 0:82a5f03b7eb4 102
anfontanelli 0:82a5f03b7eb4 103 /*printf("q=");
anfontanelli 0:82a5f03b7eb4 104 for(int i = 0; i<6; i++){
anfontanelli 0:82a5f03b7eb4 105 printf(" %4.1f",q_out[i]);
anfontanelli 0:82a5f03b7eb4 106 }
anfontanelli 0:82a5f03b7eb4 107 printf("\r\n"); */
anfontanelli 0:82a5f03b7eb4 108
anfontanelli 0:82a5f03b7eb4 109 /* printf("q_sat=");
anfontanelli 0:82a5f03b7eb4 110 for(int i = 0; i<6; i++){
anfontanelli 0:82a5f03b7eb4 111 printf(" %4.1f",q_out_sat[i]);
anfontanelli 0:82a5f03b7eb4 112 }
anfontanelli 0:82a5f03b7eb4 113 printf("\r\n\n"); */
anfontanelli 0:82a5f03b7eb4 114
anfontanelli 0:82a5f03b7eb4 115 //q_act = arm.getJointPos();
anfontanelli 0:82a5f03b7eb4 116
anfontanelli 0:82a5f03b7eb4 117
anfontanelli 0:82a5f03b7eb4 118
anfontanelli 0:82a5f03b7eb4 119 /* printf("q_act = ");
anfontanelli 0:82a5f03b7eb4 120 for(int i = 0; i<6; i++){
anfontanelli 0:82a5f03b7eb4 121 printf(" %4.2f ",q_act[i]);
anfontanelli 0:82a5f03b7eb4 122 }
anfontanelli 0:82a5f03b7eb4 123 printf("\r\n");*/
anfontanelli 0:82a5f03b7eb4 124
anfontanelli 0:82a5f03b7eb4 125 arm.setJointPos(q_out);
anfontanelli 0:82a5f03b7eb4 126
anfontanelli 0:82a5f03b7eb4 127 //arm.gravityVector(q);
anfontanelli 0:82a5f03b7eb4 128 //arm.inertiaMatrix(q);
anfontanelli 0:82a5f03b7eb4 129 //arm.stiffnessVector(q);
anfontanelli 0:82a5f03b7eb4 130 //arm.frictionVector(q);
anfontanelli 0:82a5f03b7eb4 131
anfontanelli 0:82a5f03b7eb4 132
anfontanelli 0:82a5f03b7eb4 133
anfontanelli 0:82a5f03b7eb4 134
anfontanelli 0:82a5f03b7eb4 135 t = t+0.001;
anfontanelli 0:82a5f03b7eb4 136 wait_us(1000);
anfontanelli 0:82a5f03b7eb4 137
anfontanelli 0:82a5f03b7eb4 138 }
anfontanelli 0:82a5f03b7eb4 139
anfontanelli 0:82a5f03b7eb4 140
anfontanelli 0:82a5f03b7eb4 141 }
anfontanelli 0:82a5f03b7eb4 142