-
Notifications
You must be signed in to change notification settings - Fork 5
3.2 Controls
The control system's primary task is to listen to desired setpoint commands from the planner and continually generate an effort (force/torque) that is most likely to guide the AUV towards the desired setpoint.
The controls package operates in two modes: State mode and Superimposer mode. In both cases, the control system runs an action server that listens to commands from an action client in the planner package. When the server receives a command, it is processed depending on the mode of operation.
In State mode, the control system comprises three layers:
-
State Server: The state server listens to state goals from the planner. State goals may contain four optional degrees of freedom: x, y, z, and orientation. These degrees of freedom are optional, meaning not all of them need to be enabled. If a degree of freedom is not enabled, the control system makes no effort to adjust the AUV's pose in that specific degree of freedom. For example, if the state server receives a state goal that only specifies a z position, the AUV will ascend or descend to the desired depth without controlling its x and y position or orientation. After receiving a goal, the state server publishes setpoints to the PID nodes. It monitors the progress of the goal and informs the planner when the goal pose has been achieved and maintained for a certain period.
-
PID nodes: Detailed information about PID and quaternions is covered later in this document. There are four PID nodes: x, y, z, and quaternion (orientation). Each PID node is responsible for a specific degree of freedom. The x, y, and z PID nodes publish topics called global_x, global_y, and global_z, respectively. These topics represent the desired force to be exerted by the thrusters in the global coordinate frame. It's important to note that the effort published by the controls is body-framed, not global-framed. The transformation to the body frame is handled by the superimposer. The quaternion PID node is responsible for running PID controls on the AUV's rotation. Unlike the x, y, and z PID nodes, the quaternion PID node outputs three topics: roll, pitch, and yaw. These topics correspond to the x, y, and z components of the desired torque.
-
Superimposer: The superimposer subscribes to the global_x, global_y, global_z, roll, pitch, and yaw topics and combines them into a single effort wrench. The global x, y, z forces are combined into a single global force vector and rotated to find the desired linear force within the AUV's body frame. This force constitutes the force component of the wrench. The torque component of the wrench is simply the roll, pitch, and yaw values. No further transformation is necessary for the torques.
In Superimposer mode, the controls package consists of two layers:
-
Superimposer Server: The superimposer server is an action server that listens to superimposer goals from the planner package. A superimposer goal consists of six optional effort components: force x, y, z, and torque x, y, z. If a specific degree of freedom is specified, that value is passed to the superimposer to be incorporated into the effort. For the force x, y, z degrees of freedom, the topics are surge, sway, and heave. These forces are body-framed. For the torque x, y, z degrees of freedom, the topics are roll, pitch, and yaw.
-
Superimposer: The superimposer subscribes to the surge, sway, heave, roll, pitch, and yaw topics and combines them into a single effort vector.
It's worth noting that these modes of operation are not mutually exclusive and can be used simultaneously. For instance, you can use the state server to maintain the AUV's orientation while simultaneously using the superimposer server to apply a constant surge force.
Interacting with the controls package should be done using an action client in the planner. The planner features a Controller class that provides abstractions for all control actions. You can find this class in controller.py. In a mission plan, you can instantiate a controller object from controller.py and use its methods to control the AUV.
- controls.launch: Launches servers.launch, superimposer.launch, and pid.launch or pid-sim.launch if running the simulation.
- superimposer.launch: Runs the superimposer node.
- servers.launch: Creates the state server and the superimposer server.
- pid.launch: Launches x, y, z, and quaternion PID nodes with gains for the real AUV.
- pid-sim.launch: Launches x, y, z, and quaternion PID nodes with gains for the simulated AUV.
- joystick.launch: Launches the joystick node.
- pid_config_gui.launch: Launches an rqt GUI for easier PID gain tuning.
If your package has configurable parameters, explain how to configure them. This could involve editing configuration files or using ROS parameter