Get best of the week

Wir beleben den Hexapod wieder. Zweiter Teil

Video eines sich bewegenden Hexapods


Im Vergleich zur vorherigen Veröffentlichung erwies sich ihr Vorgänger dank einer großen Anzahl von Fotos als spektakulärer. Ich möchte die Lücke in dieser Angelegenheit schließen und Ihnen einige Videos präsentieren, die eine kleine Probefahrt des Roboters durch die Wohnung festhalten.


Der Roboter wird vom Bediener vom Telefon aus über WLAN gesteuert. Wenn Sie sich jedoch die ersten beiden Videos ansehen, haben Sie möglicherweise den falschen Eindruck, dass der Hexapod die Grundlagen der Intelligenz besitzt. Leider ist das nicht so ...


Ferner wurde der Roboter vom weichen Teppich zu einem ziemlich rutschigen Linoleum ausgestoßen, was ein gutes Ergebnis zeigte.


Implementierung von Hexapod-Bewegungen


Wie im vorherigen Artikel erwähnt , sind die MotionJob- Klasse und abgeleitete Klassen, die von der Motion- Klasse geerbt wurden, für die Implementierung von Roboterbewegungen verantwortlich .

Die MotionJob- Klasse verfügt nur über eine öffentliche Ausführungsmethode , mit der eine bestimmte Bewegungsausführung gestartet wird .

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;
}


Die Logik ist ziemlich einfach. Als Parameter wird ein Zeiger auf ein Objekt verwendet, das den Bewegungsprozess beschreibt. Wenn zum aktuellen Zeitpunkt keine andere Bewegung in der Ausführung war, wird die angegebene Bewegung aktiv und wechselt in den Ausführungsstatus ( RUNING ). Falls bei der Ausführung bereits eine Bewegung stattgefunden hat , übersetzen wir diese in einen Stoppzustand ( STOPING ), und die neue Bewegung wird als nächste im Feld m_pNext aufgezeichnet .

WICHTIG: Es wird vereinbart, dass alle von Motion geerbten Objekte eine Erstellung im dynamischen Speicherbereich (über new ) implizieren und von der MotionJob- Klasse freigegeben werden , nachdem die Bewegung abgeschlossen oder abgebrochen wurde.

Die MotionJob- Klasse , die von AJob geerbt wurde , implementiert die gesamte Magie der Bewegung in der überlappenden onRun- Methode , die während des Betriebs des Roboters mit einer bestimmten Häufigkeit aufgerufen wird.

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; 
  }
}


Der obige Code enthält genügend Kommentare zu seiner Arbeit, daher stelle ich als Ergänzung nur einige wichtige Punkte fest:
  • Motion (Vector3D points[FOOTS_COUNT]), (int pose[FOOTS_COUNT][3]). , . getPose , getPose. _setPose, .
  • Bewegungen können regelmäßig oder zyklisch sein. Die normale Bewegung nach Erreichen des für die Ausführung eines Bewegungszyklus vorgesehenen Zeitraums (zurückgegeben von der Motion :: maxTime () -Methode ) wird sofort abgeschlossen.
    Zyklische Bewegungen werden erst nach einem erzwungenen Stopp abgeschlossen, der auftritt, wenn die Ausführungsmethode aufgerufen wird (kann mit einem Nullzeiger aufgerufen werden) oder wenn ein positiver Zeitpunkt für die Ausführung der Bewegung zugewiesen wird ( zurückgegeben von der Motion :: totalTime () -Methode ).

Die folgenden Klassen werden zur Beschreibung verschiedener Bewegungsoptionen definiert:

Klasse 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]);
};

Klasse 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]);
};

Klasse 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]);
};

Klasse 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]);
};

TransPoseMotion-Klasse


Mit der TransPoseMotion- Klasse können Sie Gliedmaßen von einer Position zur anderen bewegen. Beim Erstellen eines Objekts werden die Koordinaten der Punkte und der Zeitraum, ab dem die Bewegung ausgeführt werden soll, an den Klassenkonstruktor übertragen.

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];
  }
}


Wenn wir die Anfangsposition und den Verschiebungsvektor für jedes Glied kennen, können wir die einfache Position der Pfoten in der getPose- Methode im Zeitintervall von 0 bis maxTime leicht ermitteln, indem wir einfache lineare Berechnungen durchführen.

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]);
}


Lineare Berechnungen werden der Hilfsfunktion linearFunc zugewiesen, einer gewöhnlichen linearen Gleichung mit Parametern, die durch die Argumente der Funktion angegeben werden.

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


TransAnglesMotion-Klasse


Die Logik dieser Klasse ähnelt der vorherigen. Der einzige Unterschied besteht darin, dass in der TransAnglesMotion- Klasse die Positionen der Hexapod- Gliedmaßen durch den Satz von Winkeln festgelegt werden, in denen die Gelenke gebogen werden sollen, und nicht durch die Raumkoordinaten der Enden der Roboterpfoten.

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;
}


LinearMotion-Klasse


