Module: hingedRigidBodyStateEffector

Executive Summary

The hinged rigid body class is an instantiation of the state effector abstract class. The integrated test is validating the interaction between the hinged rigid body module and the rigid body hub that it is attached to. In this case, a hinged rigid body has an inertia tensor and is attached to the hub by a single degree of freedom torsional hinged with a linear spring constant and linear damping term.

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.

../../../../_images/moduleHRBStateEffector.svg

Figure 1: HingedRigidBodyStateEffector() Module I/O Illustration

Module I/O Messages

Msg Variable Name

Msg Type

Description

hingedRigidBodyRefMsg

HingedRigidBodyMsgPayload

(Optional) Input message of the reference angle and angle rate

motorTorqueInMsg

ArrayMotorTorqueMsgPayload

(Optional) Input message of the hinge motor torque value

hingedRigidBodyOutMsg

HingedRigidBodyMsgPayload

Output message containing the panel hinge state angle and angle rate

hingedRigidBodyConfigLogOutMsg

SCStatesMsgPayload

Output message containing the panel inertial position and attitude states

Detailed Module Description

Mathematical Modeling

See Allard, Schaub, and Piggott paper: General Hinged Solar Panel Dynamics Approximating First-Order Spacecraft Flexing for a detailed description of this model. A hinged rigid body has 2 states: theta and thetaDot.

For additional information about connecting a reference, see Bascom and Schaub paper: Modular Dynamic Modeling of Hinged Solar Panel Deployments

The module PDF Description contains further information on this module’s function, how to run it, as well as testing.

Note

In contrast to Module: spinningBodyOneDOFStateEffector, this module assumes:

  • rigid body inertia matrix is diagonal as seen in the hinged body \(\cal S\) frame

  • the center of mass lies on the \(\hat{\bf s}_1\) axis

Module Testing

The integrated tests has six scenarios it is testing. The first three are: one with gravity and no damping, one without gravity and without damping, and one without gravity with damping. These first three tests are verifying energy and momentum conservation. In the first two cases orbital energy, orbital momentum, rotational energy, and rotational angular momentum should all be conserved. In the third case orbital momentum, orbital energy, and rotational momentum should be conserved. This integrated test validates for all three scenarios that all of these parameters are conserved. The fourth scenario is verifying that the steady state deflection while a constant force is being applied matches the back of the envelope (BOE) calculation. The fifth scenario applies a constant force and removes the force and the test verifies that the frequency and amplitude match the BOE calculations. And the sixth scenario verifies that Basilisk gives identical results to a planar Lagrangian dynamical system created independently.

The document PDF Description contains a more detailed discussion of the testing, as well as the expected results.

User Guide

This section is to outline the steps needed to setup a Hinged Rigid Body State Effector in python using Basilisk.

  1. Import the hingedRigidBodyStateEffector class:

    from Basilisk.simulation import hingedRigidBodyStateEffector
    
  2. Create an instantiation of a Hinged Rigid body:

    panel1 = hingedRigidBodyStateEffector.HingedRigidBodyStateEffector()
    
  3. Define all physical parameters for a Hinged Rigid Body. For example:

    IPntS_S = [[100.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 50.0]]
    

    Do this for all of the parameters for a Hinged Rigid Body seen in the Hinged Rigid Body 1 Parameters Table.

  4. (Optional) Define a unique name for each state. If you have multiple panels, 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:

    panel1.thetaInit = 5*numpy.pi/180.0
    panel1.thetaDotInit = 0.0
    
  5. Define a unique name for each state:

    panel1.nameOfThetaState = "hingedRigidBodyTheta1"
    panel1.nameOfThetaDotState = "hingedRigidBodyThetaDot1"
    
  6. Define an optional motor torque input message:

    panel1.motorTorqueInMsg.subscribeTo(msg)
    
  7. The angular states of the panel are created using an output message hingedRigidBodyOutMsg.

  8. The panel config log state output message is hingedRigidBodyConfigLogOutMsg.

  9. Add the panel to your spacecraft:

    scObject.addStateEffector(panel1)
    

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

  10. Add the module to the task list:

    unitTestSim.AddModelToTask(unitTaskName, panel1)
    

class HingedRigidBodyStateEffector : public StateEffector, public SysModel
#include <hingedRigidBodyStateEffector.h>

hinged rigid body state effector class

Public Functions

HingedRigidBodyStateEffector()

&#8212; Contructor

This is the constructor, setting variables to default values

~HingedRigidBodyStateEffector()

&#8212; Destructor

This is the destructor, nothing to report here

void writeOutputStateMessages(uint64_t CurrentClock)

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

Parameters

CurrentClock – The current simulation time (used for time stamping)

Returns

void

void UpdateState(uint64_t CurrentSimNanos)

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

Parameters

CurrentSimNanos – The current simulation time in nanoseconds

Returns

void

void registerStates(DynParamManager &statesIn)

&#8212; Method for registering the HRB states

This method allows the HRB state effector to register its states: theta and thetaDot with the dyn param manager

void linkInStates(DynParamManager &states)

&#8212; Method for getting access to other states

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

