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.

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]
```
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 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, const JointArrayStateMsgPayload &desJointStatesIn)

Method for computing the low pass filtered joint reference profile

Parameters:

CurrentSimNanos – current simulation time in nanoseconds
desJointStatesIn – desired joint states input message payload

void computeLinearProfile(uint64_t CurrentSimNanos, const JointArrayStateMsgPayload &desJointStatesIn)

Method for computing the linear joint reference profile

Parameters:

CurrentSimNanos – current simulation time in nanoseconds
desJointStatesIn – desired joint states input message payload

void computeCubicProfile(uint64_t CurrentSimNanos, const JointArrayStateMsgPayload &desJointStatesIn)

Method for computing the cubic joint reference profile

Parameters:

CurrentSimNanos – current simulation time in nanoseconds
desJointStatesIn – desired joint states input message payload

void computeQuinticProfile(uint64_t CurrentSimNanos, const JointArrayStateMsgPayload &desJointStatesIn)

Method for computing the quintic joint reference profile

Parameters:

CurrentSimNanos – current simulation time in nanoseconds
desJointStatesIn – desired joint states input message payload

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”

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