Module: linearTranslationNDOFStateEffector
Executive Summary
The N-DoF linear translation body class is an instantiation of the state effector abstract class with \(N\) degrees of freedom. The integrated test is validating the interaction between the linear translation body module and the rigid body hub that it is attached to. In this case, a 4-DoF linear translation body has an inertia tensor and is attached to the hub by four single-degree-of-freedom axes. Each spinning axis is fixed in the parent’s body frame and the effector is rigid, which means that its center of mass location does not move in the \(F\) frame. An optional motor force can be applied on the spinning axis, and the user can also lock the axis through a command. Moreover, the user can input a displacement reference that the effector will track through a spring and damper.
Message Connection Descriptions
The following table lists all the module input and output messages. The module msg variable name is set by the user from python. The msg type contains a link to the message structure definition, while the description provides information on what this message is used for.
Msg Variable Name |
Msg Type |
Description |
---|---|---|
translatingBodyOutMsg |
Output message containing the linear translation body state displacement and displacement rate |
|
motorForceInMsg |
(Optional) Input message of the motor force value |
|
motorLockInMsg |
(Optional) Input message for locking the axis |
|
translatingBodyRefInMsg |
(Optional) Input message for prescribing the displacement and displacement rate |
|
translatingBodyConfigLogOutMsg |
Output message containing the translating body inertial position and attitude states |
Detailed Module Description
For each degree of freedom, the user must create a translating body inside the \(N\)-DoF module. Each body represents a link and has 2 states: rho
and rhoDot
. The displacement and displacement rate can change due to the interaction with the hub, but also because of applied forces (control, spring and damper). The displacement remains fixed and the displacement rate is set to zero when the axis is locked.
Mathematical Modeling
See the following conference paper for a detailed description of this model.
Note
P. Johnson and J. Vaz Carneiro, “Backsubstitution Method For Spacecraft With Generally Translating Appendages,” AAS Astrodynamics Specialist Conference, Broomfield, CO, Aug. 11-15, 2024
User Guide
This section is to outline the steps needed to setup a Translating Body State Effector in Python using Basilisk.
Import the linearTranslatingBodyNDOFStateEffector class:
from Basilisk.simulation import linearTranslatingBodyNDOFStateEffector
Create an instantiation of a Translating body:
translatingBodyEffector = linearTranslatingBodyNDOFStateEffector.linearTranslatingBodyNDOFStateEffector()
For each degree of freedom, create and set the properties of a translating body:
translatingBody = linearTranslationNDOFStateEffector.translatingBody() translatingBody.setMass(50.0) translatingBody.setIPntFc_F([[100.0, 0.0, 0.0], [0.0, 80.0, 0.0], [0.0, 0.0, 50.0]]) translatingBody.setDCM_FP([[0.0, -1.0, 0.0], [0.0, 0.0, -1.0], [1.0, 0.0, 0.0]]) translatingBody.setR_FcF_F([[0.8], [0.5], [-0.3]]) translatingBody.setR_F0P_P([[0.1], [-0.2], [0.4]]) translatingBody.setFHat_P([[3.0 / 5.0], [4.0 / 5.0], [0.0]]) translatingBodyEffector.addTranslatingBody(translatingBody)
(Optional) Define initial conditions of the effector. Default values are zero states:
translatingBody.setRhoInit(1.0) translatingBody.setRhoDotInit(0.05)
(Optional) Define spring and damper coefficients. Default values are zero:
translatingBody.setK(100.0) translatingBody.setC(0.0)
(Optional) Define a unique name for each state. If you have multiple translating bodies, they each must have a unique name. If these names are not specified, then the default names are used which are incremented by the effector number:
translatingBody.nameOfRhoState = "translatingBodyRho" translatingBody.nameOfRhoDotState = "translatingBodyRhoDot"
(Optional) Connect a command force message:
cmdArray = messaging.ArrayMotorForceMsgPayload() cmdArray.motorForce = [cmdForce] # [Nm] cmdMsg = messaging.ArrayMotorForceMsg().write(cmdArray) translatingBody.motorForceInMsg.subscribeTo(cmdMsg)
(Optional) Connect an axis-locking message (0 means the axis is free to move and 1 locks the axis):
lockArray = messaging.ArrayEffectorLockMsgPayload() lockArray.effectorLockFlag = [1] lockMsg = messaging.ArrayEffectorLockMsg().write(lockArray) translatingBody.motorLockInMsg.subscribeTo(lockMsg)
(Optional) Connect a displacement and displacement rate reference message:
translationRef = messaging.LinearTranslationRigidBodyMsgPayload() translationRef.rho = 0.2 translationRef.rhoDot = 0.0 translationRefMsg = messaging.LinearTranslationRigidBodyMsg().write(translationRef) translatingBody.translatingBodyRefInMsg.subscribeTo(translationRefMsg)
The linear states of the body are created using an output message
translatingBodyOutMsg
.The translating body config log state output message is
translatingBodyConfigLogOutMsg
.Add the effector to your spacecraft:
scObject.addStateEffector(translatingBodyEffector)
See Module: spacecraft documentation on how to set up a spacecraft object.
-
struct translatingBody
- #include <linearTranslationNDOFStateEffector.h>
translating body structure
Public Functions
-
void setMass(double mass)
setter for
mass
property
-
void setK(double k)
setter for
k
property
-
void setC(double c)
setter for
c
property
-
inline void setRhoInit(double rhoInit)
setter for
rhoInit
property
-
inline void setRhoDotInit(double rhoDotInit)
setter for
rhoDotInit
property
-
inline double getMass() const
getter for
mass
property
-
inline double getK() const
getter for
k
property
-
inline double getC() const
getter for
c
property
-
inline double getRhoInit() const
getter for
rhoInit
property
-
inline double getRhoDotInit() const
getter for
rhoDotInit
property
Private Members
-
double mass = 0.0
[kg] mass of translating arm
-
double k = 0.0
[N/m] translational spring constant
-
double c = 0.0
[N-s/m] translational damping coefficient
-
double rhoInit = 0.0
[m] initial translating body distance from equilibrium
-
double rhoDotInit = 0.0
[m/s] initial translating body velocity of F frame wrt F0 frame
-
double rhoRef = 0.0
[m] reference translating body distance from equilibrium
-
double rhoDotRef = 0.0
[m/s] reference translating body velocity of F frame wrt F0 frame
-
double u = 0.0
[N] motor force acting along the translating axis of the body
-
bool isAxisLocked = false
— lock flag
-
Eigen::Matrix3d IPntFc_F = Eigen::Matrix3d::Identity()
[kg-m^2] Inertia of body about point Fc in F frame components
-
Eigen::Vector3d r_FcF_F = Eigen::Vector3d::Zero()
[m] vector pointing from translating frame F origin to point Fc (center of mass of arm) in F frame components
-
Eigen::Vector3d r_F0P_P = Eigen::Vector3d::Zero()
[m] vector pointing from parent origin to translating frame F origin in F frame components
-
Eigen::Matrix3d dcm_FP = Eigen::Matrix3d::Identity()
— DCM from parent frame to current F frame
-
double rho = 0.0
[m] translating body distance from equilibrium
-
double rhoDot = 0.0
[m/s] translating body velocity of F frame wrt F0 frame
-
Eigen::Vector3d r_FF0_B
[m] vector pointing from translating frame F to translating frame F0 (magnitude rho)
-
Eigen::Vector3d r_F0P_B
[m] vector pointing from parent translating frame P to translating frame F0
-
Eigen::Vector3d r_FcF_B
[m] vector pointing from translating frame F origin to point Fc (center of mass of arm) in B frame components
-
Eigen::Vector3d r_FB_B
[m] vector pointing from body frame B origin to F frame in B frame components
-
Eigen::Vector3d omega_FN_B
[rad/s] angular velocity of the F frame wrt the N frame in B frame components
-
Eigen::Vector3d r_FcN_N
[m] position vector of translating body’s center of mass Fc relative to the inertial frame origin N
-
BSKLogger bskLogger
Friends
- friend class linearTranslationNDOFStateEffector
-
void setMass(double mass)
-
class linearTranslationNDOFStateEffector : public StateEffector, public SysModel
- #include <linearTranslationNDOFStateEffector.h>
translating body state effector class
Public Functions
-
linearTranslationNDOFStateEffector()
— Constructor
This is the constructor, setting variables to default values
-
~linearTranslationNDOFStateEffector() final
— Destructor
This is the destructor, nothing to report here
-
void addTranslatingBody(translatingBody const &newBody)
method for adding a new translating body
This method is used to add a translating body.
-
inline void setNameOfRhoState(const std::string &nameOfRhoState)
setter for
nameOfRhoState
property
-
inline void setNameOfRhoDotState(const std::string &nameOfRhoDotState)
setter for
nameOfRhoDotState
property
-
inline std::string getNameOfRhoState() const
getter for
nameOfRhoState
property
-
inline std::string getNameOfRhoDotState() const
getter for
nameOfRhoDotState
property
Public Members
-
std::vector<Message<LinearTranslationRigidBodyMsgPayload>*> translatingBodyOutMsgs
vector of state output messages
-
std::vector<Message<SCStatesMsgPayload>*> translatingBodyConfigLogOutMsgs
vector of translating body state config log messages
-
std::vector<ReadFunctor<LinearTranslationRigidBodyMsgPayload>> translatingBodyRefInMsgs
(optional) reference state input message
-
ReadFunctor<ArrayMotorForceMsgPayload> motorForceInMsg
— (optional) motor force input message name
-
ReadFunctor<ArrayEffectorLockMsgPayload> motorLockInMsg
— (optional) motor lock input message name
Private Functions
-
void Reset(uint64_t CurrentClock) final
This method is used to reset the module.
-
void readInputMessages()
This method reads motor force, lock flag, and reference state messages.
-
void writeOutputStateMessages(uint64_t CurrentClock) final
This method takes the computed rho states and outputs them to the messaging system.
-
void UpdateState(uint64_t CurrentSimNanos) final
This method is used so that the simulation will ask TB to update messages
-
void registerStates(DynParamManager &statesIn) final
This method allows the TB state effector to register its states: rho and rhoDot with the dynamic parameter manager
-
void linkInStates(DynParamManager &states) final
This method allows the TB state effector to have access to the hub states and gravity
-
void updateContributions(double integTime, BackSubMatrices &backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N) final
This method allows the TB state effector to give its contributions to the matrices needed for the back-sub method
-
void computeCRhoStar(Eigen::VectorXd &CRhoStar, const Eigen::Vector3d &g_N)
This method compute CRhoStar for back-sub
-
void computeBackSubContributions(BackSubMatrices &backSubContr) const
This method computes the back-sub contributions of the system
-
void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) final
This method is used to find the derivatives for the TB stateEffector: rhoDDot and the kinematic derivative
-
void updateEffectorMassProps(double integTime) final
This method allows the TB state effector to provide its contributions to the mass props and mass prop rates of the spacecraft
-
void updateEnergyMomContributions(double integTime, Eigen::Vector3d &rotAngMomPntCContr_B, double &rotEnergyContr, Eigen::Vector3d omega_BN_B) final
This method is for calculating the contributions of the TB state effector to the energy and momentum of the spacecraft
-
void prependSpacecraftNameToStates() final
This method prepends the name of the spacecraft for multi-spacecraft simulations.
-
void computeTranslatingBodyInertialStates()
This method computes the translating body states relative to the inertial frame
Private Members
-
int N = 0
— number of translating body axes defined in the system
-
std::vector<translatingBody> translatingBodyVec
— vector of TB effector structs
-
Eigen::Vector3d omega_BN_B
[rad/s] angular velocity of the B frame wrt the N frame in B frame components
-
Eigen::MatrixXd *inertialPositionProperty = nullptr
[m] r_N inertial position relative to system spice zeroBase/refBase
-
Eigen::MatrixXd *inertialVelocityProperty = nullptr
[m] v_N inertial velocity relative to system spice zeroBase/refBase
-
std::string nameOfRhoState
— identifier for the theta state data container
-
std::string nameOfRhoDotState
— identifier for the thetaDot state data container
Private Static Attributes
-
static uint64_t effectorID = 1
[] ID number of this effector
-
linearTranslationNDOFStateEffector()