Alright, Looks like I found how to do it.
In the solveForwardKinematic() function, I added the equations before solving the forward kinematics.
void SolveFowardKinematic() {
robot_set_AR3();
float target_xyzuvw[6];
float joints[ROBOT_nDOFs];
float joints3;
float joints4;
float joints5;
for (int i = 0; i < ROBOT_nDOFs; i++) {
joints[i] = JangleIn[i];
}
//Conversion differential wrist
joints3 = joints[3];
joints4 = joints[4];
joints5 = joints[5];
joints[4] = (20/16) * (joints4 - joints3);
joints[5] = (20/16) * (joints5 - joints3) - (20/16) * (joints4 - joints3);
forward_kinematics_robot_xyzuvw(joints, target_xyzuvw);
xyzuvw_Out[0] = target_xyzuvw[0];
xyzuvw_Out[1] = target_xyzuvw[1];
xyzuvw_Out[2] = target_xyzuvw[2];
xyzuvw_Out[3] = target_xyzuvw[3] / M_PI * 180;
xyzuvw_Out[4] = target_xyzuvw[4] / M_PI * 180;
xyzuvw_Out[5] = target_xyzuvw[5] / M_PI * 180;
}
Also, I added the inverse equations in the solveInverseKinematics function, after solving the inverse kinematics.
void SolveInverseKinematic() {
float joints[ROBOT_nDOFs];
float target[6];
float JangleOut3;
float JangleOut4;
float JangleOut5;
float solbuffer[ROBOT_nDOFs] = {0};
int NumberOfSol = 0;
int solVal = 0;
KinematicError = 0;
JointEstimate();
target[0] = xyzuvw_In[0];
target[1] = xyzuvw_In[1];
target[2] = xyzuvw_In[2];
target[3] = xyzuvw_In[3] * M_PI / 180;
target[4] = xyzuvw_In[4] * M_PI / 180;
target[5] = xyzuvw_In[5] * M_PI / 180;
// Serial.println("X : " + String(target[0]) + " Y : " + String(target[1]) + " Z : " + String(target[2]) + " rx : " + String(xyzuvw_In[3]) + " ry : " + String(xyzuvw_In[4]) + " rz : " + String(xyzuvw_In[5]));
for (int i = -3; i <= 3; i++) {
joints_estimate[4] = i * 30;
int success = inverse_kinematics_robot_xyzuvw(target, joints, joints_estimate);
if (success) {
if (solbuffer[4] != joints[4]) {
if (robot_joints_valid(joints)) {
for (int j = 0; j < ROBOT_nDOFs; j++) {
solbuffer[j] = joints[j];
SolutionMatrix[j][NumberOfSol] = solbuffer[j];
}
if (NumberOfSol <= 6) {
NumberOfSol++;
}
}
}
} else {
KinematicError = 1;
}
}
joints_estimate[4] = JangleIn[4];
solVal = 0;
for (int i = 0; i < ROBOT_nDOFs; i++) {
if ((abs(joints_estimate[i] - SolutionMatrix[i][0]) > 20) and NumberOfSol > 1) {
solVal = 1;
} else if ((abs(joints_estimate[i] - SolutionMatrix[i][1]) > 20) and NumberOfSol > 1) {
solVal = 0;
}
// Serial.println(String(i) + " Joint estimate : " + String(joints_estimate[i]) + " // Joint sol 1 : " + String(SolutionMatrix[i][0]) + " // Joint sol 2 : " + String(SolutionMatrix[i][1]));
}
if (NumberOfSol==0) {
KinematicError = 1;
}
// Serial.println("Sol : " + String(solVal) + " Nb sol : " + String(NumberOfSol));
for (int i = 0; i < ROBOT_nDOFs; i++) {
JangleOut[i] = SolutionMatrix[i][solVal];
}
//Conversion differential wrist
JangleOut3 = JangleOut[3];
JangleOut4 = JangleOut[4];
JangleOut5 = JangleOut[5];
JangleOut[4] = JangleOut4 * 16/20 + JangleOut3;
JangleOut[5] = (JangleOut5 + 20/16 * JangleOut3 + JangleOut4) * 16/20;
}
I haven't tested the code, as the robot isn't built yet. I will give updates once I make it work.
See ya !
