Get best of the week

We revive the hexapod. Part two

Video of a moving hexapod


Compared to the previous publication, her predecessor turned out to be more spectacular, thanks to a large number of photos. I would like to fill in the gap in this matter and present you a few videos that capture a small test drive of a robot around the apartment.


The robot is controlled by the operator from the phone via Wi-Fi. But when watching the first two videos, you might get the false impression that the hexapod has the rudiments of intelligence. Unfortunately, this is not so ...


Further, the robot was expelled from the soft carpet to a rather slippery linoleum, which showed a good result.


Implementation of hexapod movements


As was mentioned in the previous article , the MotionJob class and derived classes inherited from the Motion class are responsible for the implementation of robot movements .

The MotionJob class has just one public execute method , which launches a given motion for execution.

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


Its logic is pretty simple. As a parameter, it takes a pointer to an object that describes the process of movement. If at the current moment there was no other movement in the execution, then the indicated movement becomes active, and it enters the execution state ( RUNING ). In the case when there was already some movement at the execution, we translate it into a stop state ( STOPING ), and the new movement is recorded as the next in the m_pNext field .

IMPORTANT: It is agreed that all objects inherited from Motion imply creation in the dynamic memory area (via new ) and are freed by the MotionJob class after the movement is completed or canceled.

The MotionJob class , being inherited from AJob , implements all the magic of movement in the overlapped onRun method , which is called with a given frequency during the operation of the 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; 
  }
}


The above code contains enough comments about its work, therefore, as an addition, I note only a few key points:
  • Motion (Vector3D points[FOOTS_COUNT]), (int pose[FOOTS_COUNT][3]). , . getPose , getPose. _setPose, .
  • Movements can be regular or cyclical. Normal movement upon reaching the end of the time period allotted for the execution of one movement cycle (returned by the Motion :: maxTime () method ) will be immediately completed.
    Cyclic movements will be completed only after a forced stop that occurs when the execute method is called (can be called with a null pointer), or when a positive point in time is allotted for the execution of the movement (returned by the Motion :: totalTime () method ).

The following classes are defined for describing various motion options:

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

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

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

class 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 Class


The TransPoseMotion class allows you to move limbs from one position to another. When creating an object, the coordinates of points and the period of time beyond which the movement should be carried out are transferred to the class constructor.

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


Knowing the initial position and the displacement vector for each limb, we can easily get the new position of the paws in the getPose method in the time interval from 0 to maxTime by performing simple linear calculations.

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


Linear computations are assigned to the auxiliary function linearFunc, which is an ordinary linear equation with parameters specified through the arguments of the function.

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


TransAnglesMotion Class


The logic of this class is similar to the previous one. The only difference is that in the TransAnglesMotion class , the positions of the hexapod limbs are set by the set of angles at which its joints should be bent, and not by the spatial coordinates of the ends of the robot paws.

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 Class


Implementing straightforward motion requires a bit more code and your attention. The hexapod design has an optimal number of limbs, allowing you to always have three points of support when moving, so that the projection of the center of mass of the robot is always located inside the triangle formed by them. This ensures a stable position of the robot when moving. Therefore, the rectilinear movement of the robot can be considered as a set of identical steps. In each such step, two phases can be distinguished. In the first phase, one triad of legs touches the surface and their ends move parallel to the surface in the direction opposite to the direction of movement. In this case, the second triad of paws, without touching the surface, moves along a curved path in the direction of movement of the robot.At a certain point in time, they reach their extreme position, and the second triad begins to come into contact with the surface. In the second phase, the process of motion is repeated, only this time the second triad moves the robot, and the first returns to the starting point.



This animation was found while preparing an article on the Internet on this page , but after reviewing it I was convinced that its original source was habr , and the author of the publication was the respected conKORD .

When realizing the movement in a straight line, we need to set the height of the legs off the surface ( heigth ), the position of the robot limbs at the beginning (and at the end) of the movement ( pose0 ), the velocity vector ( speed ), the time it takes to complete one step ( maxTime ), and how long the robot should perform movement ( totalTime ). If you specify a negative value for totalTime, the movement will continue indefinitely until the robot’s charge runs out or the movement is forcibly completed.

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

Despite the fact that the getPose method contains an impressive amount of code, it is quite 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;
}


We divide the whole step into four quarters:

  1. In the first quarter, the first triad of limbs (left front, left back and right middle) perform a push movement, that is, they move along the surface in the opposite direction to the movement. The second triad of paws (right front, right back and left middle) moves in the direction of movement, while the height of their rise varies from the maximum value of the specified height of the legs (at the beginning of the quarter) to zero (at the end of the quarter) as a function of sine.
  2. In the second quarter, the directions of the triads change. Now the second triad of paws moves the robot, and the first moves in the same direction as the robot. In this case, the height of the paws of the first triad changes in a sinusoid from zero to the maximum value.
  3. In the third quarter, the direction of movement of the paws is maintained, and the ends of the first triad go down, reaching the surface at the end of the third quarter.
  4. In the last quarter, the first triad becomes the support, and the second changes direction and moves upward along the sinusoid, reaching the maximum height at the end of the quarter.

RotateMotion Class


Having understood the logic of the LinearMotion class , we can easily figure out the implementation of the RotateMotion class .

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


The constructors of both classes are almost identical in the composition of the arguments passed. Only in the case of a reversal of speed is the angular velocity vector defining the axis and speed of rotation of the robot.

In the getPose method of the RotateMotion class , the logic of work is also divided into four stages. However, to calculate the position of the paws, instead of the linear function linearFunc , the radial function rotFunc is used .

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


The radial function allows you to calculate the position of a point in space after its rotation around the center of the robot, which coincides with the origin.

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


Inside the function, a mathematical apparatus is involved, allowing us to work with vector and matrix quantities.

That's it for now. We have analyzed an essential part of the program code responsible for implementing the movement of the robot. If you have any questions or comments, I will be glad to answer them in the comments. I will try to devote the following article to the remainder of the code, the source code of which will be available for download.
Thank you for the attention!

To be continued…

More articles:


All Articles