Get best of the week

Revivimos el hexápodo. La segunda parte

Video de un hexápodo en movimiento


En comparación con la publicación anterior, su predecesora resultó ser más espectacular, gracias a una gran cantidad de fotos. Me gustaría llenar el vacío en este asunto y presentarles algunos videos que capturan una pequeña prueba de manejo de un robot alrededor del apartamento.


El operador controla el robot desde el teléfono a través de Wi-Fi. Pero cuando mira los primeros dos videos, puede tener la falsa impresión de que el hexápodo tiene los rudimentos de la inteligencia. Desafortunadamente, esto no es así ...


Además, el robot fue expulsado de la alfombra suave a un linóleo bastante resbaladizo, que mostró un buen resultado.


Implementación de movimientos hexapod


Como se mencionó en el artículo anterior , la clase MotionJob y las clases derivadas heredadas de la clase Motion son responsables de la implementación de los movimientos del robot .

La clase MotionJob tiene solo un método de ejecución público , que inicia un movimiento dado para la ejecución.

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


Su lógica es bastante simple. Como parámetro, toma un puntero a un objeto que describe el proceso de movimiento. Si en el momento actual no hubo otro movimiento en la ejecución, entonces el movimiento indicado se activa y entra en el estado de ejecución ( RUNING ). En el caso de que ya hubiera algún movimiento en la ejecución, lo traducimos a un estado de detención ( STOPING ), y el nuevo movimiento se registra como el siguiente en el campo m_pNext .

IMPORTANTE: se acuerda que todos los objetos heredados de Motion implican la creación en el área de memoria dinámica (a través de nuevo ) y son liberados por la clase MotionJob después de que el movimiento se haya completado o cancelado.

La clase MotionJob , que se hereda de AJob , implementa toda la magia del movimiento en el método onRun superpuesto , que se llama con una frecuencia dada durante la operación del robot.

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


El código anterior contiene suficientes comentarios sobre su trabajo, por lo tanto, como complemento, solo observo algunos puntos clave:
  • Motion (Vector3D points[FOOTS_COUNT]), (int pose[FOOTS_COUNT][3]). , . getPose , getPose. _setPose, .
  • Los movimientos pueden ser regulares o cíclicos. El movimiento normal al llegar al final del período de tiempo asignado para la ejecución de un ciclo de movimiento (devuelto por el método Motion :: maxTime () ) se completará de inmediato.
    Los movimientos cíclicos se completarán solo después de una detención forzada que se produce cuando se llama al método de ejecución (se puede llamar con un puntero nulo) o cuando se asigna un punto positivo en el tiempo para la ejecución del movimiento (devuelto por el método Motion :: totalTime () ).

Las siguientes clases se definen para describir varias opciones de movimiento:

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

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

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

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

Clase TransPoseMotion


La clase TransPoseMotion le permite mover extremidades de una posición a otra. Al crear un objeto, las coordenadas de los puntos y el período de tiempo más allá del cual se debe llevar a cabo el movimiento se transfieren al constructor de la clase.

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


Conociendo la posición inicial y el vector de desplazamiento para cada extremidad, podemos obtener fácilmente la nueva posición de las patas en el método getPose en el intervalo de tiempo de 0 a maxTime realizando cálculos lineales simples.

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


Los cálculos lineales se asignan a la función auxiliar linearFunc, que es una ecuación lineal ordinaria con parámetros especificados a través de los argumentos de la función.

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


Clase TransAnglesMotion


La lógica de esta clase es similar a la anterior. La única diferencia es que en la clase TransAnglesMotion, las posiciones de las extremidades del hexápodo se establecen por el conjunto de ángulos en los que deben doblarse sus articulaciones, y no por las coordenadas espaciales de los extremos de las patas del 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;
}


Clase de movimiento lineal


