问:
6DOF Kinematics moveo
I asked on stackoverflow for the DH-Parameter for my robotic arm (moveo bcn3d).
I also copied the arduino code of Skyentific (YouTuber). Modified it to match my settings but i still does not work. Can someone please help me??? I am getting depressed if this keeps going like that.

In the home position, all joints have angle 0.
If I tell the forward kinematic that I want the 3rd joint and the 5th joint rotated 90 degrees, the values should be: (224, 0, 280) {x, y, z} should come out + some euler angles (can be ignored for now).
But I don't understand why these values come out, because the r and d values of the DH matrix are correct(https://stackoverflow.com/questions/67159164/denavit-hartenberg-6dof-moveo-inverse-kinematic-robot-arm?noredirect=1#comment118726837_67159164)
Output of the Arduino:\

inverse:
0.00
0.00
-0.00
-180.00
180.00
-180.00

forward:
-224.00
0.00
280.00
45.00
180.00
45.00

In this example you can see that the inverse kinematic calculates three times 180 although the arm is already in the home position.
Also you can see that the forward kinematics calculates a negative number for the x axis and strange rotations for joints 4, 5 and 6.

Translated with www.DeepL.com/Translator (free version)

The DH-Parameters: https://stackoverflow.com/questions/67159164/denavit-hartenberg-6dof-moveo-inverse-kinematic-robot-arm?noredirect=1#comment118726837_67159164

His github:https://github.com/SkyentificGit/SmallRobotArm

The YT-video:https://youtu.be/Sgsn2CM3bjY

You can ignore the Pins and just focus on the inverse and forward kinematics

