Get best of the week

Nous ressuscitons l'hexapode. Deuxième partie

Vidéo d'un hexapode en mouvement


Par rapport à la publication précédente, son prédécesseur s'est avéré plus spectaculaire, grâce à un grand nombre de photos. Je voudrais combler cette lacune et vous présenter quelques vidéos qui capturent un petit essai routier d'un robot autour de l'appartement.


Le robot est contrôlé par l'opérateur depuis le téléphone via Wi-Fi. Mais en regardant les deux premières vidéos, vous pourriez avoir la fausse impression que l'hexapode a les rudiments de l'intelligence. Malheureusement, ce n'est pas le cas ...


De plus, le robot a été expulsé du tapis mou vers un linoléum plutôt glissant, ce qui a donné un bon résultat.


Mise en place de mouvements hexapodes


Comme mentionné dans l' article précédent , la classe MotionJob et les classes dérivées héritées de la classe Motion sont responsables de l'implémentation des mouvements du robot .

La classe MotionJob n'a qu'une seule méthode d' exécution publique , qui lance une motion d'exécution donnée.

int MotionJob :: execute (Motion * pMotion)
int MotionJob::execute(Motion* pMotion) {
  if(m_Status == NONE && pMotion != NULL) {
     
     m_pMotion = pMotion;
     m_MotionTime = 0;
     m_TotalTime = m_pMotion->totalTime();
     m_Status = RUNING;
  } else if(pMotion != NULL){
    
    if(m_pMotion->m_pNext != NULL) 
      delete m_pMotion->m_pNext;
      
    m_pMotion->m_pNext = pMotion;
     m_Status = STOPING;
  }
  return S_OK;
}


Sa logique est assez simple. En tant que paramètre, il prend un pointeur sur un objet qui décrit le processus de mouvement. Si au moment actuel il n'y a pas eu d'autre mouvement dans l'exécution, alors le mouvement indiqué devient actif et il entre dans l'état d'exécution ( RUNING ). Dans le cas où il y avait déjà un mouvement à l'exécution, nous le traduisons dans un état d'arrêt ( STOPING ), et le nouveau mouvement est enregistré comme le suivant dans le champ m_pNext .

IMPORTANT: Il est convenu que tous les objets hérités de Motion impliquent la création dans la zone de mémoire dynamique (via new ) et sont libérés par la classe MotionJob une fois le mouvement terminé ou annulé.

La classe MotionJob , héritée d' AJob , implémente toute la magie du mouvement dans la méthode onRun superposée , qui est appelée avec une fréquence donnée pendant le fonctionnement du robot.

void MotionJob :: onRun ()
void MotionJob::onRun() {

  Vector3D points[FOOTS_COUNT];
  int pose[FOOTS_COUNT][3];
  //    ,  
  if(m_pMotion == NULL)
    return;
  //           ,
  //            .
  //      .
  int res;
  if(m_pMotion->getPose(min(m_MotionTime, m_pMotion->maxTime()), pose) == S_OK) {
     res = m_pGeksapod->_setPose(pose);
  } else if(m_pMotion->getPose(min(m_MotionTime, m_pMotion->maxTime()), points) == S_OK) {
     res = m_pGeksapod->_setPose(points);
  }
  //     
  m_MotionTime += MOTION_JOB_PERIOD;
  //     ,    ,
  //      :
  //   m_TotalTime -     
  if(m_TotalTime > 0) { 
    m_TotalTime -= MOTION_JOB_PERIOD;
    m_TotalTime = max(0,m_TotalTime); 
  }
  //      , 
  //      
  if(m_TotalTime == 0 && m_pMotion->isLooped()) {
     m_Status = STOPING;
  }
  //         
  //          .
  if( (m_MotionTime - m_pMotion->maxTime() >= MOTION_JOB_PERIOD && (!m_pMotion->isLooped() || m_Status == STOPING)) ) { //  
    //      
    Motion* pNextMotion = m_pMotion->m_pNext;
    m_pMotion->m_pNext = NULL;
    delete m_pMotion;
    m_pMotion = NULL;
    //   
    m_Status = NONE;
    execute(pNextMotion);
  } else if(m_MotionTime - m_pMotion->maxTime() >= MOTION_JOB_PERIOD) {
    //      
    //         
    m_MotionTime = 0; 
  }
}


Le code ci-dessus contient suffisamment de commentaires sur son travail, donc, en complément, je ne note que quelques points clés:
  • Motion (Vector3D points[FOOTS_COUNT]), (int pose[FOOTS_COUNT][3]). , . getPose , getPose. _setPose, .
  • Les mouvements peuvent être réguliers ou cycliques. Le mouvement normal à la fin de la période de temps allouée pour l'exécution d'un cycle de mouvement (retourné par la méthode Motion :: maxTime () ) sera immédiatement terminé.
    Les mouvements cycliques ne seront terminés qu'après un arrêt forcé qui se produit lorsque la méthode d' exécution est appelée (peut être appelée avec un pointeur nul), ou lorsqu'un moment positif est alloué pour l'exécution du mouvement (retourné par la méthode Motion :: totalTime () ).

Les classes suivantes sont définies pour décrire diverses options de mouvement:

classe TransPoseMotion
class TransPoseMotion: public Motion {
  Vector3D m_Pose0[FOOTS_COUNT];  //  
  Vector3D m_Offset[FOOTS_COUNT];  //  
public:  
  TransPoseMotion(Vector3D begPose[FOOTS_COUNT], Vector3D endPose[FOOTS_COUNT],
        long maxTime, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

classe TransAnglesMotion
class TransAnglesMotion: public Motion {
  char m_Pose0[FOOTS_COUNT][3];
  char m_Offset[FOOTS_COUNT][3];
public:  
  TransAnglesMotion(int begPose[FOOTS_COUNT][3], int endPose[FOOTS_COUNT][3],
        long maxTime, Motion* pNext = NULL);
  int getPose(long actionTime, int pose[FOOTS_COUNT][3]);
};

classe LinearMotion
class LinearMotion: public Motion {
  //     ( ) 
  Vector3D m_Pose0[FOOTS_COUNT];
  Vector3D m_Offset; //     
  int m_Heigth;          //   
public:  
  LinearMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed, 
        long maxTime, long totalTime = -1, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

classe RotateMotion
class RotateMotion: public Motion {
  //     ( ) 
  Vector3D m_Pose0[FOOTS_COUNT]; 
  Vector3D m_Angles; //     
  int m_Heigth;           //   
public:  
  RotateMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D rotor, 
        long maxTime, long totalTime = -1, Motion* pNext = NULL);
  int getPose(long time, Vector3D points[FOOTS_COUNT]);
};

Classe TransPoseMotion


La classe TransPoseMotion vous permet de déplacer des membres d'une position à une autre. Lors de la création d'un objet, les coordonnées des points et la période au-delà de laquelle le mouvement doit être effectué sont transférées au constructeur de classe.

TransPoseMotion :: TransPoseMotion (...)
TransPoseMotion::TransPoseMotion(Vector3D begPose[FOOTS_COUNT],
  Vector3D endPose[FOOTS_COUNT], long maxTime, Motion* pNext) : Motion(maxTime, -1, pNext) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    m_Pose0[i] = begPose[i];
    m_Offset[i] = endPose[i] - begPose[i];
  }
}


Connaissant la position initiale et le vecteur de déplacement pour chaque membre, nous pouvons facilement obtenir la nouvelle position des pattes dans la méthode getPose dans l'intervalle de temps de 0 à maxTime en effectuant de simples calculs linéaires.

int TransPoseMotion :: getPose (...)
int TransPoseMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  for(int i = 0; i < FOOTS_COUNT; i++)
    linearFunc(1.0, m_Offset[i], m_Pose0[i], 1.0*time/m_MaxTime, points[i]);
}


Les calculs linéaires sont affectés à la fonction auxiliaire linearFunc, qui est une équation linéaire ordinaire avec des paramètres spécifiés par les arguments de la fonction.

void linearFunc (...)
void linearFunc(float a, Vector3D offset, Vector3D p0, float k,  Vector3D& res) {
  res = a*offset*k + p0;
}


TransAnglesMotion, classe


La logique de cette classe est similaire à la précédente. La seule différence est que dans la classe TransAnglesMotion, les positions des membres hexapodes sont définies par l'ensemble des angles auxquels ses articulations doivent être pliées, et non par les coordonnées spatiales des extrémités des pattes du robot.

TransAnglesMotion :: TransAnglesMotion (...)
TransAnglesMotion::TransAnglesMotion(int begPose[FOOTS_COUNT][3],
  int endPose[FOOTS_COUNT][3], long maxTime, Motion* pNext) : Motion(maxTime, -1, pNext) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    for(int j = 0; j < 3; j++) {
      m_Pose0[i][j] = begPose[i][j];
      m_Offset[i][j] = endPose[i][j] - begPose[i][j];
    }
  }
}



int TransAnglesMotion :: getPose (...)
int TransAnglesMotion::getPose(long time, int pose[FOOTS_COUNT][3]) {
  for(int i = 0; i < FOOTS_COUNT; i++) {
    for(int j = 0; j < 3; j++) 
      pose[i][j] = m_Pose0[i][j] + (int)(1.0*m_Offset[i][j]*time/m_MaxTime);
  }
  return S_OK;
}


Classe LinearMotion


La mise en œuvre d'un mouvement simple nécessite un peu plus de code et votre attention. La conception hexapode a un nombre optimal de membres, vous permettant d'avoir toujours trois points d'appui lors du déplacement, de sorte que la projection du centre de masse du robot soit toujours située à l'intérieur du triangle formé par eux. Cela garantit une position stable du robot lors du déplacement. Par conséquent, le mouvement rectiligne du robot peut être considéré comme un ensemble d'étapes identiques. Dans chacune de ces étapes, deux phases peuvent être distinguées. Dans la première phase, une triade de jambes touche la surface et leurs extrémités se déplacent parallèlement à la surface dans la direction opposée à la direction du mouvement. Dans ce cas, la deuxième triade de pattes, sans toucher la surface, se déplace le long d'une trajectoire courbe dans le sens du mouvement du robot.À un certain moment, ils atteignent leur position extrême et la deuxième triade commence à entrer en contact avec la surface. Dans la deuxième phase, le processus de mouvement est répété, mais cette fois, la deuxième triade déplace le robot et la première revient au point de départ.



Cette animation a été trouvée lors de la préparation d'un article sur Internet sur cette page , mais après l'avoir revue, j'étais convaincue que sa source d'origine était habr , et l'auteur de la publication était le respecté conKORD .

Lors de la réalisation du mouvement en ligne droite, nous devons définir la hauteur des jambes par rapport à la surface ( hauteur ), la position des membres du robot au début (et à la fin) du mouvement ( pose0 ), le vecteur de vitesse ( vitesse ), le temps qu'il faut pour effectuer une étape ( maxTime ) et combien de temps le robot doit effectuer le mouvement ( totalTime ). Si vous spécifiez une valeur négative pour totalTime, le mouvement se poursuivra indéfiniment jusqu'à ce que la charge du robot s'épuise ou que le mouvement soit terminé de force.

LinearMotion :: LinearMotion (...)
LinearMotion::LinearMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed,
  long maxTime, long totalTime, Motion* pNext = NULL) 
    : Motion(maxTime, true, totalTime, pNext) {
  copyPose(m_Pose0, pose0);
  m_Offset = 1.0*speed*maxTime/1000;
  m_Heigth = heigth;
}

Malgré le fait que la méthode getPose contient une quantité impressionnante de code, c'est assez simple.

int LinearMotion :: getPose (...)
int LinearMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  double k = 1.0*time/m_MaxTime;

  if(k < 0.25) {  //   
    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*cos(M_PI*k*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;

  } else if(k < 0.5) { //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*sin(M_PI*(k - 0.25)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;

  } else if(k < 0.75) { //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
    
    int h = (int)m_Heigth*cos(M_PI*(k - 0.5)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;
    
  } else {  //   
    
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], 1 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], 1 - k,
      points[LEFT_BACK_FOOT_IDX]);
    linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 1 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);

    linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], 1 - k,
      points[LEFT_MIDLE_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], 1 - k,
      points[RIGTH_FRONT_FOOT_IDX]);
    linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], 1 - k,
      points[RIGTH_BACK_FOOT_IDX]);
    
    int h = (int)m_Heigth*sin(M_PI*(k - 0.75)*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;
  }

  return S_OK;
}


Nous divisons l'ensemble de l'étape en quatre trimestres:

  1. Au premier trimestre, la première triade de membres (avant gauche, arrière gauche et milieu droit) effectue un mouvement de poussée, c'est-à-dire qu'ils se déplacent le long de la surface dans la direction opposée au mouvement. La deuxième triade de pattes (avant droit, arrière droit et milieu gauche) se déplace dans le sens du mouvement, tandis que la hauteur de leur élévation varie de la valeur maximale de la hauteur spécifiée des jambes (au début du quartier) à zéro (à la fin du trimestre) en fonction du sinus.
  2. Au deuxième trimestre, les directions des triades changent. Maintenant, la deuxième triade de pattes déplace le robot et la première se déplace dans la même direction que le robot. Dans ce cas, la hauteur des pattes de la première triade change dans une sinusoïde de zéro à la valeur maximale.
  3. Au troisième trimestre, la direction du mouvement des pattes est maintenue et les extrémités de la première triade descendent pour atteindre la surface à la fin du troisième trimestre.
  4. Au dernier trimestre, la première triade devient le support, et la seconde change de direction et se déplace vers le haut le long de la sinusoïde, atteignant la hauteur maximale à la fin du trimestre.

Classe RotateMotion


Ayant compris la logique de la classe LinearMotion , nous pouvons facilement comprendre l'implémentation de la classe RotateMotion .

RotateMotion :: RotateMotion (...)
RotateMotion::RotateMotion(int heigth, Vector3D pose0[FOOTS_COUNT], Vector3D speed,
  long maxTime, long totalTime = -1, Motion* pNext = NULL) 
    : Motion(maxTime, true, totalTime, pNext) {
  copyPose(m_Pose0, pose0);
  m_Angles = 0.001*maxTime*speed*M_PI/180;
  m_Heigth = heigth;
}


Les constructeurs des deux classes sont presque identiques dans la composition des arguments passés. Ce n'est que dans le cas d'une inversion de vitesse que le vecteur vitesse angulaire définit l'axe et la vitesse de rotation du robot.

Dans la méthode getPose de la classe RotateMotion , la logique de travail est également divisée en quatre étapes. Cependant, pour calculer la position des pattes, au lieu de la fonction linéaire linearFunc , la fonction radiale rotFunc est utilisée .

int RotateMotion :: getPose (...)
int RotateMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
  double k = 1.0*time/m_MaxTime;
  if(k < 0.25) {  //   

    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], -k,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], -k,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], -k,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*cos(M_PI*k*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;

  } else if(k < 0.5) {  //   
    
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], 0.5 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], 0.5 - k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 0.5 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*sin(M_PI*(k - 0.25)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;

  } else if(k < 0.75) {  //   
  
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], 0.5 - k,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], 0.5 - k,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 0.5 - k,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
      points[RIGTH_BACK_FOOT_IDX]);
  
    int h = (int)m_Heigth*cos(M_PI*(k - 0.5)*2);
    points[LEFT_FRONT_FOOT_IDX].z += h;
    points[LEFT_BACK_FOOT_IDX].z += h;
    points[RIGTH_MIDLE_FOOT_IDX].z += h;
  } else { //   
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 1,
      points[LEFT_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_BACK_FOOT_IDX], k - 1,
      points[LEFT_BACK_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 1,
      points[RIGTH_MIDLE_FOOT_IDX]);
      
    rotFunc(2.0, m_Angles, m_Pose0[LEFT_MIDLE_FOOT_IDX], 1 - k,
      points[LEFT_MIDLE_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_FRONT_FOOT_IDX], 1 - k,
      points[RIGTH_FRONT_FOOT_IDX]);
    rotFunc(2.0, m_Angles, m_Pose0[RIGTH_BACK_FOOT_IDX], 1 - k,
      points[RIGTH_BACK_FOOT_IDX]);

    int h = (int)m_Heigth*sin(M_PI*(k - 0.75)*2);
    points[LEFT_MIDLE_FOOT_IDX].z += h;
    points[RIGTH_FRONT_FOOT_IDX].z += h;
    points[RIGTH_BACK_FOOT_IDX].z += h;
  }
  return S_OK;
}


La fonction radiale vous permet de calculer la position d'un point dans l'espace après sa rotation autour du centre du robot, qui coïncide avec l'origine.

void rotFunc (...)
void rotFunc(float a, Vector3D angles, Vector3D p0, float k, Vector3D& res) {
  Matrix3D m = rotMatrix2(a*angles*k);
  res = m*p0;
}


À l'intérieur de la fonction, un appareil mathématique est impliqué, nous permettant de travailler avec des quantités vectorielles et matricielles.

C'est tout pour le moment. Nous avons analysé une partie essentielle du code de programme responsable de la mise en œuvre du mouvement du robot. Si vous avez des questions ou des commentaires, je serai heureux d'y répondre dans les commentaires. J'essaierai de consacrer l'article suivant au reste du code, dont le code source sera disponible en téléchargement.
Merci pour l'attention!

À suivre…

More articles:


All Articles