Catalogue of series articles

Tip: you can add the directories of all articles in the series here. You need to add the directories manually
TODO: finishing after writing

preface

Limited cognition, I hope you will forgive me. If you have any problems, I hope to communicate with you and grow together!

This paper first gives a brief introduction to the code analysis of footswing trajectory (bezier curve to calculate the foot swing trajectory). The specific content will be more later. Other modules can refer to my other articles

Tip: the following is the main content of this article

1, Contents of footswing trajectory (bezier curve)

  Vec3<T> _p0, _pf, _p, _v, _a;
  T _height;

  /*!
   * Function: create a new foot swing track and set everything to zero function
   */
  FootSwingTrajectory() 
  {
    _p0.setZero();    //Initial point
    _pf.setZero();    //End
    _p.setZero();     //Trajectory point
    _v.setZero();     //Trajectory velocity
    _a.setZero();     //Trajectory acceleration
    _height = 0;      //Track height
  }

.
.

2, Set / get correlation function of footswing trajectory (bezier curve)

1. Set foot start position function

  /*!
   * Function: set the starting position function of the foot
   * @param p0 : Initial position of the foot
   */
  void setInitialPosition(Vec3<T> p0) 
  {
    _p0 = p0;
  }

2. Set the end position function of the foot

  /*!
   * Function: set the end position function of the foot
   * @param pf :Final foot position
   */
  void setFinalPosition(Vec3<T> pf) 
  {
    _pf = pf;
  }

3. Set the maximum height function of the swing leg

  /*!
   * Function: set the maximum height function of swing leg
   * @param h :The maximum height of the swing leg is reached when the swing leg is halfway through
   */
  void setHeight(T h) 
  {
    _height = h;
  }

4. Obtain the track coordinates at this time and obtain the function of the current point position of the swing leg

  /*!
   * Function: obtain the track coordinates and the function of the current point position of the swing leg
   * @return :Foot position
   */
  Vec3<T> getPosition() 
  {
    return _p;
  }

5. Obtain the trajectory derivative at this time to obtain the current foot speed function on the swinging leg

  /*!
   * Function: get the trajectory derivative at this time and get the current foot speed function on the swinging leg
   * @return : Foot speed
   */
  Vec3<T> getVelocity() 
  {
    return _v;
  }

6. Obtain the second derivative of the trajectory at this time, and obtain the acceleration function of the foot at the current point

  /*!
   * Function: get the second derivative of the trajectory at this time and get the acceleration function of the foot at the current point
   * @return : Foot acceleration
   */
  Vec3<T> getAcceleration() 
  {
    return _a;
  }

3, Calculation of foot swing trajectory with (third order) bezier curve

principle

code

(1) The bezier curve is used to calculate the swing trajectory of the foot in two stages: lifting and falling

/*!
 * Calculation of foot swing trajectory with bezier curve
 * @tparam T
 * @param phase
 * @param swingTime
 */
template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime) 
{
  _p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);                                          //Linear interpolation between y0 and yf. x interpolates between 0 and 1 between two values  
  _v = Interpolate::cubicBezierFirstDerivative<Vec3<T>>(_p0, _pf, phase) / swingTime;               //Cubic bezier interpolation derivative between y0 and yf. x between 0 and 1
  _a = Interpolate::cubicBezierSecondDerivative<Vec3<T>>(_p0, _pf, phase) / (swingTime * swingTime);//Cubic bezier interpolation derivative between y0 and yf. x between 0 and 1

  T zp, zv, za;
  if(phase < T(0.5)) //Phase less than 0.5, foot lifting stage
  {
    zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 4 / (swingTime * swingTime);
  } 
  else              //When the phase is greater than 0.5, the foot is placed
  {
    zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 4 / (swingTime * swingTime);
  }

  _p[2] = zp;
  _v[2] = zv;
  _a[2] = za;
}

template class FootSwingTrajectory<double>;
template class FootSwingTrajectory<float>;

(2) Cubic bezier interpolation between two points y0 and yf (x between 0 and 1)

/*!
 * y0 Cubic bezier interpolation between yf and two points. x between 0 and 1
 */
template <typename y_t, typename x_t>
y_t cubicBezier(y_t y0, y_t yf, x_t x) 
{
  static_assert(std::is_floating_point<x_t>::value,
                "must use floating point value");
  assert(x >= 0 && x <= 1);
  y_t yDiff = yf - y0;
  x_t bezier = x * x * x + x_t(3) * (x * x * (x_t(1) - x));
  return y0 + bezier * yDiff;
}

(3) Cubic bezier interpolation derivative between two points y0 and yf (x between 0 and 1)

/*!
 * y0 And yf between two points. x between 0 and 1
 */
template <typename y_t, typename x_t>
y_t cubicBezierFirstDerivative(y_t y0, y_t yf, x_t x) 
{
  static_assert(std::is_floating_point<x_t>::value,
                "must use floating point value");
  assert(x >= 0 && x <= 1);
  y_t yDiff = yf - y0;
  x_t bezier = x_t(6) * x * (x_t(1) - x);
  return bezier * yDiff;
}

(4) Cubic bezier interpolation derivative between two points y0 and yf (x between 0 and 1)

/*!
 *  y0 And yf between two points. x between 0 and 1
 */
template <typename y_t, typename x_t>
y_t cubicBezierSecondDerivative(y_t y0, y_t yf, x_t x) 
{
  static_assert(std::is_floating_point<x_t>::value,
                "must use floating point value");
  assert(x >= 0 && x <= 1);
  y_t yDiff = yf - y0;
  x_t bezier = x_t(6) - x_t(12) * x;
  return bezier * yDiff;
}

summary

Input the starting point, target point and control duty cycle factor (0,1), carry out the third-order Bessel curve, carry out the interpolation between two points, output the position track, carry out the derivation, or the velocity track and acceleration track

be careful:
(1) The derivation method here is not to derive the position trajectory after obtaining it, but to derive the third-order Bessel curve formula during calculation

(2) The starting point and target point here are not three-dimensional, but a scalar, and the foot end takes the height coordinates of the Z axis

(3) The trajectory planning here is to plan from the starting point to the leg lifting height point (foot lifting), and then from the leg lifting height point to the target point (foot landing)

(4) The starting point, target point, height and control duty cycle factor (0,1) of the project are given externally with API
.
.

reference material

The theory part can refer to my blog
https://blog.csdn.net/qq_35635374/article/details/120730023