Python Module: jointThrAllocation

Executive Summary

This module allocates commanded translational force and body torque across thrusters mounted on articulated arms. It solves for joint angle commands and thruster force commands using the arm configuration message and spacecraft state messages.

The vector from the hub body-frame origin to the instantaneous spacecraft center of mass is computed internally from the current joint configuration. As a result, the articulated arm configuration message must provide both the arm kinematics and the mass properties needed to reconstruct the system center of mass.

The optimizer uses SciPy. Importing the module from Basilisk.fswAlgorithms does not require SciPy, but executing JointThrAllocation.UpdateState does.

Message Connection Descriptions

The following diagram and table list the module input and output messages.

Module I/O Messages
Msg Variable Name	Msg Type	Description
armConfigInMsg	THRArmConfigMsgPayload	Input articulated thruster-arm configuration and mass-property message.
hubStatesInMsg	SCStatesMsgPayload	Input spacecraft hub state message.
transForceInMsg	CmdForceInertialMsgPayload	Input inertial-frame commanded force message.
rotTorqueInMsg	CmdTorqueBodyMsgPayload	Input body-frame commanded torque message.
jointStatesInMsgs	ScalarJointStateMsgPayload	(optional) Vector of current joint-state input messages used by the joint-motion penalty.
thrForceOutMsg	THRArrayCmdForceMsgPayload	Output thruster force command message.
desJointAnglesOutMsg	JointArrayStateMsgPayload	Output desired joint angle command message.

Module Assumptions and Limitations

The implementation assumes serial arm chains packed in arm order, one spacecraft tree, and one thruster per arm. The thruster parent joint is assumed to be the configured joint index on that arm.

Mathematical Modeling

The following conference papers provide detailed descriptions of the joint-thruster allocation problem solved by this module.

Note

W. Schwend and H. Schaub, “Cascaded Control Architecture for Spacecraft with Arm-Mounted Thrusters Using Reaction Torque Compensation”, AAS Astrodynamics Specialist Conference, Whistler, Canada, July 26–30, 2026.

W. Schwend, A. Rogers and H. Schaub, “Hub Reaction Torque Reduction for Spacecraft Control Using Robotic Arm-Mounted Thrusters”, AAS Astrodynamics Specialist Conference, Whistler, Canada, July 26–30, 2026.

User Guide

The module is imported through the standard flight-software package:

from Basilisk.fswAlgorithms import jointThrAllocation

allocation = jointThrAllocation.JointThrAllocation()
allocation.ModelTag = "jointThrAllocation"

The armConfigInMsg input must define both the articulated thruster-arm kinematics and the body mass properties used to compute the instantaneous spacecraft center of mass. The required message fields are:

hubMass: hub mass \([\text{kg}]\)
r_BcB_B: hub center of mass relative to the hub body-frame origin \([\text{m}]\)
bodyArmIdx: arm index for each mass-carrying body
bodyJointIdx: local parent-joint index for each mass-carrying body
bodyMass: mass of each arm body \([\text{kg}]\)
r_LcP_P: body center of mass relative to the parent joint, expressed in the parent-joint frame \([\text{m}]\)
armJointCount: number of joints in each arm
r_CP_P: parent-joint to child-joint position vectors \([\text{m}]\)
shat_P: child-joint spin axes
dcm_C0P: zero-angle child-to-parent direction cosine matrices
armTreeIdx: kinematic-tree index for each arm
thrArmIdx: arm index for each thruster
thrArmJointIdx: local parent-joint index for each thruster
r_TP_P: thruster position relative to the parent joint \([\text{m}]\)
fhat_P: thruster force direction unit vectors

The thrust upper bound can be provided as either a scalar or a per-thruster vector:

allocation.setThrForceMax(2.5)

The wrench tracking weights can be provided as a scalar, a length-six vector, or a 6-by-6 matrix:

allocation.setWc(1.0)

The thrust weighting term can be provided as either a scalar or a per-thruster vector:

allocation.setWf(1.0e-6)

To penalize deviations from the current joint angles, set Wtheta and add one ScalarJointStateMsg reader for every configured joint. The readers must be added in the same order as the joints in armConfigInMsg:

allocation.setWtheta(1.0e-5)
for i, jointStateMsg in enumerate(jointStateMsgs):
    allocation.addHingedJoint()
    allocation.jointStatesInMsgs[i].subscribeTo(jointStateMsg)

When the motion penalty is enabled, UpdateState() requires every measured joint angle to be finite. NaN or infinite measurements trigger BSK_ERROR through the module logger, raising BasiliskError before optimization or output publication. Existing output messages retain their previous payloads and timestamps. Finite angles may include multiple revolutions. Joint-state inputs are unused when the penalty is disabled.

setWtheta() accepts a scalar, a vector of length nJoint containing diagonal weights, or an nJoint-by-nJoint matrix. All entries must be finite. Scalar and vector weights must be nonnegative, and matrix weights must be symmetric positive semidefinite. Zero weights and singular matrices are allowed; negative off-diagonal entries are also allowed when the matrix is positive semidefinite.

The weights are validated during Reset(), after the arm configuration determines the number of joints. Invalid weights raise ValueError before optimization. Matrix symmetry and eigenvalue checks allow a roundoff tolerance of 10 * nJoint * numpy.finfo(float).eps relative to the largest absolute matrix entry. Negative diagonal entries are always rejected. Roundoff-level asymmetry is removed by averaging unequal transpose entries; entries that are already symmetric are preserved. Accepted negative eigenvalues within the roundoff tolerance are projected to zero. The motion cost is bounded below by zero to suppress negative residuals from floating-point cancellation near a null direction of the weighting matrix. Non-finite motion costs remain non-finite, so overflow or invalid joint-state data cannot become a zero-cost candidate.

Reset() clears the output commands and restores the initial diagnostic state: solutionFound is zero, and costVal, bestErrInf, and all six entries of wrenchError are NaN. These values indicate that no allocation has been attempted since reset. The next UpdateState() replaces them with the result of the new allocation attempt.

class jointThrAllocation.JointThrAllocation(*args, **kwargs)[source]

Bases: SysModel

Allocate thruster forces and joint angles for thrusters mounted on arms.

This implementation is intentionally example-oriented with explicit assumptions:

Arms are serial chains packed in arm order.
Arm geometry/config comes from THRArmConfigMsgPayload.
One spacecraft tree is supported (all arms in same kinematic tree).
Exactly one thruster per arm (configurable check).
Thruster parent joint is the last joint in each arm (configurable check).

addHingedJoint()[source]: Add a joint-state input used by the optional joint-motion penalty.

computeComFromTheta(dcm_CB, r_CB_B)[source]

Compute the position of the system CoM relative to the body frame origin.

Parameters:

dcm_CB – List of direction cosine matrices for each joint.
r_CB_B – List of joint-frame origins in body-frame coordinates.

Returns:

Position vector of the system CoM relative to the body frame origin.

cost(decisionVar: ndarray, desiredWrench_B: ndarray, currentJointAngles: ndarray = None) → float[source]

Compute the cost function for the given decision variables and desired wrench.

Parameters:

decisionVar – Concatenated vector of joint angles and thruster forces.
desiredWrench_B – Desired force and torque in body-frame coordinates.
currentJointAngles – Current joint angles when using the joint-motion penalty.

Returns:

Cost function value.

jointPoseFromTheta(theta: ndarray)[source]

Return joint frame poses in body-frame coordinates.

Parameters:: theta – Joint angle vector.
Returns:: Direction cosine matrices and joint-frame origins in body-frame coordinates.

mapping(theta: ndarray)[source]

Compute the thruster force-to-wrench map for given joint angles.

Parameters:: theta – Joint angle vector.
Returns:: Thruster force-to-wrench map for the given joint angles.

resolveThrForceMax()[source]: Resolve thrForceMax to a length-nThr vector after nThr is known.

resolveWf()[source]: Resolve Wf to a length-nThr vector after nThr is known.

resolveWtheta()[source]

Validate and resolve Wtheta to an nJoint-by-nJoint weighting matrix.

Matrix symmetry and eigenvalue checks use a tolerance of 10 * nJoint * numpy.finfo(float).eps after scaling by the largest absolute matrix entry. Negative diagonal entries are always rejected. Roundoff-level asymmetry is removed by averaging unequal transpose entries; already symmetric entries are preserved. Negative eigenvalues within the tolerance are projected to zero. Zero and singular positive-semidefinite matrices are accepted.

Raises:: ValueError – If the shape is invalid, an entry is non-finite, a scalar/vector weight or matrix diagonal entry is negative, or a matrix is asymmetric or has a negative eigenvalue beyond the roundoff tolerance.

setThrForceMax(thrForceMaxIn)[source]

Set thrust upper bounds.

Accepted inputs are:

scalar: same upper bound for all thrusters
vector length nThr: per-thruster upper bounds

setWc(wcIn)[source]

Set wrench tracking weights for the cost function.

Accepted inputs are:

scalar: wc * I6
length-6 vector: diag(wc)
6-by-6 matrix

setWf(wfIn)[source]

Set thrust-weight term for the cost function.

Accepted inputs are:

scalar: applies same weight to all thrusters
vector length nThr: per-thruster weights

setWtheta(wThetaIn)[source]

Configure the optional joint-angle deviation penalty.

Validation occurs in resolveWtheta() during Reset(), after the number of joints is known. All weights must be finite. Scalar and vector weights must be nonnegative; a matrix must be symmetric positive semidefinite to floating-point roundoff.

Parameters:: wThetaIn – Scalar weight, length-nJoint vector of diagonal weights, or nJoint-by-nJoint weighting matrix. Zero weights are allowed.

validateInputMessages()[source]: Raise BasiliskError if a required input message is not linked.

jointThrAllocation.mapMatrix(rVec_B: ndarray, fHatVec_B: ndarray, r_ComB_B: ndarray) → ndarray[source]

Build the thruster force-to-wrench map.

Parameters:

rVec_B – Thruster locations in body-frame coordinates.
fHatVec_B – Thruster unit force directions in body-frame coordinates.
r_ComB_B – Center-of-mass location relative to the hub origin in body-frame coordinates.

Returns:

Matrix mapping thruster magnitudes to stacked force and torque.

jointThrAllocation.wrapAngle(angles: ndarray) → ndarray[source]: Wrap an angle or angle array to the range \([-\pi, \pi]\).