Module: forceTorqueThrForceMapping

Executive Summary

This module maps commanded forces and torques defined in the body frame of the spacecraft to a set of thrusters. It is capable of handling Center of Mass (CoM) offsets and non-controllable axis. In contrast to Module: thrForceMapping, this module only handles on-pulsing, but not off-pulsing. Further, it provides a single force/torque projection onto the thrusters and thus lacks some of the robustness features of Module: thrForceMapping.

The commanded force and torque input messages are optional, and the associated vectors are zeroed if no input message is connected. This provides a general capability to map control torques, forces or torques and forces onto a set of thrusters.

Message Connection Descriptions

The following table lists all the module input and output messages. The module msg connection 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. Both the cmdTorqueInMsg and cmdForceInMsg are optional.

Module I/O Messages

Msg Variable Name

Msg Type

Description

cmdTorqueInMsg

CmdTorqueBodyMsgPayload

(optional) The name of the vehicle control torque (Lr) input message

cmdForceInMsg

CmdForceBodyMsgPayload

(optional) The name of the vehicle control force input message

thrConfigInMsg

THRArrayConfigMsgPayload

The name of the thruster cluster input message

vehConfigInMsg

VehicleConfigMsgPayload

The name of the vehicle config input message

thrForceCmdOutMsg

THRArrayCmdForceMsgPayload

The name of the output thruster force message

Detailed Module Description

Force and Torque Mapping

The desired force and torque are given as \(\mathbf{F}_{req}\) and \(\boldsymbol{\tau}_{req}\), respectively. These are both stacked into a single vector.

(1)\[\begin{split}\begin{bmatrix} \boldsymbol{\tau}_{req} \\ \mathbf{F}_{req} \end{bmatrix}\end{split}\]

The \(i_{th}\) thruster position expressed in spacecraft body-fixed coordinates is given by \(\mathbf{r}_i\). The unit direction vector of the thruster force is \(\hat{\mathbf{g}}_{t_i}\). The thruster force is given as:

(2)\[\mathbf{F}_i = F_i\hat{\mathbf{g}}_{t_i}\]

The torque produced by each thruster about the body-fixed CoM is:

(3)\[\boldsymbol{\tau}_i = ((\mathbf{r}_i - \mathbf{r}_{\text{COM}}) \times \hat{\mathbf{g}}_{t_i})F_i = \mathbf{d}_iF_i\]

The total force and torque on the spacecraft may be represented as:

(4)\[\begin{split}\begin{bmatrix} \boldsymbol{\tau}_{req} \\ \mathbf{F}_{req} \end{bmatrix} = \begin{bmatrix} \mathbf{d}_i \ldots \mathbf{d}_N \\ \hat{\mathbf{g}}_{t_i} \ldots \hat{\mathbf{g}}_{t_N} \end{bmatrix} \begin{bmatrix} F_1 \\ \vdots \\ F_N \end{bmatrix} = [D]\mathbf{F}\end{split}\]

The force required by each thruster can computed by the following equation. Any rows within the \([D]\) matrix that contain only zeros are removed beforehand.

(5)\[\begin{split}\mathbf{F} = [D]^T([D][D]^T)^{-1}\begin{bmatrix} \boldsymbol{\tau}_{req} \\ \mathbf{F}_{req} \end{bmatrix}\end{split}\]

To ensure no commanded thrust is less than zero, the minimum thrust is subtracted from the thrust vector

(6)\[\mathbf{F} = \mathbf{F} - \text{min}(\mathbf{F})\]

These thrust commands are then written to the output message.

User’s Guide

To set up this module users must create the config data and module wrap:

moduleConfig = forceTorqueThrForceMapping.forceTorqueThrForceMappingConfig()
moduleWrap = unitTestSim.setModelDataWrap(moduleConfig)
moduleWrap.ModelTag = "forceTorqueThrForceMappingTag"
unitTestSim.AddModelToTask(unitTaskName, moduleWrap, moduleConfig)

The cmdForceInMsg and cmdTorqueInMsg are optional. However, the thrConfigInMsg and vehConfigInMsg are not. These can both be set up as follows, where rcsLocationData is a list of the thruster positions and rcsDirectionData is a list of thruster directions. CoM_B is the center of mass of the spacecraft in the body frame.:

fswSetupThrusters.clearSetup()
for i in range(numThrusters):
    fswSetupThrusters.create(rcsLocationData[i], rcsDirectionData[i], maxThrust)
thrConfigInMsg = fswSetupThrusters.writeConfigMessage()
vehConfigInMsgData = messaging.VehicleConfigMsgPayload()
vehConfigInMsgData.CoM_B = CoM_B
vehConfigInMsg = messaging.VehicleConfigMsg().write(vehConfigInMsgData)

Then, the relevant messages must be subscribed to by the module:

moduleConfig.cmdTorqueInMsg.subscribeTo(cmdTorqueInMsg)
moduleConfig.cmdForceInMsg.subscribeTo(cmdForceInMsg)
moduleConfig.thrConfigInMsg.subscribeTo(thrConfigInMsg)
moduleConfig.vehConfigInMsg.subscribeTo(vehConfigInMsg)

For more information on how to set up and use this module, see the unit test.


Functions

void SelfInit_forceTorqueThrForceMapping(forceTorqueThrForceMappingConfig *configData, int64_t moduleID)

This method initializes the output messages for this module.

Parameters
  • configData – The configuration data associated with this module

  • moduleID – The module identifier

Returns

void

void Update_forceTorqueThrForceMapping(forceTorqueThrForceMappingConfig *configData, uint64_t callTime, int64_t moduleID)

Add a description of what this main Update() routine does for this module

Parameters
  • configData – The configuration data associated with the module

  • callTime – The clock time at which the function was called (nanoseconds)

  • moduleID – The module identifier

Returns

void

void Reset_forceTorqueThrForceMapping(forceTorqueThrForceMappingConfig *configData, uint64_t callTime, int64_t moduleID)

This method performs a complete reset of the module. Local module variables that retain time varying states between function calls are reset to their default values. Check if required input messages are connected.

Parameters
  • configData – The configuration data associated with the module

  • callTime – [ns] time the method is called

  • moduleID – The module identifier

Returns

void

struct forceTorqueThrForceMappingConfig
#include <forceTorqueThrForceMapping.h>

This module maps thruster forces for arbitrary forces and torques.

Public Members

double rThruster_B[MAX_EFF_CNT][3]

[m] local copy of the thruster locations

double gtThruster_B[MAX_EFF_CNT][3]

[] local copy of the thruster force unit direction vectors

uint32_t numThrusters

[] The number of thrusters available on vehicle

double CoM_B[3]

[m] CoM of the s/c

CmdTorqueBodyMsg_C cmdTorqueInMsg

(optional) vehicle control (Lr) input message

CmdForceBodyMsg_C cmdForceInMsg

(optional) vehicle control force input message

THRArrayConfigMsg_C thrConfigInMsg

thruster cluster configuration input message

VehicleConfigMsg_C vehConfigInMsg

vehicle config input message

THRArrayCmdForceMsg_C thrForceCmdOutMsg

thruster force command output message

BSKLogger *bskLogger

BSK Logging.