Module: spinningBodyOneDOFStateEffector

Executive Summary

The spinning body class is an instantiation of the state effector abstract class. The integrated test is validating the interaction between the spinning body module and the rigid body hub that it is attached to. In this case, a 1-DoF spinning body has an inertia tensor and is attached to the hub by a single degree of freedom axis. This module can represent multiple different effectors, such as a hinged solar panel, a reaction wheel or a single-gimbal. The spinning axis is fixed in the body frame and the effector is rigid, which means that its center of mass location does not move in the spinning frame S. An optional motor torque can be applied on the spinning axis, and the user can also lock the axis through a command.

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.

Module I/O Messages

Msg Variable Name

Msg Type

Description

spinningBodyOutMsg

HingedRigidBodyMsgPayload

Output message containing the spinning body state angle and angle rate

motorTorqueInMsg

ArrayMotorTorqueMsgPayload

(Optional) Input message of the motor torque value

motorLockInMsg

ArrayEffectorLockMsgPayload

(Optional) Input message for locking the axis

spinningBodyRefInMsg

HingedRigidBodyMsgPayload

(Optional) Input message for prescribing the angle and angle rate

spinningBodyConfigLogOutMsg

SCStatesMsgPayload

Output message containing the spinning body inertial position and attitude states

Detailed Module Description

A 1 DoF spinning body has 2 states: theta and thetaDot. The angle and angle rate can change due to the interaction with the hub, but also because of applied torques (control, spring and damper). The angle remains fixed and the angle 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

J. Vaz Carneiro, C. Allard and H. Schaub, “Rotating Rigid Body Dynamics Architecture for Spacecraft Simulation Software Implementation”, AAS Rocky Mountain GN&C Conference, Breckenridge, CO, Feb. 2–8, 2023

User Guide

This section is to outline the steps needed to setup a Spinning Body State Effector in Python using Basilisk.

  1. Import the spinningBodyOneDOFStateEffector class:

    from Basilisk.simulation import spinningBodyOneDOFStateEffector
    
  2. Create an instantiation of a Spinning body:

    spinningBody = spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()
    
  3. Define all physical parameters for a Spinning Body. For example:

    spinningBody.mass = 100.0
    spinningBody.IPntSc_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]]
    spinningBody.dcm_S0B = [[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]]
    spinningBody.r_ScS_S = [[0.5], [0.0], [1.0]]
    spinningBody.r_SB_B = [[1.5], [-0.5], [2.0]]
    spinningBody.sHat_S = [[0], [0], [1]]
    
  4. (Optional) Define initial conditions of the effector. Default values are zero states:

    spinningBody.thetaInit = 5 * macros.D2R
    spinningBody.thetaDotInit = 1 * macros.D2R
    
  5. (Optional) Define spring and damper coefficients. Default values are zero states:

    spinningBody.k = 1.0
    spinningBody.c = 0.5
    
  6. (Optional) Define a unique name for each state. If you have multiple spinning 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:

    spinningBody.nameOfThetaState = "spinningBodyTheta"
    spinningBody.nameOfThetaDotState = "spinningBodyThetaDot"
    
  7. (Optional) Connect a command torque message:

    cmdArray = messaging.ArrayMotorTorqueMsgPayload()
    cmdArray.motorTorque = [cmdTorque]  # [Nm]
    cmdMsg = messaging.ArrayMotorTorqueMsg().write(cmdArray)
    spinningBody.motorTorqueInMsg.subscribeTo(cmdMsg)
    
  8. (Optional) Connect an axis-locking message (0 means the axis is free to rotate and 1 locks the axis):

    lockArray = messaging.ArrayEffectorLockMsgPayload()
    lockArray.motorTorque = [1]
    lockMsg = messaging.ArrayEffectorLockMsg().write(lockArray)
    spinningBody.motorLockInMsg.subscribeTo(lockMsg)
    
  9. (Optional) Connect an angle and angle rate reference message:

    angleRef = messaging.HingedRigidBodyMsgPayload()
    angleRef.theta = thetaRef
    angleRef.thetaDot = thetaDotRef
    angleRefMsg = messaging.HingedRigidBodyMsg().write(angleRef)
    spinningBody.spinningBodyRefInMsg.subscribeTo(angleRefMsg)
    
  10. The angular states of the body are created using an output message spinningBodyOutMsg.

  11. The spinning body config log state output message is spinningBodyConfigLogOutMsg.

  12. Add the effector to your spacecraft:

    scObject.addStateEffector(spinningBody)
    

    See Module: spacecraft documentation on how to set up a spacecraft object.

  13. Add the module to the task list:

    unitTestSim.AddModelToTask(unitTaskName, spinningBody)
    

