NASA Astrobee Robot Software  Astrobee Version:
Flight software for the Astrobee robots operating inside the International Space Station.
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ctl.h
Go to the documentation of this file.
1 /* Copyright (c) 2017, United States Government, as represented by the
2  * Administrator of the National Aeronautics and Space Administration.
3  *
4  * All rights reserved.
5  *
6  * The Astrobee platform is licensed under the Apache License, Version 2.0
7  * (the "License"); you may not use this file except in compliance with the
8  * License. You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15  * License for the specific language governing permissions and limitations
16  * under the License.
17  */
18 
19 #ifndef CTL_CTL_H_
20 #define CTL_CTL_H_
21 
22 #include <Eigen/Dense>
23 
24 namespace config_reader {
25  class ConfigReader;
26 }
27 
28 namespace constants {
29 const unsigned int ase_status_converged = 0U;
30 const unsigned int ctl_idle_mode = 0U;
31 const unsigned int ctl_stopping_mode = 1U;
32 const unsigned int ctl_stopped_mode = 3U;
33 } // namespace constants
34 
35 namespace ctl {
36 
37 struct ControlState {
38  Eigen::Vector3f est_P_B_ISS_ISS;
39  Eigen::Quaternionf est_quat_ISS2B;
40  Eigen::Vector3f est_V_B_ISS_ISS;
41  Eigen::Vector3f est_omega_B_ISS_B;
42 
43  Eigen::Matrix<float, 3, 3> inertia;
44 
45  // configuration
46  Eigen::Vector3f att_kp;
47  Eigen::Vector3f att_ki;
48  Eigen::Vector3f omega_kd;
49  Eigen::Vector3f pos_kp;
50  Eigen::Vector3f pos_ki;
51  Eigen::Vector3f vel_kd;
52 
53  float mass;
54  uint8_t est_confidence;
55 };
56 
58  Eigen::Vector3f P_B_ISS_ISS;
59  Eigen::Quaternionf quat_ISS2B;
60  Eigen::Vector3f V_B_ISS_ISS;
61  Eigen::Vector3f A_B_ISS_ISS;
62  Eigen::Vector3f omega_B_ISS_ISS;
63  Eigen::Vector3f alpha_B_ISS_ISS;
64 
65  uint8_t mode;
66 };
67 
68 struct ControlOutput {
69  Eigen::Vector3f body_force_cmd;
70  Eigen::Vector3f body_torque_cmd;
71  Eigen::Vector3f body_accel_cmd;
72  Eigen::Vector3f body_alpha_cmd;
73  Eigen::Vector3f pos_err;
74  Eigen::Vector3f pos_err_int;
75  Eigen::Vector3f att_err;
76  Eigen::Vector3f att_err_int;
77 
78  Eigen::Vector3f traj_pos;
79  Eigen::Quaternionf traj_quat;
80  Eigen::Vector3f traj_vel;
81  Eigen::Vector3f traj_accel;
82  Eigen::Vector3f traj_omega;
83  Eigen::Vector3f traj_alpha;
84 
85  float att_err_mag;
86 
91 
92  uint8_t ctl_status;
93 };
94 
95 class Control {
96  public:
97  Control(void);
98 
99  virtual void Initialize(void);
100  virtual void Step(float time_delta, ControlState & state, ControlCommand & cmd, ControlOutput* out);
101  virtual void ReadParams(config_reader::ConfigReader* config);
102 
103  private:
104  int mode_cmd_;
105  bool stopped_mode_;
106  Eigen::Vector3f prev_filter_vel_;
107  Eigen::Vector3f prev_filter_omega_;
108  int prev_mode_cmd_[5]; // for the 4 ticks required to switch to stopped; newest val at index 0
109  Eigen::Vector3f prev_position_;
110  Eigen::Quaternionf prev_att_;
111  Eigen::Vector3f linear_integrator_;
112  Eigen::Vector3f rotational_integrator_;
113 
114  bool FilterThreshold(Eigen::Vector3f vec, float threshhold, Eigen::Vector3f & previous);
115  float ButterWorthFilter(float input, float delay_val, float* sum_out);
116  float QuatError(Eigen::Quaternionf cmd, Eigen::Quaternionf actual);
117  void UpdateCtlStatus(const ControlState & state, ControlOutput* out);
118  Eigen::Vector3f SafeDivide(const Eigen::Vector3f & num, const Eigen::Vector3f & denom);
119  void FindPosErr(const ControlState & state, const ControlCommand & cmd, ControlOutput* out);
120  Eigen::Vector3f DiscreteTimeIntegrator(const Eigen::Vector3f input, Eigen::Vector3f & accumulator,
121  uint8_t ctl_status, float upper_limit, float lower_limit);
122  void FindBodyForceCmd(const ControlState & state, ControlOutput* out);
123  Eigen::Vector3f RotateVectorAtoB(Eigen::Vector3f, Eigen::Quaternionf);
124  Eigen::Vector3f SaturateVector(Eigen::Vector3f, float limit);
125  void FindBodyAlphaTorqueCmd(const ControlState & state, ControlOutput* out);
126  void FindAttErr(const ControlState & state, ControlOutput* out);
127 
128  void ForwardTrajectory(float time_delta, const ControlState & state, const ControlCommand & cmd,
129  ControlOutput* out);
130  void UpdateMode(const ControlState & state, const ControlCommand & cmd);
131  void UpdatePrevious(const ControlState & state);
132 
133  Eigen::Matrix<float, 4, 4> OmegaMatrix(Eigen::Vector3f input);
134 
135  float tun_vel_gain;
136  Eigen::Vector3f tun_accel_gain;
137  Eigen::Vector3f tun_alpha_gain;
138  float tun_ctl_stopping_omega_thresh;
139  float tun_ctl_stopping_vel_thresh;
140  float tun_ctl_stopped_pos_thresh;
141  float tun_ctl_stopped_quat_thresh;
142  float tun_ctl_pos_sat_upper;
143  float tun_ctl_pos_sat_lower;
144  float tun_ctl_linear_force_limit;
145  float tun_ctl_att_sat_upper;
146  float tun_ctl_att_sat_lower;
147 };
148 } // end namespace ctl
149 
150 #endif // CTL_CTL_H_
151 
ctl::ControlOutput::traj_quat
Eigen::Quaternionf traj_quat
Definition: ctl.h:79
ctl::ControlOutput::pos_err_int
Eigen::Vector3f pos_err_int
Definition: ctl.h:74
ctl::ControlState
Definition: ctl.h:37
ctl::ControlOutput::body_torque_cmd
Eigen::Vector3f body_torque_cmd
Definition: ctl.h:70
config_reader
Definition: config_reader.h:46
ctl::ControlOutput::traj_accel
Eigen::Vector3f traj_accel
Definition: ctl.h:81
ctl::ControlOutput::traj_omega
Eigen::Vector3f traj_omega
Definition: ctl.h:82
ctl::ControlCommand::A_B_ISS_ISS
Eigen::Vector3f A_B_ISS_ISS
Definition: ctl.h:61
ctl::ControlState::att_ki
Eigen::Vector3f att_ki
Definition: ctl.h:47
ctl::ControlState::inertia
Eigen::Matrix< float, 3, 3 > inertia
Definition: ctl.h:43
ctl::ControlOutput::att_err_mag
float att_err_mag
Definition: ctl.h:85
constants::ctl_idle_mode
const unsigned int ctl_idle_mode
Definition: ctl.h:30
ctl::ControlOutput::ctl_status
uint8_t ctl_status
Definition: ctl.h:92
ctl::ControlCommand
Definition: ctl.h:57
ctl::ControlCommand::V_B_ISS_ISS
Eigen::Vector3f V_B_ISS_ISS
Definition: ctl.h:60
constants::ctl_stopped_mode
const unsigned int ctl_stopped_mode
Definition: ctl.h:32
ctl::ControlOutput::body_alpha_cmd
Eigen::Vector3f body_alpha_cmd
Definition: ctl.h:72
ctl::ControlOutput::traj_error_att
float traj_error_att
Definition: ctl.h:88
ctl::ControlState::est_V_B_ISS_ISS
Eigen::Vector3f est_V_B_ISS_ISS
Definition: ctl.h:40
ctl::ControlState::est_omega_B_ISS_B
Eigen::Vector3f est_omega_B_ISS_B
Definition: ctl.h:41
ctl::ControlOutput::traj_error_vel
float traj_error_vel
Definition: ctl.h:89
ctl::ControlState::est_quat_ISS2B
Eigen::Quaternionf est_quat_ISS2B
Definition: ctl.h:39
ctl::Control::ReadParams
virtual void ReadParams(config_reader::ConfigReader *config)
Definition: ctl.cc:353
ctl::ControlCommand::P_B_ISS_ISS
Eigen::Vector3f P_B_ISS_ISS
Definition: ctl.h:58
constants::ase_status_converged
const unsigned int ase_status_converged
Definition: ctl.h:29
ctl
Definition: ctl.h:35
ctl::ControlOutput::att_err_int
Eigen::Vector3f att_err_int
Definition: ctl.h:76
ctl::ControlState::est_confidence
uint8_t est_confidence
Definition: ctl.h:54
ctl::ControlCommand::mode
uint8_t mode
Definition: ctl.h:65
ctl::Control::Step
virtual void Step(float time_delta, ControlState &state, ControlCommand &cmd, ControlOutput *out)
Definition: ctl.cc:56
ctl::ControlState::mass
float mass
Definition: ctl.h:53
ctl::ControlOutput::traj_vel
Eigen::Vector3f traj_vel
Definition: ctl.h:80
config_reader::ConfigReader
Definition: config_reader.h:48
ctl::ControlOutput::traj_error_pos
float traj_error_pos
Definition: ctl.h:87
ctl::ControlOutput::body_accel_cmd
Eigen::Vector3f body_accel_cmd
Definition: ctl.h:71
ctl::ControlOutput::pos_err
Eigen::Vector3f pos_err
Definition: ctl.h:73
ctl::ControlOutput::body_force_cmd
Eigen::Vector3f body_force_cmd
Definition: ctl.h:69
ctl::ControlState::omega_kd
Eigen::Vector3f omega_kd
Definition: ctl.h:48
ctl::ControlOutput::att_err
Eigen::Vector3f att_err
Definition: ctl.h:75
ctl::ControlState::pos_kp
Eigen::Vector3f pos_kp
Definition: ctl.h:49
ctl::ControlState::att_kp
Eigen::Vector3f att_kp
Definition: ctl.h:46
constants::ctl_stopping_mode
const unsigned int ctl_stopping_mode
Definition: ctl.h:31
ctl::Control::Initialize
virtual void Initialize(void)
Definition: ctl.cc:338
ctl::ControlCommand::omega_B_ISS_ISS
Eigen::Vector3f omega_B_ISS_ISS
Definition: ctl.h:62
ctl::ControlCommand::alpha_B_ISS_ISS
Eigen::Vector3f alpha_B_ISS_ISS
Definition: ctl.h:63
constants
Definition: ctl.h:28
ctl::ControlState::est_P_B_ISS_ISS
Eigen::Vector3f est_P_B_ISS_ISS
Definition: ctl.h:38
state
uint8_t state
Definition: signal_lights.h:90
ctl::ControlState::pos_ki
Eigen::Vector3f pos_ki
Definition: ctl.h:50
ctl::ControlOutput::traj_error_omega
float traj_error_omega
Definition: ctl.h:90
ctl::ControlState::vel_kd
Eigen::Vector3f vel_kd
Definition: ctl.h:51
ctl::ControlOutput::traj_pos
Eigen::Vector3f traj_pos
Definition: ctl.h:78
ctl::Control
Definition: ctl.h:95
ctl::ControlOutput
Definition: ctl.h:68
ctl::ControlCommand::quat_ISS2B
Eigen::Quaternionf quat_ISS2B
Definition: ctl.h:59
ctl::ControlOutput::traj_alpha
Eigen::Vector3f traj_alpha
Definition: ctl.h:83
ctl::Control::Control
Control(void)
Definition: ctl.cc:52