diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index f1ea1b2cd..0a60f5ff4 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -163,9 +163,167 @@ 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::SetJointMinPosition( + 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; + } +#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); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxPosition( + 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; + } +#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); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinVelocity( + 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; + } +#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); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMaxVelocity( + 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; + } +#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); +} + +///////////////////////////////////////////////// +void JointFeatures::SetJointMinEffort( + 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::SetJointMaxEffort( + 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..3cdb783fc 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -52,7 +52,10 @@ struct JointFeatureList : FeatureList< GetPrismaticJointProperties, AttachPrismaticJointFeature, - SetJointVelocityCommandFeature + SetJointVelocityCommandFeature, + SetJointPositionLimitsFeature, + SetJointVelocityLimitsFeature, + SetJointEffortLimitsFeature > { }; 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 SetJointMinPosition( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMaxPosition( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMinVelocity( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMaxVelocity( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + public: void SetJointMinEffort( + const Identity &_id, const std::size_t _dof, + const double _value) override; + + 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 3d30b2429..d8ff7d7de 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::SetJointPositionLimitsFeature, + physics::SetJointVelocityLimitsFeature, + physics::SetJointEffortLimitsFeature >; 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,475 @@ 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->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(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->SetMinPosition(0, pos - 0.5); + joint->SetMaxPosition(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->SetMinPosition(0, -math::INF_D); + joint->SetMaxPosition(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)); +} + +#if DART_VERSION_AT_LEAST(6, 10, 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->SetMinVelocity(0, -0.25); + joint->SetMaxVelocity(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->SetMinVelocity(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->SetMinVelocity(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->SetMinVelocity(0, -math::INF_D); + joint->SetMaxVelocity(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->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(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->SetMinEffort(0, -80); + joint->SetMaxEffort(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->SetMinEffort(0, -math::INF_D); + joint->SetMaxEffort(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->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) + { + 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->SetMinEffort(0, -500); + joint->SetMaxEffort(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->SetMinVelocity(0, -1); + joint->SetMaxVelocity(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->SetMinPosition(0, -1e6); + joint->SetMaxPosition(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 +// 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"); + 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->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(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); + } + } +} + +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->SetMinVelocity(0, -0.1); + joint->SetMaxVelocity(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); + + 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) + { + 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->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(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->SetMinEffort(0, -80); + joint->SetMaxEffort(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->SetMinEffort(0, -math::INF_D); + joint->SetMaxEffort(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->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) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -1e6); + joint->SetMaxEffort(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); +} +#endif + // Test detaching joints. TEST_F(JointFeaturesFixture, JointDetach) { 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); 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..005640ce9 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 SetJointPositionLimitsFeature + : 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 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 SetMinPosition( + const std::size_t _dof, const Scalar _value); + + /// \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 SetMaxPosition( + 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 SetJointMinPosition( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxPositionCommand above + public: virtual void SetJointMaxPosition( + 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 SetJointVelocityLimitsFeature + : 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 SetMinVelocity( + 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 SetMaxVelocity( + 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 SetJointMinVelocity( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxVelocityCommand above + 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 SetJointEffortLimitsFeature + : 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 SetMinEffort(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 SetMaxEffort(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 SetJointMinEffort( + const Identity &_id, std::size_t _dof, Scalar _value) = 0; + + // See Joint::SetMaxEffortCommand above + public: virtual void SetJointMaxEffort( + 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..e8ed197f3 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 SetJointPositionLimitsFeature::Joint:: + SetMinPosition(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinPosition(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointPositionLimitsFeature::Joint:: + SetMaxPosition(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxPosition(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointVelocityLimitsFeature::Joint:: + SetMinVelocity(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinVelocity(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointVelocityLimitsFeature::Joint:: + SetMaxVelocity(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxVelocity(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointEffortLimitsFeature::Joint:: + SetMinEffort(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMinEffort(this->identity, _dof, _value); + } + + ///////////////////////////////////////////////// + template + void SetJointEffortLimitsFeature::Joint:: + SetMaxEffort(const std::size_t _dof, const Scalar _value) + { + this->template Interface() + ->SetJointMaxEffort(this->identity, _dof, _value); + } + ///////////////////////////////////////////////// template void DetachJointFeature::Joint::Detach()