Implementar un movimiento directo requiere un poco más de código y su atención. El diseño del hexápodo tiene un número óptimo de extremidades, lo que le permite tener siempre tres puntos de apoyo cuando se mueve, de modo que la proyección del centro de masa del robot siempre se encuentra dentro del triángulo formado por ellos. Esto asegura una posición estable del robot cuando se mueve. Por lo tanto, el movimiento rectilíneo del robot puede considerarse como un conjunto de pasos idénticos. En cada uno de estos pasos, se pueden distinguir dos fases. En la primera fase, una tríada de patas toca la superficie y sus extremos se mueven paralelos a la superficie en la dirección opuesta a la dirección del movimiento. En este caso, la segunda tríada de patas, sin tocar la superficie, se mueve a lo largo de una trayectoria curva en la dirección del movimiento del robot.En cierto punto en el tiempo, alcanzan su posición extrema, y ​​la segunda tríada comienza a entrar en contacto con la superficie. En la segunda fase, el proceso de movimiento se repite, solo que esta vez la segunda tríada mueve el robot y la primera vuelve al punto de partida.



Esta animación se encontró al preparar un artículo en Internet en esta página , pero después de revisarla, estaba convencido de que su fuente original era habr , y el autor de la publicación era el respetado conKORD .

Al realizar el movimiento en línea recta, necesitamos establecer la altura de las piernas fuera de la superficie ( altura ), la posición de las extremidades del robot al principio (y al final) del movimiento ( pose0 ), el vector de velocidad ( velocidad ), el tiempo que lleva completar un paso ( maxTime ) y cuánto tiempo debe el robot realizar movimiento ( totalTime ). Si especifica un valor negativo para totalTime, el movimiento continuará indefinidamente hasta que la carga del robot se agote o el movimiento se complete por la fuerza.

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

A pesar de que el método getPose contiene una cantidad impresionante de código, es bastante 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;
}


Dividimos todo el paso en cuatro cuartos:

  1. En el primer trimestre, la primera tríada de extremidades (parte delantera izquierda, parte posterior izquierda y centro derecha) realiza un movimiento de empuje, es decir, se mueve a lo largo de la superficie en la dirección opuesta al movimiento. La segunda tríada de patas (derecha delantera, derecha atrás y centro izquierda) se mueve en la dirección del movimiento, mientras que la altura de su elevación varía desde el valor máximo de la altura especificada de las patas (al comienzo del cuarto) a cero (al final del cuarto) en función del seno.
  2. En el segundo trimestre, las direcciones de las tríadas cambian. Ahora la segunda tríada de patas mueve al robot, y la primera se mueve en la misma dirección que el robot. En este caso, la altura de las patas de la primera tríada cambia en una sinusoide de cero al valor máximo.
  3. En el tercer trimestre, se mantiene la dirección de movimiento de las patas, y los extremos de la primera tríada bajan, alcanzando la superficie al final del tercer trimestre.
  4. En el último cuarto, la primera tríada se convierte en el soporte, y el segundo cambia de dirección y se mueve hacia arriba a lo largo de la sinusoide, alcanzando la altura máxima al final del cuarto.

Clase RotateMotion


Habiendo entendido la lógica de la clase LinearMotion , podemos descubrir fácilmente la implementación de la clase 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;
}


Los constructores de ambas clases son casi idénticos en la composición de los argumentos pasados. Solo en el caso de una inversión de velocidad, el vector de velocidad angular define el eje y la velocidad del robot.

En el método getPose de la clase RotateMotion , la lógica del trabajo también se divide en cuatro etapas. Sin embargo, para calcular la posición de las patas, en lugar de la función lineal linearFunc , se utiliza la función radial rotFunc .

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 función radial le permite calcular la posición de un punto en el espacio después de su rotación alrededor del centro del robot, que coincide con el origen.

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


Dentro de la función, está involucrado un aparato matemático, que nos permite trabajar con cantidades vectoriales y matriciales.

Eso es todo por ahora. Hemos analizado una parte esencial del código del programa responsable de implementar el movimiento del robot. Si tiene preguntas o comentarios, me complacerá responderlos en los comentarios. Intentaré dedicar el siguiente artículo al resto del código, cuyo código fuente estará disponible para descargar.
¡Gracias por la atención!

Continuará…

More articles:


All Articles