&#8212; Method for back-sub contributions

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

&#8212; Method for HRB to compute its derivatives

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

void updateEffectorMassProps(double integTime)

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

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

&#8212; Computing energy and momentum for HRBs

This method is for calculating the contributions of the HRB state effector to the energy and momentum of the s/c

void calcForceTorqueOnBody(double integTime, Eigen::Vector3d omega_BN_B)

&#8212; Force and torque on s/c due to HRBs

void prependSpacecraftNameToStates()

class method

Public Members

double mass

[kg] mass of hinged rigid body

double d

[m] distance from hinge point H to hinged rigid body center of mass S

double k

[N-m/rad] torsional spring constant of hinge

double c

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

double thetaInit

[rad] Initial hinged rigid body angle

double thetaDotInit

[rad/s] Initial hinged rigid body angle rate

double thetaRef

[rad] hinged rigid body reference angle

double thetaDotRef

[rad/s] hinged rigid body reference 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::Matrix3d IPntS_S

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

Eigen::Vector3d r_HB_B

[m] vector pointing from body frame origin to Hinge location

Eigen::Matrix3d dcm_HB

&#8212; DCM from body frame to hinge frame

Message<HingedRigidBodyMsgPayload> hingedRigidBodyOutMsg

&#8212; state output message name

ReadFunctor<ArrayMotorTorqueMsgPayload> motorTorqueInMsg

&#8212; (optional) motor torque input message name

ReadFunctor<HingedRigidBodyMsgPayload> hingedRigidBodyRefMsg

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

Message<SCStatesMsgPayload> hingedRigidBodyConfigLogOutMsg

panel state config log message name

HingedRigidBodyMsgPayload HRBoutputStates

instance of messaging system message struct

BSKLogger bskLogger

&#8212; BSK Logging

Private Functions

void computePanelInertialStates()

This method computes the panel states relative to the inertial frame

Returns

void

Private Members

double theta

[rad] hinged rigid body angle

double thetaDot

[rad/s] hinged rigid body angle rate

double cTheta

&#8212; term needed for back substitution

double u

[N-m] optional motor torque

Eigen::Vector3d r_HP_P

[m] vector pointing from primary body frame P origin to Hinge location. If a single spacecraft body is modeled than P is the same as B

Eigen::Matrix3d dcm_HP

&#8212; DCM from primary body frame to hinge frame

Eigen::Vector3d aTheta

&#8212; term needed for back substitution

Eigen::Vector3d bTheta

&#8212; term needed for back substitution

Eigen::Matrix3d rTilde_HP_P

&#8212; Tilde matrix of rHB_B

Eigen::Matrix3d dcm_SH

&#8212; DCM from hinge to hinged rigid body frame, S

Eigen::Matrix3d dcm_SP

&#8212; DCM from body to S frame

Eigen::Vector3d omega_PN_S

[rad/s] omega_BN in S frame components

Eigen::Vector3d sHat1_P

&#8212; unit direction vector for the first axis of the S frame

Eigen::Vector3d sHat2_P

&#8212; unit direction vector for the second axis of the S frame

Eigen::Vector3d sHat3_P

&#8212; unit direction vector for the third axis of the S frame

Eigen::Vector3d r_SP_P

&#8212; Vector pointing from B to CoM of hinged rigid body in B frame components

Eigen::Matrix3d rTilde_SP_P

&#8212; Tilde matrix of rSB_B

Eigen::Vector3d rPrime_SP_P

[m/s] Body time derivative of rSB_B

Eigen::Matrix3d rPrimeTilde_SP_P

&#8212; Tilde matrix of rPrime_SB_B

Eigen::Matrix3d ISPrimePntS_P

[kg-m^2/s] time body derivative IPntS in body frame components

Eigen::Vector3d omegaLoc_PN_P

[rad/s] local copy of omegaBN

Eigen::Matrix3d omegaTildeLoc_PN_P

&#8212; tilde matrix of omegaBN

Eigen::Vector3d r_SN_N

[m] position vector of hinge CM S relative to inertial frame

Eigen::Vector3d v_SN_N

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

Eigen::Vector3d sigma_SN

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

Eigen::Vector3d omega_SN_S

[rad/s] inertial panel frame angular velocity vector

StateData *sigma_BN

Hub/Inertial attitude represented by MRP.

StateData *omega_BN_B

Hub/Inertial angular velocity vector in B frame components.

StateData *thetaState

&#8212; state manager of theta for hinged rigid body

Eigen::MatrixXd *inertialPositionProperty

[m] r_N inertial position relative to system spice zeroBase/refBase

Eigen::MatrixXd *inertialVelocityProperty

[m] v_N inertial velocity relative to system spice zeroBase/refBase

StateData *thetaDotState

&#8212; state manager of thetaDot for hinged rigid body

Eigen::MatrixXd *c_B

[m] Vector from point B to CoM of s/c in B frame components

Eigen::MatrixXd *cPrime_B

[m/s] Body time derivative of vector c_B in B frame components

Private Static Attributes

static uint64_t effectorID = 1

[] ID number of this panel