C++ Module: constraintDynamicEffector

Executive Summary

The constraint effector class is used to define a physical connection between two separate spacecraft. This class takes in the connection location on each spacecraft in their respective body frames as well as a relative vector between these two connection points. It also takes in two gain tuning parameters to tailor the stiffness and damping of the connection between the two spacecraft.

The constraint effector class is an instantiation of the dynamic effector abstract class. The included test is validating the interaction between two spacecraft rigid body hubs that are attached through a constraint effector. In this case, two identical spacecraft are connected by a 0.1 meter long arm which is enforced with high stiffness and damping to be virtually rigid.

Message Connection Descriptions

The following table lists the module input and output messages. The module message connections are set by the user from Python. The message type links to the corresponding message structure definition, while the description explains how the message is used.

Module I/O Messages
Msg Variable Name	Msg Type	Description
effectorStatusInMsg	DeviceStatusMsgPayload	(Optional) Device status input; a zero status disables the constraint force and torque but not the reported violation. The effector is active when this message is not linked.
constraintElements	ConstDynEffectorMsgPayload	Constraint force, torque, violation, and filtered force and torque magnitude output message.

Detailed Module Description

A constraint effector is a combination of two separate 3-degree-of-freedom holonomic constraints: a direction constraint which enforces the position of the connection point on each spacecraft relative to the other, and an attitude constraint which enforces the attitude of each spacecraft relative to the other. The constraint effector works by correcting violations of these constraints as well as their derivatives.

Mathematical Modeling

See the following conference paper for a detailed description of the mathematical model.

Note

J. Vaz Carneiro, A. Morell and H. Schaub, “Post-Docking Complex Spacecraft Dynamics Using Baumgarte Stabilization”, AAS/AIAA Astrodynamics Specialist Conference, Big Sky, Montana, Aug. 13-17, 2023

User Guide

This section outlines the steps needed to setup a Constraint Dynamic Effector in Python using Basilisk.

Import the constraintDynamicEffector class:

from Basilisk.simulation import constraintDynamicEffector

Create an instantiation of a holonomic constraint:

constraintEffector = constraintDynamicEffector.ConstraintDynamicEffector()

Define all physical parameters for the constraint:

constraintEffector.setR_P1B1_B1(r_P1B1_B1)
constraintEffector.setR_P2B2_B2(r_P2B2_B2)
constraintEffector.setR_P2P1_B1Init(r_P2P1_B1Init)

Define the stiffness and damping of the connection. See the recommended starting gains here:
```
constraintEffector.setAlpha(1E2)
constraintEffector.setBeta(1e3)
```
(Optional) Define exact gains for the direction and attitude constraints separately. These explicit gains are preserved during attachment initialization and reset; gains without an explicit override are derived from alpha and beta:
```
constraintEffector.setK_d(alpha**2)
constraintEffector.setC_d(2*beta)
constraintEffector.setK_a(alpha**2)
constraintEffector.setC_a(2*beta)
```
Add the effector to both spacecraft:
```
scObject1.addDynamicEffector(constraintEffector)
scObject2.addDynamicEffector(constraintEffector)
```
See C++ Module: spacecraft documentation on how to set up a spacecraft object.
Add the spacecraft and module to the task list:
```
Sim.AddModelToTask(TaskName, scObject1)
Sim.AddModelToTask(TaskName, scObject2)
Sim.AddModelToTask(TaskName, constraintEffector)
```
Important

The order in which the spacecraft are solved in the task MUST compute spacecraft 1 first and spacecraft 2 second. Regardless of what your spacecraft objects are named, the spacecraft considered “1” and “2” are dictated when setting R_P1B1_B1, R_P2B2_B2, and R_P2P1_B1Init where spacecraft 1 hosts connection point P1 and parent frame B1, and spacecraft 2 hosts connection point P2 and parent frame B2.

When using the SimulationBaseClass AddModelToTask() model priority to set the order of execution, ensure that spacecraft 1 is set to be solved before spacecraft 2. Note that model priority execution is from highest value to lowest value, with the default being -1 when unspecified. When using the model priority, the setup could look like:
```
Sim.AddModelToTask(TaskName, scObject2, ModelPriority=5) # spacecraft 2 solved second
Sim.AddModelToTask(TaskName, scObject1, ModelPriority=10) # spacecraft 1 solved first
```
When not setting any model priority, and therefore letting all models default to a -1 model priority, models of the same priority are computed in the order that they are added. So if not setting a model priority, the setup MUST look like:
```
Sim.AddModelToTask(TaskName, scObject1) # spacecraft 1 solved first
Sim.AddModelToTask(TaskName, scObject2) # spacecraft 2 solved second
```

Ensure that the dynamics integration is synced between the connected spacecraft. For best results use a variable timestep integrator:

integratorObject1 = svIntegrators.svIntegratorRKF45(scObject1)
scObject1.setIntegrator(integratorObject1)
scObject1.syncDynamicsIntegration(scObject2)

(Optional) Retrieve the constraint effector’s physical parameters, gain tuning parameters, or exact gains for the direction and attitude constraints:

constraintEffector.getR_P1B1_B1(r_P1B1_B1)
constraintEffector.getR_P2B2_B2(r_P2B2_B2)
constraintEffector.getR_P2P1_B1Init(r_P2P1_B1Init)
constraintEffector.getAlpha(1E2)
constraintEffector.getBeta(1e3)
constraintEffector.getK_d(alpha**2)
constraintEffector.getC_d(2*beta)
constraintEffector.getK_a(alpha**2)
constraintEffector.getC_a(2*beta)

(Optional) Define a input device status message.(1 means constraintEffector is connected.0 means constraintEffector is disconnected). If not set, it defaults to being connected:

effectorStatusMsgPayload = messaging.DeviceStatusMsgPayload()
effectorStatusMsgPayload.deviceStatus = 1
effectorStatusMsg = messaging.DeviceStatusMsg().write(effectorStatusMsgPayload)
constraintEffector.effectorStatusInMsg.subscribeTo(effectorStatusMsg)

(Optional) Setup Low Pass Filtering for the Constraint Forces and Torques acting on the two satellites. Define a positive cut-off frequency wc for the low-pass filter. If not set, defaults to 0:
```
constraintEffector.setFilterData(wc)
```
The constraintEffector output message records the raw and filtered constraint forces and torques acting on the two spacecraft using the variable constraintElements.

Gain Units

The direction gains multiply position error and its rate to produce force. The attitude gains multiply the dimensionless MRP error and its rate to produce torque. Their SI units are:

Constraint gain units
Gain	Units
`k_d`	N/m
`c_d`	N s/m
`k_a`	N m
`c_a`	N m s

alpha and beta are dimensionless numerical tuning parameters. Squaring alpha or doubling beta supplies the numerical value of each derived gain in its respective SI units. For example, alpha = 3 gives k_d = 9 N/m and k_a = 9 N m, while beta = 2 gives c_d = 4 N s/m and c_a = 4 N m s. Use the individual gain setters to choose the direction and attitude stiffness or damping independently.

Initialization and Reset

The effector can be attached to spacecraft hubs or to state-effector branches. Both attachment paths require positive alpha and beta tuning parameters when no individual gain has been explicitly set. Each gain without an explicit override is derived as k_d = k_a = alpha**2 or c_d = c_a = 2*beta. This setup occurs while the parent states or properties are linked, so it does not depend on separately scheduling the effector.

The effector records which individual gain setters have been called. Changing alpha or beta and then calling Reset() refreshes every derived gain while preserving explicit overrides, even when an override equals a previously derived value. Individual gain setters take effect immediately; tuning-parameter setters affect the derived gains at the next reset or attachment initialization. Repeated resets with unchanged inputs produce the same gains.

All gain and tuning-parameter setters require finite, strictly positive values and reject invalid inputs without replacing the prior value or changing whether a gain is explicit. Derived gains must also remain finite; overflow raises BasiliskError before any derived gain is replaced. Existing partial explicit configurations remain supported: unset tuning parameters retain their zero defaults, which can leave the corresponding unspecified gains at zero.

Constraint force and torque evaluation is driven by the parent dynamics. Add the effector to a task to process its optional status input and to update its filtered-force and filtered-torque output message.

When the effector is scheduled, its Reset() repeats the gain validation and derivation. It does not clear the constraint filter history or change the parent attachments.

Attaching to a State Effector

This effector supports the branching described in Advanced: Effector Module Branching. A constraint needs two endpoints, and either or both of them may be a state effector rather than a hub:

stateEff.addDynamicEffector(constraintEffector, segment)
scObject2.addDynamicEffector(constraintEffector)

The endpoint added first becomes body 1 of the constraint and the one added second becomes body 2, so r_P1B1_B1 and r_P2B2_B2 are each measured in the frame their own endpoint belongs to, the parent segment’s frame for a state effector and the hub body frame for a spacecraft. Adding two state effectors connects two appendages directly, with no hub as an endpoint. The segment argument is omitted for a parent with a single degree of freedom.

Both the parent and the child are still added to the task in the usual way, the same as when the child is attached to the hub.

struct parentID

#include <constraintDynamicEffector.h>

Struct containing parent classification variables.

Public Members

int idx: index of effector’s parent within parent type (ex. hub #1 or state eff #2)

std::string parentType: type of parent attached to (hub or state effector)

class ConstraintDynamicEffector : public SysModel, public DynamicEffector 

#include <constraintDynamicEffector.h>

constraint dynamic effector class

Public Functions

ConstraintDynamicEffector(): The Constructor

~ConstraintDynamicEffector(): This is the destructor, nothing to report here

void Reset(uint64_t CurrentSimNanos) override

Validate and initialize the constraint gains when the scheduler resets the model.

Parameters:: CurrentSimNanos – [ns] Time at which the reset occurs

void linkInStates(DynParamManager &states) override

This method allows the constraint effector to have access to the parent states

Parameters:: states – The states to link

void linkInProperties(DynParamManager &properties) override

This method is used to link properties to the constraint effector.

Parameters:: properties – The parameter manager to collect from

void computeForceTorque(double integTime, double timeStep) override

This method computes the forces on torques on each spacecraft body.

Parameters:

integTime – Integration time
timeStep – Current integration time step used

void UpdateState(uint64_t CurrentSimNanos) override

Update state method

Parameters:: CurrentSimNanos – The current simulation time

void writeOutputStateMessage(uint64_t CurrentClock)

This method takes the computed constraint force and torque states and outputs them to the messaging system.

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

void computeFilteredForce(uint64_t CurrentClock)

Filtering method to calculate filtered Constraint Force

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

void computeFilteredTorque(uint64_t CurrentClock)

Filtering method to calculate filtered Constraint Torque

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

void readInputMessage(): This method allows the user to set the status of the constraint dynamic effector

void setR_P2P1_B1Init(Eigen::Vector3d r_P2P1_B1Init)

Set the initial separation between connection points.

setter for r_P2P1_B1Init initial spacecraft separation

Parameters:: r_P2P1_B1Init – [in] [m] Initial separation from connection point P1 to P2.

void setR_P1B1_B1(Eigen::Vector3d r_P1B1_B1)

Set the first body’s connection-point position.

setter for r_P1B1_B1 connection point position on spacecraft 1

Parameters:: r_P1B1_B1 – [in] [m] Position of connection point P1 relative to hub B1.

void setR_P2B2_B2(Eigen::Vector3d r_P2B2_B2)

Set the second body’s connection-point position.

setter for r_P2B2_B2 connection point position on spacecraft 2

Parameters:: r_P2B2_B2 – [in] [m] Position of connection point P2 relative to hub B2.

void setSigma_B2B1Init(Eigen::MRPd sigma_B2B1Init)

Set the initial relative attitude constraint.

setter for sigma_B2B1_Init initial spacecraft relative attitude

Parameters:: sigma_B2B1Init – [in] Initial attitude of body B2 relative to body B1.

void setAlpha(double alpha)

Set the proportional Baumgarte tuning parameter.

setter for alpha gain tuning parameter

Note

Its square supplies the numerical SI values of derived k_d [N/m] and k_a [N*m].

Parameters:: alpha – [in] [-] Numerical proportional Baumgarte tuning parameter.

void setBeta(double beta)

Set the derivative Baumgarte tuning parameter.

setter for beta gain tuning parameter

Note

Twice its value supplies the numerical SI values of derived c_d [N*s/m] and c_a [N*m*s].

Parameters:: beta – [in] [-] Numerical derivative Baumgarte tuning parameter.

void setK_d(double k_d)

Set the direction-constraint proportional gain.

setter for k_d gain

Parameters:: k_d – [in] [N/m] Direction-constraint proportional gain.

void setC_d(double c_d)

Set the direction-constraint derivative gain.

setter for c_d gain

Parameters:: c_d – [in] [N*s/m] Direction-constraint derivative gain.

void setK_a(double k_a)

Set the attitude-constraint proportional gain.

setter for k_a gain

Parameters:: k_a – [in] [N*m] Attitude-constraint proportional gain multiplying dimensionless MRP error.

void setC_a(double c_a)

Set the attitude-constraint derivative gain.

setter for c_a gain

Parameters:: c_a – [in] [N*m*s] Attitude-constraint derivative gain multiplying the MRP error rate.

void setFilter_Data(double wc, double h, double k)

setter for a,b,s,c,d,e coefficients of low pass filter

This method allows the user to set the cut-off frequency of the low pass filter which is then used to calculate the coefficients for numerical low pass filtering based on a second-order low pass filter design.

Parameters:

wc – The cut-off frequency of the low pass filter.
h – The constant digital time step.
k – The damping coefficient

void setStateNameOfPosition(std::string value) override

Add a spacecraft position-state name.

setter for stateNameOfPosition property

Parameters:: value – [in] Position-state name to add.

void setStateNameOfVelocity(std::string value) override

Add a spacecraft velocity-state name.

setter for stateNameOfVelocity property

Parameters:: value – [in] Velocity-state name to add.

void setStateNameOfSigma(std::string value) override

Add a spacecraft attitude-state name.

setter for stateNameOfSigma property

Parameters:: value – [in] Attitude-state name to add.

void setStateNameOfOmega(std::string value) override

Add a spacecraft angular-velocity-state name.

setter for stateNameOfOmega property

Parameters:: value – [in] Angular-velocity-state name to add.

void setPropName_inertialPosition(std::string value) override

Add an inertial-position property name.

setter for propName_inertialPosition property

Parameters:: value – [in] Inertial-position property name to add.

inline const std::vector<std::string> getPropName_inertialPosition() const: getter for propName_inertialPosition property

void setPropName_inertialVelocity(std::string value) override

Add an inertial-velocity property name.

setter for propName_inertialVelocity property

Parameters:: value – [in] Inertial-velocity property name to add.

inline const std::vector<std::string> getPropName_inertialVelocity() const: getter for propName_inertialVelocity property

void setPropName_inertialAttitude(std::string value) override

Add an inertial-attitude property name.

setter for propName_inertialAttitude property

Parameters:: value – [in] Inertial-attitude property name to add.

inline const std::vector<std::string> getPropName_inertialAttitude() const: getter for propName_inertialAttitude property

void setPropName_inertialAngVelocity(std::string value) override

Add an inertial-angular-velocity property name.

setter for propName_inertialAngVelocity property

Parameters:: value – [in] Inertial-angular-velocity property name to add.

inline const std::vector<std::string> getPropName_inertialAngVelocity() const: getter for propName_inertialAngVelocity property

inline Eigen::Vector3d getR_P2P1_B1Init() const: getter for r_P2P1_B1Init initial spacecraft separation

inline Eigen::Vector3d getR_P1B1_B1() const: getter for r_P1B1_B1 connection point position on spacecraft 1

inline Eigen::Vector3d getR_P2B2_B2() const: getter for r_P2B2_B2 connection point position on spacecraft 2

inline double getAlpha() const: getter for alpha gain tuning parameter

inline double getBeta() const: getter for beta gain tuning parameter

inline double getK_d() const: getter for k_d gain

inline double getC_d() const: getter for c_d gain

inline double getK_a() const: getter for k_a gain

inline double getC_a() const: getter for c_a gain

Public Members

Message<ConstDynEffectorMsgPayload> constraintElements: output message with constraint force and torque on connected s/c

ReadFunctor<DeviceStatusMsgPayload> effectorStatusInMsg: input message to toggle effector on/off

uint64_t effectorStatus = 1: effector toggle on/off (1-active, 0-inactive)

Private Functions

void validateConfiguration()

Validate the user-supplied gain configuration.

Validate that either the tuning parameters or individual gains were configured.

void initializeGains()

Refresh derived gains while preserving explicit individual gains.

Recompute derived gains without replacing explicitly configured individual gains.

Private Members

int scInitCounter = 0: counter to kill simulation if more than two spacecraft initialized

int scID = 1: 0,1 alternating spacecraft toggle to output appropriate force/torque

parentID parent1

parentID parent2

int hubCounter = 0

int effectorCounter = 0

Eigen::Vector3d r_P1B1_B1 = Eigen::Vector3d::Zero(): [m] position vector from spacecraft 1 hub to its connection point P1

Eigen::Vector3d r_P2B2_B2 = Eigen::Vector3d::Zero(): [m] position vector from spacecraft 2 hub to its connection point P2

Eigen::Vector3d r_P2P1_B1Init = Eigen::Vector3d::Zero(): [m] prescribed position vector from spacecraft 1 connection point to spacecraft 2 connection point

Eigen::Matrix3d dcm_B2B1Init = Eigen::Matrix3d::Identity(): attitude constraint violation

double alpha = 0.0: [-] Numerical tuning parameter whose square sets proportional gains in SI units

double beta = 0.0: [-] Numerical tuning parameter doubled to set derivative gains in SI units

double k_d = 0.0: [N/m] direction constraint proportional gain

double c_d = 0.0: [N*s/m] direction constraint derivative gain

double k_a = 0.0: [N*m] attitude constraint proportional gain multiplying dimensionless MRP error

double c_a = 0.0: [N*m*s] attitude constraint derivative gain multiplying the MRP error rate

bool k_dExplicit = false: Whether setK_d supplied the direction proportional gain.

bool c_dExplicit = false: Whether setC_d supplied the direction derivative gain.

bool k_aExplicit = false: Whether setK_a supplied the attitude proportional gain.

bool c_aExplicit = false: Whether setC_a supplied the attitude derivative gain.

double a = 0.0: coefficient in numerical low pass filter

double b = 0.0: coefficient in numerical low pass filter

double c = 0.0: coefficient in numerical low pass filter

double d = 0.0: coefficient in numerical low pass filter

double e = 0.0: coefficient in numerical low pass filter

double F_mag_tminus2 = 0.0: Magnitude of unfiltered constraint force at t-2 time step.

double F_mag_tminus1 = 0.0: Magnitude of unfiltered constraint force at t-1 time step.

double F_mag_t = 0.0: Magnitude of unfiltered constraint force at t time step.

double F_filtered_mag_t = 0.0: Magnitude of filtered constraint force at t time step.

double F_filtered_mag_tminus1 = 0.0: Magnitude of filtered constraint force at t-1 time step.

double F_filtered_mag_tminus2 = 0.0: Magnitude of filtered constraint force at t-2 time step.

double T1_mag_tminus2 = 0.0: Magnitude of unfiltered constraint torque on s/c 1 at t-2 time step.

double T1_mag_tminus1 = 0.0: Magnitude of unfiltered constraint torque on s/c 1 at t-1 time step.

double T1_mag_t = 0.0: Magnitude of unfiltered constraint torque on s/c 1 at t time step.

double T1_filtered_mag_t = 0.0: Magnitude of filtered constraint torque on s/c 1 at t time step.

double T1_filtered_mag_tminus1 = 0.0: Magnitude of filtered constraint torque on s/c 1 at t-1 time step.

double T1_filtered_mag_tminus2 = 0.0: Magnitude of filtered constraint torque on s/c 1 at t-2 time step.

double T2_mag_tminus2 = 0.0: Magnitude of unfiltered constraint torque on s/c 2 at t-2 time step.

double T2_mag_tminus1 = 0.0: Magnitude of unfiltered constraint torque on s/c 2at t-1 time step.

double T2_mag_t = 0.0: Magnitude of unfiltered constraint torque on s/c 2 at t time step.

double T2_filtered_mag_t = 0.0: Magnitude of filtered constraint torque on s/c 2 at t time step.

double T2_filtered_mag_tminus1 = 0.0: Magnitude of filtered constraint torque on s/c 2 at t-1 time step.

double T2_filtered_mag_tminus2 = 0.0: Magnitude of filtered constraint torque on s/c 2 at t-2 time step.

std::vector<std::string> stateNameOfPosition: state engine name of the parent rigid body inertial position vector

std::vector<std::string> stateNameOfVelocity: state engine name of the parent rigid body inertial velocity vector

std::vector<std::string> stateNameOfSigma: state engine name of the parent rigid body inertial attitude

std::vector<std::string> stateNameOfOmega: state engine name of the parent rigid body inertial angular velocity vector

std::vector<StateData*> hubPosition: [m] parent inertial position vector

std::vector<StateData*> hubVelocity: [m/s] parent inertial velocity vector

std::vector<StateData*> hubSigma: parent attitude Modified Rodrigues Parameters (MRPs)

std::vector<StateData*> hubOmega: [rad/s] parent inertial angular velocity vector

std::vector<std::string> propName_inertialPosition: property name of inertialPosition

std::vector<std::string> propName_inertialVelocity: property name of inertialVelocity

std::vector<std::string> propName_inertialAttitude: property name of inertialAttitude

std::vector<std::string> propName_inertialAngVelocity: property name of inertialAngVelocity

std::vector<Eigen::MatrixXd*> inertialPositionProperty: [m] position relative to inertial frame

std::vector<Eigen::MatrixXd*> inertialVelocityProperty: [m/s] velocity relative to inertial frame

std::vector<Eigen::MatrixXd*> inertialAttitudeProperty: attitude relative to inertial frame

std::vector<Eigen::MatrixXd*> inertialAngVelocityProperty: [rad/s] inertial angular velocity relative to inertial frame

Eigen::Vector3d psi_N = Eigen::Vector3d::Zero(): [m] direction constraint violation in inertial frame

Eigen::Vector3d psiPrime_N = Eigen::Vector3d::Zero(): [m/s] direction rate constraint violation in inertial frame

Eigen::MRPd phi = Eigen::MRPd::Identity(): attitude constraint violation

Eigen::Vector3d omega_B2B1_B2 = Eigen::Vector3d::Zero(): [rad/s] angular velocity constraint violation in spacecraft 2 body frame

Eigen::Vector3d Fc_N = Eigen::Vector3d::Zero(): [N] force applied on each spacecraft COM in the inertial frame

Eigen::Vector3d T_B2 = Eigen::Vector3d::Zero(): [N-m] torque applied on spacecraft 2 in its body frame

Eigen::Vector3d T_B1 = Eigen::Vector3d::Zero(): [N-m] torque applied on spacecraft 1 in its body frame