```/*
Simple script to move my tiny 6dof robotic arm
*/
#include <math.h>

#define PI 3.1415926535897932384626433832795

//driver for the axis 1
#define PUL1_PIN 39
#define DIR1_PIN 37
//driver for the axis 2
#define PUL2_PIN 43
#define DIR2_PIN 41
//driver for the axis 3
#define PUL3_PIN 47
#define DIR3_PIN 45
//driver for the axis 4
#define PUL4_PIN 46
#define DIR4_PIN 48
//driver for the axis 5
#define PUL5_PIN A6
#define DIR5_PIN A7
//driver for the axis 6
#define PUL6_PIN A0
#define DIR6_PIN A1

//enable pin for the axis 3, 2 and 1
#define EN321_PIN 32
#define EN4_PIN A8
#define EN5_PIN A2
#define EN6_PIN 38

double curPos1 = 0.0;
double curPos2 = 0.0;
double curPos3 = 0.0;
double curPos4 = 0.0;
double curPos5 = 0.0;
double curPos6 = 0.0;

boolean PULstat1 = 0;
boolean PULstat2 = 0;
boolean PULstat3 = 0;
boolean PULstat4 = 0;
boolean PULstat5 = 0;
boolean PULstat6 = 0;

//robot geometry
const double dl1 = 360.0/200.0/32.0;
const double dl2 = 360.0/200.0/32.0;
const double dl3 = 360.0/200.0/32.0;
const double dl4 = 360.0/200.0/32.0;
const double dl5 = 360.0/200.0/32.0;
const double dl6 = 360.0/200.0/32.0;
const double r1 = 0.0;
const double r2 = 223.0;
const double r3 = 0.0;
const double d1 = 232.0;
const double d3 = 0.0;
const double d4 = 224.0;
const double d6 = 175.0;

void setup()
{
  Serial.begin(9600);
  pinMode(PUL1_PIN, OUTPUT);
  pinMode(DIR1_PIN, OUTPUT);
  pinMode(PUL2_PIN, OUTPUT);
  pinMode(DIR2_PIN, OUTPUT);
  pinMode(PUL3_PIN, OUTPUT);
  pinMode(DIR3_PIN, OUTPUT);
  pinMode(PUL4_PIN, OUTPUT);
  pinMode(DIR4_PIN, OUTPUT);
  pinMode(PUL5_PIN, OUTPUT);
  pinMode(DIR5_PIN, OUTPUT);
  pinMode(PUL6_PIN, OUTPUT);
  pinMode(DIR6_PIN, OUTPUT);

  pinMode(EN321_PIN, OUTPUT);
  pinMode(EN4_PIN, OUTPUT);
  pinMode(EN5_PIN, OUTPUT);
  pinMode(EN6_PIN, OUTPUT);

  digitalWrite(PUL1_PIN, LOW); // gear ratio = 96/20 = 4.8
  digitalWrite(DIR1_PIN, LOW); //LOW = negative direction

  digitalWrite(PUL2_PIN, LOW); // gear ratio = 4
  digitalWrite(DIR2_PIN, LOW); //LOW = positive direction

  digitalWrite(PUL3_PIN, LOW); // gear ratio = 5
  digitalWrite(DIR3_PIN, LOW); //LOW = negative direction

  digitalWrite(PUL4_PIN, LOW); // gear ratio = 56/20 = 2.8
  digitalWrite(DIR4_PIN, LOW); //LOW = positive direction

  digitalWrite(PUL5_PIN, LOW); // gear ratio = 42/20 = 2.1
  digitalWrite(DIR5_PIN, LOW); //LOW = positive direction

  digitalWrite(PUL6_PIN, LOW); // gear ratio = 1
  digitalWrite(DIR6_PIN, LOW); //LOW = positive direction

  // all joints disabled!
  digitalWrite(EN32A1_PIN, HIGH);
  digitalWrite(EN4_PIN, HIGH);
  digitalWrite(EN5_PIN, HIGH);
  digitalWrite(EN6_PIN, HIGH);
}

void loop()
{
  // enable all joints
  delay(5000);
  Serial.println("gogo");
  digitalWrite(EN321_PIN, LOW);
  digitalWrite(EN4_PIN, LOW);
  digitalWrite(EN5_PIN, LOW);
  digitalWrite(EN6_PIN, LOW);
  delay(1000);
  //--------------------------------------------------------GoGoGo-------------------
  curPos1=0.0;
  curPos2=0.0;
  curPos3=0.0;
  curPos4=0.0;
  curPos5=0.0;
  curPos6=0.0;

  //float Xhome[6]={0.0, 0.0, 854.0, 0.0, 0.0, 0.0};
  float Xhome[6]={224.0, 0.0, 280.0, 0.0, 180.0, 0.0};
  float Yhome[6]={0.0, 0.0, -90.0, 0.0, -90.0, 0.0};

  float X1[6]={164.5, 0.0, 141.0, 90.0, 180.0, -90.0};
  float X11[6]={164.5+14.7, 35.4, 141.0, 90.0, 180.0, -90.0};
  float X12[6]={164.5+50.0, 50.0, 141.0, 90.0, 180.0, -90.0};
  float X13[6]={164.5+85.3, 35.4, 141.0, 90.0, 180.0, -90.0};
  float X14[6]={164.5+100.0, 0.0, 141.0, 90.0, 180.0, -90.0};
  float X15[6]={164.5+85.3, -35.4, 141.0, 90.0, 180.0, -90.0};
  float X16[6]={164.5+50.0, -50.0, 141.0, 90.0, 180.0, -90.0};
  float X17[6]={164.5+14.7, -35.4, 141.0, 90.0, 180.0, -90.0};

  float X18[6]={164.5+50.0, 0.0, 141.0, 90.0, 180.0, -90.0};

  float X2[6]={264.5, 0.0, 141.0, 0.0, 90.0, 0.0};
  float X3[6]={164.5, 100.0, 141.0, 90.0, 90.0, 0.0};
  float X4[6]={164.5, -100.0, 141.0, 90.0, -90.0, 0.0};

  float Jhome[6], J1[6], J11[6], J12[6], J13[6], J14[6], J15[6], J16[6], J17[6], J18[6], J2[6], J3[6], J4[6];
  float Fhome[6];
  InverseK(Xhome, Jhome);
  ForwardK(Yhome, Fhome);
  Serial.println("inverse:");
  Serial.println(Jhome[0]);
  Serial.println(Jhome[1]);
  Serial.println(Jhome[2]);
  Serial.println(Jhome[3]);
  Serial.println(Jhome[4]);
  Serial.println(Jhome[5]);
  Serial.println("------");
  Serial.println("forward:");
  Serial.println(Fhome[0]);
  Serial.println(Fhome[1]);
  Serial.println(Fhome[2]);
  Serial.println(Fhome[3]);
  Serial.println(Fhome[4]);
  Serial.println(Fhome[5]);
  Serial.println("------");
  InverseK(X1, J1);
  InverseK(X11, J11);
  InverseK(X12, J12);
  InverseK(X13, J13);
  InverseK(X14, J14);
  InverseK(X15, J15);
  InverseK(X16, J16);
  InverseK(X17, J17);
  InverseK(X18, J18);
  InverseK(X2, J2);
  InverseK(X3, J3);
  InverseK(X4, J4);

  goStrightLine(Jhome, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  float velG=0.25e-4;
  goStrightLine(J1, J11, 0.25e-4, 0.75e-10, 0.0, 0.5*velG);
  goStrightLine(J11, J12, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J12, J13, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J13, J14, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J14, J15, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J15, J16, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J16, J17, 0.25e-4, 0.75e-10, 0.5*velG, 0.5*velG);
  goStrightLine(J17, J1, 0.25e-4, 0.75e-10, 0.5*velG, 0.0);

  goStrightLine(J1, J18, 0.25e-4, 0.75e-10, 0.0, 0.8*velG);
  goStrightLine(J18, J14, 0.25e-4, 0.75e-10, 0.8*velG, 0.0);
  goStrightLine(J14, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  goStrightLine(J1, J2, 0.25e-4, 0.75e-10, 0.0, 0.0);
  goStrightLine(J2, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  goStrightLine(J1, J3, 0.25e-4, 0.75e-10, 0.0, 0.0);
  goStrightLine(J3, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  goStrightLine(J1, J4, 0.25e-4, 0.75e-10, 0.0, 0.0);
  goStrightLine(J4, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  goStrightLine(J1, Jhome, 0.25e-4, 0.75e-10, 0.0, 0.0);
  //--------------------------------------------------------GoGoGoBack---------------
}

void goStrightLine(float* xfi, float* xff, float vel0, float acc0, float velini, float velfin){
  //
  float lmax = max(abs(xff[0]-xfi[0]),abs(xff[1]-xfi[1]));
  lmax = max(lmax,abs(xff[2]-xfi[2]));
  lmax = max(lmax,abs(xff[3]-xfi[3]));
  lmax = max(lmax,abs(xff[4]-xfi[4]));
  lmax = max(lmax,abs(xff[5]-xfi[5]));
  unsigned long preMil = micros();
  double l = 0.0;
  vel0 = min(vel0,sqrt(lmax*acc0+0.5*velini*velini+0.5*velfin*velfin));
  unsigned long curMil = micros();
  unsigned long t = 0;
  double tap = vel0/acc0-velini/acc0;
  double lap = velini*tap+acc0*tap*tap/2.0;
  double lcsp = lmax-(vel0*vel0/2.0/acc0-velfin*velfin/2.0/acc0);
  double tcsp = (lcsp-lap)/vel0+tap;
  double tfin = vel0/acc0-velfin/acc0+tcsp;
  while (curMil-preMil<=tfin){
    t = curMil-preMil;
    //acceleration phase
    if (t<=tap) {
      l = velini*t+acc0*t*t/2.0;
    }
    //contant maximum speed phase
    if (t>tap && t<=tcsp) {
      l = lap+vel0*(t-tap);
    }
    //deceleration phase
    if (t>tcsp) {
      l = lcsp+vel0*(t-tcsp)-acc0*(t-tcsp)*(t-tcsp)/2.0;
    }

    //trajectory x and y as a function of l
    float Xx[6];
    Xx[0]=xfi[0]+(xff[0]-xfi[0])/lmax*l;
    Xx[1]=xfi[1]+(xff[1]-xfi[1])/lmax*l;
    Xx[2]=xfi[2]+(xff[2]-xfi[2])/lmax*l;
    Xx[3]=xfi[3]+(xff[3]-xfi[3])/lmax*l;
    Xx[4]=xfi[4]+(xff[4]-xfi[4])/lmax*l;
    Xx[5]=xfi[5]+(xff[5]-xfi[5])/lmax*l;

    goTrajectory(Xx);
    curMil = micros();
  }
}

void goTrajectory(float* Jf){

  //execution
  int delF=2;
  // joint #1
  if (Jf[0]-curPos1>0.0) { // positive direction of rotation
    digitalWrite(DIR1_PIN, HIGH);
    while (Jf[0]-curPos1>dl1/2.0) {
      if (PULstat1 == 0) {
        digitalWrite(PUL1_PIN, HIGH);
        PULstat1 = 1;
      } else {
        digitalWrite(PUL1_PIN, LOW);
        PULstat1 = 0;
      }
      //curPos1 = Jf[0];
      curPos1 = curPos1 + dl1/2.0;
      if (Jf[0]-curPos1>dl1/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR1_PIN, LOW);
    while (-Jf[0]+curPos1>dl1/2.0) {
      if (PULstat1 == 0) {
        digitalWrite(PUL1_PIN, HIGH);
        PULstat1 = 1;
      } else {
        digitalWrite(PUL1_PIN, LOW);
        PULstat1 = 0;
      }
      //curPos1 = Jf[0];
      curPos1 = curPos1 - dl1/2.0;
      if (-Jf[0]+curPos1>dl1/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
  // joint #2
  if (Jf[1]-curPos2>0.0) { // positive direction of rotation
    digitalWrite(DIR2_PIN, HIGH);
    while (Jf[1]-curPos2>dl2/2.0) {
      if (PULstat2 == 0) {
        digitalWrite(PUL2_PIN, HIGH);
        PULstat2 = 1;
      } else {
        digitalWrite(PUL2_PIN, LOW);
        PULstat2 = 0;
      }
      //curPos2 = Jf[1];
      curPos2 = curPos2 + dl2/2.0;
      if (Jf[1]-curPos2>dl2/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR2_PIN, LOW);
    while (-Jf[1]+curPos2>dl2/2.0) {
      if (PULstat2 == 0) {
        digitalWrite(PUL2_PIN, HIGH);
        PULstat2 = 1;
      } else {
        digitalWrite(PUL2_PIN, LOW);
        PULstat2 = 0;
      }
      //curPos2 = Jf[1];
      curPos2 = curPos2 - dl2/2.0;
      if (-Jf[1]+curPos2>dl2/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
  // joint #3
  if (Jf[2]-curPos3>0.0) { // positive direction of rotation
    digitalWrite(DIR3_PIN, LOW);
    while (Jf[2]-curPos3>dl3/2.0) {
      if (PULstat3 == 0) {
        digitalWrite(PUL3_PIN, HIGH);
        PULstat3 = 1;
      } else {
        digitalWrite(PUL3_PIN, LOW);
        PULstat3 = 0;
      }
      //curPos3 = Jf[2];
      curPos3 = curPos3 + dl3/2.0;
      if (Jf[2]-curPos3>dl3/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR3_PIN, HIGH);
    while (-Jf[2]+curPos3>dl3/2.0) {
      if (PULstat3 == 0) {
        digitalWrite(PUL3_PIN, HIGH);
        PULstat3 = 1;
      } else {
        digitalWrite(PUL3_PIN, LOW);
        PULstat3 = 0;
      }
      //curPos3 = Jf[2];
      curPos3 = curPos3 - dl3/2.0;
      if (-Jf[2]+curPos3>dl3/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
  // joint #4
  if (Jf[3]-curPos4>0.0) { // positive direction of rotation
    digitalWrite(DIR4_PIN, HIGH);
    while (Jf[3]-curPos4>dl4/2.0) {
      if (PULstat4 == 0) {
        digitalWrite(PUL4_PIN, HIGH);
        PULstat4 = 1;
      } else {
        digitalWrite(PUL4_PIN, LOW);
        PULstat4 = 0;
      }
      //curPos4 = Jf[3];
      curPos4 = curPos4 + dl4/2.0;
      if (Jf[3]-curPos4>dl4/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR4_PIN, LOW);
    while (-Jf[3]+curPos4>dl4/2.0) {
      if (PULstat4 == 0) {
        digitalWrite(PUL4_PIN, HIGH);
        PULstat4 = 1;
      } else {
        digitalWrite(PUL4_PIN, LOW);
        PULstat4 = 0;
      }
      //curPos4 = Jf[3];
      curPos4 = curPos4 - dl4/2.0;
      if (-Jf[3]+curPos4>dl4/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
  // joint #5
  if (Jf[4]-curPos5>0.0) { // positive direction of rotation
    digitalWrite(DIR5_PIN, HIGH);
    while (Jf[4]-curPos5>dl5/2.0) {
      if (PULstat5 == 0) {
        digitalWrite(PUL5_PIN, HIGH);
        PULstat5 = 1;
      } else {
        digitalWrite(PUL5_PIN, LOW);
        PULstat5 = 0;
      }
      //curPos5 = Jf[4];
      curPos5 = curPos5 + dl5/2.0;
      if (Jf[4]-curPos5>dl5/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR5_PIN, LOW);
    while (-Jf[4]+curPos5>dl5/2.0) {
      if (PULstat5 == 0) {
        digitalWrite(PUL5_PIN, HIGH);
        PULstat5 = 1;
      } else {
        digitalWrite(PUL5_PIN, LOW);
        PULstat5 = 0;
      }
      //curPos5 = Jf[4];
      curPos5 = curPos5 - dl5/2.0;
      if (-Jf[4]+curPos5>dl5/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
  // joint #6
  if (Jf[5]-curPos6>0.0) { // positive direction of rotation
    digitalWrite(DIR6_PIN, HIGH);
    while (Jf[5]-curPos6>dl6/2.0) {
      if (PULstat6 == 0) {
        digitalWrite(PUL6_PIN, HIGH);
        PULstat6 = 1;
      } else {
        digitalWrite(PUL6_PIN, LOW);
        PULstat6 = 0;
      }
      //curPos6 = Jf[5];
      curPos6 = curPos6 + dl6/2.0;
      if (Jf[5]-curPos6>dl6/2.0) {
        delayMicroseconds(delF);
      }
    }
  } else {
    digitalWrite(DIR6_PIN, LOW);
    while (-Jf[5]+curPos6>dl6/2.0) {
      if (PULstat6 == 0) {
        digitalWrite(PUL6_PIN, HIGH);
        PULstat6 = 1;
      } else {
        digitalWrite(PUL6_PIN, LOW);
        PULstat6 = 0;
      }
      //curPos6 = Jf[5];
      curPos6 = curPos6 - dl6/2.0;
      if (-Jf[5]+curPos6>dl6/2.0) {
        delayMicroseconds(delF);
      }
    }
  }
}

void InverseK(float* Xik, float* Jik) {
  // inverse kinematics
  // input: Xik - pos value for the calculation of the inverse kinematics
  // output: Jfk - joints value for the calculation of the inversed kinematics

  // from deg to rad
  // Xik(4:6)=Xik(4:6)*pi/180;
  Xik[3]=Xik[3]*PI/180.0;
  Xik[4]=Xik[4]*PI/180.0;
  Xik[5]=Xik[5]*PI/180.0;

  // Denavit-Hartenberg matrix
  float theta[6]={0.0, 90.0, -90.0, 0.0, 0.0, 0.0}; // theta=[0; -90+0; 0; 0; 0; 0];
  float alfa[6]={90.0, 0.0, -90.0, 90.0, -90.0, 0.0}; // alfa=[-90; 0; -90; 90; -90; 0];
  float r[6]={r1, r2, r3, 0.0, 0.0, 0.0}; // r=[47; 110; 26; 0; 0; 0];
  float d[6]={d1, 0.0, d3, d4, 0.0, d6}; // d=[133; 0; 7; 117.5; 0; 28];
  // from deg to rad
  MatrixScale(theta, 6, 1, PI/180.0); // theta=theta*pi/180;
  MatrixScale(alfa, 6, 1, PI/180.0); // alfa=alfa*pi/180;

  // work frame
  float Xwf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xwf=[0; 0; 0; 0; 0; 0];

  // tool frame
  float Xtf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xtf=[0; 0; 0; 0; 0; 0];

  // work frame transformation matrix
  float Twf[16];
  pos2tran(Xwf, Twf); // Twf=pos2tran(Xwf);

  // tool frame transformation matrix
  float Ttf[16];
  pos2tran(Xtf, Ttf); // Ttf=pos2tran(Xtf);

  // total transformation matrix
  float Twt[16];
  pos2tran(Xik, Twt); // Twt=pos2tran(Xik);

  // find T06
  float inTwf[16], inTtf[16], Tw6[16], T06[16];
  invtran(Twf, inTwf); // inTwf=invtran(Twf);
  invtran(Ttf, inTtf); // inTtf=invtran(Ttf);
  MatrixMultiply(Twt, inTtf, 4, 4, 4, Tw6); // Tw6=Twt*inTtf;
  MatrixMultiply(inTwf, Tw6, 4, 4, 4, T06); // T06=inTwf*Tw6;

  // positon of the spherical wrist
  float Xsw[3];
  // Xsw=T06(1:3,4)-d(6)*T06(1:3,3);
  Xsw[0]=T06[0*4 + 3]-d[5]*T06[0*4 + 2];
  Xsw[1]=T06[1*4 + 3]-d[5]*T06[1*4 + 2];
  Xsw[2]=T06[2*4 + 3]-d[5]*T06[2*4 + 2];

  // joints variable
  // Jik=zeros(6,1);
  // first joint
  Jik[0]=atan2(Xsw[1],Xsw[0])-atan2(d[2],sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])); // Jik(1)=atan2(Xsw(2),Xsw(1))-atan2(d(3),sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2));
  // second joint
  Jik[1]=PI/2.0
  -acos((r[1]*r[1]+(Xsw[2]-d[0])*(Xsw[2]-d[0])+(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])-(r[2]*r[2]+d[3]*d[3]))/(2.0*r[1]*sqrt((Xsw[2]-d[0])*(Xsw[2]-d[0])+(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0]))))
  -atan((Xsw[2]-d[0])/(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])); // Jik(2)=pi/2-acos((r(2)^2+(Xsw(3)-d(1))^2+(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2-(r(3)^2+d(4)^2))/(2*r(2)*sqrt((Xsw(3)-d(1))^2+(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)))-atan((Xsw(3)-d(1))/(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1)));
  // third joint
  Jik[2]=PI
  -acos((r[1]*r[1]+r[2]*r[2]+d[3]*d[3]-(Xsw[2]-d[0])*(Xsw[2]-d[0])-(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0])*(sqrt(Xsw[0]*Xsw[0]+Xsw[1]*Xsw[1]-d[2]*d[2])-r[0]))/(2*r[1]*sqrt(r[2]*r[2]+d[3]*d[3])))
  -atan(d[3]/r[2]); // Jik(3)=pi-acos((r(2)^2+r(3)^2+d(4)^2-(Xsw(3)-d(1))^2-(sqrt(Xsw(1)^2+Xsw(2)^2-d(3)^2)-r(1))^2)/(2*r(2)*sqrt(r(3)^2+d(4)^2)))-atan(d(4)/r(3));
  // last three joints
  float T01[16], T12[16], T23[16], T02[16], T03[16], inT03[16], T36[16];
  DH1line(theta[0]+Jik[0], alfa[0], r[0], d[0], T01); // T01=DH1line(theta(1)+Jik(1),alfa(1),r(1),d(1));
  DH1line(theta[1]+Jik[1], alfa[1], r[1], d[1], T12); // T12=DH1line(theta(2)+Jik(2),alfa(2),r(2),d(2));
  DH1line(theta[2]+Jik[2], alfa[2], r[2], d[2], T23); // T23=DH1line(theta(3)+Jik(3),alfa(3),r(3),d(3));
  MatrixMultiply(T01, T12, 4, 4, 4, T02); // T02=T01*T12;
  MatrixMultiply(T02, T23, 4, 4, 4, T03); // T03=T02*T23;
  invtran(T03, inT03); // inT03=invtran(T03);
  MatrixMultiply(inT03, T06, 4, 4, 4, T36); // T36=inT03*T06;
  // forth joint
  Jik[3]=atan2(-T36[1*4+2], -T36[0*4+2]); // Jik(4)=atan2(-T36(2,3),-T36(1,3));
  // fifth joint
  Jik[4]=atan2(sqrt(T36[0*4+2]*T36[0*4+2]+T36[1*4+2]*T36[1*4+2]), T36[2*4+2]); // Jik(5)=atan2(sqrt(T36(1,3)^2+T36(2,3)^2),T36(3,3));
  // sixth joints
  Jik[5]=atan2(-T36[2*4+1], T36[2*4+0]); // Jik(6)=atan2(-T36(3,2),T36(3,1));
  // rad to deg
  MatrixScale(Jik, 6, 1, 180.0/PI); // Jik=Jik/pi*180;
}

void ForwardK(float* Jfk, float* Xfk)
{
  // forward kinematics
  // input: Jfk - joints value for the calculation of the forward kinematics
  // output: Xfk - pos value for the calculation of the forward kinematics

  // Denavit-Hartenberg matrix
  float theTemp[6]={0.0, 90.0, -90.0, 0.0, 0.0, 0.0};
  float theta[6];
  MatrixAdd(theTemp, Jfk, 6, 1, theta); // theta=[Jfk(1); -90+Jfk(2); Jfk(3); Jfk(4); Jfk(5); Jfk(6)];
  float alfa[6]={90.0, 0.0, -90.0, 90.0, -90.0, 0.0}; // alfa=[-90; 0; -90; 90; -90; 0];
  float r[6]={r1, r2, r3, 0.0, 0.0, 0.0}; // r=[47; 110; 26; 0; 0; 0];
  float d[6]={d1, 0.0, d3, d4, 0.0, d6}; // d=[133; 0; 7; 117.5; 0; 28];
  // from deg to rad
  MatrixScale(theta, 6, 1, PI/180.0); // theta=theta*pi/180;
  MatrixScale(alfa, 6, 1, PI/180.0); // alfa=alfa*pi/180;

  // work frame
  float Xwf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xwf=[0; 0; 0; 0; 0; 0];

  // tool frame
  float Xtf[6]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Xtf=[0; 0; 0; 0; 0; 0];

  // work frame transformation matrix
  float Twf[16];
  pos2tran(Xwf, Twf); // Twf=pos2tran(Xwf);

  // tool frame transformation matrix
  float Ttf[16];
  pos2tran(Xtf, Ttf); // Ttf=pos2tran(Xtf);

  // DH homogeneous transformation matrix
  float T01[16], T12[16], T23[16], T34[16], T45[16], T56[16];
  DH1line(theta[0], alfa[0], r[0], d[0], T01); // T01=DH1line(theta(1),alfa(1),r(1),d(1));
  DH1line(theta[1], alfa[1], r[1], d[1], T12); // T12=DH1line(theta(2),alfa(2),r(2),d(2));
  DH1line(theta[2], alfa[2], r[2], d[2], T23); // T23=DH1line(theta(3),alfa(3),r(3),d(3));
  DH1line(theta[3], alfa[3], r[3], d[3], T34); // T34=DH1line(theta(4),alfa(4),r(4),d(4));
  DH1line(theta[4], alfa[4], r[4], d[4], T45); // T45=DH1line(theta(5),alfa(5),r(5),d(5));
  DH1line(theta[5], alfa[5], r[5], d[5], T56); // T56=DH1line(theta(6),alfa(6),r(6),d(6));

  float Tw1[16], Tw2[16], Tw3[16], Tw4[16], Tw5[16], Tw6[16], Twt[16];
  MatrixMultiply(Twf, T01, 4, 4, 4, Tw1); // Tw1=Twf*T01;
  MatrixMultiply(Tw1, T12, 4, 4, 4, Tw2); // Tw2=Tw1*T12;
  MatrixMultiply(Tw2, T23, 4, 4, 4, Tw3); // Tw3=Tw2*T23;
  MatrixMultiply(Tw3, T34, 4, 4, 4, Tw4); // Tw4=Tw3*T34;
  MatrixMultiply(Tw4, T45, 4, 4, 4, Tw5); // Tw5=Tw4*T45;
  MatrixMultiply(Tw5, T56, 4, 4, 4, Tw6); // Tw6=Tw5*T56;
  MatrixMultiply(Tw6, Ttf, 4, 4, 4, Twt); // Twt=Tw6*Ttf;

  // calculate pos from transformation matrix
  tran2pos(Twt, Xfk); // Xfk=tran2pos(Twt);
  // Xfk(4:6)=Xfk(4:6)/pi*180;
  Xfk[3]=Xfk[3]/PI*180.0;
  Xfk[4]=Xfk[4]/PI*180.0;
  Xfk[5]=Xfk[5]/PI*180.0;
}

void invtran(float* Titi, float* Titf)
{
  // finding the inverse of the homogeneous transformation matrix
  // first row
  Titf[0*4 + 0] = Titi[0*4 + 0];
  Titf[0*4 + 1] = Titi[1*4 + 0];
  Titf[0*4 + 2] = Titi[2*4 + 0];
  Titf[0*4 + 3] = -Titi[0*4 + 0]*Titi[0*4 + 3]-Titi[1*4 + 0]*Titi[1*4 + 3]-Titi[2*4 + 0]*Titi[2*4 + 3];
  // second row
  Titf[1*4 + 0] = Titi[0*4 + 1];
  Titf[1*4 + 1] = Titi[1*4 + 1];
  Titf[1*4 + 2] = Titi[2*4 + 1];
  Titf[1*4 + 3] = -Titi[0*4 + 1]*Titi[0*4 + 3]-Titi[1*4 + 1]*Titi[1*4 + 3]-Titi[2*4 + 1]*Titi[2*4 + 3];
  // third row
  Titf[2*4 + 0] = Titi[0*4 + 2];
  Titf[2*4 + 1] = Titi[1*4 + 2];
  Titf[2*4 + 2] = Titi[2*4 + 2];
  Titf[2*4 + 3] = -Titi[0*4 + 2]*Titi[0*4 + 3]-Titi[1*4 + 2]*Titi[1*4 + 3]-Titi[2*4 + 2]*Titi[2*4 + 3];
  // forth row
  Titf[3*4 + 0] = 0.0;
  Titf[3*4 + 1] = 0.0;
  Titf[3*4 + 2] = 0.0;
  Titf[3*4 + 3] = 1.0;
}

void tran2pos(float* Ttp, float* Xtp)
{
  // pos from homogeneous transformation matrix
  Xtp[0] = Ttp[0*4 + 3];
  Xtp[1] = Ttp[1*4 + 3];
  Xtp[2] = Ttp[2*4 + 3];
  Xtp[4] = atan2(sqrt(Ttp[2*4 + 0]*Ttp[2*4 + 0] + Ttp[2*4 + 1]*Ttp[2*4 + 1]),Ttp[2*4 + 2]);
  Xtp[3] = atan2(Ttp[1*4 + 2]/sin(Xtp[4]),Ttp[0*4 + 2]/sin(Xtp[4]));
  Xtp[5] = atan2(Ttp[2*4 + 1]/sin(Xtp[4]),-Ttp[2*4 + 0]/sin(Xtp[4]));
}

void pos2tran(float* Xpt, float* Tpt)
{
  // pos to homogeneous transformation matrix
  // first row
  Tpt[0*4 + 0] = cos(Xpt[3])*cos(Xpt[4])*cos(Xpt[5])-sin(Xpt[3])*sin(Xpt[5]);
  Tpt[0*4 + 1] = -cos(Xpt[3])*cos(Xpt[4])*sin(Xpt[5])-sin(Xpt[3])*cos(Xpt[5]);
  Tpt[0*4 + 2] = cos(Xpt[3])*sin(Xpt[4]);
  Tpt[0*4 + 3] = Xpt[0];
  // second row
  Tpt[1*4 + 0] = sin(Xpt[3])*cos(Xpt[4])*cos(Xpt[5])+cos(Xpt[3])*sin(Xpt[5]);
  Tpt[1*4 + 1] = -sin(Xpt[3])*cos(Xpt[4])*sin(Xpt[5])+cos(Xpt[3])*cos(Xpt[5]);
  Tpt[1*4 + 2] = sin(Xpt[3])*sin(Xpt[4]);
  Tpt[1*4 + 3] = Xpt[1];
  // third row
  Tpt[2*4 + 0] = -sin(Xpt[4])*cos(Xpt[5]);
  Tpt[2*4 + 1] = sin(Xpt[4])*sin(Xpt[5]);
  Tpt[2*4 + 2] = cos(Xpt[4]);
  Tpt[2*4 + 3] = Xpt[2];
  // forth row
  Tpt[3*4 + 0] = 0.0;
  Tpt[3*4 + 1] = 0.0;
  Tpt[3*4 + 2] = 0.0;
  Tpt[3*4 + 3] = 1.0;
}

void DH1line(float thetadh, float alfadh, float rdh, float ddh, float* Tdh)
{
  // creats Denavit-Hartenberg homogeneous transformation matrix
  // first row
  Tdh[0*4 + 0] = cos(thetadh);
  Tdh[0*4 + 1] = -sin(thetadh)*cos(alfadh);
  Tdh[0*4 + 2] = sin(thetadh)*sin(alfadh);
  Tdh[0*4 + 3] = rdh*cos(thetadh);
  // second row
  Tdh[1*4 + 0] = sin(thetadh);
  Tdh[1*4 + 1] = cos(thetadh)*cos(alfadh);
  Tdh[1*4 + 2] = -cos(thetadh)*sin(alfadh);
  Tdh[1*4 + 3] = rdh*sin(thetadh);
  // third row
  Tdh[2*4 + 0] = 0.0;
  Tdh[2*4 + 1] = sin(alfadh);
  Tdh[2*4 + 2] = cos(alfadh);
  Tdh[2*4 + 3] = ddh;
  // forth row
  Tdh[3*4 + 0] = 0.0;
  Tdh[3*4 + 1] = 0.0;
  Tdh[3*4 + 2] = 0.0;
  Tdh[3*4 + 3] = 1.0;
}

void MatrixPrint(float* A, int m, int n, String label)
{
  // A = input matrix (m x n)
  int i, j;
  Serial.println();
  Serial.println(label);
  for (i = 0; i < m; i++)
  {
    for (j = 0; j < n; j++)
    {
      Serial.print(A[n * i + j]);
      Serial.print("\t");
    }
    Serial.println();
  }
}

void MatrixCopy(float* A, int n, int m, float* B)
{
  int i, j;
  for (i = 0; i < m; i++)
    for(j = 0; j < n; j++)
    {
      B[n * i + j] = A[n * i + j];
    }
}

//Matrix Multiplication Routine
// C = A*B
void MatrixMultiply(float* A, float* B, int m, int p, int n, float* C)
{
  // A = input matrix (m x p)
  // B = input matrix (p x n)
  // m = number of rows in A
  // p = number of columns in A = number of rows in B
  // n = number of columns in B
  // C = output matrix = A*B (m x n)
  int i, j, k;
  for (i = 0; i < m; i++)
    for(j = 0; j < n; j++)
    {
      C[n * i + j] = 0;
      for (k = 0; k < p; k++)
        C[n * i + j] = C[n * i + j] + A[p * i + k] * B[n * k + j];
    }
}

//Matrix Addition Routine
void MatrixAdd(float* A, float* B, int m, int n, float* C)
{
  // A = input matrix (m x n)
  // B = input matrix (m x n)
  // m = number of rows in A = number of rows in B
  // n = number of columns in A = number of columns in B
  // C = output matrix = A+B (m x n)
  int i, j;
  for (i = 0; i < m; i++)
    for(j = 0; j < n; j++)
      C[n * i + j] = A[n * i + j] + B[n * i + j];
}

//Matrix Subtraction Routine
void MatrixSubtract(float* A, float* B, int m, int n, float* C)
{
  // A = input matrix (m x n)
  // B = input matrix (m x n)
  // m = number of rows in A = number of rows in B
  // n = number of columns in A = number of columns in B
  // C = output matrix = A-B (m x n)
  int i, j;
  for (i = 0; i < m; i++)
    for(j = 0; j < n; j++)
      C[n * i + j] = A[n * i + j] - B[n * i + j];
}

//Matrix Transpose Routine
void MatrixTranspose(float* A, int m, int n, float* C)
{
  // A = input matrix (m x n)
  // m = number of rows in A
  // n = number of columns in A
  // C = output matrix = the transpose of A (n x m)
  int i, j;
  for (i = 0; i < m; i++)
    for(j = 0; j < n; j++)
      C[m * j + i] = A[n * i + j];
}

void MatrixScale(float* A, int m, int n, float k)
{
  for (int i = 0; i < m; i++)
    for (int j = 0; j < n; j++)
      A[n * i + j] = A[n * i + j] * k;
}
```

