C++ Module: jointArrayRefProfiler

Executive Summary

This module converts instantaneous reference joint angle changes to either filtered or time-profiled reference joint angles, rates, and accelerations.

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
jointStatesInMsgs	ScalarJointStateMsgPayload	Vector of current joint state input messages.
jointStateDotsInMsgs	ScalarJointStateMsgPayload	Vector of current joint state-derivative input messages.
desJointStatesInMsg	JointArrayStateMsgPayload	Desired joint-array state input message.
desJointStatesOutMsg	JointArrayStateMsgPayload	Joint-array reference output message.

Module Assumptions and Limitations

This module assumes the desired joint-array command remains constant between message updates and that a new profile should begin whenever the values within the desJointStatesInMsg are updated. In instances where there is a timing mismatch between the write time of desJointStatesInMsg and the module update time, the new profile begins based on the module update time rather than the time the message was written.

The lowPass mode applies a first-order discrete low-pass filter to the desired joint angles using the user-specified wc and filterDt parameters. This mode smooths the reference command but does not guarantee finite-time convergence like the time-profiled modes.

For the linear, cubic, and quintic modes, the module is designed to drive each joint to the desired final angle over the specified profileDuration. The cubic and quintic implementations assume the terminal joint rate and terminal joint acceleration are zero. The linear mode does not preserve the initial joint rates, performs constant-rate interpolation, and only reaches zero rate once the profile completes.

Angle Handling

By default, joint-angle wrapping is disabled and useShortestPath is False. The module therefore preserves the input joint-angle representation and profiles directly to the commanded angle.

Call setJointAngleWrapStart() to enable wrapping and define the lower bound of the output interval. The module normalizes rotational joint inputs, commanded angles, and output references to \([\theta_{start}, \theta_{start} + 2\pi)\). For example, a lower bound of \(-\pi\) selects \([-\pi, \pi)\), while a lower bound of \(0\) selects \([0, 2\pi)\). Call disableJointAngleWrapping() to disable wrapping after it has been configured. This feature is intended for periodic rotational joints and should not be used for multi-turn joint-coordinate commands.

In lowPass mode, the internal filter state remains unwrapped so that it can converge continuously across an interval boundary. Only the joint-angle value written to the output message is normalized to the configured interval.

Set useShortestPath to True to profile to the closest \(2\pi\)-equivalent of each commanded angle relative to the joint angle at the start of a new profile. The effective target displacement is in \([-\pi, \pi]\). For example, a transition from \(-170^\circ\) to \(170^\circ\) follows a \(-20^\circ\) path.

The shortest-path option is independent of angle wrapping. When useShortestPath is enabled while joint-angle wrapping is disabled, the output reference retains an unwrapped angle representation to preserve a continuous short-path profile across a canonical interval boundary.

Note

Changing the wrapping interval or enabling or disabling wrapping affects the next output message. The sampled start angle and effective target remain fixed for the current profile, including after changes to useShortestPath. They are recomputed only when the desired message values change or on the first UpdateState() following Reset(). Rewriting an identical command with a new timestamp does not restart the profile. Call Reset() before the next update to apply changed settings to an unchanged command.

User Guide

This section outlines the steps needed to set up the jointArrayRefProfiler module in Python using Basilisk.

Import the module:

from Basilisk.simulation import jointArrayRefProfiler

Create an instance of the module:

module = jointArrayRefProfiler.JointArrayRefProfiler()
module.ModelTag = "jointArrayRefProfiler"

Set the profile type to either linear, cubic, quintic, or lowPass:
```
module.setProfileType("linear")
```
For the low pass filter mode, set the cutoff frequency and filter time step:
```
module.setWc(1.0)  # [rad/s]
module.setFilterDt(0.01)  # [s]
```
For the time-profiled modes, set the profile duration:
```
module.setProfileDuration(2.0)  # [s]
```

(Optional) Enable joint angle wrapping and set its output interval:

module.setJointAngleWrapStart(0.0)  # [rad], wraps to [0.0, 2*pi)

(Optional) Enable shortest path routing:
```
module.setUseShortestPath(True)
```
For each hinged joint in the system, add a hinged joint to the module:
```
module.addHingedJoint()
```

Add the module to the task list:

unitTestSim.AddModelToTask(unitTaskName, module)

class JointArrayRefProfiler : public SysModel 

