From 361df80726b22fc6b8690a1f8007f76c056328bc Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 5 Dec 2025 14:42:00 -0700 Subject: [PATCH 1/5] Add new message definition --- .../JointArrayStateMsgPayload.h | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100755 src/architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h diff --git a/src/architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h b/src/architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h new file mode 100755 index 0000000000..da5340da74 --- /dev/null +++ b/src/architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h @@ -0,0 +1,36 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef JOINT_ARRAY_STATE_MESSAGE_H +#define JOINT_ARRAY_STATE_MESSAGE_H + +#include + +/*! @brief Structure used by the messaging system to communicate details about the array of joint states in the system.*/ +typedef struct +//@cond DOXYGEN_IGNORE +JointArrayStateMsgPayload +//@endcond + +{ + std::vector states; //!< [-] list of joint states + std::vector stateDots; //!< [-] list of joint state derivatives +} JointArrayStateMsgPayload; + +#endif From 66905ed192ff69af5de4742dc865f6fe5bd41664 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 5 Dec 2025 14:43:17 -0700 Subject: [PATCH 2/5] Add new hingedJointArray module --- .../hingedJointArrayMotor.cpp | 307 ++++++++++++++++++ .../hingedJointArrayMotor.h | 94 ++++++ .../hingedJointArrayMotor.i | 52 +++ 3 files changed, 453 insertions(+) create mode 100644 src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.cpp create mode 100644 src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.h create mode 100644 src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.i diff --git a/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.cpp b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.cpp new file mode 100644 index 0000000000..f438656181 --- /dev/null +++ b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.cpp @@ -0,0 +1,307 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + + +#include "fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.h" +#include +#include +#include + +void HingedJointArrayMotor::Reset(uint64_t CurrentSimNanos) +{ + // check that required input messages are connected + if (!this->massMatrixInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.massMatrixInMsg was not linked."); + } + if (!this->reactionForcesInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.reactionForcesInMsg was not linked."); + } + if (!this->desJointStatesInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.desJointStatesInMsg was not linked."); + } + if (this->jointStatesInMsgs.empty()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStatesInMsgs vector is empty."); + } else { + if (this->jointStatesInMsgs.size() != static_cast(this->numHingedJoints)) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStatesInMsgs size does not match numHingedJoints."); + } + for (std::size_t i = 0; i < this->jointStatesInMsgs.size(); ++i) { + if (!this->jointStatesInMsgs[i].isLinked()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStatesInMsgs[%zu] was not linked.", i); + } + } + } + if (this->jointStateDotsInMsgs.empty()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStateDotsInMsgs vector is empty."); + } else { + if (this->jointStateDotsInMsgs.size() != static_cast(this->numHingedJoints)) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStateDotsInMsgs size does not match numHingedJoints."); + } + for (std::size_t i = 0; i < this->jointStateDotsInMsgs.size(); ++i) { + if (!this->jointStateDotsInMsgs[i].isLinked()) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor.jointStateDotsInMsgs[%zu] was not linked.", i); + } + } + } + + // Check that the gains have been set properly + if (this->Ktheta.rows() != this->numHingedJoints || this->Ktheta.cols() != this->numHingedJoints) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor Ktheta gains matrix does not have dimensions of %d x %d.", this->numHingedJoints, this->numHingedJoints); + } + if (this->Ptheta.rows() != this->numHingedJoints || this->Ptheta.cols() != this->numHingedJoints) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor Ptheta gains matrix does not have dimensions of %d x %d.", this->numHingedJoints, this->numHingedJoints); + } + + + this->treeInfoInitialized = false; + +} + +void HingedJointArrayMotor::UpdateState(uint64_t CurrentSimNanos) +{ + MJSysMassMatrixMsgPayload massMatrixIn = this->massMatrixInMsg(); + MJJointReactionsMsgPayload reactionForcesIn = this->reactionForcesInMsg(); + JointArrayStateMsgPayload desJointStatesIn = this->desJointStatesInMsg(); + + // Extract the information on the kinematic trees if not already done + if (!this->treeInfoInitialized) { + this->numKinematicTrees = 0; + this->treeMap.clear(); + int nextHingeIdx = 0; + for (std::size_t joint = 0; joint < reactionForcesIn.jointTreeIdx.size(); ++joint) { + const int tree = reactionForcesIn.jointTreeIdx[joint]; + const int jointType = reactionForcesIn.jointTypes[joint]; + + TreeInfo& info = this->treeMap[tree]; + + if (info.freeJointIdx < 0) { + // first time seeing this tree + ++this->numKinematicTrees; + // check that first joint in tree is free + if (jointType != 0) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor: first joint in kinematic tree %d is not a free joint.", tree); + } + info.freeJointIdx = static_cast(joint); + } else { + // subsequent joints must be hinges + if (jointType != 3) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor: joint %zu in kinematic tree %d is not a hinged joint.", joint, tree); + } + info.hingeJointIdxs.push_back(static_cast(joint)); + info.hingeGlobalIdxs.push_back(nextHingeIdx); + ++nextHingeIdx; + } + } + if (this->numHingedJoints != nextHingeIdx) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor: numHingedJoints does not match the number of hinge joints found in the kinematic trees."); + } + this->treeInfoInitialized = true; + } + + // Loop through the joint states and joint state dot input messages + Eigen::VectorXd jointStates(this->numHingedJoints); + jointStates.setZero(); + Eigen::VectorXd jointStateDots(this->numHingedJoints); + jointStateDots.setZero(); + for (size_t i = 0; i < this->jointStatesInMsgs.size(); ++i) { + ScalarJointStateMsgPayload jointStateIn = this->jointStatesInMsgs[i](); + ScalarJointStateMsgPayload jointStateDotIn = this->jointStateDotsInMsgs[i](); + jointStates(i) = jointStateIn.state; + jointStateDots(i) = jointStateDotIn.state; + + } + + // Compute the motor torques for each hinged joint + Eigen::VectorXd uH(this->numHingedJoints); + uH.setZero(); + const int nDOF = static_cast(reactionForcesIn.biasForces.size()); + Eigen::VectorXd nonActuatorForces(nDOF); + nonActuatorForces.setZero(); + for (int k = 0; k < nDOF; ++k) { + nonActuatorForces(k) = reactionForcesIn.passiveForces[k] + reactionForcesIn.constraintForces[k] + + reactionForcesIn.appliedForces[k] - reactionForcesIn.biasForces[k]; + } + + constexpr int nTransDOF = 3; // number of translational DOFs for free joint base + for (const auto& [treeId, info] : this->treeMap) { + const int freeJointIdx = info.freeJointIdx; + const auto& hingeJointIdxs = info.hingeJointIdxs; + const auto& hingeGlobalIdxs = info.hingeGlobalIdxs; + const int nHingeJoints = static_cast(hingeJointIdxs.size()); + + if (nHingeJoints == 0) { + continue; // no hinged joints in this tree + } + + // Extract the joint DOF indicies for this tree + std::vector dofIdx; + dofIdx.reserve(nTransDOF + nHingeJoints); + + const int freeStart = reactionForcesIn.jointDOFStart[freeJointIdx]; + for (int i = 0; i < nTransDOF; ++i) { + dofIdx.push_back(freeStart + i); + } + + for (int mhJointIdx : hingeJointIdxs) { + const int hingeStart = reactionForcesIn.jointDOFStart[mhJointIdx]; + dofIdx.push_back(hingeStart); // hinged joints have 1 DOF + } + + // Build the mass matrix for this kinematic tree + const int nTreeDOF = static_cast(dofIdx.size()); + Eigen::MatrixXd Mfull(nTreeDOF, nTreeDOF); + Mfull.setZero(); + const auto& massMatrixData = massMatrixIn.massMatrix; + + for (int r = 0; r < nTreeDOF; ++r) { + const int rowIdx = dofIdx[r]; + for (int c = 0; c < nTreeDOF; ++c) { + const int colIdx = dofIdx[c]; + Mfull(r, c) = massMatrixData[rowIdx * nDOF + colIdx]; + } + } + + // Extract the submatrices + Eigen::Matrix3d Mtt = Mfull.block<3, 3>(0, 0); + Eigen::MatrixXd Mtth = Mfull.block(0, nTransDOF, 3, nHingeJoints); + Eigen::MatrixXd Mtht = Mfull.block(nTransDOF, 0, nHingeJoints, 3); + Eigen::MatrixXd Mthth = Mfull.block(nTransDOF, nTransDOF, nHingeJoints, nHingeJoints); + + // Build the non-actuator force vectors + Eigen::Vector3d baseTransBias; + baseTransBias.setZero(); + Eigen::VectorXd jointBias(nHingeJoints); + jointBias.setZero(); + for (int i = 0; i < nTransDOF; ++i) { + baseTransBias(i) = nonActuatorForces(dofIdx[i]); + } + for (int i = 0; i < nHingeJoints; ++i) { + jointBias(i) = nonActuatorForces(dofIdx[nTransDOF + i]); + } + + // Build the gain matrices for the tree + Eigen::MatrixXd Ktheta_tree(nHingeJoints, nHingeJoints); + Ktheta_tree.setZero(); + Eigen::MatrixXd Ptheta_tree(nHingeJoints, nHingeJoints); + Ptheta_tree.setZero(); + for (int i = 0; i < nHingeJoints; ++i) { + const int gi = hingeGlobalIdxs[i]; + for (int j = 0; j < nHingeJoints; ++j) { + const int gj = hingeGlobalIdxs[j]; + Ktheta_tree(i, j) = this->Ktheta(gi, gj); + Ptheta_tree(i, j) = this->Ptheta(gi, gj); + } + } + + // Compute desired joint accelerations for this tree + Eigen::VectorXd theta_ddot_des(nHingeJoints); + theta_ddot_des.setZero(); + Eigen::VectorXd theta(nHingeJoints), thetaDot(nHingeJoints), theta_des(nHingeJoints), thetaDot_des(nHingeJoints); + theta.setZero(); + thetaDot.setZero(); + theta_des.setZero(); + thetaDot_des.setZero(); + for (int i = 0; i < nHingeJoints; ++i) { + const int jointIdx = hingeGlobalIdxs[i]; + theta(i) = jointStates[jointIdx]; + thetaDot(i) = jointStateDots[jointIdx]; + theta_des(i) = desJointStatesIn.states[jointIdx]; + thetaDot_des(i) = desJointStatesIn.stateDots[jointIdx]; + } + + auto wrap = [](double a){ return std::atan2(std::sin(a), std::cos(a)); }; + Eigen::VectorXd e(nHingeJoints), eDot(nHingeJoints); + e.setZero(); + eDot.setZero(); + for (int i = 0; i < nHingeJoints; ++i) { + e(i) = wrap(theta(i) - theta_des(i)); + eDot(i) = thetaDot(i) - thetaDot_des(i); + } + theta_ddot_des = -Ktheta_tree * e - Ptheta_tree * eDot; + + // Use Schur complement to compute the motor torques + Eigen::Vector3d rhs = -Mtth * theta_ddot_des + baseTransBias; + Eigen::LDLT ldlt(Mtt); + Eigen::Vector3d lambda = ldlt.solve(rhs); + Eigen::VectorXd uH_tree = (Mthth * theta_ddot_des) + (Mtht*lambda) - jointBias; + + for (int i = 0; i < nHingeJoints; ++i) { + const int hingedIdx = hingeGlobalIdxs[i]; + uH(hingedIdx) = uH_tree(i); + } + + } + + // Apply torque limits if specified + if (!this->uMax.empty()) { + if (static_cast(this->uMax.size()) != this->numHingedJoints) { + bskLogger.bskLog(BSK_ERROR, "HingedJointArrayMotor: size of uMax does not match numHingedJoints."); + } + for (int i = 0; i < this->numHingedJoints; ++i) { + uH(i) = std::max(-this->uMax[i], std::min(this->uMax[i], uH(i))); + } + } + + // write to the output messages + for (int i=0; i < this->numHingedJoints; ++i) { + SingleActuatorMsgPayload motorTorquesOutMsg = this->motorTorquesOutMsgs[i]->zeroMsgPayload; + motorTorquesOutMsg.input = uH(i); + this->motorTorquesOutMsgs[i]->write(&motorTorquesOutMsg, this->moduleID, CurrentSimNanos); + } +} + +void HingedJointArrayMotor::setKtheta(std::vector var) +{ + const std::size_t N = var.size(); + const int m = static_cast(std::llround(std::sqrt(static_cast(N)))); + + using MatMap = Eigen::Map>; + MatMap Kmap(var.data(), m, m); + + this->Ktheta = Kmap; +} + +void HingedJointArrayMotor::setPtheta(std::vector var) +{ + const std::size_t N = var.size(); + const int m = static_cast(std::llround(std::sqrt(static_cast(N)))); + + using MatMap = Eigen::Map>; + MatMap Pmap(var.data(), m, m); + + this->Ptheta = Pmap; +} + +void HingedJointArrayMotor::setUMax(std::vector var) +{ + this->uMax = var; +} + +void HingedJointArrayMotor::addHingedJoint() +{ + // increase the number of hinged joints by 1 + this->numHingedJoints++; + + // add a new input message reader for the new hinged joint + this->jointStatesInMsgs.push_back(ReadFunctor()); + this->jointStateDotsInMsgs.push_back(ReadFunctor()); + + // add a new output message for the new hinged joint + this->motorTorquesOutMsgs.push_back(new Message()); +} diff --git a/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.h b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.h new file mode 100644 index 0000000000..2939b8832c --- /dev/null +++ b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.h @@ -0,0 +1,94 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + + +#ifndef HINGEDJOINTARRAYMOTOR_H +#define HINGEDJOINTARRAYMOTOR_H + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h" +#include "architecture/msgPayloadDefCpp/MJJointReactionsMsgPayload.h" +#include "architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h" +#include "architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h" +#include "architecture/msgPayloadDefC/SingleActuatorMsgPayload.h" +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" +#include "architecture/utilities/avsEigenSupport.h" +#include + +/*! @brief This module determines the motor torques for an array of hinged joints based on commanded angles and the current system status. + */ +class HingedJointArrayMotor: public SysModel { +public: + HingedJointArrayMotor() = default; //!< This is the constructor for the module class. + ~HingedJointArrayMotor() = default; //!< This is the destructor for the module class. + + /*! + * This method is used to reset the module and checks that required input messages are connected. + */ + void Reset(uint64_t CurrentSimNanos); + + /*! + * This is the main method that gets called every time the module is updated. + * It computes the motor torques for the full array of hinged joints. + * */ + void UpdateState(uint64_t CurrentSimNanos); + +public: + ReadFunctor massMatrixInMsg; //!< system mass matrix input msg + ReadFunctor reactionForcesInMsg; //!< joint reaction forces and torques input msg + ReadFunctor desJointStatesInMsg; //!< desired joint states input msg + std::vector> jointStatesInMsgs; //!< vector of joint state input msgs + std::vector> jointStateDotsInMsgs; //!< vector of joint state derivative input msgs + + std::vector*> motorTorquesOutMsgs; //!< vector of joint motor torque output msgs + + BSKLogger bskLogger; //!< BSK Logging + /** setter for `Ktheta` property */ + void setKtheta(std::vector); + /** getter for `Ktheta` property */ + Eigen::MatrixXd getKtheta() const {return this->Ktheta;} + /** setter for `Ptheta` property */ + void setPtheta(std::vector); + /** getter for `Ptheta` property */ + Eigen::MatrixXd getPtheta() const {return this->Ptheta;} + /** setter for `uMax` property */ + void setUMax(std::vector); + /** getter for `uMax` property */ + std::vector getUMax() const {return this->uMax;} + /** method for adding a new hinged joint to the system */ + void addHingedJoint(); + +private: + int numHingedJoints = 0; //!< number of hinged joints in the system + Eigen::MatrixXd Ktheta = Eigen::MatrixXd{}; //!< [1/s^2] proportional gain for hinged joints + Eigen::MatrixXd Ptheta = Eigen::MatrixXd{}; //!< [1/s] proportional gain for hinged joints + std::vector uMax = {}; //!< [Nm] (optional) maximum joint motor torque + struct TreeInfo { + int freeJointIdx = -1; + std::vector hingeJointIdxs; + std::vector hingeGlobalIdxs; + }; //< struct to hold info about each kinematic tree + bool treeInfoInitialized = false; //!< flag indicating if tree info has been initialized + int numKinematicTrees = 0; //!< number of kinematic trees in the system + std::unordered_map treeMap; //!< map from kinematic tree index to tree info +}; + + +#endif diff --git a/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.i b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.i new file mode 100644 index 0000000000..c9eb379cf4 --- /dev/null +++ b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.i @@ -0,0 +1,52 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + +%module hingedJointArrayMotor + +%include "architecture/utilities/bskException.swg" +%default_bsk_exception(); + +%{ + #include "hingedJointArrayMotor.h" +%} + +%include "std_vector.i" +%template(DoubleVector) std::vector; + +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_conly_data.i" + +%include "sys_model.i" +%include "hingedJointArrayMotor.h" + +%include "architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h" +%include "architecture/msgPayloadDefCpp/MJJointReactionsMsgPayload.h" +%include "architecture/msgPayloadDefCpp/JointArrayStateMsgPayload.h" +%include "architecture/msgPayloadDefC/ScalarJointStateMsgPayload.h" +struct ScalarJointStateMsg_C; +%include "architecture/msgPayloadDefC/SingleActuatorMsgPayload.h" +struct SingleActuatorMsg_C; + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} From e47dda9d8e61715fc3c0d3346a4b20b8dcf1e79d Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 5 Dec 2025 14:43:31 -0700 Subject: [PATCH 3/5] Add new hingedJointArray documentation --- .../hingedJointArrayMotor.rst | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.rst diff --git a/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.rst b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.rst new file mode 100644 index 0000000000..30c6192fed --- /dev/null +++ b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/hingedJointArrayMotor.rst @@ -0,0 +1,71 @@ +Executive Summary +----------------- +The ``hingedJointArrayMotor`` module determines the motor torques for an array of hinged joints based on commanded angles and the current system configuration. + +.. note:: + This module is designed to work for systems with multiple spacecraft, however, each spacecraft must be comprised of six degree-of-freedom rigid hub with attached hinged joints only. Additionally, the math assumes the reaction torques experienced by the hub due to the hinged joint motion are cancelled. + +Message Connection Descriptions +------------------------------- +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description +provides information on what this message is used for. + +.. list-table:: Module I/O Messages + :widths: 25 25 50 + :header-rows: 1 + + * - Msg Variable Name + - Msg Type + - Description + * - massMatrixInMsg + - :ref:`MJSysMassMatrixMsgPayload` + - System mass matrix input msg + * - reactionForcesInMsg + - :ref:`MJJointReactionsMsgPayload` + - Joint reaction forces and torques input msg + * - desJointStatesInMsg + - :ref:`JointArrayStateMsgPayload` + - Desired joint states input msg + * - jointStatesInMsg + - :ref:`ScalarJointStateMsgPayload` + - Vector of joint state input msgs + * - jointStateDotsInMsg + - :ref:`ScalarJointStateMsgPayload` + - Vector of joint state derivative input msgs + * - motorTorquesOutMsg + - :ref:`SingleActuatorMsgPayload` + - Vector of joint motor torque output msgs + +User Guide +---------- +This section is to outline the steps needed to setup the ``hingedJointArrayMotor`` module in Python using Basilisk. + +#. Import the hingedJointArrayMotor class:: + + from Basilisk.fswAlgorithms import hingedJointArrayMotor + +#. Create an instance of hingedJointArrayMotor:: + + module = hingedJointArrayMotor.HingedJointArrayMotor() + +#. Set the control gains:: + + Ktheta = 10.0 * np.eye(numJoints * numSpacecraft) + Ptheta = 2.0 * np.sqrt(10.0) * np.eye(numJoints * numSpacecraft) + module.setKtheta(Ktheta.flatten().tolist()) + module.setPtheta(Ptheta.flatten().tolist()) + +#. (Optional) set the maximum motor torque values:: + + uMax = [0.03] * (numJoints * numSpacecraft) + module.setUMax(uMax) + +#. 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) From c35bc529d55993c25ad400b071e8a8902ae9a280 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 5 Dec 2025 14:43:47 -0700 Subject: [PATCH 4/5] Add new hingedJointArray unit test --- .../_UnitTest/test_hingedJointArrayMotor.py | 260 ++++++++++++++++++ 1 file changed, 260 insertions(+) create mode 100644 src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/_UnitTest/test_hingedJointArrayMotor.py diff --git a/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/_UnitTest/test_hingedJointArrayMotor.py b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/_UnitTest/test_hingedJointArrayMotor.py new file mode 100644 index 0000000000..41e55dd92e --- /dev/null +++ b/src/fswAlgorithms/effectorInterfaces/hingedJointArrayMotor/_UnitTest/test_hingedJointArrayMotor.py @@ -0,0 +1,260 @@ +# +# ISC License +# +# Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# +# + +import numpy as np +import pytest + +from Basilisk.utilities import SimulationBaseClass, macros +from Basilisk.architecture import messaging +from Basilisk.fswAlgorithms import hingedJointArrayMotor + +def jointTorques(M, K, P, theta, thetaDot, desTheta, desThetaDot, bias, maxTorque=None): + M = np.asarray(M, dtype=float) + K = np.asarray(K, dtype=float) + P = np.asarray(P, dtype=float) + theta = np.asarray(theta, dtype=float).reshape(-1) + thetaDot = np.asarray(thetaDot, dtype=float).reshape(-1) + desTheta = np.asarray(desTheta, dtype=float).reshape(-1) + desThetaDot= np.asarray(desThetaDot, dtype=float).reshape(-1) + bias = np.asarray(bias, dtype=float).reshape(-1) + + nj = theta.size + e = theta - desTheta + edot = thetaDot - desThetaDot + theta_ddot_des = -(K @ e) - (P @ edot) + + Mtt = M[0:3,0:3] + Mtth = M[0:3,6:6+nj] + Mtht = M[6:6+nj,0:3] + Mthth = M[6:6+nj,6:6+nj] + + transBias = bias[0:3] + jointBias = bias[6:6+nj] + + rhs = -Mtth @ theta_ddot_des + transBias + + x = np.linalg.solve(Mtt, rhs) + + u_H = Mthth @ theta_ddot_des + Mtht @ x - jointBias + + if maxTorque is not None: + for i in range(nj): + if u_H[i] > maxTorque[i]: + u_H[i] = maxTorque[i] + elif u_H[i] < -maxTorque[i]: + u_H[i] = -maxTorque[i] + + return u_H + +@pytest.mark.parametrize("nonActForces", [False, True]) +@pytest.mark.parametrize("maxTorque", [False, True]) +@pytest.mark.parametrize("numJoints", [1,4]) +@pytest.mark.parametrize("numSpacecraft", [1, 2]) + +def test_hingedJointArrayMotor(nonActForces, maxTorque, numJoints, numSpacecraft): + r""" + **Validation Test Description** + + This unit test validates the motor torques found by the joint array controller for spacecraft with attached jointed arms. + + **Test Parameters** + + The presence of non actuator based forces, presence of max torque limits, number of joints, and number of spacecraft can be varied. + + Args: + nonActForces (bool): Flag to indicate presence of non-actuator forces + maxTorque (bool): Flag to indicate presence of max torque limits + numJoints (int): Number of joints in the arm + numSpacecraft (int): Number of spacecraft in the simulation + + **Description of Variables Being Tested** + + In this test the motor torques output by the module are compared to the expected torques calculated in python. + """ + unitTaskName = "unitTask" + unitProcessName = "TestProcess" + + unitTestSim = SimulationBaseClass.SimBaseClass() + testProcessRate = macros.sec2nano(0.1) + testProc = unitTestSim.CreateNewProcess(unitProcessName) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) + + # setup module to be tested + module = hingedJointArrayMotor.HingedJointArrayMotor() + module.ModelTag = "hingedJointArrayMotorTag" + Ktheta = 10.0 * np.eye(numJoints * numSpacecraft) + Ptheta = 2.0 * np.sqrt(10.0) * np.eye(numJoints * numSpacecraft) + module.setKtheta(Ktheta.flatten().tolist()) + module.setPtheta(Ptheta.flatten().tolist()) + if maxTorque: + uMax = [0.03] * (numJoints * numSpacecraft) + module.setUMax(uMax) + for sc in range(numSpacecraft): + for j in range(numJoints): + module.addHingedJoint() + unitTestSim.AddModelToTask(unitTaskName, module) + + # create mass matrix input message + M = np.eye((6 + numJoints) * numSpacecraft) + massMatrixInMsgData = messaging.MJSysMassMatrixMsgPayload() + massMatrixInMsgData.massMatrix.clear() + for v in M.flatten(): + massMatrixInMsgData.massMatrix.push_back(v) + + # create the joint states input messages vectors + if numJoints == 1: + theta = [0.1] + thetaDot = [0.5] + elif numJoints == 4: + theta = [0.1, -0.5, 0.3, 0.7] + thetaDot = [0.5, -0.2, 0.1, -0.3] + jointStatesInMsgDataList = [] + jointStateDotsInMsgDataList = [] + for sc in range(numSpacecraft): + for j in range(numJoints): + jointStatesInMsgData = messaging.ScalarJointStateMsgPayload() + jointStatesInMsgData.state = theta[j] + jointStatesInMsgDataList.append(jointStatesInMsgData) + + jointStateDotsInMsgData = messaging.ScalarJointStateMsgPayload() + jointStateDotsInMsgData.state = thetaDot[j] + jointStateDotsInMsgDataList.append(jointStateDotsInMsgData) + + # create the desired joint state input message + desJointStateInMsgData = messaging.JointArrayStateMsgPayload() + desTheta = [0.12] * (numJoints * numSpacecraft) + desThetaDot = [0.05] * (numJoints * numSpacecraft) + desJointStateInMsgData.states.clear() + desJointStateInMsgData.stateDots.clear() + for v in np.asarray(desTheta).flatten(): + desJointStateInMsgData.states.push_back(float(v)) + for v in np.asarray(desThetaDot).flatten(): + desJointStateInMsgData.stateDots.push_back(float(v)) + + # create the joint reactions input message + JointReactionsInMsgData = messaging.MJJointReactionsMsgPayload() + jointTreeIdx = [] + jointTypes = [] + jointDOFStart = [] + currentDOF = 0 + for sc in range(numSpacecraft): + idx = [sc] * (numJoints+1) + jointTreeIdx.extend(idx) + types = [0] + [3] * numJoints + jointTypes.extend(types) + jointDOFStart.append(currentDOF) + currentDOF += 6 + for j in range(numJoints): + jointDOFStart.append(currentDOF) + currentDOF += 1 + if nonActForces: + biasForces = [-2.0] * ((6 + numJoints) * numSpacecraft) + else: + biasForces = [0.0] * ((6 + numJoints) * numSpacecraft) + passiveForces = [0.0] * ((6 + numJoints) * numSpacecraft) + constraintForces = [0.0] * ((6 + numJoints) * numSpacecraft) + appliedForces = [0.0] * ((6 + numJoints) * numSpacecraft) + JointReactionsInMsgData.jointTreeIdx.clear() + JointReactionsInMsgData.jointTypes.clear() + JointReactionsInMsgData.jointDOFStart.clear() + JointReactionsInMsgData.biasForces.clear() + JointReactionsInMsgData.passiveForces.clear() + JointReactionsInMsgData.constraintForces.clear() + JointReactionsInMsgData.appliedForces.clear() + for v in jointTreeIdx: + JointReactionsInMsgData.jointTreeIdx.push_back(v) + for v in jointTypes: + JointReactionsInMsgData.jointTypes.push_back(v) + for v in jointDOFStart: + JointReactionsInMsgData.jointDOFStart.push_back(v) + for v in biasForces: + JointReactionsInMsgData.biasForces.push_back(v) + for v in passiveForces: + JointReactionsInMsgData.passiveForces.push_back(v) + for v in constraintForces: + JointReactionsInMsgData.constraintForces.push_back(v) + for v in appliedForces: + JointReactionsInMsgData.appliedForces.push_back(v) + + # write input messages + massMatrixInMsg = messaging.MJSysMassMatrixMsg().write(massMatrixInMsgData) + reactionForcesInMsg = messaging.MJJointReactionsMsg().write(JointReactionsInMsgData) + desJointStateInMsg = messaging.JointArrayStateMsg().write(desJointStateInMsgData) + jointStatesInMsgList = [] + jointStateDotsInMsgList = [] + for i in range(numJoints * numSpacecraft): + jointStatesInMsg = messaging.ScalarJointStateMsg().write(jointStatesInMsgDataList[i]) + jointStatesInMsgList.append(jointStatesInMsg) + jointStateDotsInMsg = messaging.ScalarJointStateMsg().write(jointStateDotsInMsgDataList[i]) + jointStateDotsInMsgList.append(jointStateDotsInMsg) + + # subscribe input messages to module + module.massMatrixInMsg.subscribeTo(massMatrixInMsg) + module.reactionForcesInMsg.subscribeTo(reactionForcesInMsg) + module.desJointStatesInMsg.subscribeTo(desJointStateInMsg) + for i in range(numJoints * numSpacecraft): + module.jointStatesInMsgs[i].subscribeTo(jointStatesInMsgList[i]) + module.jointStateDotsInMsgs[i].subscribeTo(jointStateDotsInMsgList[i]) + + # setup output message recorder objects + motorTorqueRecorders = [] + for i in range(numJoints * numSpacecraft): + rec = module.motorTorquesOutMsgs[i].recorder() + motorTorqueRecorders.append(rec) + unitTestSim.AddModelToTask(unitTaskName, rec) + + unitTestSim.InitializeSimulation() + unitTestSim.ConfigureStopTime(macros.sec2nano(0.1)) + unitTestSim.ExecuteSimulation() + + # pull module data + moduleTorques = [] + for rec in motorTorqueRecorders: + moduleTorques.append(rec.input[-1]) + moduleTorques = np.asarray(moduleTorques, dtype=float).reshape(-1) + + # compute expected results + reactionForces = np.asarray(biasForces, dtype=float).reshape(-1) + expectedTorques = np.zeros(numJoints * numSpacecraft, dtype=float) + for sc in range(numSpacecraft): + startIdx = sc * (6 + numJoints) + M_sc = M[startIdx:startIdx+6+numJoints, startIdx:startIdx+6+numJoints] + K_sc = Ktheta[sc*numJoints:(sc+1)*numJoints, sc*numJoints:(sc+1)*numJoints] + P_sc = Ptheta[sc*numJoints:(sc+1)*numJoints, sc*numJoints:(sc+1)*numJoints] + theta_sc = theta[0:numJoints] + thetaDot_sc = thetaDot[0:numJoints] + desTheta_sc = desTheta[sc*numJoints:(sc+1)*numJoints] + desThetaDot_sc = desThetaDot[sc*numJoints:(sc+1)*numJoints] + bias_sc = -reactionForces[startIdx:startIdx+6+numJoints] + + if maxTorque: + uMax_sc = uMax + else: + uMax_sc = None + + torques_sc = jointTorques(M_sc, K_sc, P_sc, theta_sc, thetaDot_sc, + desTheta_sc, desThetaDot_sc, bias_sc, maxTorque=uMax_sc) + expectedTorques[sc*numJoints:(sc+1)*numJoints] = torques_sc + + # Assert the motor torques are correct + np.testing.assert_allclose(moduleTorques, expectedTorques, rtol=1e-8, + err_msg=f"Motor torques {moduleTorques} do not match expected values {expectedTorques}") + +if __name__ == "__main__": + test_hingedJointArrayMotor(True, False, 1, 1) From b9758569806f6ee1c49cc9a5aa8c28ab90325179 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 5 Dec 2025 14:43:57 -0700 Subject: [PATCH 5/5] update release notes --- docs/source/Support/bskReleaseNotes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 1545321ebc..1f4d52f181 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -44,6 +44,7 @@ Version |release| - Added new module :ref:`MJSystemCoM` to extract the system center of mass position and velocity from a MuJoCo simulation. - Added new module :ref:`MJSystemMassMatrix` to extract the system mass matrix from a MuJoCo simulation. - Added new module :ref:`MJJointReactionForces` to extract the reaction forces and torques acting on the joints from a MuJoCo simulation. +- Added new module :ref:`hingedJointArrayMotor` to determine the motor torques for an array of hinged joints. - Refactored the CI build system scripts - Removed deprecated use of ``Basilisk.simulation.planetEphemeris.ClassicElementsMsgPayload``. Users need to use ``ClassicalElements()`` defined in ``orbitalMotion``.