答:
Well, i would address one of your problem :

Weird result

One thing you need to understand while working with serial manipulator control inverse kinematic is concept of multi solution. With understanding that 6 dof serial manipulator usually consist of anthromorphic arm (basic 2 dof planar robot) and spherical wrist, therefore a multi solution possibly exist for one point.

Fig 1. Solution A and solution B for point x,y

Simple testing give result that configuration :

q = [0 0 90 0 90 0]

Didn't give position 224, 0, 280 (x,y,z)

But it give
HTM :

```<code>    -1         0         0        -1
     0         1         0         0
     0         0        -1        57
     0         0         0         1
</code>```

Read : -1, 0, 57 (x,y,z)

while this configuration would give u the position u want :

q = [ 0 0 0 -180 180 -180]

q = [0  -84.3114   19.6625 0   65.1509 0]

Additional note :

for -224 in x axis, maybe u wanna check if ur code have additional minus symbol or anything that accidentally make the value of x is -1. Testing with another configuration may help.

Title: 6DOF Kinematics moveo

Body:
Well, i would address one of your problem :

Weird result

One thing you need to understand while working with serial manipulator control inverse kinematic is concept of multi solution. With understanding that 6 dof serial manipulator usually consist of anthromorphic arm (basic 2 dof planar robot) and spherical wrist, therefore a multi solution possibly exist for one point.

