Forum Comments

Modifying the AR4 program to fit a differential wrist
In Questions
Alexandre Le Roux
Feb 12, 2022
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<float>(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 !
0
1
Alexandre Le Roux
More actions