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 + + + + + + + +