Position controller - possible improvement

[brunopinto900]

The position controller is defined as follows

Eigen::Vector3d geometricCtrl::poscontroller(const Eigen::Vector3d &pos_error, const Eigen::Vector3d &vel_error) {
  Eigen::Vector3d a_fb =
      Kpos_.asDiagonal() * pos_error + Kvel_.asDiagonal() * vel_error;  // feedforward term for trajectory error

  if (a_fb.norm() > max_fb_acc_)
    a_fb = (max_fb_acc_ / a_fb.norm()) * a_fb;  // Clip acceleration if reference is too large

  return a_fb;
}

However, i was wondering with the following approach (cascade PID controller):

//Position Control

float v_xc = k_p*(traj.pos_d[0]-pos[0])+traj.dpos_d[0];
  float v_yc = k_p*(traj.pos_d[1]-pos[1])+traj.dpos_d[1];
  float v_zc = k_pz*(traj.pos_d[2]-pos[2])+traj.dpos_d[2];

  float a_x = k_v*(v_xc-v[0])+traj.ddpos_d[0];
  float a_y = k_v*(v_yc-v[1])+traj.ddpos_d[1];
  float a_z = k_vz*(v_zc-v[2])+traj.ddpos_d[2];

Where traj can be the circle trajectory.

I can work on this.

[Jaeyoung-Lim]

@brunopinto900 Thanks for the suggestions, but it would be more clear for me if the suggestions were in a form of a pull request so that we can discuss properly

[brunopinto900]

You are right. I can work on that and do a pull request.