Fig 1. Solution A and solution B for point x,y

Simple testing give result that configuration :

q = [0 0 90 0 90 0]

Didn't give position 224, 0, 280 (x,y,z)

But it give
HTM :

    -1         0         0        -1
     0         1         0         0
     0         0        -1        57
     0         0         0         1

Read : -1, 0, 57 (x,y,z)

while this configuration would give u the position u want :

q = [ 0 0 0 -180 180 -180]

q = [0  -84.3114   19.6625 0   65.1509 0]

Additional note :

for -224 in x axis, maybe u wanna check if ur code have additional minus symbol or anything that accidentally make the value of x is -1. Testing with another configuration may help.


问:
Question about robot jacobian
I try to understand jacobian for robot control. I want to precisely understand how to build that matrix.
Can someone advice me course which explain how to create that matrix on a simple sample?

An other question about that matrix, I read that this matrix had to be process every step. Is it true?

答:
Jacobian matrix is used to describe the relationship between the configuration space (space of joint angles) and the task space (space of end-effector pose)
as follows:

$\textbf{$\dot{p}$} = \textbf{J(q)}\textbf{$\dot{q}$}$,

where $\textbf{J(q)}$ is the Jacobian matrix, $\textbf{p}$ and $\textbf{q}$ are vectors represented for the end-effector pose and joint angles, respectively.

Several tutorials:

https://realitybytes.blog/2017/06/20/forward-and-inverse-kinematics-jacobians-and-differential-motion/

https://automaticaddison.com/the-ultimate-guide-to-jacobian-matrices-for-robotics/

http://www8.tfe.umu.se/courses/elektro/RobotControl/Lecture07_5EL158.pdf

You can do further searching with keywords: Denavit–Hartenberg, kinematics modeling, etc.

ContentLicense: CC BY-SA 4.0

Title: Question about robot jacobian


问:
calculation sequences when using RPY-transformation vs Euler-transformation
I have trouble understanding calculation sequences for RPY-transformation and Euler-transformation.

Let's assume that there are 3 (rotation/translation) instructions.

I will notate transformation matrix for instruction 1 as [1],

transformation matrix for instruction 2 as [2],

transformation matrix for instruction 3 as [3]

In this situation when using RPY-method, calculation order will be [3][2][1] -> [3][2 * 1] -> [3 * 2 * 1].

First multiply [2] and [1], and then multiply [3]. Right?

When using Euler-method, I am not sure what is correct order.

[1][2][3] -> [1 * 2][3] -> [1 * 2 * 3]

First multiply [1] and [2], and then multiply [3]

[3][2][1] -> [3 * 2][1] -> [3 * 2 * 1]

First multiply [3] and [2], and then multiply [1]

What is correct sequence when calculation Euler-method? Is it 1) or 2) ?

Also I think the resulting 4x4 matrix should be same whether I use RPY or Euler transformation. Is it correct?

答:
I was quite confused but now got an answer by myself.

Let's assume 3 instructions like below.

#1 rotation about world x axis(reference frame)

#2 rotation about world z axis(reference frame)

#3 rotation about world y axis(reference frame)

When using RPY convention, you process in order of #1 -> #2 -> #3.

Then your matrix will be like [3][2][1]

because in absolute frame, we use pre-multiplication. So new matrix goes to the left side of an existing matrix.

On the other hand, when using Euler convention, you process in order of #3 -> #2 -> #1.

In this case, matrix will also be like [3][2][1]

because in relative frame, we use post-multiplication. So new matrix goes to the right side of an existing matrix.


问:
Can ROS run on a Raspberry Pi?
Can ROS run on a Raspberry Pi?

ROS is resigned to run on a network of machines, with different machines, even different cores on the same machine doing different jobs. Can one of those machines be a Raspberry Pi?

I am considering using an R-Pi as the EtherCAT master on a mobile robot, communicating with the main PC over WiFi, using a dongle.

Can an R-Pi even run ROS at all?

Would an R-Pi have enough processing power to do some 1kHz servoing?

Would it be possible to run some servoing on the host through the WiFi connecion?

答:
ROS runs amazingly well on my "Raspberry PI 3B+" with any of the below images from Ubiquity robotics page.

https://downloads.ubiquityrobotics.com/pi.html

It is ROS Kinetic and you can install other packages like Gazebo as well.

ContentLicense: CC BY-SA 3.0

Title: Can ROS run on a Raspberry Pi?


问:
How to perform map matching algo using lidar scan s
I have run a code to extract features from lidar scans like poles and facades, and I created the reference map just concatenating results from each scan (i.e local map) now my question is;
How can I run map matching algo to get the robots poses?</p>

答:
Concatenation will work only if your positioning/localization is super precise which is seldom the case. What you want to be doing is scan registration. ICP and NDT are the two most widely used registration techniques which you can speed up by matching only the features you are extracting. In scan registration, your sensor gets a scan (from which you will extract features) and match them to the next scan which will give you a pose between them. So if you know your starting pose, you can get your new pose!

Of course you will find a lot of resources using C/C++, but PCL has a python version of icp which I haven't used. So if you do end up using it, please let us know!

Finally, if you are working within ROS, there are several packages/projects that can perform SLAM for you like rtabmap, hector_mapping, etc.


问:
How to choose a servo motor for solar tracking mechanism?
I design a solar tracking mechanisim. You can see the very undetailed representation in the pic. For choosing a servo motor how can I calculate the torque etc?

答:
Most solar tracking rigs only move along one axis because the efficiency savings from moving along 2 doesn't make up for the energy losses of having 2 motors.

If you're just doing this for fun, then you have questions to ask yourself:

How heavy is the system that you're moving?

Is it so heavy that a normal hobby servo couldn't handle it?

Is this application better suited for something like a large brushless motor & an O-Drive?

You really didn't provide a ton of detail here, so it's hard to help with motor selection.


问:
Navigation - GPS + IMU; how to make it more accurate?
Currently, I am trying to navigate a small robot car to point A from my current position. The car has a GPS sensor and a BNO055 IMU(Gyro + Mag + Acc). I know the GPS co-ordinates of point A. Using the GPS co-ordinates of my car, I can calculate the bearing angle and the distance. From the fusion mode in the BNO055, I get the tilt compensated heading again(It gives me heading, roll & pitch). Based on the heading angle and the bearing angle, my car makes its turns. However, here is what I want to build on this:

The GPS readings sometimes are a bit far from my actual position. How to get rid of these outliers? I've read of the use of EKF. But they seem to be all mathematical formulas and it's tough to keep up with them!

For when the GPS signal is lost, I've read about Dead Reckoning methods being used. I'm not sure how to implement those. Can someone shed some light on that?

Can I calculate the speed with just the GPS and an IMU?
Any help is appreciated, thanks!

答:
One way to improve GPS accuracy is RTK GPS.   RTK GPS is what some of these robot lawnmowers use because of its centimeters accuracy.
https://learn.sparkfun.com/tutorials/what-is-gps-rtk/all</p>


问:
Can I find the end effector position from joint velocities?
I would like to know if I can find the end effector position according to joint velocities. I have found joint velocities of a manipulator. I need to check if it is correct. So I need to know that how can I combine joints veloties and the end effector postion. Thank you in advance.</p>

答:
No, you cannot determine the end effector position by using only joint velocities.  If you use the Jacobian matrix, you can determine the velocity vector of the end effector given a set of joint velocities.  If you have a finite time over which the joint velocities were measured, you can know how much the end effector has moved in that short time.  But unless you have a starting position, the joint velocities are insufficient for determining end effector position.

However, if you do know the forward kinematics as well as the joint velocities, you can use $\dot{x} = \mathrm{J} \dot{\theta}$ to determine incremental end effector motion.


答:
As of 2020/2021, the answer is yes, ROS runs on Raspberry Pi.

https://roboticsbackend.com/install-ros2-on-raspberry-pi/
https://www.youtube.com/watch?v=AmuLiA840fA

Both of these resources use Ubuntu; RaspberryPi is a first class target for Ubuntu now.  https://ubuntu.com/raspberry-pi

Title: Can ROS run on a Raspberry Pi?

Body:
Can ROS run on a Raspberry Pi?

ROS is resigned to run on a network of machines, with different machines, even different cores on the same machine doing different jobs. Can one of those machines be a Raspberry Pi?

I am considering using an R-Pi as the EtherCAT master on a mobile robot, communicating with the main PC over WiFi, using a dongle.

Can an R-Pi even run ROS at all?

