From 062deb776cd453bc8e3d1dc20b865f726db19a4a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 5 Apr 2024 18:45:53 +0000 Subject: [PATCH 1/6] Add Kinematic feature Signed-off-by: Ian Chen --- bullet-featherstone/src/KinematicFeatures.cc | 69 ++++ bullet-featherstone/src/KinematicFeatures.hh | 52 +++ bullet-featherstone/src/SDFFeatures.cc | 9 + bullet-featherstone/src/plugin.cc | 3 + include/gz/physics/Kinematic.hh | 65 ++++ include/gz/physics/detail/Kinematic.hh | 46 +++ test/common_test/kinematic_features.cc | 316 +++++++++++++++++++ 7 files changed, 560 insertions(+) create mode 100644 bullet-featherstone/src/KinematicFeatures.cc create mode 100644 bullet-featherstone/src/KinematicFeatures.hh create mode 100644 include/gz/physics/Kinematic.hh create mode 100644 include/gz/physics/detail/Kinematic.hh 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 cd5d0d6dd..69efc8e4b 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -879,6 +879,15 @@ Identity SDFFeatures::ConstructSdfModelImpl( // with AttachHeightmap. this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); } + + // 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); + } } // 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..82468b8b8 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,7 @@ struct BulletFeatures : FeatureList < EntityManagementFeatureList, SimulationFeatureList, FreeGroupFeatureList, + KinematicFeatureList, KinematicsFeatureList, LinkFeatureList, SDFFeatureList, @@ -53,6 +55,7 @@ class Plugin : public virtual EntityManagementFeatures, public virtual SimulationFeatures, public virtual FreeGroupFeatures, + public virtual KinematicFeatures, 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..d2348e778 --- /dev/null +++ b/include/gz/physics/Kinematic.hh @@ -0,0 +1,65 @@ +/* + * 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. + /// \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. + 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..42f1dc584 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 @@ -146,6 +152,7 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) EXPECT_EQ( F_WCexpected.pose.rotation(), childLinkFrameData.pose.rotation()); + // TODO(ahcorde): Rewiew this in bullet-featherstone if(this->PhysicsEngineName(name) == "bullet_featherstone") { @@ -166,6 +173,315 @@ 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 + 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) +{ + 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); From 6a4862a860d9aff27407a05c3ac3dc3248fbe7a9 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 5 Apr 2024 21:46:46 +0000 Subject: [PATCH 2/6] add doc to test Signed-off-by: Ian Chen --- test/common_test/kinematic_features.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 42f1dc584..0faa25095 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -152,7 +152,6 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) EXPECT_EQ( F_WCexpected.pose.rotation(), childLinkFrameData.pose.rotation()); - // TODO(ahcorde): Rewiew this in bullet-featherstone if(this->PhysicsEngineName(name) == "bullet_featherstone") { @@ -189,7 +188,9 @@ using SetKinematicTestFeaturesList = TEST_F(SetKinematicTestFeaturesList, SetKinematic) { - // Test toggling a link between kinematic and dynamic type + // 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; @@ -307,6 +308,8 @@ TEST_F(SetKinematicTestFeaturesList, SetKinematic) 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; From 6030c1c48347f8455d42f43924f6c3c845933d71 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 5 Apr 2024 22:11:38 +0000 Subject: [PATCH 3/6] lint Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 69efc8e4b..d886c772d 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -880,14 +880,17 @@ Identity SDFFeatures::ConstructSdfModelImpl( 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); + model->body->setLinkDynamicType(indexInModel, + btCollisionObject::CF_KINEMATIC_OBJECT); } +#endif } // Add the remaining links in the model without constructing the bullet From d175730c06571bd47e7773a652f196a13cc10a55 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 6 Apr 2024 00:48:26 +0000 Subject: [PATCH 4/6] fix build Signed-off-by: Ian Chen --- bullet-featherstone/src/plugin.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bullet-featherstone/src/plugin.cc b/bullet-featherstone/src/plugin.cc index 82468b8b8..6e7319fcc 100644 --- a/bullet-featherstone/src/plugin.cc +++ b/bullet-featherstone/src/plugin.cc @@ -40,7 +40,9 @@ struct BulletFeatures : FeatureList < EntityManagementFeatureList, SimulationFeatureList, FreeGroupFeatureList, +#if BT_BULLET_VERSION >= 307 KinematicFeatureList, +#endif KinematicsFeatureList, LinkFeatureList, SDFFeatureList, @@ -55,7 +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, From c2e2f407981fa60f67c7aa2d709b87208a9ce7f0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Jul 2024 17:39:40 +0000 Subject: [PATCH 5/6] expand API doc Signed-off-by: Ian Chen --- include/gz/physics/Kinematic.hh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/gz/physics/Kinematic.hh b/include/gz/physics/Kinematic.hh index d2348e778..a1ba94573 100644 --- a/include/gz/physics/Kinematic.hh +++ b/include/gz/physics/Kinematic.hh @@ -33,11 +33,15 @@ namespace gz 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; }; From 25fa2e420ac255c7463558e52e167d6a63e2d669 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Jul 2024 17:44:03 +0000 Subject: [PATCH 6/6] line wrap Signed-off-by: Ian Chen --- include/gz/physics/Kinematic.hh | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/gz/physics/Kinematic.hh b/include/gz/physics/Kinematic.hh index a1ba94573..cdb1669bc 100644 --- a/include/gz/physics/Kinematic.hh +++ b/include/gz/physics/Kinematic.hh @@ -33,9 +33,10 @@ namespace gz 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. + /// 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);