class SpinningBodyOneDOFStateEffector : public StateEffector, public SysModel
#include <spinningBodyOneDOFStateEffector.h>

spinning body state effector class

Public Functions

SpinningBodyOneDOFStateEffector()

&#8212; Contructor

This is the constructor, setting variables to default values

~SpinningBodyOneDOFStateEffector() override

&#8212; Destructor

This is the destructor, nothing to report here

void Reset(uint64_t CurrentClock) override

&#8212; Method for reset

This method is used to reset the module.

void writeOutputStateMessages(uint64_t CurrentClock) override

&#8212; Method for writing the output messages

This method takes the computed theta states and outputs them to the messaging system.

void UpdateState(uint64_t CurrentSimNanos) override

&#8212; Method for updating information

This method is used so that the simulation will ask SB to update messages

void registerStates(DynParamManager &statesIn) override

&#8212; Method for registering the SB states

This method allows the SB state effector to register its states: theta and thetaDot with the dynamic parameter manager

void linkInStates(DynParamManager &states) override

&#8212; Method for getting access to other states

This method allows the SB 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) override

&#8212; Method for back-substitution contributions

This method allows the SB state effector to give its contributions to the matrices needed for the back-sub method

void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN) override

&#8212; Method for SB to compute its derivatives

This method is used to find the derivatives for the SB stateEffector: thetaDDot and the kinematic derivative

void updateEffectorMassProps(double integTime) override

&#8212; Method for giving the s/c the HRB mass props and prop rates

This method allows the SB 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) override

&#8212; Method for computing energy and momentum for SBs

This method is for calculating the contributions of the SB state effector to the energy and momentum of the spacecraft

void prependSpacecraftNameToStates() override

Method used for multiple spacecraft.

This method prepends the name of the spacecraft for multi-spacecraft simulations.

void computeSpinningBodyInertialStates()

Method for computing the SB’s states.

This method computes the spinning body states relative to the inertial frame

Public Members

double mass = 1.0

[kg] mass of spinning body

double k = 0.0

[N-m/rad] torsional spring constant

double c = 0.0

[N-m-s/rad] rotational damping coefficient

double thetaInit = 0.0

[rad] initial spinning body angle

double thetaDotInit = 0.0

[rad/s] initial spinning body angle rate

std::string nameOfThetaState

&#8212; identifier for the theta state data container

std::string nameOfThetaDotState

&#8212; identifier for the thetaDot state data container

Eigen::Vector3d r_SB_B = {0.0, 0.0, 0.0}

[m] vector pointing from body frame B origin to spinning frame S origin in B frame components

Eigen::Vector3d r_ScS_S = {0.0, 0.0, 0.0}

[m] vector pointing from spinning frame S origin to point Sc (center of mass of the spinner) in S frame components

Eigen::Vector3d sHat_S = {1.0, 0.0, 0.0}

&#8212; spinning axis in S frame components.

Eigen::Matrix3d IPntSc_S

[kg-m^2] Inertia of spinning body about point Sc in S frame components

Eigen::Matrix3d dcm_S0B

&#8212; DCM from the body frame to the S0 frame (S frame for theta=0)

Message<HingedRigidBodyMsgPayload> spinningBodyOutMsg

state output message

Message<SCStatesMsgPayload> spinningBodyConfigLogOutMsg

spinning body state config log message

ReadFunctor<ArrayMotorTorqueMsgPayload> motorTorqueInMsg

