diff --git a/bullet-featherstone/src/KinematicFeatures.cc b/bullet-featherstone/src/KinematicFeatures.cc new file mode 100644 index 000000000..225bfc209 --- /dev/null +++ b/bullet-featherstone/src/KinematicFeatures.cc @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2024 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 "KinematicFeatures.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +#if BT_BULLET_VERSION >= 307 +///////////////////////////////////////////////// +void KinematicFeatures::SetLinkKinematic( + const Identity &_id, bool _kinematic) +{ + auto *link = this->ReferenceInterface(_id); + auto *model = this->ReferenceInterface(link->model); + + int collisionFlags = _kinematic ? btCollisionObject::CF_KINEMATIC_OBJECT : + btCollisionObject::CF_DYNAMIC_OBJECT; + + if (link->indexInModel.has_value()) + { + model->body->setLinkDynamicType(link->indexInModel.value(), collisionFlags); + if (_kinematic) + { + model->body->getLink( + link->indexInModel.value()).m_absFrameTotVelocity.setZero(); + model->body->getLink( + link->indexInModel.value()).m_absFrameLocVelocity.setZero(); + } + } + else + { + model->body->setBaseDynamicType(collisionFlags); + if (_kinematic) + { + model->body->clearVelocities(); + } + } +} + +///////////////////////////////////////////////// +bool KinematicFeatures::GetLinkKinematic(const Identity &_id) const +{ + auto *link = this->ReferenceInterface(_id); + auto *model = this->ReferenceInterface(link->model); + + int indexInModel = link->indexInModel.value_or(-1); + return model->body->isLinkKinematic(indexInModel); +} +#endif + +} // namespace bullet_featherstone +} // namespace physics +} // namespace gz diff --git a/bullet-featherstone/src/KinematicFeatures.hh b/bullet-featherstone/src/KinematicFeatures.hh new file mode 100644 index 000000000..cd98f593f --- /dev/null +++ b/bullet-featherstone/src/KinematicFeatures.hh @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2024 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. + * +*/ + +#ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_KINEMATICFEATURES_HH_ +#define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_KINEMATICFEATURES_HH_ + +#include + +#include "Base.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +#if BT_BULLET_VERSION >= 307 +struct KinematicFeatureList : FeatureList< + Kinematic +> { }; + +class KinematicFeatures : + public virtual Base, + public virtual Implements3d +{ + // ----- Set / Get Kinematic ----- + public: void SetLinkKinematic( + const Identity &_id, + bool _kinematic) override; + + public: bool GetLinkKinematic( + const Identity &_id) const override; +}; +#endif + +} // namespace bullet_featherstone +} // namespace physics +} // namespace gz + +#endif diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3ad3b3f5b..71184d3e6 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -880,6 +880,18 @@ Identity SDFFeatures::ConstructSdfModelImpl( // with AttachHeightmap. this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); } + +#if BT_BULLET_VERSION >= 307 + // Set kinematic mode + // Do this after adding collisions + if (linkSdf->Kinematic()) + { + auto *linkInfo = this->ReferenceInterface(linkID); + int indexInModel = linkInfo->indexInModel.value_or(-1); + model->body->setLinkDynamicType(indexInModel, + btCollisionObject::CF_KINEMATIC_OBJECT); + } +#endif } // Add the remaining links in the model without constructing the bullet diff --git a/bullet-featherstone/src/plugin.cc b/bullet-featherstone/src/plugin.cc index 46fcc8e13..6e7319fcc 100644 --- a/bullet-featherstone/src/plugin.cc +++ b/bullet-featherstone/src/plugin.cc @@ -25,6 +25,7 @@ #include "FreeGroupFeatures.hh" #include "ShapeFeatures.hh" #include "JointFeatures.hh" +#include "KinematicFeatures.hh" #include "KinematicsFeatures.hh" #include "LinkFeatures.hh" #include "SDFFeatures.hh" @@ -39,6 +40,9 @@ struct BulletFeatures : FeatureList < EntityManagementFeatureList, SimulationFeatureList, FreeGroupFeatureList, +#if BT_BULLET_VERSION >= 307 + KinematicFeatureList, +#endif KinematicsFeatureList, LinkFeatureList, SDFFeatureList, @@ -53,6 +57,9 @@ class Plugin : public virtual EntityManagementFeatures, public virtual SimulationFeatures, public virtual FreeGroupFeatures, +#if BT_BULLET_VERSION >= 307 + public virtual KinematicFeatures, +#endif public virtual KinematicsFeatures, public virtual LinkFeatures, public virtual SDFFeatures, diff --git a/include/gz/physics/Kinematic.hh b/include/gz/physics/Kinematic.hh new file mode 100644 index 000000000..cdb1669bc --- /dev/null +++ b/include/gz/physics/Kinematic.hh @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2024 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. + * + */ + +#ifndef GZ_PHYSICS_KINEMATIC_HH_ +#define GZ_PHYSICS_KINEMATIC_HH_ + +#include + +namespace gz +{ + namespace physics + { + ///////////////////////////////////////////////// + class GZ_PHYSICS_VISIBLE Kinematic + : public virtual Feature + { + /// \brief The Link API for setting link to be kinematic + public: template + class Link : public virtual Feature::Link + { + /// \brief Set link to be kinematic. + /// A kinematic link does not react to forces, e.g. gravity or other + /// dynamic objects. It reacts to pose or velocity commands that are + /// set on the link (via a FreeGroup) or on the joint that connects the + /// kinematic links. + /// \param[i] _kinematic True to make this link kinematic. + public: void SetKinematic(bool _kinematic); + + /// \brief Get whether this link is kinematic. + /// \return True if the link is kinematic, false otherwise. + /// \sa SetKinematic + public: bool GetKinematic() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + /// \brief Implementation API for setting a link to be kinematic + /// \param[in] _id Identity of the link + /// \param[in] _kinematic True to make this link kinematic + public: virtual void SetLinkKinematic( + const Identity &_shapeID, bool _kinematic) = 0; + + /// \brief Implementation API for getting whether a link is kinematic + /// \param[in] _id Identity of the link + /// \return True if the link is kinematic, false otherwise. + public: virtual bool GetLinkKinematic( + const Identity &_shapeID) const = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/detail/Kinematic.hh b/include/gz/physics/detail/Kinematic.hh new file mode 100644 index 000000000..feca1d660 --- /dev/null +++ b/include/gz/physics/detail/Kinematic.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2024 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. + * + */ + +#ifndef GZ_PHYSICS_DETAIL_KINEMATIC_HH_ +#define GZ_PHYSICS_DETAIL_KINEMATIC_HH_ + +#include +#include + +namespace gz +{ +namespace physics +{ +///////////////////////////////////////////////// +template +void Kinematic::Link::SetKinematic(bool _kinematic) +{ + this->template Interface() + ->SetLinkKinematic(this->identity, _kinematic); +} + +///////////////////////////////////////////////// +template +bool Kinematic::Link::GetKinematic() const +{ + return this->template Interface() + ->GetLinkKinematic(this->identity); +} +} // namespace physics +} // namespace gz + +#endif diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index effa773dc..0faa25095 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -16,7 +16,10 @@ */ #include +#include + #include +#include #include #include "test/TestLibLoader.hh" @@ -25,11 +28,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include #include @@ -166,6 +172,319 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) } } + +using SetKinematicFeaturesList = gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::ForwardStep, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::Kinematic, + gz::physics::LinkFrameSemantics +>; + +using SetKinematicTestFeaturesList = + KinematicFeaturesTest; + +TEST_F(SetKinematicTestFeaturesList, SetKinematic) +{ + // Test toggling a link between kinematic and dynamic type. + // When dynamic, the link should fall due to gravity. It should also + // stop falling when made kinematic again. + 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; + sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + const std::string modelStr = R"( + + + 0 0 10.0 0 0 0 + + true + + + + 0.1 + + + + + + )"; + + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("M1"); + ASSERT_NE(nullptr, model); + auto link = model->GetLink("link"); + ASSERT_NE(nullptr, link); + + // verify sphere initial state + gz::math::Pose3d initialPose(0, 0, 10, 0, 0, 0); + auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + // Step physics and verify sphere is at the same location because + // it is kinematic + double time = 1.0; + double stepSize = 0.001; + std::size_t steps = static_cast(time / stepSize); + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + // Make link dynamic and step + link->SetKinematic(false); + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + frameData = link->FrameDataRelativeToWorld(); + + // Verify that sphere is falling by checking its pos and vel + double gravity = -9.8; + double distZ = 0.5 * gravity; + double expectedPosZ = initialPose.Pos().Z() + distZ; + double expectedVelZ = gravity * time; + EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); + EXPECT_NEAR(expectedPosZ, + frameData.pose.translation().z(), 1e-2); + EXPECT_NEAR(0.0, frameData.linearVelocity.x(), 1e-3); + EXPECT_NEAR(0.0, frameData.linearVelocity.y(), 1e-3); + EXPECT_NEAR(expectedVelZ, frameData.linearVelocity.z(), 1e-2); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + // Make link kinematic again and step + link->SetKinematic(true); + + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + frameData = link->FrameDataRelativeToWorld(); + + // Verify the sphere did not move + EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); + EXPECT_NEAR(expectedPosZ, + frameData.pose.translation().z(), 1e-2); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + } +} + +TEST_F(SetKinematicTestFeaturesList, SetKinematicLinksWithJoint) +{ + // Load 2 kinematic links connected by a revolute joint. + // Make one of the links dynamic and verify its motion + 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; + sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + const std::string modelStr = R"( + + + 0 0 1.0 0 0 0 + + true + 0 0.25 0.0 1.57 0 0 + + + + 0.1 + 0.5 + + + + + + true + 0 -0.25 0.0 1.57 0 0 + + + + 0.1 + 0.5 + + + + + + 0 0 -0.25 0 0 0 + link1 + link2 + + 1.0 0 0 + + + + + )"; + + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("M1"); + ASSERT_NE(nullptr, model); + auto link1 = model->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto link2 = model->GetLink("link2"); + ASSERT_NE(nullptr, link2); + + // Verify links initial state + gz::math::Pose3d initialLink1Pose(0, 0.25, 1, 1.57, 0, 0); + auto frameData1 = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink1Pose, gz::math::eigen3::convert(frameData1.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.angularVelocity)); + gz::math::Pose3d initialLink2Pose(0, -0.25, 1, 1.57, 0, 0); + auto frameData2 = link2->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink2Pose, gz::math::eigen3::convert(frameData2.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.angularVelocity)); + + // Step physics and verify links are at the same location because they + // are kinematic + double time = 1.0; + double stepSize = 0.001; + std::size_t steps = static_cast(time / stepSize); + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + + frameData1 = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink1Pose, gz::math::eigen3::convert(frameData1.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.angularVelocity)); + frameData2 = link2->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink2Pose, gz::math::eigen3::convert(frameData2.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.angularVelocity)); + + // Make link2 dynamic and step + link2->SetKinematic(false); + + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + // Verify link1 remains still + frameData1 = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink1Pose, gz::math::eigen3::convert(frameData1.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.angularVelocity)); + + // Link2 should start rotating due to gravity + frameData2 = link2->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameData2.pose.translation().x(), 1e-3); + EXPECT_LT(initialLink2Pose.Y(), frameData2.pose.translation().y()); + EXPECT_GT(initialLink2Pose.Z(), frameData2.pose.translation().z()); + + // \todo(iche033) bullet-feathersone implementation does not return + // correct velocities for non-base links when they are attached to a parent + // base link that is either fixed to the world or kinematic + // see https://github.com/gazebosim/gz-physics/issues/617 + if (this->PhysicsEngineName(name) != "bullet-featherstone") + { + EXPECT_NEAR(0.0, frameData2.linearVelocity.x(), 1e-3); + EXPECT_LT(0.0, frameData2.linearVelocity.y()); + EXPECT_GT(0.0, frameData2.linearVelocity.z()); + EXPECT_LT(0.0, frameData2.angularVelocity.x()); + EXPECT_NEAR(0.0, frameData2.angularVelocity.y(), 1e-3); + EXPECT_NEAR(0.0, frameData2.angularVelocity.z(), 1e-3); + } + auto updatedLink2Pose = gz::math::eigen3::convert(frameData2.pose); + + // Make link2 kinematic again and step + link2->SetKinematic(true); + + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + + // Verify the links did not move + frameData1 = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(initialLink1Pose, gz::math::eigen3::convert(frameData1.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData1.angularVelocity)); + frameData2 = link2->FrameDataRelativeToWorld(); + EXPECT_EQ(updatedLink2Pose, gz::math::eigen3::convert(frameData2.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData2.angularVelocity)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv);