[Spinal] Add an option for body rate cmd

[Li-Jinjie]

What is this

Add body rate option in cmd msg.

Details

First, I add SetControlMode.srv to set mode. The default set is is_attitude == True and is_body_rate == False.

If is_attitude == True and is_body_rate == False, everything remains the same with previous version.

If is_attitude == True and is_body_rate == True, the controller will use error_ang_vel = target_ang_vel - ang_vel to get D term.

If is_attitude == False and is_body_rate == True, the controller becomes a body_rate controller with only P term.

is_attitude == False and is_body_rate == False are temporarily not allowed.

TODO:

Note that I have changed the sign of d term in control_feedback_state_msg_ to be compatible with p and i. I think this change will not affect anything. Please check the 309 line of attitude_control.cpp

[tongtybj]

I think this is a very useful contribution to our control framework.

[sugikazu75]

@Li-Jinjie

If is_attitude == False and is_body_rate == True, the controller becomes a body_rate controller with only P term.

In my understanding, in this mode, we use only D-term in our original system for attitude control. Is this correct ?
If so, we don't have to add p_term and i_term to roll_pitch_term.

Moreover, where is is_attitude_ctrl_ used other than in callback function? It seems to me that there is no difference between two modes: (is_attitude, is_body_rate) = (True, True) and (False, True).

[Li-Jinjie]

@Li-Jinjie

If is_attitude == False and is_body_rate == True, the controller becomes a body_rate controller with only P term.

In my understanding, in this mode, we use only D-term in our original system for attitude control. Is this correct ? If so, we don't have to add p_term and i_term to roll_pitch_term.

Yes correct, only D-term of attitude controller (or we can call it the P gain for angular rate) is used.

Moreover, where is is_attitude_ctrl_ used other than in callback function? It seems to me that there is no difference between two modes: (is_attitude, is_body_rate) = (True, True) and (False, True).

You are right, I truly missed something during cherry-pick. Now I have fixed this part and please check.

[tongtybj]

Yes correct, only D-term of attitude controller (or we can call it the P gain for angular rate) is used.

For the naming of the control term, I also agree with @Li-Jinjie . Although using d_term for angular control is convenient (saving several code lines), but it might be theorerically confusing. So, I suggest @Li-Jinjie to use p_term for angular control.