Modifying the AR4 program to fit a differential wrist

Hello, Im happy to see people modify the program and share it with everyone, I only ask people not resell for profit. You dont need to use encoders. the software has an option for open loop mode on the settings tab.

Thank you for your answer Chris, I found how to disable the encoders in the software.

I understood how to modify the DH parameters to change the dimensions of the robot.

I also have made progress in implementing the wrist kinematics equations in the program. I modified the AR2 kinematics spreadsheet to find the wrist angles of the motors after reduction.

Here's the spreadsheet : https://docs.google.com/spreadsheets/d/1iReknbEAj4RcIj4BmtkDDe6l3gSqDisI/edit?usp=sharing&ouid=103973936673798901416&rtpof=true&sd=true

Now I just need to modify the teensy 4.1 program in the same way. I have found where the forward and inverse kinematics are programmed in the code.

If anyone has a tip on how to do this, again, any help is greatly appreciated.

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 !

Ovviamente non ho capito nulla ,spero un giorno di riuscire a programmare,se avete fa suggerire un percorso vi sarò grato.