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.
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.

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)

---

class jointThrAllocation.JointThrAllocation(*args, **kwargs)

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).

computeComFromTheta(dcm_CB, r_CB_B)

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) → float

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.

Returns:

Cost function value.

jointPoseFromTheta(theta: ndarray)

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)

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()

Resolve thrForceMax to a length-nThr vector after nThr is known.

resolveWf()

Resolve Wf to a length-nThr vector after nThr is known.

setThrForceMax(thrForceMaxIn)

Set thrust upper bounds.

Accepted inputs are:

• scalar: same upper bound for all thrusters

• vector length nThr: per-thruster upper bounds

setWc(wcIn)

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)

Set thrust-weight term for the cost function.

Accepted inputs are:

• scalar: applies same weight to all thrusters

• vector length nThr: per-thruster weights

validateInputMessages()

Raise BasiliskError if a required input message is not linked.

jointThrAllocation.mapMatrix(rVec_B: ndarray, fHatVec_B: ndarray, r_ComB_B: ndarray) → ndarray

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.