From 9f47486e2d6f4f8ebfc2fe9e0ed1f58535124267 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 19 Sep 2023 15:50:31 +0000 Subject: [PATCH] Dartsim joint features issues Signed-off-by: Michael Carroll --- dartsim/src/JointFeatures.cc | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 14c5fc7a4..aee0ec6a8 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -124,8 +124,21 @@ void JointFeatures::SetJointAcceleration( void JointFeatures::SetJointForce( const Identity &_id, std::size_t _dof, double _value) { + auto *jointInfo = this->ReferenceInterface(_id); + if (jointInfo == nullptr) + { + gzerr << "Invalid joint id value in SetJointForce" << std::endl; + return; + } + auto joint = this->ReferenceInterface(_id)->joint; + if (joint == nullptr) + { + gzerr << "Invalid joint in SetJointForce" << std::endl; + return; + } + // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail