test_jointThrAllocation

test_jointThrAllocation.single_arm_allocation()

Connect a two-joint arm with an analytically known force-to-wrench map.

Both hinges rotate about the body x axis. The thruster points along that axis and acts at the center of mass, so any pair of joint angles produces the same body-x force and zero torque. This makes the wrench-tracking and joint-motion contributions to the cost independently predictable.

The hub attitude is zero, the requested force is 1 N along body x, and the requested torque is zero. Two writable joint-state messages are connected even when the optional motion penalty is disabled. Each test configures its weights before calling Reset().

Returns:

Allocation module and a dictionary retaining its writable input messages. The jointStatesInMsgs entry contains the two joint-state messages in configuration order.

test_jointThrAllocation.test_allocation_configuration_helpers()

Validation Test Description

This unit test verifies that jointThrAllocation.JointThrAllocation can be constructed from the standard Basilisk.fswAlgorithms import path and that the helper methods resolve scalar and vector configuration values.

Description of Variables Being Tested

This unit test checks the resolved thrust force bounds, thrust weights, and initial decision vector shape.

test_jointThrAllocation.test_cost_wraps_joint_motion(single_arm_allocation, weight, expected_penalty, extra_turns)

Validation Test Description

Call cost() directly for the axial-thruster fixture. A 0.5 N candidate tracks a 1 N request with a squared wrench error of 0.25. The thrust weight is one, giving a thrust-use term of 0.5 and a baseline cost of 0.75.

Commanded and measured angles lie on opposite sides of the wrapping boundary. Their shortest signed differences are 0.1 rad and -0.2 rad. Expected penalties are calculated from those differences without calling the module’s wrapping helper. This detects omitted penalties, unwrapped differences, and implementations that correct only one revolution.

Test Parameter Discussion

• weight selects the disabled, scalar, vector, or full-matrix case. None leaves the penalty disabled and passes no current angles.

• expected_penalty is 0 for the disabled case; 0.10 for scalar weight 2; 0.13 for diagonal weights [1, 3]; and 0.07 for matrix [[1, 0.5], [0.5, 2]]. The matrix case includes a cross term of -0.02.

• extra_turns is either zero or three. Extra revolutions are added to the first measured angle and subtracted from the second; the expected wrapped errors and penalty remain unchanged.

Description of Variables Being Tested

The returned scalar cost must equal 0.75 + expected_penalty within pytest.approx tolerances. The full-matrix case also checks that off-diagonal weights contribute to the cost.

test_jointThrAllocation.test_joint_motion_penalty_configuration()

Validation Test Description

This unit test verifies that jointThrAllocation.JointThrAllocation configures the optional joint-motion penalty and its joint-state readers.

Description of Variables Being Tested

This unit test checks scalar, vector, and matrix joint-motion weights and the optional joint-state message connections.

test_jointThrAllocation.test_map_matrix()

Validation Test Description

This unit test verifies that jointThrAllocation.mapMatrix() maps thruster force magnitudes into the expected stacked force and torque wrench.

Description of Variables Being Tested

This unit test checks the force block and torque block of the returned wrench mapping matrix.

test_jointThrAllocation.test_motion_penalty_does_not_hide_overflow(single_arm_allocation)

Verify that the nonnegative cost bound cannot turn overflow into zero cost.

This finite, nearly singular matrix and angle error have a positive motion penalty, but intermediate products can overflow to negative infinity. Resolve the weights and call the real cost with zero wrench and thrust. Depending on floating-point evaluation order, the result can be positive or NaN, but it must never be zero or negative. Either sign of infinity in the motion term must produce a positive-infinite cost rather than being clipped to zero and treated as a favorable allocation.

test_jointThrAllocation.test_motion_penalty_is_nonnegative(single_arm_allocation, weight, angle_error)

Validation Test Description

Resolve nearly positive-semidefinite weights and evaluate cost() near their null directions with zero requested wrench and zero thrust. There are no other cost contributions, so a negative result would reward motion.

Test Parameter Discussion

weight includes a matrix with a -1 eigenvalue hidden by a 1e15 scale, a matrix with a negative eigenvalue of roundoff magnitude, and a singular matrix susceptible to cancellation in the quadratic form. Negative modes within the documented relative tolerance must be projected out, rather than retained. angle_error gives the tested near-null direction [rad].

Description of Variables Being Tested

The resolved matrix must have no eigenvalue below -1e-12 in these cases. The actual motion cost must be finite, nonnegative, and at most 1e-12. The first input previously produced a cost of approximately -1 despite passing validation, while its projected penalty is approximately zero.

test_jointThrAllocation.test_motion_penalty_preserves_nan(single_arm_allocation)

Verify that bounding motion cost below by zero does not hide invalid inputs.

A NaN measured joint angle makes only the motion penalty invalid; the candidate angles, force-to-wrench map, requested wrench, and thrust are otherwise finite. cost() must remain NaN so the candidate cannot be mistaken for a finite, zero-cost allocation.

test_jointThrAllocation.test_reset_accepts_semidefinite_motion_weights(single_arm_allocation, weight, expected_matrix)

Validation Test Description

Initialize the connected allocation with valid motion weights at the boundaries of the allowed domain. Zero and singular weights must remain usable, including matrices with negative off-diagonal entries. Validation must handle small and large finite scales without overflow or an arbitrary minimum weight.

Test Parameter Discussion

weight covers scalar/vector zeros, zero and singular matrices, a floating-point Gram matrix with a mathematically zero eigenvalue, extreme scales including the smallest positive float, and asymmetry of one machine epsilon. expected_matrix contains the resolved diagonal or full matrix. In the asymmetric case, the expected off-diagonal entries are the mean of the configured pair. Singular matrices may change within roundoff when negative computed eigenvalues are projected to zero.

Description of Variables Being Tested

Reset() must succeed, retain finite entries, and produce an exactly symmetric Wtheta matching the expected matrix. Comparison uses zero absolute tolerance so that small weights cannot pass after being erased. The caller’s input array must remain unchanged.

test_jointThrAllocation.test_reset_clears_solution_diagnostics(single_arm_allocation, monkeypatch, previous_success)

Validation Test Description

Run an allocation, reset the module, and run another allocation. The optimizer stub supplies a 0.5 N candidate for a 1 N request with unit thrust-use weight, giving cost 0.75 and wrench error 0.5 N when successful. Reset must discard these diagnostics together with the output commands.

Test Parameter Discussion

previous_success marks all optimizer results either successful or failed before reset. The failed case starts with the failure sentinel bestErrInf == inf and a 1 N wrench error. Both cases must return to the constructor’s no-allocation state after reset. The subsequent update uses successful 0.75 N candidates, with cost 0.8125 and wrench error 0.25 N.

Description of Variables Being Tested

Before reset, verify the expected status, cost, and wrench-error diagnostics so the test cannot pass by resetting an unused module. After reset, solutionFound must be zero; bestErrInf, costVal, and all six entries of wrenchError must be NaN. Thrust outputs must be zero and all three joint-command arrays must be empty. The next update must populate the commands and diagnostics with the new successful allocation.

test_jointThrAllocation.test_reset_rejects_invalid_motion_weights(single_arm_allocation, weight, error_message)

Validation Test Description

Configure an invalid motion weight on a connected two-joint allocation and call Reset(). Validation must fail during initialization, before any optimizer can use a negative or non-finite motion penalty.

Test Parameter Discussion

weight covers negative scalar/vector entries, NaN and infinity in each supported input form, an asymmetric matrix, indefinite matrices, and wrong dimensions. The indefinite matrix has positive entries but eigenvalues -1 and 3, so checking entries alone is insufficient. Small negative and indefinite weights ensure that an absolute tolerance cannot hide invalid weights merely because their magnitude is small. Negative diagonals must also be rejected regardless of other entries’ scale, even at the smallest representable magnitude. error_message names the expected failure.

Description of Variables Being Tested

Reset() must raise ValueError with a message identifying the failed condition. Required messages and arm geometry are valid, so each failure must originate from motion-weight validation.

test_jointThrAllocation.test_reset_rejects_missing_input_message(missing_msg_name)

Validation Test Description

This unit test verifies that jointThrAllocation.JointThrAllocation rejects reset calls when any required input message is not connected.

Description of Variables Being Tested

This unit test checks each required input message reader.

test_jointThrAllocation.test_spacecraft_layout(theta)

Validation Test Description

This unit test verifies that jointThrAllocation.jointPoseFromTheta() computes the correct joint frame poses and vectors for a given set of joint angles. It also verifies that jointThrAllocation.computeComFromTheta() computes the correct center of mass vector for a given set of joint angles.

Description of Variables Being Tested

This unit test checks the DCM and position vectors for each joint. It also checks the computed center of mass vector.

test_jointThrAllocation.test_spacecraft_layout_asymmetric_body_masses_shift_com()

Validation Test Description

This unit test verifies that jointThrAllocation.computeComFromTheta() responds correctly to asymmetric arm-body masses by shifting the system center of mass toward the heavier side.

Description of Variables Being Tested

This unit test checks the computed center-of-mass vector for an asymmetric set of arm-body masses.

test_jointThrAllocation.test_spacecraft_layout_includes_hub_com_offset()

Validation Test Description

This unit test verifies that jointThrAllocation.computeComFromTheta() includes the hub center-of-mass offset contribution when computing the system center of mass.

Description of Variables Being Tested

This unit test checks the computed center-of-mass vector for a nonzero hub center-of-mass offset.

test_jointThrAllocation.test_update_optimizer_failure(single_arm_allocation, monkeypatch, use_motion_penalty)

Validation Test Description

First run a successful update using prescribed candidates with joint angles [0.4, -0.3] rad and 0.5 N thrust. Then mark all five optimizer results unsuccessful and run another update without resetting the module. Starting with a nonzero command ensures that the failure path must overwrite previous outputs and clear the previous success diagnostics.

Test Parameter Discussion

use_motion_penalty selects the fallback joint command. When true, unit motion weighting is enabled and the command must hold the measured angles [0.2, -0.4] rad. When false, the command must be [0, 0] rad even though joint-state messages are connected. Both cases use the real cost function with an optimizer stub that supplies the success/failure flags.

Description of Variables Being Tested

Confirm that the first update reports success and commands 0.5 N thrust, and that both updates together visit ten seeds. After failure, every thrust slot must be zero, the joint-angle array must contain exactly the two fallback angles, and commanded rates and accelerations must be zero. Check solutionFound == 0, the positive-infinity failure sentinel in bestErrInf, and NaN in costVal. With zero thrust, wrenchError must equal the original 1 N body-x request with all other components zero.

test_jointThrAllocation.test_update_rejects_nonfinite_joint_states(single_arm_allocation, monkeypatch, invalid_state, invalid_joint, use_motion_penalty)

Validation Test Description

Run a successful allocation, then replace one measured joint angle with a non-finite value and call UpdateState() again. When the motion penalty is enabled, the update must use the module logger’s BSK_ERROR path and raise BasiliskError before invoking the optimizer or publishing commands. This prevents the failure fallback from copying an invalid measurement into a joint command.

Test Parameter Discussion

invalid_state covers NaN and both signs of infinity. invalid_joint places the invalid measurement in either input, leaving the other finite, so validating only the first joint or accepting any finite joint fails. use_motion_penalty also covers the disabled case, where joint-state inputs are unused and must not prevent allocation.

Description of Variables Being Tested

The optimizer stub must receive no additional calls after rejection. Both output timestamps and their previously valid commands must remain unchanged. With the penalty disabled, another five optimizer calls and fresh output timestamps confirm normal operation. Finally, restore a finite measured angle containing a full revolution and verify that a new allocation succeeds, so the check does not restrict valid unwrapped angles or leave the module unable to process corrected input.

test_jointThrAllocation.test_update_selects_minimum_total_cost(single_arm_allocation, monkeypatch, lowest_cost_succeeds)

Validation Test Description

Exercise UpdateState() with the real cost function and a deterministic optimizer stub. With a 1 N requested force and unit thrust weight, a candidate force f has numerical cost (1 - f)**2 + f. The five prescribed forces [N] are [1, 1.5, 0.5, 0.75, 2], with costs [1, 1.75, 0.75, 0.8125, 3].

The first candidate has zero wrench error, but a later candidate has lower total cost. The last candidate is also worse than the expected winner. This catches selection by wrench error, stopping at the first accurate result, and publishing the last result instead of the best result. The optimizer stub makes these checks independent of SciPy’s convergence.

Test Parameter Discussion

lowest_cost_succeeds controls the success flag of the third candidate. If true, its 0.5 N force and 0.75 cost must win. If false, that candidate must be ignored and the fourth candidate must win with 0.75 N force and 0.8125 cost. All other candidates are marked successful.

Description of Variables Being Tested

Check that all five configured seeds are visited in order and that every objective evaluation has the analytic cost. Verify the selected thrust and joint-angle messages, zero commanded joint rates and accelerations, solutionFound == 1, and the selected costVal. wrenchError and bestErrInf must describe the selected candidate’s 0.5 N or 0.25 N residual, even though the first candidate had no residual.

test_jointThrAllocation.test_update_uses_current_joint_states(single_arm_allocation, monkeypatch)

Validation Test Description

Run two updates with the same five prescribed optimizer candidates. Each produces the requested 1 N force and zero torque. Thrust-use weighting is zero and joint-motion weighting is the identity, so only motion cost can distinguish the candidates. Four candidates command zero joint angles; the third commands angles near the wrapping boundary.

Initially, measured angles are on the opposite side of that boundary. The third candidate has wrapped errors [0.1, -0.2] rad and cost 0.05, so it must win. Rewrite both measured angles to zero before the second update. A zero-angle candidate must then win with zero cost. This detects stale or ignored joint-state inputs, missing wrapping in the objective, and omission of motion cost from solution selection.

Test Parameter Discussion

This test has one case containing two consecutive updates. The optimizer stub evaluates the actual objective for every candidate on both updates; it does not test SciPy convergence. The larger motion cost in each update is calculated independently as (pi - 0.05)**2 + (pi - 0.1)**2.

Description of Variables Being Tested

Compare every recorded objective value with its analytic expectation, verify the selected joint angles and 1 N thrust command, and check solutionFound and costVal after each update. The wrench-error vector must remain zero to absolute tolerances of 1e-14 N for force and 1e-14 N m for torque. Exactly ten optimizer calls are expected across the two updates.