From af451941dd96f746ebe9927a1553634d94c49f3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 5 Sep 2022 15:33:30 +0200 Subject: [PATCH] Added kinematic features common tests (#409) Signed-off-by: ahcorde --- dartsim/src/KinematicsFeatures_TEST.cc | 126 -------------- test/common_test/CMakeLists.txt | 1 + test/common_test/kinematic_features.cc | 157 ++++++++++++++++++ .../common_test}/worlds/string_pendulum.sdf | 0 4 files changed, 158 insertions(+), 126 deletions(-) delete mode 100644 dartsim/src/KinematicsFeatures_TEST.cc create mode 100644 test/common_test/kinematic_features.cc rename {dartsim => test/common_test}/worlds/string_pendulum.sdf (100%) diff --git a/dartsim/src/KinematicsFeatures_TEST.cc b/dartsim/src/KinematicsFeatures_TEST.cc deleted file mode 100644 index 632a3d877..000000000 --- a/dartsim/src/KinematicsFeatures_TEST.cc +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include - -#include - -#include -#include -#include - -#include - -// Features -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "test/Utils.hh" - -using namespace gz; - -using TestFeatureList = gz::physics::FeatureList< - physics::ForwardStep, - physics::GetEntities, - physics::JointFrameSemantics, - physics::LinkFrameSemantics, - physics::sdf::ConstructSdfModel, - physics::sdf::ConstructSdfWorld ->; - -using TestEnginePtr = physics::Engine3dPtr; - -class KinematicsFeaturesFixture : public ::testing::Test -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -// Test joint frame semantics -TEST_F(KinematicsFeaturesFixture, JointFrameSemantics) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "string_pendulum.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - ASSERT_NE(nullptr, world); - - auto model = world->GetModel("pendulum"); - ASSERT_NE(nullptr, model); - auto pivotJoint = model->GetJoint("pivot"); - ASSERT_NE(nullptr, pivotJoint); - auto childLink = model->GetLink("bob"); - ASSERT_NE(nullptr, childLink); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - for (std::size_t i = 0; i < 100; ++i) - { - world->Step(output, state, input); - } - // Pose of Child link (C) in Joint frame (J) - physics::Pose3d X_JC = physics::Pose3d::Identity(); - X_JC.translate(physics::Vector3d(0, 0, -1)); - - // Notation: Using F_WJ for the frame data of frame J (joint) relative to - // frame W (world). - auto F_WJ = pivotJoint->FrameDataRelativeToWorld(); - physics::FrameData3d F_WCexpected = F_WJ; - - physics::Vector3d pendulumArmInWorld = - F_WJ.pose.rotation() * X_JC.translation(); - - F_WCexpected.pose = F_WJ.pose * X_JC; - // angular acceleration of the child link is the same as the joint, so we - // don't need to assign a new value. - - // Note that the joint's linear velocity and linear acceleration are zero, so - // they are omitted here. - F_WCexpected.linearAcceleration = - F_WJ.angularAcceleration.cross(pendulumArmInWorld) + - F_WJ.angularVelocity.cross( - F_WJ.angularVelocity.cross(pendulumArmInWorld)); - - F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld); - - auto childLinkFrameData = childLink->FrameDataRelativeToWorld(); - EXPECT_TRUE( - physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6)); -} diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index d561e2a59..152128e7f 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -5,6 +5,7 @@ set(tests construct_empty_world free_joint_features joint_features + kinematic_features simulation_features ) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc new file mode 100644 index 000000000..19baff522 --- /dev/null +++ b/test/common_test/kinematic_features.cc @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include + +#include +#include + +#include "../helpers/TestLibLoader.hh" +#include "../Utils.hh" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +template +class KinematicFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(KinematicFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + // TODO(ahcorde): SKIP bullet, review this test again. + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) == "bullet") + { + GTEST_SKIP(); + } + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +struct KinematicFeaturesList : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::GetJointFromModel, + gz::physics::LinkFrameSemantics, + gz::physics::JointFrameSemantics +> { }; + +using KinematicFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(KinematicFeaturesTest, + KinematicFeaturesTestTypes); + +TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = + root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "string_pendulum.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("pendulum"); + ASSERT_NE(nullptr, model); + auto pivotJoint = model->GetJoint("pivot"); + ASSERT_NE(nullptr, pivotJoint); + auto childLink = model->GetLink("bob"); + ASSERT_NE(nullptr, childLink); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + for (std::size_t i = 0; i < 100; ++i) + { + world->Step(output, state, input); + } + // Pose of Child link (C) in Joint frame (J) + gz::physics::Pose3d X_JC = gz::physics::Pose3d::Identity(); + X_JC.translate(gz::physics::Vector3d(0, 0, -1)); + + // Notation: Using F_WJ for the frame data of frame J (joint) relative to + // frame W (world). + auto F_WJ = pivotJoint->FrameDataRelativeToWorld(); + gz::physics::FrameData3d F_WCexpected = F_WJ; + + gz::physics::Vector3d pendulumArmInWorld = + F_WJ.pose.rotation() * X_JC.translation(); + + F_WCexpected.pose = F_WJ.pose * X_JC; + // angular acceleration of the child link is the same as the joint, so we + // don't need to assign a new value. + + // Note that the joint's linear velocity and linear acceleration are zero, so + // they are omitted here. + F_WCexpected.linearAcceleration = + F_WJ.angularAcceleration.cross(pendulumArmInWorld) + + F_WJ.angularVelocity.cross( + F_WJ.angularVelocity.cross(pendulumArmInWorld)); + + F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld); + + auto childLinkFrameData = childLink->FrameDataRelativeToWorld(); + EXPECT_TRUE( + gz::physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6)); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!KinematicFeaturesTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/dartsim/worlds/string_pendulum.sdf b/test/common_test/worlds/string_pendulum.sdf similarity index 100% rename from dartsim/worlds/string_pendulum.sdf rename to test/common_test/worlds/string_pendulum.sdf