bullet-featherstone: Add Kinematic feature

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<LinkInfo>(_id);
+  auto *model = this->ReferenceInterface<ModelInfo>(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<LinkInfo>(_id);
+  auto *model = this->ReferenceInterface<ModelInfo>(link->model);
+
+  int indexInModel = link->indexInModel.value_or(-1);
+  return model->body->isLinkKinematic(indexInModel);
+}
+#endif
+
+}  // namespace bullet_featherstone
+}  // namespace physics
+}  // namespace gz

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 <gz/physics/Kinematic.hh>
+
+#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<KinematicFeatureList>
+{
+  // ----- 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

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<LinkInfo>(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

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,

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 <gz/physics/FeatureList.hh>
+
+namespace gz
+{
+  namespace physics
+  {
+    /////////////////////////////////////////////////
+    class GZ_PHYSICS_VISIBLE Kinematic
+      : public virtual Feature
+    {
+      /// \brief The Link API for setting link to be kinematic
+      public: template <typename PolicyT, typename FeaturesT>
+      class Link : public virtual Feature::Link<PolicyT, FeaturesT>
+      {
+        /// \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 <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        /// \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 <gz/physics/detail/Kinematic.hh>
+
+#endif

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 <gz/physics/Kinematic.hh>
+#include <gz/physics/FeatureList.hh>
+
+namespace gz
+{
+namespace physics
+{
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Kinematic::Link<PolicyT, FeaturesT>::SetKinematic(bool _kinematic)
+{
+  this->template Interface<Kinematic>()
+      ->SetLinkKinematic(this->identity, _kinematic);
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+bool Kinematic::Link<PolicyT, FeaturesT>::GetKinematic() const
+{
+  return this->template Interface<Kinematic>()
+      ->GetLinkKinematic(this->identity);
+}
+}  // namespace physics
+}  // namespace gz
+
+#endif