#include <jointArrayRefProfiler.h>

This module converts instantaneous reference angle changes for an array of joints to smooth profiles.

Public Functions

JointArrayRefProfiler() = default: This is the constructor for the module class.

~JointArrayRefProfiler() = default: This is the destructor for the module class.

void Reset(uint64_t CurrentSimNanos): This method is used to reset the module and checks that required input messages are connected.

void UpdateState(uint64_t CurrentSimNanos): This is the main method that gets called every time the module is updated. It determines the current joint reference states based on the profile shape and duration.

void setProfileType(std::string profileType): setter for the profileType property

inline std::string getProfileType() const: getter for the profileType property

void setProfileDuration(double profileDuration): setter for the profileDuration property

inline double getProfileDuration() const: getter for the profileDuration property

void setWc(double wc): setter for the wc property

inline double getWc() const: getter for the wc property

void setFilterDt(double filterDt): setter for the filterDt property

inline double getFilterDt() const: getter for the filterDt property

void setJointAngleWrapStart(double jointAngleWrapStart): setter for the jointAngleWrapStart property that enables joint-angle wrapping

inline double getJointAngleWrapStart() const: getter for the jointAngleWrapStart property

inline bool getJointAngleWrappingEnabled() const: getter for the joint-angle wrapping enabled status

void disableJointAngleWrapping(): method for disabling joint-angle wrapping

void setUseShortestPath(bool useShortestPath): setter for the useShortestPath property

inline bool getUseShortestPath() const: getter for the useShortestPath property

void addHingedJoint(): method for adding a new hinged joint to the system

Public Members

std::vector<ReadFunctor<ScalarJointStateMsgPayload>> jointStatesInMsgs: vector of current joint state input messages

std::vector<ReadFunctor<ScalarJointStateMsgPayload>> jointStateDotsInMsgs: vector of current joint state-derivative input messages

ReadFunctor<JointArrayStateMsgPayload> desJointStatesInMsg: Desired joint-array state input message.

Message<JointArrayStateMsgPayload> desJointStatesOutMsg: Joint-array reference output message.

BSKLogger bskLogger: BSK Logging.

Private Functions

void computeLowPassFilter(uint64_t CurrentSimNanos)

Method for computing the low pass filtered joint reference profile

Parameters:: CurrentSimNanos – current simulation time in nanoseconds

void computeLinearProfile(uint64_t CurrentSimNanos)

Method for computing the linear joint reference profile

Parameters:: CurrentSimNanos – current simulation time in nanoseconds

void computeCubicProfile(uint64_t CurrentSimNanos)

Method for computing the cubic joint reference profile

Parameters:: CurrentSimNanos – current simulation time in nanoseconds

void computeQuinticProfile(uint64_t CurrentSimNanos)

Method for computing the quintic joint reference profile

Parameters:: CurrentSimNanos – current simulation time in nanoseconds

Private Members

std::string profileType: [-] Reference profile type

double profileDuration = -1.0: [s] Reference profile duration, used for all profile types except “lowPass”

double wc = -1.0: [rad/s] low pass filter cutoff frequency, only used if profileType is “lowPass”

double filterDt = -1.0: [s] low pass filter time step, only used if profileType is “lowPass”

bool jointAngleWrappingEnabled = false: Whether rotational joint angles are wrapped before profiling.

double jointAngleWrapStart = -M_PI: [rad] Lower bound of the joint-angle wrapping interval

bool useShortestPath = false: Whether rotational joint profiles use the shortest angular path.

int numHingedJoints = 0: number of hinged joints in the system

uint64_t profileStartTime = 0: [ns] simulation time at which the current profile started

bool profileStartTimeSet = false: flag indicating whether the profile start time has been set

Eigen::VectorXd startJointAngles: [rad] joint angles at the start of the current reference profile

Eigen::VectorXd startJointRates: [rad/s] joint angle rates at the start of the current reference profile

Eigen::VectorXd targetJointAngles: [rad] effective joint-angle targets for the current reference profile

Eigen::VectorXd refJointAngles: [rad] current profiled joint angles

Eigen::VectorXd refJointRates: [rad/s] current profiled joint angle rates

Eigen::VectorXd refJointAccels: [rad/s^2] current profiled joint angle accelerations

JointArrayStateMsgPayload prevDesJointStates: previous desired joint states, used to detect changes in the desired state command