From 8e9430c8d3d169a9b5cbe2e4a3f776751ef75ece Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 2 Jun 2021 00:55:34 +0200 Subject: [PATCH 1/6] Added DART feature for setting joint limits dynamically. Signed-off-by: Martin Pecka --- dartsim/src/JointFeatures.cc | 142 +++++++ dartsim/src/JointFeatures.hh | 29 +- dartsim/src/JointFeatures_TEST.cc | 468 ++++++++++++++++++++++- dartsim/worlds/test.world | 43 +++ include/ignition/physics/Joint.hh | 151 ++++++++ include/ignition/physics/detail/Joint.hh | 54 +++ 6 files changed, 884 insertions(+), 3 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index f1ea1b2cd..c4f206afb 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -163,9 +163,151 @@ void JointFeatures::SetJointVelocityCommand( { joint->setActuatorType(dart::dynamics::Joint::SERVO); } + // warn about bug https://github.com/dartsim/dart/issues/1583 + if ((joint->getPositionLowerLimit(_dof) > -1e16 || + joint->getPositionUpperLimit(_dof) < 1e16 ) && + (!std::isfinite(joint->getForceUpperLimit(_dof)) || + !std::isfinite(joint->getForceLowerLimit(_dof)))) + { + static bool informed = false; + if (!informed) + { + ignerr << "Velocity control does not respect positional limits of " + << "joints if these joints do not have an effort limit. Please, " + << "set min and max effort for joint [" << joint->getName() + << "] to values about -1e6 and 1e6 (or higher if working with " + << "heavy links)." << std::endl; + informed = true; + } + } + joint->setCommand(_dof, _value); } +///////////////////////////////////////////////// +void JointFeatures::SetJointMinPositionCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid minimum joint position value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + joint->setLimitEnforcement(true); + // We do not check min/max mismatch, we leave that to DART. + joint->setPositionLowerLimit(_dof, _value); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxPositionCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid maximum joint position value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + joint->setLimitEnforcement(true); + // We do not check min/max mismatch, we leave that to DART. + joint->setPositionUpperLimit(_dof, _value); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinVelocityCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid minimum joint velocity value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + joint->setLimitEnforcement(true); + // We do not check min/max mismatch, we leave that to DART. + joint->setVelocityLowerLimit(_dof, _value); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxVelocityCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid maximum joint velocity value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + joint->setLimitEnforcement(true); + // We do not check min/max mismatch, we leave that to DART. + joint->setVelocityUpperLimit(_dof, _value); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinEffortCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid minimum joint effort value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + // We do not check min/max mismatch, we leave that to DART. + joint->setForceLowerLimit(_dof, _value); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxEffortCommand( + const Identity &_id, const std::size_t _dof, const double _value) +{ + auto joint = this->ReferenceInterface(_id)->joint; + + // Take extra care that the value is valid. A nan can cause the DART + // constraint solver to fail, which will in turn either cause a crash or + // collisions to fail + if (std::isnan(_value)) + { + ignerr << "Invalid maximum joint effort value [" << _value + << "] commanded on joint [" << joint->getName() << " DOF " << _dof + << "]. The command will be ignored\n"; + return; + } + // We do not check min/max mismatch, we leave that to DART. + joint->setForceUpperLimit(_dof, _value); +} + ///////////////////////////////////////////////// std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const { diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 562319051..86609416d 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -52,7 +52,10 @@ struct JointFeatureList : FeatureList< GetPrismaticJointProperties, AttachPrismaticJointFeature, - SetJointVelocityCommandFeature + SetJointVelocityCommandFeature, + SetJointPositionLimitsCommandFeature, + SetJointVelocityLimitsCommandFeature, + SetJointEffortLimitsCommandFeature > { }; class JointFeatures : @@ -171,6 +174,30 @@ class JointFeatures : public: void SetJointVelocityCommand( const Identity &_id, const std::size_t _dof, const double _value) override; + + public: void SetJointMinPositionCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMaxPositionCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMinVelocityCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMaxVelocityCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMinEffortCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMaxEffortCommand( + const Identity &_id, const std::size_t _dof, + const double _value) override; }; } diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 3d30b2429..0aaf68b82 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -62,7 +62,10 @@ using TestFeatureList = ignition::physics::FeatureList< physics::SetBasicJointState, physics::SetJointVelocityCommandFeature, physics::sdf::ConstructSdfModel, - physics::sdf::ConstructSdfWorld + physics::sdf::ConstructSdfWorld, + physics::SetJointPositionLimitsCommandFeature, + physics::SetJointVelocityLimitsCommandFeature, + physics::SetJointEffortLimitsCommandFeature >; using TestEnginePtr = physics::Engine3dPtr; @@ -104,7 +107,7 @@ TEST_F(JointFeaturesFixture, JointSetCommand) ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = - dartWorld->getSkeleton(modelName); + dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); const auto *dartBaseLink = skeleton->getBodyNode("base"); @@ -163,6 +166,467 @@ TEST_F(JointFeaturesFixture, JointSetCommand) } } +TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPositionCommand(0, pos - 0.1); + joint->SetMaxPositionCommand(0, pos + 0.1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos - 0.1, joint->GetPosition(0), 1e-3); + + joint->SetMinPositionCommand(0, pos - 0.5); + joint->SetMaxPositionCommand(0, pos + 0.5); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.5, joint->GetPosition(0), 1e-2); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos - 0.5, joint->GetPosition(0), 1e-2); + + joint->SetMinPositionCommand(0, -math::INF_D); + joint->SetMaxPositionCommand(0, math::INF_D); + joint->SetPosition(0, pos); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_LT(pos + 0.5, joint->GetPosition(0)); +} + +TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + world->Step(output, state, input); + + joint->SetMinVelocityCommand(0, -0.25); + joint->SetMaxVelocityCommand(0, 0.5); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, -1000); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.25, joint->GetVelocity(0), 1e-6); + + // set minimum velocity above zero + joint->SetMinVelocityCommand(0, 0.25); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 0); + world->Step(output, state, input); + } + EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + // make sure the minimum velocity is kept even without velocity commands + world->Step(output, state, input); + } + EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocityCommand(0, -0.25); + joint->SetPosition(0, 0); + joint->SetVelocity(0, 0); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 0); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocityCommand(0, -math::INF_D); + joint->SetMaxVelocityCommand(0, math::INF_D); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_LT(0.5, joint->GetVelocity(0)); +} + +TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinEffortCommand(0, -1e-6); + joint->SetMaxEffortCommand(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffortCommand(0, -80); + joint->SetMaxEffortCommand(0, 80); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_LT(pos, joint->GetPosition(0)); + EXPECT_LT(0, joint->GetVelocity(0)); + + joint->SetMinEffortCommand(0, -math::INF_D); + joint->SetMaxEffortCommand(0, math::INF_D); + joint->SetPosition(0, 0); + joint->SetVelocity(0, 0); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_LT(pos, joint->GetPosition(0)); + EXPECT_LT(0, joint->GetVelocity(0)); +} + +TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPositionCommand(0, pos - 0.1); + joint->SetMaxPositionCommand(0, pos + 0.1); + joint->SetMinVelocityCommand(0, -0.25); + joint->SetMaxVelocityCommand(0, 0.5); + joint->SetMinEffortCommand(0, -1e-6); + joint->SetMaxEffortCommand(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffortCommand(0, -500); + joint->SetMaxEffortCommand(0, 1000); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + // 0.05 because we go 0.1 s with max speed 0.5 + EXPECT_NEAR(pos + 0.05, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 200; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetPosition(0, pos); + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + + joint->SetMinVelocityCommand(0, -1); + joint->SetMaxVelocityCommand(0, 1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); + + joint->SetPosition(0, pos); + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + + joint->SetMinPositionCommand(0, -1e6); + joint->SetMaxPositionCommand(0, 1e6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_LT(pos + 0.1, joint->GetPosition(0)); + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); +} + +// TODO(anyone): position limits do not work very well with velocity control +// bug https://github.com/dartsim/dart/issues/1583 +#if 0 +TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithVelocityControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"simple_joint_test"}; + const std::string jointName{"j1"}; + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto joint = model->GetJoint(jointName); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPositionCommand(0, pos - 0.1); + joint->SetMaxPositionCommand(0, pos + 0.1); + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + + if (i % 500 == 499) + { + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + } + } +} +#endif + +TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + joint->SetMinVelocityCommand(0, -0.1); + joint->SetMaxVelocityCommand(0, 0.1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, 0.1); + world->Step(output, state, input); + } + EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -0.025); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.025, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocityCommand(0, -math::INF_D); + joint->SetMaxVelocityCommand(0, math::INF_D); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); +} + +TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + joint->SetMinEffortCommand(0, -1e-6); + joint->SetMaxEffortCommand(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffortCommand(0, -80); + joint->SetMaxEffortCommand(0, 80); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-1, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffortCommand(0, -math::INF_D); + joint->SetMaxEffortCommand(0, math::INF_D); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(-100, joint->GetVelocity(0), 1e-6); +} + +TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) +{ + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + // Test joint velocity command + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + joint->SetMinVelocityCommand(0, -0.5); + joint->SetMaxVelocityCommand(0, 0.5); + joint->SetMinEffortCommand(0, -1e-6); + joint->SetMaxEffortCommand(0, 1e-6); + + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffortCommand(0, -1e6); + joint->SetMaxEffortCommand(0, 1e6); + + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); +} + // Test detaching joints. TEST_F(JointFeaturesFixture, JointDetach) { diff --git a/dartsim/worlds/test.world b/dartsim/worlds/test.world index 17bc75ab4..e45cca060 100644 --- a/dartsim/worlds/test.world +++ b/dartsim/worlds/test.world @@ -308,6 +308,49 @@ + + + 10 10 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 0 0 -1 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + 0 0 1 0 0 0 + base + bar + + 0 1 0 + + + diff --git a/include/ignition/physics/Joint.hh b/include/ignition/physics/Joint.hh index 24af4ebe6..ae60e6b51 100644 --- a/include/ignition/physics/Joint.hh +++ b/include/ignition/physics/Joint.hh @@ -366,6 +366,157 @@ namespace ignition }; }; + ///////////////////////////////////////////////// + /// \brief This feature sets the min and max generalized position of this + /// Joint. + class IGNITION_PHYSICS_VISIBLE SetJointPositionLimitsCommandFeature + : public virtual Feature + { + /// \brief The Joint API for setting position limits of a joint. + public: template + class Joint : public virtual Feature::Joint + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the minimum generalized coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The minimum generalized coordinate. Units depend on the underlying + /// joint type. + public: void SetMinPositionCommand( + const std::size_t _dof, const Scalar _value); + + /// \brief Set the maximum generalized coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The maximum generalized coordinate. Units depend on the underlying + /// joint type. + public: void SetMaxPositionCommand( + const std::size_t _dof, const Scalar _value); + }; + + /// \private The implementation API for setting position limit commands + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + // See Joint::SetMinPositionCommand above + public: virtual void SetJointMinPositionCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxPositionCommand above + public: virtual void SetJointMaxPositionCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the min and max value of generalized velocity + /// of this Joint. + class IGNITION_PHYSICS_VISIBLE SetJointVelocityLimitsCommandFeature + : public virtual Feature + { + /// \brief The Joint API for setting velocity limits of a joint. These + /// limits apply to joints commanded via velocity or positional commands. + public: template + class Joint : public virtual Feature::Joint + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the minimum value of generalized velocity of a specific + /// generalized coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The minimum generalized velocity. Units depend on the underlying + /// joint type. + public: void SetMinVelocityCommand( + const std::size_t _dof, const Scalar _value); + + /// \brief Set the maximum value of generalized velocity of a specific + /// generalized coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The maximum generalized velocity. Units depend on the underlying + /// joint type. + public: void SetMaxVelocityCommand( + const std::size_t _dof, const Scalar _value); + }; + + /// \private The implementation API for setting velocity limit commands + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + // See Joint::SetMinVelocityCommand above + public: virtual void SetJointMinVelocityCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxVelocityCommand above + public: virtual void SetJointMaxVelocityCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the min and max value of effort of this Joint. + class IGNITION_PHYSICS_VISIBLE SetJointEffortLimitsCommandFeature + : public virtual Feature + { + /// \brief The Joint API for setting effort limits of a joint. These + /// limits are applied to joints controlled via positional, velocity or + /// effort commands. + public: template + class Joint : public virtual Feature::Joint + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the minimum value of effort of a specific generalized + /// coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The minimum effort. Units depend on the underlying joint type. + public: void SetMinEffortCommand( + const std::size_t _dof, const Scalar _value); + + /// \brief Set the maximum value of effort of a specific generalized + /// coordinate within this joint. + /// \param[in] _dof + /// The desired generalized coordinate within this joint. Values start + /// from 0 and stop before Joint::GetDegreesOfFreedom(). + /// \param[in] _value + /// The maximum effort. Units depend on the underlying joint type. + public: void SetMaxEffortCommand( + const std::size_t _dof, const Scalar _value); + }; + + /// \private The implementation API for setting effort limit commands + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + // See Joint::SetMinEffortCommand above + public: virtual void SetJointMinEffortCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxEffortCommand above + public: virtual void SetJointMaxEffortCommand( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + }; + }; + class IGNITION_PHYSICS_VISIBLE DetachJointFeature : public virtual Feature { diff --git a/include/ignition/physics/detail/Joint.hh b/include/ignition/physics/detail/Joint.hh index 2bf543f80..2dfdec2ae 100644 --- a/include/ignition/physics/detail/Joint.hh +++ b/include/ignition/physics/detail/Joint.hh @@ -159,6 +159,60 @@ namespace ignition ->SetJointVelocityCommand(this->identity, _dof, _value); } + ///////////////////////////////////////////////// + template + void SetJointPositionLimitsCommandFeature::Joint:: + SetMinPositionCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinPositionCommand(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointPositionLimitsCommandFeature::Joint:: + SetMaxPositionCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxPositionCommand(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointVelocityLimitsCommandFeature::Joint:: + SetMinVelocityCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinVelocityCommand(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointVelocityLimitsCommandFeature::Joint:: + SetMaxVelocityCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxVelocityCommand(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointEffortLimitsCommandFeature::Joint:: + SetMinEffortCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinEffortCommand(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointEffortLimitsCommandFeature::Joint:: + SetMaxEffortCommand(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxEffortCommand(this->identity, _dof, _value); + } + ///////////////////////////////////////////////// template void DetachJointFeature::Joint::Detach() From e313e26dc58af36ee5457b4327ccd6edaaa31f40 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 2 Jun 2021 01:12:52 +0200 Subject: [PATCH 2/6] Make code compatible with older DART release (this commit can be reverted once newer DART is out). Signed-off-by: Martin Pecka --- dartsim/src/JointFeatures.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index c4f206afb..03c261a1c 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -200,7 +200,7 @@ void JointFeatures::SetJointMinPositionCommand( << "]. The command will be ignored\n"; return; } - joint->setLimitEnforcement(true); + joint->setPositionLimitEnforced(true); // We do not check min/max mismatch, we leave that to DART. joint->setPositionLowerLimit(_dof, _value); } @@ -221,7 +221,7 @@ void JointFeatures::SetJointMaxPositionCommand( << "]. The command will be ignored\n"; return; } - joint->setLimitEnforcement(true); + joint->setPositionLimitEnforced(true); // We do not check min/max mismatch, we leave that to DART. joint->setPositionUpperLimit(_dof, _value); } @@ -242,7 +242,7 @@ void JointFeatures::SetJointMinVelocityCommand( << "]. The command will be ignored\n"; return; } - joint->setLimitEnforcement(true); + joint->setPositionLimitEnforced(true); // We do not check min/max mismatch, we leave that to DART. joint->setVelocityLowerLimit(_dof, _value); } @@ -263,7 +263,7 @@ void JointFeatures::SetJointMaxVelocityCommand( << "]. The command will be ignored\n"; return; } - joint->setLimitEnforcement(true); + joint->setPositionLimitEnforced(true); // We do not check min/max mismatch, we leave that to DART. joint->setVelocityUpperLimit(_dof, _value); } From c8e60bee01bbe276be55548fc664461cc36cf78e Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 2 Jun 2021 01:27:23 +0200 Subject: [PATCH 3/6] Updated the number of models for test. Signed-off-by: Martin Pecka --- dartsim/src/SDFFeatures_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 58dbac8a6..ba7b7952c 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -98,7 +98,7 @@ TEST(SDFFeatures_TEST, CheckDartsimData) dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); - ASSERT_EQ(6u, dartWorld->getNumSkeletons()); + ASSERT_EQ(7u, dartWorld->getNumSkeletons()); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(1); ASSERT_NE(nullptr, skeleton); From e23f60c5256403e6645d7539a8ad0f609bbc65aa Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 12 Jul 2021 16:16:33 +0200 Subject: [PATCH 4/6] Apply suggestions from code review Signed-off-by: Martin Pecka Co-authored-by: Addisu Z. Taddese --- dartsim/src/JointFeatures.cc | 2 +- include/ignition/physics/Joint.hh | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 03c261a1c..f250e5fbb 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -200,7 +200,7 @@ void JointFeatures::SetJointMinPositionCommand( << "]. The command will be ignored\n"; return; } - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); // We do not check min/max mismatch, we leave that to DART. joint->setPositionLowerLimit(_dof, _value); } diff --git a/include/ignition/physics/Joint.hh b/include/ignition/physics/Joint.hh index ae60e6b51..50126f68c 100644 --- a/include/ignition/physics/Joint.hh +++ b/include/ignition/physics/Joint.hh @@ -378,22 +378,22 @@ namespace ignition { public: using Scalar = typename PolicyT::Scalar; - /// \brief Set the minimum generalized coordinate within this joint. + /// \brief Set the minimum allowed value of the generalized coordinate within this joint. /// \param[in] _dof /// The desired generalized coordinate within this joint. Values start /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value - /// The minimum generalized coordinate. Units depend on the underlying + /// The minimum allowed value of the generalized coordinate. Units depend on the underlying /// joint type. public: void SetMinPositionCommand( const std::size_t _dof, const Scalar _value); - /// \brief Set the maximum generalized coordinate within this joint. + /// \brief Set the maximum allowed value of the generalized coordinate within this joint. /// \param[in] _dof /// The desired generalized coordinate within this joint. Values start /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value - /// The maximum generalized coordinate. Units depend on the underlying + /// The maximum allowed value of the generalized coordinate. Units depend on the underlying /// joint type. public: void SetMaxPositionCommand( const std::size_t _dof, const Scalar _value); From 9288356f8e256d537f1daabc96509384b8da2b69 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 12 Jul 2021 16:49:15 +0200 Subject: [PATCH 5/6] Fixed issues from review. Signed-off-by: Martin Pecka --- dartsim/src/JointFeatures.cc | 18 ++-- dartsim/src/JointFeatures.hh | 18 ++-- dartsim/src/JointFeatures_TEST.cc | 114 ++++++++++++----------- include/ignition/physics/Joint.hh | 46 ++++----- include/ignition/physics/detail/Joint.hh | 48 +++++----- 5 files changed, 125 insertions(+), 119 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index f250e5fbb..c4f4e2bcb 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -185,7 +185,7 @@ void JointFeatures::SetJointVelocityCommand( } ///////////////////////////////////////////////// -void JointFeatures::SetJointMinPositionCommand( +void JointFeatures::SetJointMinPosition( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; @@ -206,7 +206,7 @@ void JointFeatures::SetJointMinPositionCommand( } ///////////////////////////////////////////////// -void JointFeatures::SetJointMaxPositionCommand( +void JointFeatures::SetJointMaxPosition( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; @@ -221,13 +221,13 @@ void JointFeatures::SetJointMaxPositionCommand( << "]. The command will be ignored\n"; return; } - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); // We do not check min/max mismatch, we leave that to DART. joint->setPositionUpperLimit(_dof, _value); } ///////////////////////////////////////////////// -void JointFeatures::SetJointMinVelocityCommand( +void JointFeatures::SetJointMinVelocity( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; @@ -242,13 +242,13 @@ void JointFeatures::SetJointMinVelocityCommand( << "]. The command will be ignored\n"; return; } - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); // We do not check min/max mismatch, we leave that to DART. joint->setVelocityLowerLimit(_dof, _value); } ///////////////////////////////////////////////// -void JointFeatures::SetJointMaxVelocityCommand( +void JointFeatures::SetJointMaxVelocity( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; @@ -263,13 +263,13 @@ void JointFeatures::SetJointMaxVelocityCommand( << "]. The command will be ignored\n"; return; } - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); // We do not check min/max mismatch, we leave that to DART. joint->setVelocityUpperLimit(_dof, _value); } ///////////////////////////////////////////////// -void JointFeatures::SetJointMinEffortCommand( +void JointFeatures::SetJointMinEffort( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; @@ -289,7 +289,7 @@ void JointFeatures::SetJointMinEffortCommand( } ///////////////////////////////////////////////// -void JointFeatures::SetJointMaxEffortCommand( +void JointFeatures::SetJointMaxEffort( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 86609416d..3cdb783fc 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -53,9 +53,9 @@ struct JointFeatureList : FeatureList< AttachPrismaticJointFeature, SetJointVelocityCommandFeature, - SetJointPositionLimitsCommandFeature, - SetJointVelocityLimitsCommandFeature, - SetJointEffortLimitsCommandFeature + SetJointPositionLimitsFeature, + SetJointVelocityLimitsFeature, + SetJointEffortLimitsFeature > { }; class JointFeatures : @@ -175,27 +175,27 @@ class JointFeatures : const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMinPositionCommand( + public: void SetJointMinPosition( const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMaxPositionCommand( + public: void SetJointMaxPosition( const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMinVelocityCommand( + public: void SetJointMinVelocity( const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMaxVelocityCommand( + public: void SetJointMaxVelocity( const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMinEffortCommand( + public: void SetJointMinEffort( const Identity &_id, const std::size_t _dof, const double _value) override; - public: void SetJointMaxEffortCommand( + public: void SetJointMaxEffort( const Identity &_id, const std::size_t _dof, const double _value) override; }; diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 0aaf68b82..e7e368844 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -63,9 +63,9 @@ using TestFeatureList = ignition::physics::FeatureList< physics::SetJointVelocityCommandFeature, physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld, - physics::SetJointPositionLimitsCommandFeature, - physics::SetJointVelocityLimitsCommandFeature, - physics::SetJointEffortLimitsCommandFeature + physics::SetJointPositionLimitsFeature, + physics::SetJointVelocityLimitsFeature, + physics::SetJointEffortLimitsFeature >; using TestEnginePtr = physics::Engine3dPtr; @@ -184,8 +184,8 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) auto pos = joint->GetPosition(0); - joint->SetMinPositionCommand(0, pos - 0.1); - joint->SetMaxPositionCommand(0, pos + 0.1); + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); for (std::size_t i = 0; i < 100; ++i) { @@ -201,8 +201,8 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) } EXPECT_NEAR(pos - 0.1, joint->GetPosition(0), 1e-3); - joint->SetMinPositionCommand(0, pos - 0.5); - joint->SetMaxPositionCommand(0, pos + 0.5); + joint->SetMinPosition(0, pos - 0.5); + joint->SetMaxPosition(0, pos + 0.5); for (std::size_t i = 0; i < 300; ++i) { @@ -218,8 +218,8 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) } EXPECT_NEAR(pos - 0.5, joint->GetPosition(0), 1e-2); - joint->SetMinPositionCommand(0, -math::INF_D); - joint->SetMaxPositionCommand(0, math::INF_D); + joint->SetMinPosition(0, -math::INF_D); + joint->SetMaxPosition(0, math::INF_D); joint->SetPosition(0, pos); for (std::size_t i = 0; i < 300; ++i) @@ -246,8 +246,8 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) world->Step(output, state, input); - joint->SetMinVelocityCommand(0, -0.25); - joint->SetMaxVelocityCommand(0, 0.5); + joint->SetMinVelocity(0, -0.25); + joint->SetMaxVelocity(0, 0.5); for (std::size_t i = 0; i < 10; ++i) { @@ -264,7 +264,7 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) EXPECT_NEAR(-0.25, joint->GetVelocity(0), 1e-6); // set minimum velocity above zero - joint->SetMinVelocityCommand(0, 0.25); + joint->SetMinVelocity(0, 0.25); for (std::size_t i = 0; i < 10; ++i) { @@ -280,7 +280,7 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) } EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); - joint->SetMinVelocityCommand(0, -0.25); + joint->SetMinVelocity(0, -0.25); joint->SetPosition(0, 0); joint->SetVelocity(0, 0); @@ -291,8 +291,8 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - joint->SetMinVelocityCommand(0, -math::INF_D); - joint->SetMaxVelocityCommand(0, math::INF_D); + joint->SetMinVelocity(0, -math::INF_D); + joint->SetMaxVelocity(0, math::INF_D); for (std::size_t i = 0; i < 10; ++i) { @@ -320,8 +320,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) auto pos = joint->GetPosition(0); - joint->SetMinEffortCommand(0, -1e-6); - joint->SetMaxEffortCommand(0, 1e-6); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { @@ -339,8 +339,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - joint->SetMinEffortCommand(0, -80); - joint->SetMaxEffortCommand(0, 80); + joint->SetMinEffort(0, -80); + joint->SetMaxEffort(0, 80); for (std::size_t i = 0; i < 100; ++i) { @@ -350,8 +350,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) EXPECT_LT(pos, joint->GetPosition(0)); EXPECT_LT(0, joint->GetVelocity(0)); - joint->SetMinEffortCommand(0, -math::INF_D); - joint->SetMaxEffortCommand(0, math::INF_D); + joint->SetMinEffort(0, -math::INF_D); + joint->SetMaxEffort(0, math::INF_D); joint->SetPosition(0, 0); joint->SetVelocity(0, 0); @@ -382,12 +382,12 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) auto pos = joint->GetPosition(0); - joint->SetMinPositionCommand(0, pos - 0.1); - joint->SetMaxPositionCommand(0, pos + 0.1); - joint->SetMinVelocityCommand(0, -0.25); - joint->SetMaxVelocityCommand(0, 0.5); - joint->SetMinEffortCommand(0, -1e-6); - joint->SetMaxEffortCommand(0, 1e-6); + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); + joint->SetMinVelocity(0, -0.25); + joint->SetMaxVelocity(0, 0.5); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { @@ -405,8 +405,8 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - joint->SetMinEffortCommand(0, -500); - joint->SetMaxEffortCommand(0, 1000); + joint->SetMinEffort(0, -500); + joint->SetMaxEffort(0, 1000); for (std::size_t i = 0; i < 100; ++i) { @@ -428,8 +428,8 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) joint->SetPosition(0, pos); EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - joint->SetMinVelocityCommand(0, -1); - joint->SetMaxVelocityCommand(0, 1); + joint->SetMinVelocity(0, -1); + joint->SetMaxVelocity(0, 1); for (std::size_t i = 0; i < 100; ++i) { @@ -442,8 +442,8 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) joint->SetPosition(0, pos); EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - joint->SetMinPositionCommand(0, -1e6); - joint->SetMaxPositionCommand(0, 1e6); + joint->SetMinPosition(0, -1e6); + joint->SetMaxPosition(0, 1e6); for (std::size_t i = 0; i < 100; ++i) { @@ -456,8 +456,8 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) // TODO(anyone): position limits do not work very well with velocity control // bug https://github.com/dartsim/dart/issues/1583 -#if 0 -TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithVelocityControl) +// resolved in DART 6.11.0 +TEST_F(JointFeaturesFixture, DISABLED_JointSetPositionLimitsWithVelocityControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); @@ -479,8 +479,8 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithVelocityControl) auto pos = joint->GetPosition(0); - joint->SetMinPositionCommand(0, pos - 0.1); - joint->SetMaxPositionCommand(0, pos + 0.1); + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, 1); @@ -493,7 +493,6 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithVelocityControl) } } } -#endif TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) { @@ -509,8 +508,8 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) physics::ForwardStep::State state; physics::ForwardStep::Input input; - joint->SetMinVelocityCommand(0, -0.1); - joint->SetMaxVelocityCommand(0, 0.1); + joint->SetMinVelocity(0, -0.1); + joint->SetMaxVelocity(0, 0.1); for (std::size_t i = 0; i < 100; ++i) { @@ -533,8 +532,15 @@ TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) } EXPECT_NEAR(-0.025, joint->GetVelocity(0), 1e-6); - joint->SetMinVelocityCommand(0, -math::INF_D); - joint->SetMaxVelocityCommand(0, math::INF_D); + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.1, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocity(0, -math::INF_D); + joint->SetMaxVelocity(0, math::INF_D); for (std::size_t i = 0; i < 100; ++i) { @@ -558,8 +564,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) physics::ForwardStep::State state; physics::ForwardStep::Input input; - joint->SetMinEffortCommand(0, -1e-6); - joint->SetMaxEffortCommand(0, 1e-6); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { @@ -568,8 +574,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - joint->SetMinEffortCommand(0, -80); - joint->SetMaxEffortCommand(0, 80); + joint->SetMinEffort(0, -80); + joint->SetMaxEffort(0, 80); for (std::size_t i = 0; i < 100; ++i) { @@ -578,8 +584,8 @@ TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) } EXPECT_NEAR(-1, joint->GetVelocity(0), 1e-6); - joint->SetMinEffortCommand(0, -math::INF_D); - joint->SetMaxEffortCommand(0, math::INF_D); + joint->SetMinEffort(0, -math::INF_D); + joint->SetMaxEffort(0, math::INF_D); for (std::size_t i = 0; i < 10; ++i) { @@ -604,10 +610,10 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) physics::ForwardStep::State state; physics::ForwardStep::Input input; - joint->SetMinVelocityCommand(0, -0.5); - joint->SetMaxVelocityCommand(0, 0.5); - joint->SetMinEffortCommand(0, -1e-6); - joint->SetMaxEffortCommand(0, 1e-6); + joint->SetMinVelocity(0, -0.5); + joint->SetMaxVelocity(0, 0.5); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 1000; ++i) { @@ -616,8 +622,8 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - joint->SetMinEffortCommand(0, -1e6); - joint->SetMaxEffortCommand(0, 1e6); + joint->SetMinEffort(0, -1e6); + joint->SetMaxEffort(0, 1e6); for (std::size_t i = 0; i < 1000; ++i) { diff --git a/include/ignition/physics/Joint.hh b/include/ignition/physics/Joint.hh index 50126f68c..005640ce9 100644 --- a/include/ignition/physics/Joint.hh +++ b/include/ignition/physics/Joint.hh @@ -369,7 +369,7 @@ namespace ignition ///////////////////////////////////////////////// /// \brief This feature sets the min and max generalized position of this /// Joint. - class IGNITION_PHYSICS_VISIBLE SetJointPositionLimitsCommandFeature + class IGNITION_PHYSICS_VISIBLE SetJointPositionLimitsFeature : public virtual Feature { /// \brief The Joint API for setting position limits of a joint. @@ -378,24 +378,26 @@ namespace ignition { public: using Scalar = typename PolicyT::Scalar; - /// \brief Set the minimum allowed value of the generalized coordinate within this joint. + /// \brief Set the minimum allowed value of the generalized coordinate + /// within this joint. /// \param[in] _dof /// The desired generalized coordinate within this joint. Values start /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value - /// The minimum allowed value of the generalized coordinate. Units depend on the underlying - /// joint type. - public: void SetMinPositionCommand( + /// The minimum allowed value of the generalized coordinate. Units + /// depend on the underlying joint type. + public: void SetMinPosition( const std::size_t _dof, const Scalar _value); - /// \brief Set the maximum allowed value of the generalized coordinate within this joint. + /// \brief Set the maximum allowed value of the generalized coordinate + /// within this joint. /// \param[in] _dof /// The desired generalized coordinate within this joint. Values start /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value - /// The maximum allowed value of the generalized coordinate. Units depend on the underlying - /// joint type. - public: void SetMaxPositionCommand( + /// The maximum allowed value of the generalized coordinate. Units + /// depend on the underlying joint type. + public: void SetMaxPosition( const std::size_t _dof, const Scalar _value); }; @@ -406,11 +408,11 @@ namespace ignition public: using Scalar = typename PolicyT::Scalar; // See Joint::SetMinPositionCommand above - public: virtual void SetJointMinPositionCommand( + public: virtual void SetJointMinPosition( const Identity &_id, std::size_t _dof, Scalar _value) = 0; // See Joint::SetMaxPositionCommand above - public: virtual void SetJointMaxPositionCommand( + public: virtual void SetJointMaxPosition( const Identity &_id, std::size_t _dof, Scalar _value) = 0; }; }; @@ -418,7 +420,7 @@ namespace ignition ///////////////////////////////////////////////// /// \brief This feature sets the min and max value of generalized velocity /// of this Joint. - class IGNITION_PHYSICS_VISIBLE SetJointVelocityLimitsCommandFeature + class IGNITION_PHYSICS_VISIBLE SetJointVelocityLimitsFeature : public virtual Feature { /// \brief The Joint API for setting velocity limits of a joint. These @@ -436,7 +438,7 @@ namespace ignition /// \param[in] _value /// The minimum generalized velocity. Units depend on the underlying /// joint type. - public: void SetMinVelocityCommand( + public: void SetMinVelocity( const std::size_t _dof, const Scalar _value); /// \brief Set the maximum value of generalized velocity of a specific @@ -447,7 +449,7 @@ namespace ignition /// \param[in] _value /// The maximum generalized velocity. Units depend on the underlying /// joint type. - public: void SetMaxVelocityCommand( + public: void SetMaxVelocity( const std::size_t _dof, const Scalar _value); }; @@ -458,18 +460,18 @@ namespace ignition public: using Scalar = typename PolicyT::Scalar; // See Joint::SetMinVelocityCommand above - public: virtual void SetJointMinVelocityCommand( + public: virtual void SetJointMinVelocity( const Identity &_id, std::size_t _dof, Scalar _value) = 0; // See Joint::SetMaxVelocityCommand above - public: virtual void SetJointMaxVelocityCommand( + public: virtual void SetJointMaxVelocity( const Identity &_id, std::size_t _dof, Scalar _value) = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature sets the min and max value of effort of this Joint. - class IGNITION_PHYSICS_VISIBLE SetJointEffortLimitsCommandFeature + class IGNITION_PHYSICS_VISIBLE SetJointEffortLimitsFeature : public virtual Feature { /// \brief The Joint API for setting effort limits of a joint. These @@ -487,8 +489,7 @@ namespace ignition /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value /// The minimum effort. Units depend on the underlying joint type. - public: void SetMinEffortCommand( - const std::size_t _dof, const Scalar _value); + public: void SetMinEffort(const std::size_t _dof, const Scalar _value); /// \brief Set the maximum value of effort of a specific generalized /// coordinate within this joint. @@ -497,8 +498,7 @@ namespace ignition /// from 0 and stop before Joint::GetDegreesOfFreedom(). /// \param[in] _value /// The maximum effort. Units depend on the underlying joint type. - public: void SetMaxEffortCommand( - const std::size_t _dof, const Scalar _value); + public: void SetMaxEffort(const std::size_t _dof, const Scalar _value); }; /// \private The implementation API for setting effort limit commands @@ -508,11 +508,11 @@ namespace ignition public: using Scalar = typename PolicyT::Scalar; // See Joint::SetMinEffortCommand above - public: virtual void SetJointMinEffortCommand( + public: virtual void SetJointMinEffort( const Identity &_id, std::size_t _dof, Scalar _value) = 0; // See Joint::SetMaxEffortCommand above - public: virtual void SetJointMaxEffortCommand( + public: virtual void SetJointMaxEffort( const Identity &_id, std::size_t _dof, Scalar _value) = 0; }; }; diff --git a/include/ignition/physics/detail/Joint.hh b/include/ignition/physics/detail/Joint.hh index 2dfdec2ae..e8ed197f3 100644 --- a/include/ignition/physics/detail/Joint.hh +++ b/include/ignition/physics/detail/Joint.hh @@ -161,56 +161,56 @@ namespace ignition ///////////////////////////////////////////////// template - void SetJointPositionLimitsCommandFeature::Joint:: - SetMinPositionCommand(const std::size_t _dof, const Scalar _value) + void SetJointPositionLimitsFeature::Joint:: + SetMinPosition(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMinPositionCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMinPosition(this->identity, _dof, _value); } ///////////////////////////////////////////////// template - void SetJointPositionLimitsCommandFeature::Joint:: - SetMaxPositionCommand(const std::size_t _dof, const Scalar _value) + void SetJointPositionLimitsFeature::Joint:: + SetMaxPosition(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMaxPositionCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMaxPosition(this->identity, _dof, _value); } ///////////////////////////////////////////////// template - void SetJointVelocityLimitsCommandFeature::Joint:: - SetMinVelocityCommand(const std::size_t _dof, const Scalar _value) + void SetJointVelocityLimitsFeature::Joint:: + SetMinVelocity(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMinVelocityCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMinVelocity(this->identity, _dof, _value); } ///////////////////////////////////////////////// template - void SetJointVelocityLimitsCommandFeature::Joint:: - SetMaxVelocityCommand(const std::size_t _dof, const Scalar _value) + void SetJointVelocityLimitsFeature::Joint:: + SetMaxVelocity(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMaxVelocityCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMaxVelocity(this->identity, _dof, _value); } ///////////////////////////////////////////////// template - void SetJointEffortLimitsCommandFeature::Joint:: - SetMinEffortCommand(const std::size_t _dof, const Scalar _value) + void SetJointEffortLimitsFeature::Joint:: + SetMinEffort(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMinEffortCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMinEffort(this->identity, _dof, _value); } ///////////////////////////////////////////////// template - void SetJointEffortLimitsCommandFeature::Joint:: - SetMaxEffortCommand(const std::size_t _dof, const Scalar _value) + void SetJointEffortLimitsFeature::Joint:: + SetMaxEffort(const std::size_t _dof, const Scalar _value) { - this->template Interface() - ->SetJointMaxEffortCommand(this->identity, _dof, _value); + this->template Interface() + ->SetJointMaxEffort(this->identity, _dof, _value); } ///////////////////////////////////////////////// From 4984ceca07eac786607ec3275066b28a17271afd Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 22 Jul 2021 12:35:53 -0500 Subject: [PATCH 6/6] Accomodate DART 6.9 Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures.cc | 16 ++++++++++++++++ dartsim/src/JointFeatures_TEST.cc | 2 ++ 2 files changed, 18 insertions(+) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index c4f4e2bcb..0a60f5ff4 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -200,7 +200,11 @@ void JointFeatures::SetJointMinPosition( << "]. The command will be ignored\n"; return; } +#if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); +#else + joint->setPositionLimitEnforced(true); +#endif // We do not check min/max mismatch, we leave that to DART. joint->setPositionLowerLimit(_dof, _value); } @@ -221,7 +225,11 @@ void JointFeatures::SetJointMaxPosition( << "]. The command will be ignored\n"; return; } +#if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); +#else + joint->setPositionLimitEnforced(true); +#endif // We do not check min/max mismatch, we leave that to DART. joint->setPositionUpperLimit(_dof, _value); } @@ -242,7 +250,11 @@ void JointFeatures::SetJointMinVelocity( << "]. The command will be ignored\n"; return; } +#if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); +#else + joint->setPositionLimitEnforced(true); +#endif // We do not check min/max mismatch, we leave that to DART. joint->setVelocityLowerLimit(_dof, _value); } @@ -263,7 +275,11 @@ void JointFeatures::SetJointMaxVelocity( << "]. The command will be ignored\n"; return; } +#if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); +#else + joint->setPositionLimitEnforced(true); +#endif // We do not check min/max mismatch, we leave that to DART. joint->setVelocityUpperLimit(_dof, _value); } diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index e7e368844..d8ff7d7de 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -230,6 +230,7 @@ TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) EXPECT_LT(pos + 0.5, joint->GetPosition(0)); } +#if DART_VERSION_AT_LEAST(6, 10, 0) TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) { sdf::Root root; @@ -632,6 +633,7 @@ TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) } EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); } +#endif // Test detaching joints. TEST_F(JointFeaturesFixture, JointDetach)