Das Implementieren von unkomplizierten Bewegungen erfordert etwas mehr Code und Ihre Aufmerksamkeit. Das Hexapod-Design verfügt über eine optimale Anzahl von Gliedmaßen, sodass Sie beim Bewegen immer drei Stützpunkte haben, sodass sich die Projektion des Schwerpunkts des Roboters immer innerhalb des von ihnen gebildeten Dreiecks befindet. Dies gewährleistet eine stabile Position des Roboters beim Bewegen. Daher kann die geradlinige Bewegung des Roboters als ein Satz identischer Schritte betrachtet werden. In jedem solchen Schritt können zwei Phasen unterschieden werden. In der ersten Phase berührt eine Dreiklang von Beinen die Oberfläche und ihre Enden bewegen sich parallel zur Oberfläche in die der Bewegungsrichtung entgegengesetzte Richtung. In diesem Fall bewegt sich die zweite Pfoten-Triade, ohne die Oberfläche zu berühren, entlang eines gekrümmten Pfades in Bewegungsrichtung des Roboters.Zu einem bestimmten Zeitpunkt erreichen sie ihre äußerste Position und die zweite Triade beginnt mit der Oberfläche in Kontakt zu kommen. In der zweiten Phase wird der Bewegungsvorgang wiederholt, nur diesmal bewegt die zweite Triade den Roboter und die erste kehrt zum Startpunkt zurück.



Diese Animation wurde bei der Vorbereitung eines Artikels im Internet auf dieser Seite gefunden , aber nach Durchsicht war ich überzeugt, dass die ursprüngliche Quelle habr war und der Autor der Veröffentlichung das angesehene conKORD war .

Wenn wir die Bewegung in einer geraden Linie realisieren, müssen wir die Höhe der Beine von der Oberfläche ( Höhe ), die Position der Roboterglieder am Anfang (und am Ende) der Bewegung ( Pose0 ), den Geschwindigkeitsvektor ( Geschwindigkeit ), die Zeit, die für einen Schritt benötigt wird ( maxTime ) und wie lange der Roboter dauern sollte , einstellen Bewegung ausführen ( totalTime ). Wenn Sie einen negativen Wert für totalTime angebenwird die Bewegung auf unbestimmte Zeit fortgesetzt, bis die Ladung des Roboters aufgebraucht ist oder die Bewegung gewaltsam abgeschlossen ist.

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;
}

Trotz der Tatsache, dass die getPose- Methode eine beeindruckende Menge an Code enthält, ist sie recht einfach.

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;
}


Wir teilen den ganzen Schritt in vier Viertel:

  1. Im ersten Quartal führt die erste Dreiklang von Gliedmaßen (links vorne, links hinten und rechts in der Mitte) eine Druckbewegung aus, dh sie bewegt sich entlang der Oberfläche in die entgegengesetzte Richtung zur Bewegung. Die zweite Dreiklang von Pfoten (rechts vorne, rechts hinten und links in der Mitte) bewegt sich in Bewegungsrichtung, während die Höhe ihres Anstiegs vom Maximalwert der angegebenen Höhe der Beine (zu Beginn des Quartals) bis Null (am Ende des Quartals) als Funktion des Sinus variiert.
  2. Im zweiten Quartal ändern sich die Richtungen der Triaden. Jetzt bewegt die zweite Pfoten-Triade den Roboter und die erste bewegt sich in die gleiche Richtung wie der Roboter. In diesem Fall ändert sich die Höhe der Pfoten der ersten Triade in einer Sinuskurve von Null auf den Maximalwert.
  3. Im dritten Quartal wird die Bewegungsrichtung der Pfoten beibehalten, und die Enden der ersten Triade sinken ab und erreichen am Ende des dritten Quartals die Oberfläche.
  4. Im letzten Quartal wird die erste Triade zur Stütze, und die zweite ändert ihre Richtung und bewegt sich entlang der Sinuskurve nach oben, wobei sie am Ende des Quartals die maximale Höhe erreicht.

RotateMotion-Klasse


Nachdem wir die Logik der LinearMotion- Klasse verstanden haben , können wir die Implementierung der RotateMotion- Klasse leicht herausfinden .

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;
}


Die Konstruktoren beider Klassen sind in der Zusammensetzung der übergebenen Argumente nahezu identisch. Nur im Fall einer Umkehrung der Geschwindigkeit ist der Winkelgeschwindigkeitsvektor , die die Achse und die Geschwindigkeit der Drehung des Roboters definieren.

In der getPose- Methode der RotateMotion- Klasse ist die Arbeitslogik ebenfalls in vier Stufen unterteilt. Zur Berechnung der Position der Pfoten wird jedoch anstelle der linearen Funktion linearFunc die radiale Funktion rotFunc verwendet .

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;
}


Mit der Radialfunktion können Sie die Position eines Punktes im Raum nach seiner Drehung um die Mitte des Roboters berechnen, die mit dem Ursprung übereinstimmt.

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


Innerhalb der Funktion befindet sich ein mathematischer Apparat, mit dem wir mit Vektor- und Matrixgrößen arbeiten können.

Das war es fürs Erste. Wir haben einen wesentlichen Teil des Programmcodes analysiert, der für die Implementierung der Bewegung des Roboters verantwortlich ist. Wenn Sie Fragen oder Kommentare haben, beantworte ich diese gerne in den Kommentaren. Ich werde versuchen, den folgenden Artikel dem Rest des Codes zu widmen, dessen Quellcode zum Download zur Verfügung steht.
Vielen Dank für Ihre Aufmerksamkeit!

Fortsetzung folgt…

More articles:


All Articles