Skip to content

Commit

Permalink
[Gimbalrotor][Spinal][Controller] workaround enabling 2-dof gimbal an…
Browse files Browse the repository at this point in the history 
…gle caluculation in Spinal
  • Loading branch information
@sugihara-16
sugihara-16 committed Apr 9, 2024
1 parent 10a0908 commit 34284b5
Show file tree
Hide file tree
Showing 3 changed files with 97 additions and 29 deletions.
107 changes: 83 additions & 24 deletions ...l_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -866,12 +866,19 @@ void AttitudeController::pwmConversion()
for(int i = 0; i < motor_number_ / rotor_coef_; i++)
{
float thrust;
if(gimbal_dof_){
thrust = ap::pythagorous2(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ *i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1]);
}
else
switch(gimbal_dof_)
{
case 2:
thrust = ap::pythagorous3(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ *i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1], base_thrust_term_[rotor_coef_ * i+2] + roll_pitch_term_[rotor_coef_ *i+2]);
break;
case 1:
thrust = ap::pythagorous2(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ *i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1]);
break;
case 0:
thrust = base_thrust_term_[i] + roll_pitch_term_[i];
break;
default:
break;
}
if(max_thrust < thrust)
{
Expand Down Expand Up @@ -900,12 +907,19 @@ void AttitudeController::pwmConversion()
for(int i = 0; i < motor_number_ / (rotor_coef_); i++)
{
float thrust;
if(gimbal_dof_){
thrust = ap::pythagorous2(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ * i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1]);
}
else
switch(gimbal_dof_)
{
case 2:
thrust = ap::pythagorous3(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ *i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1], base_thrust_term_[rotor_coef_ * i+2] + roll_pitch_term_[rotor_coef_ *i+2]);
break;
case 1:
thrust = ap::pythagorous2(base_thrust_term_[rotor_coef_ * i] + roll_pitch_term_[rotor_coef_ *i],base_thrust_term_[rotor_coef_ * i+1] + roll_pitch_term_[rotor_coef_ * i+1]);
break;
case 0:
thrust = base_thrust_term_[i] + roll_pitch_term_[i];
break;
default:
break;
}
if(max_thrust < thrust)
{
Expand Down Expand Up @@ -957,17 +971,43 @@ void AttitudeController::pwmConversion()
{
if(start_control_flag_)
{
if(gimbal_dof_){
ap::Vector3f f_i;
f_i.x = target_thrust_[i*2];
f_i.z = target_thrust_[i*2+1];
target_thrust_[i] = ap::pythagorous2(f_i.x,f_i.z);
float gimbal_candidate = atan2f(-f_i.x, f_i.z);
if(std::isfinite(gimbal_candidate))
switch(gimbal_dof_)
{
case 2:
{
ap::Vector3f f_i;
f_i.x = target_thrust_[i*3];
f_i.y = target_thrust_[i*3+1];
f_i.z = target_thrust_[i*3+2];

float gimbal_candidate_roll = atan2f(-f_i.y, f_i.z);
float gimbal_candidate_pitch = atan2f(f_i.x, -f_i.y * sin(gimbal_candidate_roll) + f_i.z * cos(gimbal_candidate_roll));
target_thrust_[i] = ap::pythagorous3(f_i.x,f_i.y,f_i.z);

/* simple lpf */
if(std::isfinite(gimbal_candidate_roll) && std::isfinite(gimbal_candidate_pitch)){
target_gimbal_angles_[2*i] =(target_gimbal_angles_[2*i]+ gimbal_candidate_roll)/2;
target_gimbal_angles_[2*i+1] =(target_gimbal_angles_[2*i+1]+ gimbal_candidate_pitch)/2;

}
break;
}
case 1:
{
target_gimbal_angles_[i] =(target_gimbal_angles_[i]+ gimbal_candidate)/2;
ap::Vector3f f_i;
f_i.x = target_thrust_[i*2];
f_i.z = target_thrust_[i*2+1];
float gimbal_candidate = atan2f(-f_i.x, f_i.z);
target_thrust_[i] = ap::pythagorous2(f_i.x,f_i.z);

/* simple lpf */
if(std::isfinite(gimbal_candidate)) target_gimbal_angles_[i] =(target_gimbal_angles_[i]+ gimbal_candidate)/2;

break;
}
}
default:
break;
}

target_pwm_[i] = convert(target_thrust_[i]);

Expand All @@ -981,14 +1021,33 @@ void AttitudeController::pwmConversion()
}
#ifdef SIMULATION
//TODO: directly send target gimbal angles to gazebo
if(gimbal_dof_){
sensor_msgs::JointState gimbal_control_msg;
gimbal_control_msg.header.stamp = ros::Time::now();
for(int i = 0; i < motor_number_ / (rotor_coef_); i++){
gimbal_control_msg.position.push_back(target_gimbal_angles_[i]);
switch(gimbal_dof_)
{
case 2:
{
sensor_msgs::JointState gimbal_control_msg;
gimbal_control_msg.header.stamp = ros::Time::now();
for(int i = 0; i < motor_number_ / (rotor_coef_); i++){
gimbal_control_msg.position.push_back(target_gimbal_angles_[2*i]);
gimbal_control_msg.position.push_back(target_gimbal_angles_[2*i+1]);
}
gimbal_control_pub_.publish(gimbal_control_msg);
break;
}
case 1:
{
sensor_msgs::JointState gimbal_control_msg;
gimbal_control_msg.header.stamp = ros::Time::now();
for(int i = 0; i < motor_number_ / (rotor_coef_); i++){
gimbal_control_msg.position.push_back(target_gimbal_angles_[i]);
}
gimbal_control_pub_.publish(gimbal_control_msg);
break;
}
default:
break;
}
gimbal_control_pub_.publish(gimbal_control_msg);
}

#else
if(gimbal_dof_){
std::map<uint16_t, float> gimbal_map;
Expand Down
2 changes: 1 addition & 1 deletion robots/gimbalrotor/config/tri_omni/GimbalrotorControl_sim.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ aerial_robot_control_name: aerial_robot_control/gimbalrotor_controller
controller:
torque_allocation_matrix_inv_pub_interval: 0.05
wrench_allocation_matrix_pub_interval: 0.1
gimbal_calc_in_fc : false
gimbal_calc_in_fc : true
gimbal_dof : 2

xy:
Expand Down
17 changes: 13 additions & 4 deletions robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,10 @@ namespace aerial_robot_control
tf::vectorTFToEigen(omega_, omega);
Eigen::Vector3d gyro = omega.cross(inertia * omega);

target_wrench_acc_cog.tail(3) = Eigen::Vector3d(target_ang_acc_x, target_ang_acc_y, target_ang_acc_z) + gyro;
if(gimbal_calc_in_fc_)
target_wrench_acc_cog.tail(3) = Eigen::Vector3d(target_ang_acc_x, target_ang_acc_y, target_ang_acc_z);
else
target_wrench_acc_cog.tail(3) = Eigen::Vector3d(target_ang_acc_x, target_ang_acc_y, target_ang_acc_z) + gyro;
pid_msg_.roll.total.at(0) = target_ang_acc_x;
pid_msg_.roll.p_term.at(0) = pid_controllers_.at(ROLL).getPTerm();
pid_msg_.roll.i_term.at(0) = pid_controllers_.at(ROLL).getITerm();
Expand Down Expand Up @@ -161,9 +164,15 @@ namespace aerial_robot_control
double max_yaw_scale = 0; // for reconstruct yaw control term in spinal
for(int i = 0; i < motor_num_; i++){
Eigen::VectorXd f_i = target_vectoring_f_trans_.segment(last_col, rotor_coef_);
target_base_thrust_.at(rotor_coef_ * i) = f_i[0];
target_base_thrust_.at(rotor_coef_ * i+1) = f_i[1];
// target_gimbal_angles_.at(i) = atan2(-f_i[0], f_i[1]);
if(gimbal_dof_ == 1)
{
target_base_thrust_.at(rotor_coef_ * i) = f_i[0];
target_base_thrust_.at(rotor_coef_ * i+1) = f_i[1];
}else if(gimbal_dof_ == 2){
target_base_thrust_.at(rotor_coef_ * i) = f_i[0];
target_base_thrust_.at(rotor_coef_ * i+1) = f_i[1];
target_base_thrust_.at(rotor_coef_ * i+2) = f_i[2];
}
if(integrated_map_inv(i, YAW) > max_yaw_scale) max_yaw_scale = integrated_map_inv(i, YAW);
last_col += rotor_coef_;
}
Expand Down

0 comments on commit 34284b5

Please sign in to comment.