Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ignore invalid joint commands #137

Merged
merged 4 commits into from
Oct 21, 2020
Merged

Ignore invalid joint commands #137

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
548b94b
Add test showing collision failure when an invalid joint velocity is …
azeey Oct 20, 2020
5a78ab4
Ignore invalid joint velocity commands
azeey Oct 20, 2020
60115c2
Codecheck
azeey Oct 20, 2020
6e532a0
Add checks for other JointFeatures::SetJoint* functions
azeey Oct 20, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 61 additions & 4 deletions dartsim/src/JointFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,29 +67,75 @@ Pose3d JointFeatures::GetJointTransform(const Identity &_id) const
void JointFeatures::SetJointPosition(
const Identity &_id, const std::size_t _dof, const double _value)
{
this->ReferenceInterface<JointInfo>(_id)->joint->setPosition(_dof, _value);
auto joint = this->ReferenceInterface<JointInfo>(_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<JointInfo>(_id)->joint->setVelocity(_dof, _value);
auto joint = this->ReferenceInterface<JointInfo>(_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<JointInfo>(_id)->joint->setAcceleration(_dof,
_value);
auto joint = this->ReferenceInterface<JointInfo>(_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);
}

/////////////////////////////////////////////////
void JointFeatures::SetJointForce(
const Identity &_id, const std::size_t _dof, const double _value)
{
auto joint = this->ReferenceInterface<JointInfo>(_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);
Expand All @@ -102,6 +148,17 @@ void JointFeatures::SetJointVelocityCommand(
const Identity &_id, const std::size_t _dof, const double _value)
{
auto joint = this->ReferenceInterface<JointInfo>(_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
<< "] 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);
Expand Down
22 changes: 22 additions & 0 deletions dartsim/src/JointFeatures_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
#include <ignition/physics/sdf/ConstructModel.hh>
#include <ignition/physics/sdf/ConstructWorld.hh>

#include <limits>
#include <sdf/Model.hh>
#include <sdf/Root.hh>
#include <sdf/World.hh>
Expand All @@ -58,6 +59,7 @@ using TestFeatureList = ignition::physics::FeatureList<
physics::GetBasicJointState,
physics::GetEntities,
physics::RevoluteJointCast,
physics::SetBasicJointState,
physics::SetJointVelocityCommandFeature,
physics::sdf::ConstructSdfModel,
physics::sdf::ConstructSdfWorld
Expand Down Expand Up @@ -105,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
Expand All @@ -119,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<double>::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
Expand All @@ -139,6 +152,15 @@ 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
for (std::size_t i = 0; i < 1000; ++i)
{
joint->SetVelocityCommand(0, std::numeric_limits<double>::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.
Expand Down