diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 69b8db238..5534c1d87 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,5 +1,4 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @mxgrey -include/* @scpeters +* @azeey @mxgrey @scpeters diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml deleted file mode 100644 index 7d3439b03..000000000 --- a/bitbucket-pipelines.yml +++ /dev/null @@ -1,60 +0,0 @@ -image: ubuntu:bionic - -pipelines: - default: - - step: - script: - - apt-get update - - apt-get -y install cmake build-essential curl git cppcheck software-properties-common g++-8 ruby-dev - - update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 - - gcc -v - - g++ -v - - gcov -v - # lcov - - git clone https://github.com/linux-test-project/lcov.git - - cd lcov - # lcov was broken briefly (see https://github.com/linux-test-project/lcov/issues/55 ) - # checkout an explicit commit to avoid future regressions - - git checkout 04335632c371b5066e722298c9f8c6f11b210201 - - make install - - cd .. - # Ignition dependencies - - echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable bionic main" > /etc/apt/sources.list.d/gazebo-stable.list - - echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease bionic main" > /etc/apt/sources.list.d/gazebo-prerelease.list - - apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743 - - apt-get update - - apt-get -y install - libignition-cmake2-dev - libignition-common3-dev - libignition-math6-dev - libignition-math6-eigen3-dev - libignition-plugin-dev - libeigen3-dev - dart6-data - libdart6-collision-ode-dev - libdart6-dev - libdart6-utils-urdf-dev - libbenchmark-dev - libsdformat10-dev - # libsdformat (uncomment if a specific branch is needed) - #- apt install -y - # libtinyxml-dev - #- git clone http://github.com/osrf/sdformat -b spec2_prototype_9 - #- cd sdformat - #- mkdir build - #- cd build - #- cmake .. - #- make -j4 install - #- cd ../.. - # Ignition physics - - mkdir build - - cd build - - cmake .. -DCMAKE_BUILD_TYPE=coverage - - make - - export CTEST_OUTPUT_ON_FAILURE=1 - - make test - - make coverage - # Use a special version of codecov for handling gcc8 output. - - bash <(curl -s https://raw.githubusercontent.com/codecov/codecov-bash/4678d212cce2078bbaaf5027af0c0dafaad6a095/codecov) -X gcovout -X gcov - - make codecheck - - make install diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 1e0aa88f0..a22893328 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -66,6 +66,7 @@ struct LinkInfo struct JointInfo { dart::dynamics::JointPtr joint; + dart::dynamics::SimpleFramePtr frame; }; struct ShapeInfo @@ -304,6 +305,13 @@ class Base : public Implements3d> this->joints.idToObject[id] = std::make_shared(); this->joints.idToObject[id]->joint = _joint; this->joints.objectToID[_joint] = id; + dart::dynamics::SimpleFramePtr jointFrame = + dart::dynamics::SimpleFrame::createShared( + _joint->getChildBodyNode(), _joint->getName() + "_frame", + _joint->getTransformFromChildBodyNode()); + + this->joints.idToObject[id]->frame = jointFrame; + this->frames[id] = this->joints.idToObject[id]->frame.get(); return id; } diff --git a/dartsim/src/KinematicsFeatures.cc b/dartsim/src/KinematicsFeatures.cc index a93ca1f9e..3408bf13f 100644 --- a/dartsim/src/KinematicsFeatures.cc +++ b/dartsim/src/KinematicsFeatures.cc @@ -40,6 +40,16 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } const dart::dynamics::Frame *frame = SelectFrame(_id); + // A missing frame ID indicates that frame semantics is not properly + // implemented for the type of frame represented by the ID. + if (nullptr == frame) + { + ignerr << "The frame ID " << _id.ID() + << " was not found in the list of known frames. This should not be " + "possible! Please report this bug!\n"; + assert(false); + return data; + } data.pose = frame->getWorldTransform(); data.linearVelocity = frame->getLinearVelocity(); @@ -62,7 +72,13 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( return model_it->second->model->getRootBodyNode(); } - return this->frames.at(_id.ID()); + auto framesIt = this->frames.find(_id.ID()); + if (framesIt == this->frames.end()) + { + return nullptr; + } + + return framesIt->second; } } diff --git a/dartsim/src/KinematicsFeatures.hh b/dartsim/src/KinematicsFeatures.hh index ecb494dd6..490cb2af3 100644 --- a/dartsim/src/KinematicsFeatures.hh +++ b/dartsim/src/KinematicsFeatures.hh @@ -30,6 +30,7 @@ namespace dartsim { struct KinematicsFeatureList : FeatureList< LinkFrameSemantics, ShapeFrameSemantics, + JointFrameSemantics, FreeGroupFrameSemantics > { }; diff --git a/dartsim/src/KinematicsFeatures_TEST.cc b/dartsim/src/KinematicsFeatures_TEST.cc new file mode 100644 index 000000000..bb917bd78 --- /dev/null +++ b/dartsim/src/KinematicsFeatures_TEST.cc @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 3.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 ignition; + +using TestFeatureList = ignition::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 + { + ignition::plugin::Loader loader; + loader.LoadLib(dartsim_plugin_LIB); + + ignition::plugin::PluginPtr dartsim = + loader.Instantiate("ignition::physics::dartsim::Plugin"); + + this->engine = + ignition::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)); +} + +///////////////////////////////////////////////// +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/dartsim/worlds/string_pendulum.sdf b/dartsim/worlds/string_pendulum.sdf new file mode 100644 index 000000000..94b3ae39d --- /dev/null +++ b/dartsim/worlds/string_pendulum.sdf @@ -0,0 +1,73 @@ + + + + + 0 0 1 0 0 0 + + + world + support + + + + + 0.001 + + + + + + 0 0 0 0.7 0 0 + support + bob + + 1 0 0 + + + + + 0 0 -1 0 0 0 + + 6 + + 0.01 + 0.01 + 0.01 + + + + + + 0.1 + + + + 0.0 0.8 0.0 1 + 0.0 0.8 0.0 1 + + + + 0 0 0.5 0 0 0 + + + 0.01 + 1.0 + + + + 0.8 0.8 0.0 1 + 0.8 0.8 0.0 1 + + + + + + 1 + + + + + + + +