&#8212; (optional) motor torque input message

ReadFunctor<ArrayEffectorLockMsgPayload> motorLockInMsg

&#8212; (optional) motor lock flag input message

ReadFunctor<HingedRigidBodyMsgPayload> spinningBodyRefInMsg

&#8212; (optional) spinning body reference input message name

Private Members

double u = 0.0

[N-m] optional motor torque

int lockFlag = 0

[] flag for locking the rotation axis

double thetaRef = 0.0

[rad] spinning body reference angle

double thetaDotRef = 0.0

[rad] spinning body reference angle rate

Eigen::Vector3d aTheta = {0.0, 0.0, 0.0}

&#8212; rDDot_BN term for back substitution

Eigen::Vector3d bTheta = {0.0, 0.0, 0.0}

&#8212; omegaDot_BN term for back substitution

double cTheta = 0.0

&#8212; scalar term for back substitution

double mTheta = 0.0

&#8212; auxiliary term for back substitution

Eigen::Vector3d sHat_B = {1.0, 0.0, 0.0}

&#8212; spinning axis in B frame components

Eigen::Vector3d r_ScS_B = {0.0, 0.0, 0.0}

[m] vector pointing from spinning frame S origin to point Sc in B frame components

Eigen::Vector3d r_ScB_B = {0.0, 0.0, 0.0}

[m] vector pointing from body frame B origin to point Sc in B frame components.

Eigen::Vector3d rPrime_ScS_B = {0.0, 0.0, 0.0}

[m/s] body frame time derivative of r_ScS_B

Eigen::Vector3d rPrime_ScB_B = {0.0, 0.0, 0.0}

[m/s] body frame time derivative of r_ScB_B

Eigen::Vector3d rDot_ScB_B = {0.0, 0.0, 0.0}

[m/s] inertial frame time derivative of r_ScB_B

Eigen::Vector3d omega_SB_B = {0.0, 0.0, 0.0}

[rad/s] angular velocity of the S frame wrt the B frame in B frame components.

Eigen::Vector3d omega_BN_B = {0.0, 0.0, 0.0}

[rad/s] angular velocity of the B frame wrt the N frame in B frame components.

Eigen::Vector3d omega_SN_B = {0.0, 0.0, 0.0}

[rad/s] angular velocity of the S frame wrt the N frame in B frame components.

Eigen::MRPd sigma_BN = {0.0, 0.0, 0.0}

&#8212; body frame attitude wrt to the N frame in MRPs

Eigen::Matrix3d rTilde_ScB_B

[m] tilde matrix of r_ScB_B

Eigen::Matrix3d omegaTilde_SB_B

[rad/s] tilde matrix of omega_SB_B

Eigen::Matrix3d omegaTilde_BN_B

[rad/s] tilde matrix of omega_BN_B

Eigen::Matrix3d dcm_BS

&#8212; DCM from spinner frame to body frame

Eigen::Matrix3d dcm_BN

&#8212; DCM from inertial frame to body frame

Eigen::Matrix3d IPntSc_B

[kg-m^2] inertia of spinning body about point Sc in B frame components

Eigen::Vector3d r_ScN_N = {0.0, 0.0, 0.0}

[m] position vector of spinning body center of mass Sc relative to the inertial frame origin N

Eigen::Vector3d v_ScN_N = {0.0, 0.0, 0.0}

[m/s] inertial velocity vector of Sc relative to inertial frame

Eigen::Vector3d sigma_SN = {0.0, 0.0, 0.0}

&#8212; MRP attitude of frame S relative to inertial frame

Eigen::Vector3d omega_SN_S = {0.0, 0.0, 0.0}

[rad/s] inertial spinning body frame angular velocity vector

double theta = 0.0

[rad] spinning body angle

double thetaDot = 0.0

[rad/s] spinning body angle rate

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

StateData *thetaState = nullptr

&#8212; state manager of theta for spinning body

StateData *thetaDotState = nullptr

&#8212; state manager of thetaDot for spinning body

Private Static Attributes

static uint64_t effectorID = 1

[] ID number of this panel