Dartsim joint features issues

Signed-off-by: Michael Carroll <[email protected]>
gazebosim · Sep 19, 2023 · 9f47486 · 9f47486
1 parent e4a12af
commit 9f47486
Showing 1 changed file with 13 additions and 0 deletions.
diff --git 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<JointInfo>(_id);
+  if (jointInfo == nullptr)
+  {
+    gzerr << "Invalid joint id value in SetJointForce" << std::endl;
+    return;
+  }
+
   auto joint = this->ReferenceInterface<JointInfo>(_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