Would an R-Pi have enough processing power to do some 1kHz servoing?

Would it be possible to run some servoing on the host through the WiFi connecion?


问:
Path planning algorithm better than RRT* and Artificial Potential Field
Hello robotics stack exchange community, hope my message finds you well during these challenging times. I need an opinion regarding path planning algorithms.

Goal

I am looking for a path planning algorithm that is able to produce smooth paths that are shorter and more predictable than RRT and RRT*. It shouldn't get stuck in local minima like Artificial Potential Field.

What I tried

I have set up a Linux 16 system with ROS Kinetic Kame and Python. I implemented Artificial Potential Field Path Planning, RRT and RRT* and ran those on a robot in a small arena. The arena was a square and I placed different objects inside of it that needed to be avoided. See the picture below for an example of how my arena configuration looks like.

Artificial Field Path Planning creates nice, smooth curves and a robot executes smooth movements to follow it. My problem with them is local minima where my robot gets stuck.

RRT produced jagged paths, but the nice thing about it was that it always found a path (albeit it was always a different one). I implemented RRT* to get smoother paths, but they're quite "branchy" looking (still better than RRT though).

I have also looked into other algorithm such as A* and MEA, but rather than trying to implement them straight away, I wanted to ask for your opinion.

答:
First some notes:

RRT* is not an optimal algorithm, it just tends to converge to the optimal path as time approaches infinity. Similarly, it is not guaranteed that it will find a path (even though it usually finds it with enough time). In other words it is a probabilistically complete algorithm.

A*-derivates are resolution complete. If the resolution is low enough to find a path, it will find it, and it will be the optimal path as long as a) the heuristic is admissible (never overestimates the real distance to the goal) and b) consistent (the heuristic of the goal node is 0 and the triangle inequality is fulfilled).

In my experience, I would encourage you to deep further into A* derived planners for your use-case. More specifically, take a look at Hybrid A* which takes into account continuous values within the discrete grid, thus allowing motion constraints to be taken into account.

Take this paper in which Hybrid A* is adapted to work in the 3D (underwater) space with AUV constraints, for example. The algorithm (for 2D) is explained there as well:
https://www.mdpi.com/1424-8220/21/4/1152.

Hope this helps.


问:
How can I automatically adjust PID parameters on the fly?
I have a simple servo system that uses a PID controller implemented in an MCU to perform the feedback. However, the properties of the system change dynamically, and so the PID parameters can never be tuned for all circumstances.

My robot is a light weight arm with back-drivable electric motors, similar to this one:

The arm performs several tasks, including picking up heavy weights, pushing and pulling objects across the desk. Each of these tasks requires different PID tuning parameters which I cannot easily predict.

What I would really like is for some higher level function which can carefully adjust the parameters in response to the arm's behaviour. For example, if it notices that the arm is oscillating, it might reduce P and increase D. Or if it noticed that the arm wasn't reaching its target, it might increase I.

Do such algorithms exist? I would be happy even if the algorithm didn't perfect the parameters immediately. E.G. the arm could oscillate a few times before the parameters were adjusted to their new values.

答:
A good approach for such a problem is called adaptive control. In short it is a control methodology that presumes the model is known but the parameters of the model (mass, inertia, etc.) are not. It's job is to estimate the unknown parameters. A brief introduction can be found on wikipedia. The text Robotics: Modelling, Planning and Control by Siciliano et al. cover the topic more thoroughly.

Edit in response to @Rocketmagnets query:

In short you must have a mathematical model of your system, i.e. the equations that describe how your system evolves over time when forced or otherwise, but you do not need to know dynamical parameters such as the mass of the various components, their inertia, etc.. It is the job of the adaptive controller to estimate these parameters. You do have to give it an initial guess for each of the unknown parameters however. Then as the system runs it uses the control signals, the output signals, and a method like linear regression or gradient descent to update the unknown parameter values. Over time the parameters will converge to values that will result in a steady-state though they may not match the real parameters, i.e. it may get the mass wrong, but the value will still work.

From here I would advise referring to a text that discusses the method. I just noticed for example that Dr. Marc Bodson is offering a copy of his text Adaptive Control: Stability, Robustness, and Convergence in PDF form on his website.


问:
Robots without microcontrolers (beam robots). Are they technologically limited?
BEAM robotics seem to be a good approach to teach learners about electronics in robotics. But can these robots be like regular programmed "cognitive" robots? Can these robots, with just analog circuits, take us to the level of robotic assistants, worker robots and other kinds of self sufficient autonomous robots?

I specifically want to know that, when creating mission critical robots ->

1) What are the areas in robotics which are practically impossible without a real time software system?

2) What areas of the field can be done without programming? If yes, are these areas feasible without an onboard software system?

3) Could an intelligent space rover, work without a cpu in the future?

答:
I think you are mixing the idea of BEAM robotics (why that, I prefer to not use this term), with analog electronics.

Analog circuits are in major applications more fast than a micro processed one, that have a clock to process instructions. The "problem" with analogs is in part with noise, but early computers are made analog, operational amplifiers are made to do computation with analog circuits.

The 74LS series, have a bunch of digital discrete ICs, they are not microprocessors, but can be flip-flops of some types, bus drivers, gate logic, and can do
a lot of "processing", again generally faster than a microprocessor.

But, today digital electronics and micro-controller or processors reached a point where cost is acceptable even for the hobbyist. Sometimes is more easy to make a software than the equivalent analog circuit if possible. There a number of reasons.

But you can see that micro-controllers still have analog circuits for the user, like analog comparators, A/D and D/A converters, and much more depending on the application, so majority of circuits are mixed type

But Software-less robots, spacecrafts etc. might never be close to feasible. Right?

That is really application and project dependent, and at some degree opinion based.
Op amps for example with an analog encoder, could close a PID loop, very fast ones.
But the signal to the PID would come from a software and a D/A. So I don't say its impossible, but there's no subject to go this way, but again, they are in major projects mixed circuits, not only digital


问:
Getting started with robotics
I'm getting started with robotics, as a side project while I'm still studying, I'm taking mechatronics at the moment, with prior knowledge of coding i learned a little bit of python and i have it easy while learning new languages, so i wanted to know what language should I start with on robotics, i plan on using arduino boards, i hear they use C and C++ i also researched that C is faster and more efficient than C++ tho it's harder to right code in since I don't know much about either Wich one should I be be learning to start with robotics?</p>

答:
To start learning robotic, the first thing you need to learn is the robotic and control it self.

About programming language, you actually can use C, C++ and Python (three most common language in robotic).

C and C++ will be used together, so learn both. But if you want to reduce the workload on programming and more focus on hardware and robotic control etc., you can use python like I did.

Upside of C & C++ :

You can achieve real time control

Downside :

Take a lot more effort in programming side, i.e. you need to create your own function for matrix operation

Upside of Python :

Reducing work load on programming side

Easier if u wanna implement image processing in robotic

Downside :

Slower process, unlikely to achieve real time control performance

Title: Getting started with robotics


问:
Robots Following Robots with Safety Distance Planning and Coordination
I have two robots, I wish for the second robot to follow the first one while keeping a constant distance d_safe between them. I am trying to implement a PID controller that takes as input the current distance between the second robot and the first one and then returns the second robot's velocity. I need help with the mapping of the information provided by the PID to the velocity. I can measure the current distance between the two robots at any time. Approaches I have tried in vain:

using the error, e(t) provided by the PID as the acceleration of the following robot over time. This seemed to me as the most appealing approach, however, its main flaws include the second robot reversing when the acceleration turns out to be negative for a given period. I wish to avoid reversing and backward motion. However, stopping is fine. So let's say it's a stop-go system.
used velocity equation:

v2 = v2 + e(t) * dt

where dt is the change in time and v2 is the velocity of the second robot that is following.

I tried setting the absolute value of the error as the velocity and only reduce the velocity component by this much if and only if the velocity remains greater or equal to zero if necessary (robots getting too close to each other). But with this approach I can never reach my desired distance between robots especially when the velocity of the first vehicle is smaller than the distance I wish.
So any approach, modification suggestions are all appreciated in advance. Implementation tests being performed in python - for what it is worth. Thanks. If there's something unclear, please let me know.

答:
I think you've got a conceptual issue here with your PID setup. You mention both:

a PID controller that takes as input the current distance between the second robot and the first one

and

using the error, e(t) provided by the PID

A PID controller accepts an error as an input and tries to drive that error to zero. If you are providing an absolute position as an input then I can definitely see where you would be having issues, because the PID controller should react to drive the input to zero. Again, then input should be error.

To get the error, you should take your inter-robot distance feedback and compare that to your desired inter-robot distance reference to get the distance error, like

```<code>float distanceError = distanceReference - distanceFeedback;
</code>```

This is your proportional error. The integral and derivative errors are calculated by taking the integral and derivative of that error, like:

```<code>float dT = // however you get elapsed time on your system.
// don't forget to reset your timer/stopwatch so you can get elapsed time on the next loop

static float distanceIntegralError = 0; // static so the value is retained call-to-call; "persistent" in Matlab
float distanceProportionalError = distanceError; // For naming consistency
distanceIntegralError += distanceError * dT;
float distanceDerivativeError = distanceError / dT;
</code>```

Once you've got your proportional, derivative, and integral errors, you can do the PID control. You multiply each error term by its corresponding gain term, and then sum those products:

```<code>float pProduct = pGain * distanceProportionalError;
float iProduct = iGain * distanceIntegralError;
float dProdcut = dGain * distanceDerivativeError;

float pidOutput = pProduct + iProduct + dProduct;
</code>```

Typically the output of the PID controller is applied to whatever units are the derivative of the reference input; if you're trying to control position (as in this example) then you'd use the output as a speed reference, or if you're trying to control speed (as in a cruise control system) then you'd use the output as acceleration (torque) reference.

It's possible that you've got some other scaling that happens on the output of your PID controller, but that generally doesn't matter because it'll be "absorbed" by the PID gains when you do your tuning.

For tuning I always point people at the Ziegler–Nichols method because it's a methodical way to tune that's been proven to work, but it's your system - feel free to adjust it however you like.

The last comment I'll make about this is that you should to save the PID controller to make up honest error in the control; if you use feed forward in addition to your PID controller then you get "bulk" movement out of the feed-forward signal and PID makes up the difference.

So, for example, if you have the speed reference going to the first robot, pass that same speed reference to the second robot. The PID controller then only has to do a little bit of work to make up for minor differences, like traction, wind, weight, motor power, etc.

You'll get much, much more consistent performance with feed-forward and PID than with just PID alone. The total output of the controller would just be the sum of the feed-forward term and the pidOutput.

Body:
I think you've got a conceptual issue here with your PID setup. You mention both:

a PID controller that takes as input the current distance between the second robot and the first one

and

using the error, e(t) provided by the PID

A PID controller accepts an error as an input and tries to drive that error to zero. If you are providing an absolute position as an input then I can definitely see where you would be having issues, because the PID controller should react to drive the input to zero. Again, then input should be error.

To get the error, you should take your inter-robot distance feedback and compare that to your desired inter-robot distance reference to get the distance error, like

float distanceError = distanceReference - distanceFeedback;

This is your proportional error. The integral and derivative errors are calculated by taking the integral and derivative of that error, like:

float dT = // however you get elapsed time on your system.
// don't forget to reset your timer/stopwatch so you can get elapsed time on the next loop

static float distanceIntegralError = 0; // static so the value is retained call-to-call; "persistent" in Matlab
float distanceProportionalError = distanceError; // For naming consistency
distanceIntegralError += distanceError * dT;
float distanceDerivativeError = distanceError / dT;

Once you've got your proportional, derivative, and integral errors, you can do the PID control. You multiply each error term by its corresponding gain term, and then sum those products:

float pProduct = pGain * distanceProportionalError;
float iProduct = iGain * distanceIntegralError;
float dProdcut = dGain * distanceDerivativeError;

float pidOutput = pProduct + iProduct + dProduct;

Typically the output of the PID controller is applied to whatever units are the derivative of the reference input; if you're trying to control position (as in this example) then you'd use the output as a speed reference, or if you're trying to control speed (as in a cruise control system) then you'd use the output as acceleration (torque) reference.

It's possible that you've got some other scaling that happens on the output of your PID controller, but that generally doesn't matter because it'll be "absorbed" by the PID gains when you do your tuning.

For tuning I always point people at the Ziegler–Nichols method because it's a methodical way to tune that's been proven to work, but it's your system - feel free to adjust it however you like.

The last comment I'll make about this is that you should to save the PID controller to make up honest error in the control; if you use feed forward in addition to your PID controller then you get "bulk" movement out of the feed-forward signal and PID makes up the difference.

So, for example, if you have the speed reference going to the first robot, pass that same speed reference to the second robot. The PID controller then only has to do a little bit of work to make up for minor differences, like traction, wind, weight, motor power, etc.

You'll get much, much more consistent performance with feed-forward and PID than with just PID alone. The total output of the controller would just be the sum of the feed-forward term and the pidOutput.


问:
What is the best way to simulate Staubli robots inside a C++/C# app
A little bit of context : I want to create an app using C++ (or C#) to handle 3D objects (obj), I will perform some operations on those 3D models to create some robotic trajectories. Then I want to simulate those trajectories showing the Staubli robot (a tx2-90) to the user.

I am totally new in the 3D coding world and so far I've only been using manufacturers softwares (for staubli it's SRS which integrates a powerful simulator) but now I want to create my own app to simulate the behavior of the robot when given different trajectories.

What librairies or 3D engines (considering I have some 3D objects to handle as well)  should I use for that? Gazebo (same thing for RoboDk apparently) seemed pretty promising but it seems that you can't modify the interface and I need something I can have the hand on as it's going to be a proprietary interface.

I am struggling to find my starting point for this so if you could guide to some ressources or anything that would be lovely!

Thank you in advance!

答:
You should look into Energid's Actin SDK, its a commercial product that allows for this.

https://www.energid.com/

Full disclosure I work for Energid, and i work with customers doing similar things


答:
What you're looking for is doable but it is difficult for beginners. There is a plethora of skills you need. The skills are

1- Mathematical modelling: The first step for any project is to derive the mathematical model of your system. This requires expertise in specific fields. For manipulators, you need to apply, for example, Netwon's laws to derive the dynamics equations of your manipulator. Please take a look at these books. These are the most common textbooks in introduction to robotics.

Robot Manipulators by Paul MIT Press

Introduction to Robotics Mechanics and Control 3th by Graig

Robot Modeling and Control by Spong and Et. al.

A mMthematical Introduction to Robotic Manipulation by Murray and Et. al.

2- Numerical Analysis: mathematical models are usually in the form of differential or difference equations. Because these equations are highly nonlinear, the only solution is to utilize numerical methods to solve them.

3- Control Strategy: to move your robot, you need control methods so that your robot follow your commands. Linear and nonlinear techniques are common. Usually for highly nonlinear models like manipulators, you need nonlinear methods.

For linear Control books: (common textbooks in universities)

Control Systems Engineering by Nise (highly recommended)

Automatic Control Systems by Golnaraghi and Et. al.

Modern Control Systems by Dorf (extremely popular but not easy for the first read)

Modern Control Engineering by Ogata

For nonlinear Control books:

Nonlinear Control Systems Analysis and Design by Marquez (highly recommended)

Nonlinear Systems by Khalil (highly recommended).

4- Computer Graphics: to draw and simulate your manipulator, you need a good background in computer graphics. For purely graphics, you may need to learn OpenGL (i.e. for all OSs) or DirectX (i.e. for Windows). You could use any programming languages for this matter. For C++, there are numerous libraries for simplifying graphics such SDL2, Glue, FreeGlut, SMFL, etc. These libraries not for drawing rather they allow the interaction with hardware (e.g. mouse, keyboard) but they are simple so you can learn them very fast.

Title: What is the best way to simulate Staubli robots inside a C++/C# app

Body:
A little bit of context : I want to create an app using C++ (or C#) to handle 3D objects (obj), I will perform some operations on those 3D models to create some robotic trajectories. Then I want to simulate those trajectories showing the Staubli robot (a tx2-90) to the user.

I am totally new in the 3D coding world and so far I've only been using manufacturers softwares (for staubli it's SRS which integrates a powerful simulator) but now I want to create my own app to simulate the behavior of the robot when given different trajectories.

What librairies or 3D engines (considering I have some 3D objects to handle as well)  should I use for that? Gazebo (same thing for RoboDk apparently) seemed pretty promising but it seems that you can't modify the interface and I need something I can have the hand on as it's going to be a proprietary interface.

I am struggling to find my starting point for this so if you could guide to some ressources or anything that would be lovely!

Thank you in advance!


问:
What are the different types of electric motors?
I am beginning to learn about the hardware aspect of robotics, and in order for a lot of this new information to be useful to me (whether on this site or elsewhere) I will need a basic understanding of the terminology.

One thing that comes up repeatedly is different electric motors: servo, DC motor, brushless motor, step motor, gear motor... etc

Is there a comprehensive list? Or at least a list of the most common ones, and their descriptions / differences?

答:
There are too much of types to someone describe. I think a simple research will help you.

Reading the other responses I will put something, to put things like they are.

Servo-motor

Near any motor can be a servo motor or not. That means a brushed-motor, brush-less motor, stepper-motor (that is a brush-less motor) can be a servo-motor or not. Servo-motors have a feed-back loop to know some variables like the position or speed, or both, and correct according to the signal you put. BUT, that is made by a controller, the motor just has the sensors to provide the signals.

Brushed-motors

As the name suggests, these motors have a rotating part that is energized (major part is the rotor), and to energize it brushes are used. There's many types of brushed-motors.

They can do its commutation mechanically, in the brushes, what generates sparks, less efficiency, EMI and RFI and more brush wear.

They can do the switching externally, and the brushes runs on continuous ring of contact, their only function is to supply power, the commutation is done by a controller.
There's nearly no sparks at the brushes (although as the ring wear this can led to bumps of the brush in the ring what result in sparks).

Their stator can be of magnets (normally the motor found on toys), or can be coils too (electro-magnets) as the motors found in power-tools.

Four brushes motor partially open, showing its brushes and contact ring, and the mechanical commutation. The brushes are inside the "yellow" casing, that holds the brush with a spring to force it over the contact ring. The brushes are made mainly from carbon, yet have other materials, the brush wear is visible on the contact ring

(source: wikimedia.org)

From left to right. Brushes of a small motor, rotor with contact ring and coils, stator and case with magnets

Brush-less motors

Brush-less motors supply the power to the rotating part of the motor (rotor) by other means.

Induction motors, as the name suggest, supply the power by induction. These motors are
the most common in industry, they are very efficient (normally ~90%). As other motors
they can have mechanical brake to have fast deceleration and holding breaking. (Ex: cable elevators).

Induction motor partially opened. The rotor, these small cuts in the metal form a low impedance coil, and case with its coils. The fins on the case help to dissipate the generated heat. Depending on the motor it normally have a fan attached to the axle, or use other motor for forced ventilation, for example the motor runs slow.

DC brush-less motors have magnets in their rotor, so no electrical power supply is necessary to the rotor. Normally the magnets have high force to power ratio, see Neodymium magnets for example, and so the rotor1 has low mass and consequently low inertia. That said, it can start and stop at higher rates/accelerations than other motors with less wear.

Stepper-motors are a type of brush-less motor, but with more poles, that means that applying power to the correct phase you can presume the position of the rotor2. But the rotor can slip, called "lose step", and so you lost the position synchronism. That means that if use a stepper motor for precise positioning you need to be sure that the motor can handle the load and not slip. How can you be sure of the position? Put a position sensor on the axle of the motor, this with a controller turns it into a servo-motor.

Stepper motor partially open. This is probably a hybrid type, with the rotor being magnetized and with gear like shape. Some steppers have round rotors, but the rotor is still magnetized with a high number of poles.

As they don't have brushes, normally the only mechanical contact between rotor and stator are the bearings. That why they have low maintenance, high efficiency, nearly no sparks3.

1 - the rotor refereed can be the inner or outer part of the motor, the part that don't rotate relative to the power supply I would consider the stator.

2 - That can be applied to others motors as well. A DC brush-less non-stepper motor, with 3 poles for example, would give you 120° of precision, but what you do is modulate the power applied to each coil (ex: less power to one more to the other) so that you get the positions between. That is used on some steppers motors to, and is called micro-stepping.

3 - although major times that means spark free, one possible mean of spark is from static energy on the rotor, that sometimes find the bearings as the short circuit to discharge, resulting in wear to the bearing, but that in not so common and have some means of counter-measure.

This is only a small list of electrical motors types.

Mechanical transmission

Note: You called that gearing but it's not the only way to do it. I will refer to mechanical transmission

That is other topic. Motors run efficiently and have more torque at certain rotations, so you put for example a reducing transmission and get less speed and more torque.
Suppose a motor spin at 4000 RPM and have a 0.1Kg-cm of torque, assuming a reduction of 100:1 (that is 100 turn in the input result in 1 turn at the output) you should get 400 RPM and 1Kg-cm of torque.1

1 - This is a theoretical example, the transmissions are not 100% efficiently so you lose some power

Some types of transmissions that can have different ratios

Gears, Hydraulics, chain sprocket, belts. There's more of course, and each of that have their subtypes. Ex: Gear reductions can be worm-gear, planetary, etc.
On some applications you can use pulleys, like cranes, elevators.

All images courtesy of Wikimedia commons http://commons.wikimedia.org


问:
What is the best SE3 library for python?
I am currently googling on this topic but I would like to hear your opinion.

What is the best self-contained SE3 library that offers interconversion between quaternion, rotation vector, transformation matrix?

Those are the functions that I often use for my projects and all implemented in Matlab. And I am looking for an alternative to avoid re-implementing them all in python.

Quaternion2RotVector and its inverse

Quaternion2RotMatrix and its inverse

RotMatrix2RotVector and its inverse

SE3 inverse, the inverse function of each rotation representation

答:
Klampt Library by Kris Hauser is very good and implement a lot of distances functions including SE3 (R3 + SO3).
I use Klampt for some robotic arms applications</p>


问:
How to calculate center of mass with a prismatic joint
Suppose we a 1DoF planar robot with only 1 prismatic joint  (for the sake of the example).
The prismatic joint can extend the link or shrink the link.
Suppose the length of link 1 is a, and suppose the center of mass is in the middle of the link.
Let d be the ony generalized variable.
My doubt is: the center of mass is (a/2)+d or (a+d)/2  (w.r.t. the frame placed in the joint) ?

答:
You have rigid parts that can move independently (subject to the degree-of-freedom constraint). Each part has its own center of mass. The overall CoM is the mean of the part CoM's, expressed in a common Cartesian coordinate system, weighted (pun unavoidable) by their respective mass fraction.

That should be general enough to compute the overall CoM for any number of parts with any number of generalized coords.


答:
Servo 1. Referring to your diagram, first consider the tilting part at the top. Imagine a line connecting its bearings. It will tilt (rotate) about that axis. The weight (force due to gravity) of the top part can be modeled as a single point, located at the Center of Gravity. The force, pointing straight down, tries to make the top part flop over -- it will exert a torque around the axis. To keep things stable, the servo has to exert a, equal but opposite torque.

That torque will be $T = F\ r\ sin\ \theta$ where

$T$ is the torque

$F$ is the weight of the top part.

$r$ is the distance of the CoG from the axis.

$\theta$ is the angle that the tabletop surface makes with the horizontal.

If things are sufficiently symmetrical, the CoG will be directly above the axis when the top surface is perfectly level. Then, $\sin \theta = 0$ so $T = 0$. When it tilts so far that the surface becomes vertical (assuming that it's not obstructed somehow), $\sin \theta = 1$ and the torque is at the max.

The rest of your design will determine the max angle -- you might have some stops or bumpers or something. Plug that max angle, along with the weight and dimensions, into the formula above.

But wait, there's more.  The servo has to do more than just hold the mechanism still. It has to move it. That means it has to exert additional torque to overcome friction, which we can't calculate here because it depends on the sort of bearings you design in. Also, in general, the servo has to accelerate the mass to start it moving, and decelerate to stop it -- but that will be negligible if you're just tracking the sun.

Servo 2 is a lot simpler, because the vertical rotation axis is always parallel with gravity. You have friction (mitigated by a lazy-susan bearing or something) and acceleration (as above). Again, there's no good way to calculate it here.

For prototyping with relatively inexpensive things like hobby servos, we typically specify torques lots beefier than the theoretical calculations suggest. If you were moving a multi-ton radio telescope dish, the engineering gets a lot more involved.

Title: How to choose a servo motor for solar tracking mechanism?

Body: I design a solar tracking mechanisim. You can see the very undetailed representation in the pic. For choosing a servo motor how can I calculate the torque etc?


问:
How to extract robots coordinates using ICP algo
I would like to know How to extract robots localization from the icp or ndt algo, currently I'm using scan registration for this reason</p>

答:
The transformation T itself contains the location and orientation of your robot. Try to search for some related information regarding SE3 pose representation.

Keywords: SE3, SO3, Rigid Body Kinematics, Matrix pose representation, coordinate transform

Some related materials:
https://www.seas.upenn.edu/~meam620/slides/kinematicsI.pdf

Important: don't try to understand lie algebra.

T is composed of rotation and translation. The rotation is 3x3 matrix that is starting from left-top of your T. Each column of the rotation matrix is the x,y,z axis direction of the robot. And the translation which start from the left-most column of your T is just xyz location of your robot with respect to the world coordinate.

Direction is important. You need to find out if your T is world to robot or robot to world.


问:
Frozen map + 'Message filter dropping' - Issues with mapping and navigation in ROS2 Foxy
sorry for the long post, just want to make sure i include all the details.

I am working on learning ROS2 by building a robot which maps and navigates my apartment.

I'm using the RPLidar A1 sensor with the RPLidar package

a Robot Create 2 as the chassis with the create_robot package

nav2 for navigation

slam-toolbox for mapping

and running everything off of a Raspberry Pi 4 4GB with Ubuntu Mate

Currently, I am able to get LIDAR readings to successfully be viewed in RVIZ. I can also get one or two frames of a map to be built. When the robot starts to move, the lidar readings update, but the map itself does not. However, i continue to 'receive' map updates at about 1 every 1.5 seconds.

Here are images from rviz showing the moving scan but with the non-moving map:
https://imgur.com/a/acfHs4I

I'm also getting a few errors, from RVIZ i am getting:
[rviz]: Message Filter dropping message: frame 'map' at time 1619194935.882 for reason 'Unknown'

from nav2, I am getting
[global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'laser' at time 1619194959.629 for reason 'Unknown'

My frame map looks like this (Most of the frames are created by the create_robot package):
https://imgur.com/a/WFDXKxj

I suspect my issue is with my transforms, either they're configured completely wrong, or there is something small

slam_toolbox does the transform from map -> odom

the create_robot package has other transforms from base_link to wheels, bumpers, etc and it also publishes odom data

using tf2, i'm publishing a transform between base_link and laser and then laser to base_scan.

I am also creating a transform from odom to base_footprint because of a ros2 navigation course i was taking on the construct.

Has anyone seen this type of error before? I feel like I am most of the way to the solution but i'm missing something easy

答:
I ended up solving my issue.

Turns out, the create_robot package publishes a transform from odom -> base_footprint. I needed to toy around with my transforms so that:

slam-toolbox created map -> odom

create_robot created odom -> base_footprint

On top of those two things, I had an issue with timing where the transform from map -> base_link did not work either. There was an issue with timings where I was getting the error:

[INFO] [1619986350.507533125] [tf2_echo]: Waiting for transform map -> base_footprint: Lookup would require extrapolation into the past. Requested time 0.200000 but the earliest data is at time 1619986346.813953, when looking up transform from frame [base_footprint] to frame [map]

To solve this error, I just had to call nav2 with the argument use_sim_time:=false

Now that I've done this, everything seems to be working completely fine.


问:
I2C Arduino Connection
I am working on a wheel mobile robot with multiple sensors which are IMU, two encoders, four ultrasonic sensors, and using two Arduino: Uno and Mega. The IMU, encoders, and ultrasonic sensors are connected to Arduino Mega while the motor controller with every other sensor attached to the robot is connected to the Arduino-Uno. The Uno is attached to the robot by the manufacturer and nearly all the pins have been used or attached to a function on the robot.
To use I2C to communicate these two Arduinos I connect SDL to SDL, SDA to SDA, GND, and VCC. Do I need to connect two separate Arduino USB cables to the Arduino simultaneously? Also, do I need to attach these lines of code from each Arduino before I can further write the codes?  Attached below the slave and then the master code

```<code>// Include the required Wire library for I2C<br>
#include <Wire.h>
int LED = 13;
int x = 0;
void setup() {
  // Define the LED pin as Output
  pinMode (LED, OUTPUT);
  // Start the I2C Bus as Slave on address 9
  Wire.begin(8);
  // Attach a function to trigger when something is received.
  Wire.onReceive(receiveEvent);
}
void receiveEvent(int bytes) {
  x = Wire.read();    // read one character from the I2C
}
void loop() {
  //If value received is 0 blink LED for 200 ms
  if (x == 0) {
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
  }
  //If value received is 3 blink LED for 400 ms
  if (x == 1) {
    digitalWrite(LED, HIGH);
    delay(400);
    digitalWrite(LED, LOW);
    delay(400);
  }
  if (x == 2){
    digitalWrite(LED,HIGH);
  }
}
</code>```

this is the master code

```<code>#include <Wire.h>

// Include the required Wire library for I2C<br>#include

int x = 0;
void setup() {
  // Start the I2C Bus as Master
  Wire.begin();
}
void loop() {
  Wire.beginTransmission(9); // transmit to device #9
  Wire.write(x);              // sends x
  Wire.endTransmission();    // stop transmitting
  Wire.beginTransmission(8);
  Wire.write(x);
  Wire.endTransmission();
  x++; // Increment x
  if (x > 3) x = 0; // `reset x once it gets 6
  delay(500);
}
</code>```

答: Dronebot workshop has a good video on communicating with I2C between two Arduinos; https://www.youtube.com/watch?v=PnG4fO5_vU4</p>

Title: I2C Arduino Connection


问:
How to draw robot manipulator diagram
I've been seeing a lot of these diagrams online and wondering how can I replicate this myself. Is there any easy-to-use software package that will allow me to draw these diagrams quickly and accurately? Ideally, I would be able to draw joints, links and label, angles and lengths of the robot manipulator.

答:
There is no specialized tool to draw it, but you can draw these with generic software.

You can get very similar diagrams from PowerPoint or Inkscape. Furthermore if you use Latex, here you can find an example using Tikz.

Title: How to draw robot manipulator diagram


问:
Collision check of a 6-DOF robotic arm
Initially we started with 3-DOF. We checked collisions in the c-space and it worked well. With 6-DOF we're having problems.
Any ideas about how to check collisions for a 6-DOF robotic arm?

--Edit
I'm going to clarify my needs. I have a algorithm based on a hybrid architecture (PRM + Q learning) that plan trajectories.
As some of you now, when building the roadmap using a PRM algorithm, you need to sample configurations (series of 6 angles for a 6R 6DOF robot in my case as we are working in the configuration space). You have to check if these configurations lead to a collision or not. If not you add them to the roadmap.
My problem is with the part where i need to check if a configuration leads to a collision.
At first, when we were testing for a 3R 3DOF robot, we had a brute-force method that worked pretty well but i'm not going into the details here unless some of you are interested.
This method didn't work for a 6R robot for many reasons.
What i need is a check-collision function that takes into account the kinematics of my robot and the environment in order to calculate if a configuration leads to a collision or not.
Thanks in advance.

答:
solution

The arm should stop on Resistence(Torque increase) or detected Impact. Build the arm to be soft.

Alternative solution

Otherwise the Software can Check if the angle are close to the folddown position and stop moving further.

Title: Collision check of a 6-DOF robotic arm


问:
How to generate a mesh file for URDF
Pybullet needs a mesh file to go along with URDF. Is there a non painful way how to generate one?

Ideally I'd love to generate one from the URDF file itself, as it has all the boxed already defined there. Less ideally, I could use a hint on how to make these mesh files without learning blender or unity or other monstrous tools.

答: The URDF file simply references DAE/COLLADA files that exist elsewhere on the filesystem. Blender and FreeCAD have exporters for DAE files. Saying Blender is a "monstrous tool" is an opinion that probably has little facts to back it up. If you plan on doing URDF file generation more in the future, you will need to learn Blender. DAE is the default exporter for Blender and my experience with it has shown that it works well. There is a Stackexchange page for Blender as well. There are also great youtube channels that show how to use blender. Your best bet is to learn Blender though.</p>

Title: How to generate a mesh file for URDF


答:
If you know how to use plot3 and scatter3 from matlab, by extension you can use plot and scatter with matplotlib and matrices with numpy. You can easily create serial linkages from FK matrices generated by numpy and then plot with matplotlib.

scatterplot
line plot

Title: How to draw robot manipulator diagram

Body: I've been seeing a lot of these diagrams online and wondering how can I replicate this myself. Is there any easy-to-use software package that will allow me to draw these diagrams quickly and accurately? Ideally, I would be able to draw joints, links and label, angles and lengths of the robot manipulator.


答:
BEAM robots are generally simple circuits that exhibit some emergent behavior that (superficially) mimics biological behavior.  Generally speaking, there is no hard criteria for what a BEAM robot must accomplish -- they are not built to handle tasks that require reliability or repeatability.

While BEAM robotics should in theory be able to handle any tasks that a microcontroller could handle, the style of construction is not geared toward ease of troubleshooting or debugging -- essential tasks for the development of any complex behavior.

Analog circuits in general are capable of very critical real-time tasks, such as guiding rockets; see -- for example -- the work of Bob Pease.  But compare the ease of development by changing a few lines of code to testing and rewiring a system like the Hawk Missile autopilot:

Analog electronics aren't incapable, they're just (in these sorts of cases) impractical.

Title: Robots without microcontrolers (beam robots). Are they technologically limited?

Body:
BEAM robotics seem to be a good approach to teach learners about electronics in robotics. But can these robots be like regular programmed "cognitive" robots? Can these robots, with just analog circuits, take us to the level of robotic assistants, worker robots and other kinds of self sufficient autonomous robots?

I specifically want to know that, when creating mission critical robots ->

1) What are the areas in robotics which are practically impossible without a real time software system?

2) What areas of the field can be done without programming? If yes, are these areas feasible without an onboard software system?

3) Could an intelligent space rover, work without a cpu in the future?


问:
Now Unable to Communicate with Create2 and Getting repeated Bat: messages via the serial port
After hooking up my Raspberry Pi4 to the Create2 for the second time, I am now unable to communicate with the Create2. When I open minicom on the pi I see the following messages repeated on the screen. I tried resetting the Create 2 and got the messages again. Has anyone seen something like this?</p>
```<code>================Pasted from minicom on the Pi4===========================================

Welcome to minicom 2.7.1

OPTIONS: I18n
Compiled on Dec 23 2019, 02:06:26.
Port /dev/ttyUSB0, 17:57:46

Press CTRL-A Z for help on special keys

key-wakeup
batt-rx-decode-error: 1 invalid-bit (0)
batt-rx-decode-error: 6 invalid-sync (20)
bat:   min 16648  sec 33  mV 16028  mA 16  rx-byte 90  mAH 1902  state 18  mode 9
    Flash CRC successful: 0x0 (0x0)
bat:   min 16648  sec 34  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 35  mV 16028  mA 0  rx-byte 65535  mAH 1902  state 14  mode 9
initial-lithium-battery-capacity: set to 2068 mAH
initial-lithium-battery-level: set to 1902 mAH
bat:   min 16648  sec 36  mV 16028  mA -16  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 37  mV 16028  mA -8  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 38  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 39  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 40  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16648  sec 40  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
key-wakeup
batt-rx-decode-error: 6 invalid-sync (2)
bat:   min 16649  sec 26  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 27  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 28  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 29  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 30  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 31  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 32  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 33  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 34  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 35  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 36  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 37  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 38  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 39  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 40  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 41  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 42  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 43  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 44  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 45  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 46  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 47  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 48  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 49  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 50  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 51  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 52  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 53  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 54  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 55  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 56  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 57  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 58  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16649  sec 59  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 0  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 1  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 2  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 3  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 4  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 5  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 6  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 7  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 8  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 9  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 10  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 11  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 12  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 13  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 14  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 15  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 16  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 17  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 18  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 19  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 20  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 21  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 22  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 23  mV 16056  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 24  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 25  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 26  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 27  mV 16028  mA 0  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 28  mV 16028  mA -56  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 29  mV 16028  mA -56  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 30  mV 16028  mA -56  rx-byte 90  mAH 1902  state 18  mode 9
bat:   min 16650  sec 30  mV 16028  mA -56  rx-byte 90  mAH 1902  state 18  mode 9
key-wakeup-wheel-drop
batt-rx-decode-error: 1 invalid-bit (0)
saving bbox vars
<span class="math-container">```</span>
</code>```

答:
These battery charging messages are normal when the battery is charging.

What happens when you take the robot off the charger? Have you tried putting the robot into safe mode (b'\x80\x83') before sending commands? Have you tried either of the scripts on the iRobot STEM github repo?

Please note that I'm an iRobot employee but the postings on this site are my own and don't necessarily represent iRobot's positions, strategies or opinions.


答: Interesting question. Those drawings are so stylized and so similar, it seems unlikely that they were done with a generic drawing program.  Next time somebody encounters one online -- if it wasn't just copied from a textbook -- try to locate contact info for the author, and ask him or her what tools were used to do it. Let's hope the answer isn't always "Dunno, it was our graphic arts people".</p>

Title: How to draw robot manipulator diagram


答:
When I used to make these types of figures, I would just use PowerPoint (for Windows) or XDraw (for Unix or Solaris), as @504k mentions.   First I would make an ellipse with the desired perspective for the front (or top) face.  Then I’d copy that ellipse and move it to the back of the viewing plane for the back face.   Next I’d draw two lines tangent to those faces for the top and bottom (or left and right) boundaries.   Then I would zoom in and, using shapes with no visible borders, create blobs that hide the lines which should not be visible.  Finally I’d make that set of shapes into a group so the entire joint could be copied, rotated, and mirrored to create the remaining joints.   It was not elegant but it was effective.

If you have access to a mechanical design package such as SolidWorks, you could extrude a cylinder to create this more accurately.   However, for the purpose of creating a kinematic model, the brute force method was effective.

Title: How to draw robot manipulator diagram


答:
This is quite a vague question, but from what you provided I would suggest:

Check how you are defining your C-Space. Do redundant configurations perform different collision checking?

Take a look at the Flexible Collision Library - FCL . Maybe their framework guides you towards a proper arrangement of your overall joint system.

Check the Open Motion Planning Library - OMPL . Combined with the FCL, you should be able to end up with a proper collision checking system for your arm.

Hope that helps.

Title: Collision check of a 6-DOF robotic arm

Body:
Initially we started with 3-DOF. We checked collisions in the c-space and it worked well. With 6-DOF we're having problems.
Any ideas about how to check collisions for a 6-DOF robotic arm?

--Edit
I'm going to clarify my needs. I have a algorithm based on a hybrid architecture (PRM + Q learning) that plan trajectories.
As some of you now, when building the roadmap using a PRM algorithm, you need to sample configurations (series of 6 angles for a 6R 6DOF robot in my case as we are working in the configuration space). You have to check if these configurations lead to a collision or not. If not you add them to the roadmap.
My problem is with the part where i need to check if a configuration leads to a collision.
At first, when we were testing for a 3R 3DOF robot, we had a brute-force method that worked pretty well but i'm not going into the details here unless some of you are interested.
This method didn't work for a 6R robot for many reasons.
What i need is a check-collision function that takes into account the kinematics of my robot and the environment in order to calculate if a configuration leads to a collision or not.
Thanks in advance.


问:
gear motor with encoder and Arduino Uno
I'm working on a project where using a gear motor with an encoder and an Arduino Uno. I have not done Arduino in a long time and am struggling getting it to do what I want. If you look at the wiring guide for the motor below here is what I have currently wired up:

1 - M1 (-) wired to the ground of the voltage source, 2-GND Encoder wired to the GND pin on the Arduino, 3-Encoder A phase wired to Pin9, 4-Encoder B phase wired to Pin10, 5-3V3 Encoder + wired to 3V3 pin on the Arduino, 6-M1 motor (+) wired to the hot voltage source.

When I supply a voltage source the motor begins to spin. I want the motor to start not moving even when the power is supplied. Then I want to be able to set the angle that the position of the motor will turn to that spot and have it stop moving. I will then turn off the power supply and the motor will stay in that position until power is supplied again a new position is programed into the code. This motor can rotate in both directions. Please help even suggestions what commands to use will be great.

答: M1- and M1+ are motor inputs that should come from the outputs of the HBridge; you control current and direction to the motor using the HBridge.  I like the L9119S hbridge; it's inexpensive and efficient.  I used it in this project as an example.</p>

Title: gear motor with encoder and Arduino Uno


问:
Raspberry Pi 4 to PCA9685 Steering Working but not Throttle
I am using a raspberry pi 4 connecting to a PCA9685 and following the instruction on the donkeycar doc site (the link can be found below). I am at the step "fine tuning calibration." When I run "python manage.py drive" the steering works using my keyboard controls but the throttle is not causing any movement with the keys. I know that the throttle is not broken because the wheels turn when I do the calibration of the throttle. How do I solve this?

https://docs.donkeycar.com/guide/calibrate/

答:
There can be a few explanations for this:

Does your motor controller (ESC) have an on-off switch? If so, try flipping that

Some ESCs need to "arm". Try going to full throttle and back and see if that makes a difference

I'm assuming you have your battery plugged in ;-)

Try reversing the direction in the myconfig.py file


问:
Autonomous harvesting robots. What are model possible components and interactions for the entire system?
I found a video which talk about best autonomous robots that use IA technology to harvest crops. The video is very interesting because, the “ecosystem” of each robot varies a lot from company to company. I am trying to identify with a list of the components which interact (wireless communication) with the robot in such a case, here what I found so far:

The robot that uses IA techniques for harvesting

A supporting robot or an autonomous truck which provides empty storage to carry the harvested crops,

A laptop used to monitor the whole operation for quality purpose

Are there any other components that I might have missed? The only that I can think of is the communication between the robot arm and the carrier which provides the autonomous mobility of the entire robot (this is part of the robot). What are other components that participate in this harvesting operation?

PS: Robotics is an aera outside of my expertise, so please feel free to correct me if I said anything wrong.

答:
Guessing which part is missing in the list is pretty easy. What was provided so far is the robot system itself which contains of the mechanical device, a computer controlled robot and a PC on which the AI software is running. The assumption is that the overall system provides a closed system, which means that the intelligence in the system is able to solve the harvesting task and no further help for the robot is needed. This is equal to an autonomous robot which is self-sufficient because of what?

Right, the reason why the robot doesn't need further advice is because the AI software has reached a certain amount of intelligence which ensures that the system is able to avoid obstacles, can grasp the fruits and can switch off itself if the task is done ...

But let us speculate which part is missing so that the system is working fully autonomous. Or let me answer the question the other way around. Which part is redundant? Perhaps it is possible to combine the laptop with the robot into a single apparatus so that the overall system is reduced to it's minimum and can solve the task more efficient. This would minimize the amount of interface and the amount of communication as well.


问:
homogenous transformation matrix variants
I am working on some manipulator projects and come across a few variants of the homogenous transformation matrix. I've been working on matlab and they give slightly different answers. Assuming both are correct, I would like to ask which one to use and under what circumstances? Are there any conventions that follow along with these trransformation matrices.

答:
On serial manipulator modelling, we commonly use Denavit-Hartenberg system. One important thing you need to know, there are 2 variant of it, that is Denavit-Hartenberg system and Modified Denavit-Hartenberg. The main difference between two is coordinates system attachment to links and order of performed transformation [1].

Denavit-Hartenberg System

Fig 1. DH Coordinate System Attachment

Will have HTM

Modified Denavit-Hartenberg System

Fig 2. MDH Coordinate System Attachment

Will have HTM

And both are correct. My professor recommendation was to use MDH because it use less variable to represent manipulator.

Reference :

Denavit Hartenberg Modelling Wikipedia


问:
AUV LiPo battery
I'm making an AUV that requires 15-20 minutes of operating time. I'm using 6 of these motors (Blue Robotics T100 Thrusters with specs at max are 135 watts and 12.5 amp ) I don't know how to calculate the capacity (mAh) I need to run my vehicle. Is there any formula that allows me to calculate for specify the battery which I will select?</p>

答:
If you want to run one motor at maximum output, it will draw 12.5 amps. If you run it for an hour, it will require 12.5 amp-hours of capacity.

If you have six motors running for an hour at maximum output, then you will need (6)*(12.5) = 75 amp-hours of capacity.

If you want to run the six motors at maximum output for 1/3 of an hour (20 minutes), then you need (1/3)*(75) = 25 amp-hours, or 25,000 mAh.

You should also take into account battery discharge capability (the "C" rating) and min/max states of charge.

For the "C" rating, it's referring to how quickly it can charge or discharge. If you find a battery pack that does have 25,000 mAh, but it's rated for 1C, then it would safely discharge all 25,000 mA in one hour. You need it discharged as quickly as 15 minutes, so you would look for a battery with a C rating of at least 4, else you would need to increase the capacity of your battery pack.

Finally, as a general rule I would generally only assume that I could charge to 80% and discharge to 20%, so I would assume I can only get 60% out of a battery; this would mean I would try to spec a pack that is (1/0.6666) = 150% of what I think I need.

Title: AUV LiPo battery


问:
Small AGV robot for ROS application
I am looking for an off-the-shelf small AGV which runs or could be used with ROS. It may include some basic sensors like a camera, an IMU, an Ultrasonic range finder, etc.

Really appreciate all the valuable suggestions from the community.

答:
The turtlebot-3 is a good option. It comes in two variants and has basic sensors ideal for teaching/learning ROS. Clearpath has several options ranging from small to large robots. These are the ones I am familiar with. If you are looking beyond these: ros-robots has an official list of robots with intended capabilities listed out.

Also, there is the option of learning ROS in simulation using either Gazebo or something like the construct.

Title: Small AGV robot for ROS application