Simulink Diagram Details
This wiki page will provide some additional details on how the Simulink diagram Template_v4_00_2023a_Jetson.slx works. This page will be useful for anyone who wants to design an experiment or expand the template for their own purposes.
The template file can be sub-divided into 7 different sections. These are:
-
Experiment Loop: Contains the logic to differentiate between the difference phases of the experiment/simulation. -
Memory Store Blocks: Contains two sub-sections; one for required memory blocks, and one for user memory blocks. -
Plant Dynamics: Contains the dynamics for the platforms. -
Log Data: Contains the data logging logic. -
External Hardware Interfaces: Contains any of the code required to interface with hardware during an experiment. -
Resources: Contains any code that doesn't fit in the other categories; for example, the code that ensures all three platforms are time synchronized.
The Experiment Loop is where most users will make changes. This contains the control laws for all platforms, including the robotic manipulator. This section is a large If-Else-Action loop; each If-Action subsystem is activated based on the phase durations that the user set up in the GUI. So, Phase #0 starts at t = 0 s and stops at t = Phase0_End. There are 6 phases that a user can modify in this section of the code, as shown:
The phases are:
- Phase #0: This phase is used to wait for data from the ground truth system.
- Phase #1: This short phase is used to turn on the flotation.
- Phase #2: During this phase, the platforms will all move to their
initiallocations using PD controllers. - Phase #3: This is the main experiment phases. User cannot directly edit the duration of this phase, and are instead required to edit the 4 sub-phases. Users can add sub-phases if desired, but will have to circumvent the use of the GUI to do so - this is generally reserved for more advanced users.
- Phase #4: During this phase, the platforms will all move to their
homelocations using PD controllers. - Phase #5: During this phase, the platforms will hold their
homepositions.
When you open a phase If-Action subsystem regardless of phase you will be presented with the following:
Here we have another set of If-Action statements. This time, the code is checking if this is EITHER a simulation (in which case all of the subsystems are activated) or in the case where it is NOT a simulation, which platform (RED, BLACK or BLUE) is the code running on. Inside each of the Change XXX Behavior you'll find platform specific commands. The code for phase #0 is not complex - all it does is ensure that the pucks are off and that no forces are being commanded to the platforms.
Moving back to the root of the diagram, we can take a look at Phase #2 (Phase #1 is the same as Phase #0, except that the pucks are turned on to allow the platforms to float). Phase #2 contains the code that controls the platforms during the Move to Initial Location phase. Navigating to Change RED Behavior as an example, we can see that RED is being controlled by a PD controller, with the goal positions having been defined in the GUI.
In this section you will also see the control law for the robotic manipulator.
This section of the code is only active if the user has opted to include the manipulator in the simulations (via the GUI). If the ARM is being simulated, then the following code is also executed:
This particular logic does one of two things: if this is an experiment, then the desired joint angles are passed through directly to the actuator position controller; if this is a simulation, then the desired joint angles are passed to a PD position controller. At the moment (2023-07-17), this PD controller is NOT equivalent to the one in the Dynamixel actuators, so don't expect identical behavior. However, it should provide a good idea of what to expect. As a general rule the arm in experiment will have little to no overshoot when commanded using the position or speed controller. It is also important to note that by default the template only includes the behavior for the position controller. It is up to the user to change the controller to meet their needs. Just remember to read up on the manipulator control block so that you send the correct commands to the actuators.
Going back once again to the top level of the diagram, let's take a look at Phase #3. This is the experiment phases. Navigating into the Phase #3: Experiment If-Action block, you'll be presented with the usual If-Statement regarding whether or not this is a simulation and and which platforms are being used. Let's open up the Change RED Behavior If-Action block; the other two blocks contain similar logic.
Inside this block, users are presented with yet another If-Statement:
This If-Statement is used to differentiate between the various sub-phases. The reason we have sub-phases is typically to ensure that your true experiment starts at rest. We typically recommend the following:
-
Sub-Phase #1: Get to desired starting position for the experiment (assuming it is different from the initial position). -
Sub-Phase #2: Turn off the pucks so that the platform is completely at rest. -
Sub-Phase #3: Turn on the pucks to allow flotation. -
Sub-Phase #4: Perform the desired experiment.
Users do not need to follow this structure; in fact, simply setting the duration for these phases to 0 in the GUI will allow you to completely skip them. Inside the sub-phase If-Action blocks, users will see identical logic to what was found in the other phases. In the template file - as of writing this entry - the RED platform is following a circular trajectory around the center of the table while rotating:
Going back to the top level of the diagram, there are three other phases. These won't be covered here, as they are identical to Phase #2 (except that the platforms head towards their home locations). Let's instead take a look at the Memory Store section.
There are two sub-sections that have memory store blocks: one contains required blocks, and the other contains user blocks. Simulink's Memory Store Blocks are crucial components when dealing with complex control systems and models that require the retention of information when data is being passed through If-Action blocks. This is because If-Action blocks execute conditionally depending on certain conditions, causing non-uniform execution order.
If data flows from an If-Action block to other blocks that execute at a different rate, it can lead to inconsistencies or data inconsistencies in your model. Memory Store Blocks help to alleviate this problem by maintaining the data from previous time steps in the simulation. This feature ensures that data is reliably stored and transferred regardless of the execution order or timing of various parts of your model. This stability is critical in developing robust, accurate simulations, particularly when working with intricate system dynamics or control strategies.
Right now the User Memory Store section is empty, but there are many required blocks already present. These include things like commanded forces and torques, spacecraft positions, experiment time, and so on.
When adding a new memory store block, you must have a Data Store Memory Simulink block at the top level of your diagram. Then when you write data to the block you add a Data Store Write block and pass data to the block. If you want to read the data, you add a Data Store Read block. You cannot write arrays to a memory store block - you can only write single values.
Let's now move on to the data logging section of the template.
Navigating into the Data Logger Subsystem, users will see the following:
This subsystem uses the mux blocks to combine the different memory store blocks into one array. This array is then written to either:
-
out.dataPacket: This array only exists in simulation. After a simulation is performed, this array is saved to the workspace. An additional function built into the GUI also converts this packet into a structure to more easily identify the variables. -
Data Logger: This subsystem is a custom device driver (i.e. a C++ function) which writes the data to a text file while an experiment is performed. The data is stored as a comma-delimited data set. When the user clicks on theSave Experiment Datain the GUI, the data is copied from the target platforms onto the ground station, where it is imported and then converted to a structure (identical to what is done to the simulation data). TheCustomDataLoggerfunction is as follows:#include <iostream> #include <iomanip> #include <fstream> #include <ctime> #include "custom_data_logger.h" using namespace std; string newName = ""; void createFile() { string name = "ExpLog"; int i = 1; newName = name; while(true){ ifstream file(newName); if(file.good()){ newName = name + to_string(i); i++; } else { break; } } ofstream file(newName); file.close(); } // This function opens a text file and appends data into it. // The function will accept an array of unknown size, and will // loop through the array and append the data to the file. void appendDataToFile(double exp_data[], int num_params) { ofstream myfile; myfile.open (newName, ios::app); for (int i = 0; i < num_params-1; i++) { myfile << exp_data[i] << ","; } myfile << exp_data[num_params-1]; myfile << endl; myfile.close(); }
The function is fairly self-descriptive, but in brief there are two functions: the createFile function takes no inputs and creates a text file with the name ExpLog*, where the * is replaced with an integer. This number is set based on how many data files exist on the platform computer. This way, no data is every accidentally overwritten. The appendDataToFile takes in an array and the number of parameters, and then writes the data to the text file that was opened using the createFile function.
Adding new data is as simple as expanding the mux block which is writing to the dataPacket_AUX_2 GoTo block. Once you have added a Memory Store block at the top level, and have passed data into a Memory Write block, then you can connect the corresponding Memory Read block to the mux block. By default, when creating the structure the function will have no way of knowing the name of the variable you have added. It will simply name the variable CustomUserDataX, where X will increment based on how many variables the user has added to the mux block.
The Plant Dynamics section of the diagram contains the dynamics models for the platforms. As of SPOT 4.0.0-RC.4 the dynamics do not include Hill's equations - this may change for future versions. Users will note that this section of code is only executed if the isSim variable is equal to 1 - thus the code is not run when a experiment is being performed. This variable is set automatically by the GUI.
Clicking into the Simulate Plant Dynamics subsystem, users will be presented with three additional subsystems:
-
RED and ARM Dynamics: Contains the dynamics for both the RED platform alone as well as the dynamics for the case where the manipulator is attached. -
BLACK Dynamics: Contains the dynamics for the BLACK platform. -
BLUE Dynamics: Contains the dynamics for the BLUE platform.
The dynamics for all three platforms are the same for case where there is no manipulator attached to RED. Let's click on the RED and ARM dynamics subsystem as an example. Inside this subsystem is another If-Block, which takes the platformSelection as an input. This parameter is determined in the GUI when the user selects different combinations of platforms. This will be discussed in the Advanced GUI Details section of the Wiki. For now, simply know that if the platformSelection is 4, 5, 10, or 11, then the coupled RED+ARM dynamics are used; otherwise, the RED dynamics are used.
For now, let's investigate the RED Only If-Action block. Inside this block is another subsystem called RED Dynamics - click on that one as well. At this point users will see the following:
From left to right, we have the saturated forces (more on them later) going into the Red Dynamics Model subsystem. This function contains the following code:
Inside the RED_dynamics MATLAB function is the following code:
function x_ddot = RED_dynamics(control_inputs, mRED, IRED)
%#codegen
% Fully define the size of x_ddot() for code generation:
x_ddot = zeros(3,1);
% Extract the servicer model parameters from the model_param vector defined
% in initialize.m:
% Extract the control inputs from the control_input vector:
Fx = control_inputs(1); % [N]
Fy = control_inputs(2); % [N]
Tz = control_inputs(3); % [N.m]
% Calculate the state acceleration vector:
x_ddot(1) = Fx/mRED; % [m/s^2]
x_ddot(2) = Fy/mRED; % [m/s^2]
x_ddot(3) = Tz/IRED; % [rad/s^2]
end
This function is taking in the control inputs, the mass of RED, and the inertia of RED, and is calculating the accelerations of for RED using the control inputs and Newton's Second Law (force = mass * acceleration). The accelerations are then integrated twice to get the velocity and position of the RED platform. These are then stored into their corresponding Memory Write blocks.
The dynamics for BLACK and BLUE are identical, with the only difference being their mass properties. So, let's go back up to the RED and ARM Dynamics subsystem and investigate the coupled RED+ARM If-Action block. Inside this block, users will see another subsystem called RED+ARM Dynamics - click on this one as well. Inside this subsystem, users will see the following:
Similar to the RED dynamics, there are forces and torques being sent into a MATLAB functions simply called Dynamics Model. Inside the dynamics model is the following code:
function xdot = DynamicsModel(x, InertiaS, CoriolisS, u,...
Gamma1_sh, Gamma2_sh, Gamma3_sh, Gamma5_sh, Gamma4_sh, Gamma6_sh,...
Gamma1_el, Gamma2_el, Gamma3_el, Gamma5_el, Gamma4_el, Gamma6_el,...
Gamma1_wr, Gamma2_wr, Gamma3_wr, Gamma5_wr, Gamma4_wr, Gamma6_wr)
fqdot1 = Gamma1_sh*(tanh(Gamma2_sh*x(10)) - tanh(Gamma3_sh*x(10))) + Gamma4_sh*tanh(Gamma5_sh*x(10)) + Gamma6_sh*x(10);
fqdot2 = Gamma1_el*(tanh(Gamma2_el*x(11)) - tanh(Gamma3_el*x(11))) + Gamma4_el*tanh(Gamma5_el*x(11)) + Gamma6_el*x(11);
fqdot3 = Gamma1_wr*(tanh(Gamma2_wr*x(12)) - tanh(Gamma3_wr*x(12))) + Gamma4_wr*tanh(Gamma5_wr*x(12)) + Gamma6_wr*x(12);
q_ddot = pinv(InertiaS)*(u - CoriolisS*x(7:12) - [0;0;0;fqdot1;fqdot2;fqdot3]);
xdot = [ x(7:12)
q_ddot ];
end
This function does two things: first, it is calculating the friction based on this model. Then, it calculates the platform and manipulator joint accelerations, which are then output and integrated to the get the velocity and position states. Users will notice that this model takes in two matrices, InertiaS and CoriolisS. These are highly complex functions which will not be covered here. For details on these equations and where they come from, please refer to this paper. The Gamma parameters were experimentally determined.
The external hardware interface section of the diagram contains 5 subsystems:
The function of each subsystem is as follows:
-
Float Code: This contains the code to float the platform, specifically a GPIO write block set to write to pin 428. -
Robotic Arm Code: This contains the code used to interface with the manipulator and send it commands. -
Manipulator Encoder Data: This contains the code used to interface with the manipulator and read the encoder values. -
PhaseSpace Camera Code: This contains the code used to receive the position data from the PhaseSpace cameras. -
Thruster Control Control: This contains the code used to command the thrusters.
Navigating into the Robotic Arm Code, users will find another If-Else block, which only activates if the user is both NOT running a simulation, and if the platform is RED (identified by the WhoAmI block). If we navigate into the Change RED Behavior subsystem, users will find the device driver used to control the robotic manipulator. The code for this block is covered in this section of the Wiki and won't be repeated here.
Going back to the top level of the diagram, we can now navigate into the Manipulator Encoder Data subsystem. Here, users will find the same logic as in the Robotic Arm Code, which prevents this code from running in simulation and on any other platform besides RED. Clicking into the Change RED Behavior subsystem, users will find the ReadArm_Position_Rates device driver:
https://github.com/Carleton-SRCL/SPOT/wiki/Dynamixel-Actuators-Code-Overview
This device driver reads the encoder data for the three Dynamixel actuators and returns both the angles and angular velocities. These are then stored into their corresponding Memory Write blocks. Users should also note that this device driver has 3 inputs; these inputs correspond to the joint angles from the previous time-step. The C++ function inside the driver uses this data to ensure that when no new encoder data is available, the previous state is held constant rather then returning 0. These encoders, as implemented in SPOT 4.0.0-RC.1, are slow to read data and typically return the joint angles at a rate of < 5 Hz.
Back at the top level of the diagram, we can now navigate into the PhaseSpace Camera Code. In this subsystem, users will find an If-Else statement. This statement checks if the diagram is running as a simulation. When it is a simulation, a continuous clock is used as the universal time. No states are set in this case, as they are created by the Plant subsystem.
In the case of an experiment, the Use Hardware to Obtain States If-Action block is activated. Within this block, a lot is happening, so let's break it down. First, a UDP receiver is used to obtain data being sent by the PhaseSpace code:
This data is ASSUMED to be a [10,1] and of type double; in other words, this block will not work if the size or data type is changed. This data is reshaped before being passed into a demux block. This block splits out the time from the rest of the data packet. The time is then passed into the Adjust Time subsystem, which looks like:
All this subsystem does is reset the time to start from 0 as soon as it has received confirmation that all of the selected platforms are active and are receiving data. This is done based on the ClockStart signal. This signal comes from the CheckForSync MATLAB function, which waits until it can communicate with all active platforms before sending a 1. Once the time has been corrected, it is passed into a mux block and merged with the rest of the packet. Then, it is demuxed again, this time into [4,3,3]. Each of these outputs from the mux block is demuxed another time so that each output from the mux block corresponds to a single variable, like so:
The demux for BLACK and BLUE are the same as for RED, except that they only have three outputs (RED includes the time). The position data is then converted from mm to m, and everything is stored into their corresponding Memory Write blocks.
Going back to the top level of the diagram one last time, we can open up the Thruster Control Code. This code controls the control mixer for the platforms.
The If-Statement blocks in this section of code check if a simulation is running (in which case everything is executed), or if an experiment is running (in which case only the code for RED, BLACK, or BLUE is executed - depending on which platform the code is running on). The logic inside the If-Action blocks is a bit different for RED, so we will use it as the primary example. Clicking into the Change Red Behavior subsystem, we see the following logic:
Thsi subsystem contains two other subsystems. The first one, Rotate Forces to Body, contains the following logic:
This takes the forces calculated in the inertial frame and rotates them into the body frame. This is done using a rotation matrix C_bI under inside the Create Rotation Matrix MATLAB User Defined Function:
function C_bI = RotateFrame(Rz)
C_bI = [ cos(Rz) sin(Rz)
-sin(Rz) cos(Rz) ];
end
Going back up two levels we can see that the body forces are being passed alongside the commanded torque into the subsystem called Calculate Thruster ON/OFF. Inside this subsystem is a fairly lengthy piece of logic. So, we will break it down in chunks, moving from left to right. The first piece of code is as follows:
This code does one of two things: if the robotic manipulator is being used, then the function MakeHWithArm is called; otherwise, the function MakeH is called. These functions are the thrust distribution matrices; these take the desired thrust and torque, and converts them into individual thruster cycles. Here is the code for MakeH:
function H = MakeH(thruster_dist2CG_RED,F_thrusters_RED)
Vec1 = [ -1
-1
0
0
1
1
0
0 ];
Vec2 = [ 0
0
1
1
0
0
-1
-1 ];
Vec3 = thruster_dist2CG_RED./1000;
Mat1 = [Vec1, Vec2, Vec3]';
Mat2 = diag((F_thrusters_RED./2));
H = Mat1*Mat2;
end
This is a [3,8] matrix. The first row represents the approximate forces being applied in the X-direction. The second row represents the same, but for the Y-direction. The last row represents the torques applied by each thruster on the platform COG.
Whichever function is used, the output matrix is passed into a pseudo-inverse sub-system. The inside of this sub-system looks like:
Once the pseudo-inverse has been calculated, it is multiplied by the commanded forces and torque in the body-fixed frame. This calculation results in duty cycle. This duty cycle is passed into the check_thrust_decay function, which is:
function [thrust_decay_factor, count] = check_thrust_decay(PWMs,thruster_count_threshold)
% normalize PWMs
if max(PWMs) > 1
PWMs = PWMs/max(PWMs);
end
count = sum(PWMs > thruster_count_threshold);
positives_only = max(PWMs,0);
sum_of_duty_cycles = sum(positives_only);
average_duty_cycle_per_thruster = sum_of_duty_cycles/count;
% Look up the thrust decay for this average duty cycle
if (average_duty_cycle_per_thruster < 0.3) || (count == 0)
thrust_decay_factor = 1;
else
thrust_decay_factor = max(0.6 - 2*average_duty_cycle_per_thruster + 1, 0.5);
end
This function normalizes the signal such that it falls between 0 and 1, then it ensures that there is no negative signal. A simple decay function is then applied to estimate how much the thrust decreases as more thrusters are turned on at the same time. Once the 'decayed' thruster forces are obtained, the thrust distribution matrix is recalculated to account for the decay, the pseuo-inverse is taken again, the inverse of H is multiplied by the forces and torques in the body frame, negatives are removed, and the new signal is passed into a function called ThrusterSat:
function ThrustPer_Final = ThrusterSat(ThrustPer, PWMFreq)
ValveTime = 0.007;
TControl = 1/PWMFreq;
ThrustPer_Final = zeros(8,1);
if max(ThrustPer) > 1
ThrustPer_Sat = ThrustPer/max(ThrustPer);
else
ThrustPer_Sat = ThrustPer;
end
for i = 1:8
if ThrustPer_Sat(i) > ValveTime/TControl
ThrustPer_Final(i) = ThrustPer_Sat(i);
else
ThrustPer_Final(i) = 0;
end
end
end
This function exists because there is a minimum time required for the thruster to fully actuate. This time is estimated to be 0.007 (from specifications), but this number is only an estimate. This function also includes a parameter called TControl; this parameter is the inverse of the software PWM frequency and is defined using a parameter from the initializer.
Next in this function, the maximum commanded thruster signal is checked to ensure it does not exceed 1. If it does, then the new thruster command is set to be the previous command divided by the maximum value (to normalize it). The final for loop in this function checks to ensure that the commanded duty cycle is greater than the ratio of the valve time and the thruster control (which is 3.5%). Essentially, this means that when a duty cycle below 3.5% is commanded, it is rounded to zero to reflect the fact that that the thruster does not have enough time to fully open.
The rest of the subsystem reverses the calculations to get a new force estimate (for example, RED_Fx_Sat) which are only used in simulations to better model the real behavior of the platforms.
Going back up to the Thruster Control Code, the output of the If-Action subsystems is demuxed into 8 individual signals for all three platforms. These are then summed up and passed to a UDP block. The summation of these signals works because in an experiment, only one of the If-Action subsystems is running on a given platform. The other two are not running, and thus output a vector of zeros. This way, we only need one UDP block for the entire diagram. For details on the software PWM code, refer to the relevant Wiki page.
Navigating back to the root of the template, we can now dig into the final section of the diagram: Resources.
This section of the template contains 4 sub-systems:
-
Platform Identification: This is a Device Driver which identifies which of the three platforms the code is currently running on. -
Check Connection: This contains a UDP Send block which sends a1to to a specific port. See thePhaseSpace Camera Codefor details on what this does. -
Simulation Status: This contains a single Memory Store block for theisSimparameter. -
Send Data to TX2: This contains a UDP Send block which sends the positions and velocities of all three platforms to the NVIDIA Jetson TX2.
The Simulation Status, Check Connection, and Send Data to TX2 subsystems are all self-explanatory, so let's focus on the Platform Identification. Navigating into this subsystem, users will see the an If-Action block that only runs in experiments. Navigating into this action subsystem, users will find a single device driver labelled IdentifyPlatform. The source code for this driver can be located here, but for completeness here is the code:
#include <iostream>
#include <unistd.h>
#include <cstring>
#include "resource_headers.h"
int getComputerIdentifier(const char* hostname) {
if (std::strcmp(hostname, "spot-red") == 0) {
return 1;
} else if (std::strcmp(hostname, "spot-black") == 0) {
return 2;
} else if (std::strcmp(hostname, "spot-blue") == 0) {
return 3;
} else {
return 0; // No match found
}
}
int WhoAmI() {
char hostname[256];
if (gethostname(hostname, sizeof(hostname)) == 0) {
int identifier = getComputerIdentifier(hostname);
return identifier;
} else {
std::cerr << "Failed to get the hostname." << std::endl;
return 1;
}
return 0;
}
In essence, this code retrieves the hostname of the computer that the code is running on. It compares the retrieved hostname with these specific strings: "spot-red", "spot-black", and "spot-blue", and returns corresponding integer identifiers.
This is accomplished using use functions. First, the getComputerIdentifier() function takes the retrieved hostname as an input and compares it with the specified strings using std::strcmp(). If a match is found, the function returns the corresponding identifier (1 for "spot-red", 2 for "spot-black", and 3 for "spot-blue"). If there is no match, the function returns 0 as the default value.
In the whoAmI() function, the code retrieves the hostname using gethostname() and calls getComputerIdentifier() to obtain the identifier based on the matched hostname. The identifier is returned as an integer and is used in identifying the active platform.