From 548b94b766f487c86ecf4df0b8a732478863aec7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 20 Oct 2020 16:06:28 -0500 Subject: [PATCH 1/4] Add test showing collision failure when an invalid joint velocity is commanded Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures_TEST.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index f64e9aa7f..9dfeb1953 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -139,6 +140,17 @@ TEST_F(JointFeaturesFixture, JointSetCommand) world->Step(output, state, input); EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); } + + // Check that invalid velocity commands don't cause collisions to fail + const dart::dynamics::BodyNodePtr dartBaseLink = skeleton->getBodyNode("base"); + ASSERT_NE(nullptr, dartBaseLink ); + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); + // expect the position of the pendulum to stay aabove ground + world->Step(output, state, input); + EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); + } } // Test detaching joints. From 5a78ab42755fd42d00864696eb3ab3485ba66454 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 20 Oct 2020 16:07:32 -0500 Subject: [PATCH 2/4] Ignore invalid joint velocity commands Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures.cc | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index c15152d01..55a83b44c 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -102,6 +102,17 @@ void JointFeatures::SetJointVelocityCommand( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is finite since this value will be used in + // DART's constraint solver. A nan will cause the solver to fail, which will + // in turn cause collisions to fail + if (!std::isfinite(_value)) + { + ignerr << "Invalid joint velocity value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } if (joint->getActuatorType() != dart::dynamics::Joint::SERVO) { joint->setActuatorType(dart::dynamics::Joint::SERVO); From 60115c278d2f3df95b902b6fd8f8311837f5b8ae Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 20 Oct 2020 16:09:34 -0500 Subject: [PATCH 3/4] Codecheck Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures_TEST.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 9dfeb1953..e5235fbf0 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -142,8 +142,9 @@ TEST_F(JointFeaturesFixture, JointSetCommand) } // Check that invalid velocity commands don't cause collisions to fail - const dart::dynamics::BodyNodePtr dartBaseLink = skeleton->getBodyNode("base"); - ASSERT_NE(nullptr, dartBaseLink ); + const dart::dynamics::BodyNodePtr dartBaseLink = + skeleton->getBodyNode("base"); + ASSERT_NE(nullptr, dartBaseLink); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); From 6e532a0727f1995cc9835bd5db9d1a29fca8c7b5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 20 Oct 2020 16:42:49 -0500 Subject: [PATCH 4/4] Add checks for other JointFeatures::SetJoint* functions Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures.cc | 60 +++++++++++++++++++++++++++---- dartsim/src/JointFeatures_TEST.cc | 15 ++++++-- 2 files changed, 65 insertions(+), 10 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 55a83b44c..f1ea1b2cd 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -67,22 +67,57 @@ Pose3d JointFeatures::GetJointTransform(const Identity &_id) const void JointFeatures::SetJointPosition( const Identity &_id, const std::size_t _dof, const double _value) { - this->ReferenceInterface(_id)->joint->setPosition(_dof, _value); + auto joint = this->ReferenceInterface(_id)->joint; + + // 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 + if (!std::isfinite(_value)) + { + ignerr << "Invalid joint position value [" << _value << "] set on joint [" + << joint->getName() << " DOF " << _dof + << "]. The value will be ignored\n"; + return; + } + joint->setPosition(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointVelocity( const Identity &_id, const std::size_t _dof, const double _value) { - this->ReferenceInterface(_id)->joint->setVelocity(_dof, _value); + auto joint = this->ReferenceInterface(_id)->joint; + + // 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 + if (!std::isfinite(_value)) + { + ignerr << "Invalid joint velocity value [" << _value << "] set on joint [" + << joint->getName() << " DOF " << _dof + << "]. The value will be ignored\n"; + return; + } + joint->setVelocity(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointAcceleration( const Identity &_id, const std::size_t _dof, const double _value) { - this->ReferenceInterface(_id)->joint->setAcceleration(_dof, - _value); + auto joint = this->ReferenceInterface(_id)->joint; + + // 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 + if (!std::isfinite(_value)) + { + ignerr << "Invalid joint acceleration value [" << _value + << "] set on joint [" << joint->getName() << " DOF " << _dof + << "]. The value will be ignored\n"; + return; + } + joint->setAcceleration(_dof, _value); } ///////////////////////////////////////////////// @@ -90,6 +125,17 @@ void JointFeatures::SetJointForce( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; + + // 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 + if (!std::isfinite(_value)) + { + ignerr << "Invalid joint force value [" << _value << "] set on joint [" + << joint->getName() << " DOF " << _dof + << "]. The value will be ignored\n"; + return; + } if (joint->getActuatorType() != dart::dynamics::Joint::FORCE) { joint->setActuatorType(dart::dynamics::Joint::FORCE); @@ -103,9 +149,9 @@ void JointFeatures::SetJointVelocityCommand( { auto joint = this->ReferenceInterface(_id)->joint; - // Take extra care that the value is finite since this value will be used in - // DART's constraint solver. A nan will cause the solver to fail, which will - // in turn cause collisions to fail + // 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 if (!std::isfinite(_value)) { ignerr << "Invalid joint velocity value [" << _value diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index e5235fbf0..3d30b2429 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -59,6 +59,7 @@ using TestFeatureList = ignition::physics::FeatureList< physics::GetBasicJointState, physics::GetEntities, physics::RevoluteJointCast, + physics::SetBasicJointState, physics::SetJointVelocityCommandFeature, physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld @@ -106,6 +107,8 @@ TEST_F(JointFeaturesFixture, JointSetCommand) dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); + const auto *dartBaseLink = skeleton->getBodyNode("base"); + ASSERT_NE(nullptr, dartBaseLink); const auto *dartJoint = skeleton->getJoint(jointName); // Default actuatore type @@ -120,6 +123,15 @@ TEST_F(JointFeaturesFixture, JointSetCommand) world->Step(output, state, input); EXPECT_LT(joint->GetVelocity(0), 0.0); + // Check that invalid velocity commands don't cause collisions to fail + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetForce(0, std::numeric_limits::quiet_NaN()); + // expect the position of the pendulum to stay aabove ground + world->Step(output, state, input); + EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); + } + joint->SetVelocityCommand(0, 1); world->Step(output, state, input); // Setting a velocity command changes the actuator type to SERVO @@ -142,9 +154,6 @@ TEST_F(JointFeaturesFixture, JointSetCommand) } // Check that invalid velocity commands don't cause collisions to fail - const dart::dynamics::BodyNodePtr dartBaseLink = - skeleton->getBodyNode("base"); - ASSERT_NE(nullptr, dartBaseLink); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN());