Adds Python bindings for the Link convenience class (#2039)

Signed-off-by: Voldivh <[email protected]> Signed-off-by: Michael Carroll <[email protected]> Signed-off-by: Eloy Briceno <[email protected]> Co-authored-by: Michael Carroll <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
gazebosim · Aug 16, 2023 · e9e2fca · e9e2fca
1 parent 5e40cb8
commit e9e2fca
Show file tree

Hide file tree

Showing 8 changed files with 377 additions and 8 deletions.
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
@@ -44,6 +44,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
   src/gz/sim/_gz_sim_pybind11.cc
   src/gz/sim/EntityComponentManager.cc
   src/gz/sim/EventManager.cc
+  src/gz/sim/Link.cc
   src/gz/sim/Model.cc
   src/gz/sim/TestFixture.cc
   src/gz/sim/Server.cc
@@ -87,6 +88,7 @@ endif()
 
 if (BUILD_TESTING)
   set(python_tests
+    link_TEST
     model_TEST
     testFixture_TEST
     world_TEST

diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2023 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 <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <iostream>
+
+#include "Link.hh"
+
+namespace py = pybind11;
+
+namespace gz
+{
+namespace sim
+{
+namespace python
+{
+void defineSimLink(py::object module)
+{
+  py::class_<gz::sim::Link>(module, "Link")
+  .def(py::init<gz::sim::Entity>())
+  .def(py::init<gz::sim::Link>())
+  .def("entity", &gz::sim::Link::Entity,
+      "Get the entity which this Link is related to.")
+  .def("reset_entity", &gz::sim::Link::ResetEntity,
+      "Reset Entity to a new one.")
+  .def("valid", &gz::sim::Link::Valid,
+      py::arg("ecm"),
+      "Check whether this link correctly refers to an entity that "
+      "has a components::Link.")
+  .def("name", &gz::sim::Link::Name,
+      py::arg("ecm"),
+      "Get the link's unscoped name.")
+  .def("parent_model", &gz::sim::Link::ParentModel,
+      py::arg("ecm"),
+      "Get the parent model.")
+  .def("is_canonical", &gz::sim::Link::IsCanonical,
+      py::arg("ecm"),
+      "Check if this is the canonical link.")
+  .def("wind_mode", &gz::sim::Link::WindMode,
+      py::arg("ecm"),
+      "Get whether this link has wind enabled.")
+  .def("collision_by_name", &gz::sim::Link::CollisionByName,
+      py::arg("ecm"),
+      py::arg("name"),
+      "Get the ID of a collision entity which is an immediate child of "
+      "this link.")
+  .def("visual_by_name", &gz::sim::Link::VisualByName,
+      py::arg("ecm"),
+      py::arg("name"),
+      "Get the ID of a visual entity which is an immediate child of "
+      "this link.")
+  .def("collisions", &gz::sim::Link::Collisions,
+      py::arg("ecm"),
+      "Get all collisions which are immediate children of this link.")
+  .def("visuals", &gz::sim::Link::Visuals,
+      py::arg("ecm"),
+      "Get all visuals which are immediate children of this link.")
+  .def("collision_count", &gz::sim::Link::CollisionCount,
+      py::arg("ecm"),
+      "Get the number of collisions which are immediate children of "
+      "this link.")
+  .def("visual_count", &gz::sim::Link::VisualCount,
+      py::arg("ecm"),
+      "Get the number of visuals which are immediate children of this "
+      "link.")
+  .def("world_pose", &gz::sim::Link::WorldPose,
+      py::arg("ecm"),
+      "Get the pose of the link frame in the world coordinate frame.")
+  .def("world_inertial_pose", &gz::sim::Link::WorldInertialPose,
+      py::arg("ecm"),
+      "Get the world pose of the link inertia.")
+  .def("world_linear_velocity",
+      py::overload_cast<const EntityComponentManager &>
+        (&gz::sim::Link::WorldLinearVelocity, py::const_),
+      py::arg("ecm"),
+      "Get the linear velocity at the origin of of the link frame "
+      "expressed in the world frame, using an offset expressed in a "
+      "body-fixed frame. If no offset is given, the velocity at the origin of "
+      "the Link frame will be returned.")
+  .def("world_linear_velocity",
+      py::overload_cast<const EntityComponentManager &, const math::Vector3d &>
+        (&gz::sim::Link::WorldLinearVelocity, py::const_),
+      py::arg("ecm"),
+      py::arg("offset"),
+      "Get the linear velocity of a point on the body in the world "
+      "frame, using an offset expressed in a body-fixed frame.")
+  .def("world_angular_velocity", &gz::sim::Link::WorldAngularVelocity,
+      py::arg("ecm"),
+      "Get the angular velocity of the link in the world frame.")
+  .def("enable_velocity_checks", &gz::sim::Link::EnableVelocityChecks,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report velocities for a link, so "
+      "functions like `world_linear_velocity` will return nullopt. This "
+      "function can be used to enable all velocity checks.")
+  .def("set_linear_velocity", &gz::sim::Link::SetLinearVelocity,
+      py::arg("ecm"),
+      py::arg("velocity"),
+      "Set the linear velocity on this link. If this is set, wrenches "
+      "on this link will be ignored for the current time step.")
+  .def("set_angular_velocity", &gz::sim::Link::SetAngularVelocity,
+      py::arg("ecm"),
+      py::arg("velocity"),
+      "Set the angular velocity on this link. If this is set, wrenches "
+      "on this link will be ignored for the current time step.")
+  .def("world_angular_acceleration", &gz::sim::Link::WorldAngularAcceleration,
+      py::arg("ecm"),
+      "Get the linear acceleration of the body in the world frame.")
+  .def("world_linear_acceleration", &gz::sim::Link::WorldLinearAcceleration,
+      py::arg("ecm"),
+      "Get the angular velocity of the link in the world frame.")
+  .def("enable_acceleration_checks", &gz::sim::Link::EnableAccelerationChecks,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report accelerations for a link, so "
+      "functions like `world_linear_acceleration` will return nullopt. This "
+      "function can be used to enable all acceleration checks.")
+  .def("world_inertia_matrix", &gz::sim::Link::WorldInertiaMatrix,
+      py::arg("ecm"),
+      "Get the inertia matrix in the world frame.")
+  .def("world_kinetic_energy", &gz::sim::Link::WorldKineticEnergy,
+      py::arg("ecm"),
+      "Get the rotational and translational kinetic energy of the "
+      "link with respect to the world frame.")
+  .def("add_world_force",
+      py::overload_cast<EntityComponentManager &, const math::Vector3d &>
+        (&gz::sim::Link::AddWorldForce, py::const_),
+      py::arg("ecm"),
+      py::arg("force"),
+      "Add a force expressed in world coordinates and applied at the "
+      "center of mass of the link.")
+  .def("add_world_force",
+      py::overload_cast<EntityComponentManager &,
+                        const math::Vector3d &,
+                        const math::Vector3d &>
+                        (&gz::sim::Link::AddWorldForce, py::const_),
+      py::arg("ecm"),
+      py::arg("force"),
+      py::arg("position"),
+      "Add a force expressed in world coordinates and applied at "
+      "an offset from the center of mass of the link.")
+  .def("add_world_wrench", &gz::sim::Link::AddWorldWrench,
+      py::arg("ecm"),
+      py::arg("force"),
+      py::arg("torque"),
+      "Add a wrench expressed in world coordinates and applied to "
+      "the link at the link's origin. This wrench is applied for one "
+      "simulation step.")
+  .def("__copy__",
+      [](const gz::sim::Link &self)
+      {
+        return gz::sim::Link(self);
+      })
+  .def("__deepcopy__",
+      [](const gz::sim::Link &self, pybind11::dict)
+      {
+        return gz::sim::Link(self);
+      });
+}
+}  // namespace python
+}  // namespace sim
+}  // namespace gz
diff --git a/python/src/gz/sim/Link.hh b/python/src/gz/sim/Link.hh
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2023 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_SIM_PYTHON__LINK_HH_
+#define GZ_SIM_PYTHON__LINK_HH_
+
+#include <pybind11/pybind11.h>
+
+#include <gz/sim/Link.hh>
+
+namespace gz
+{
+namespace sim
+{
+namespace python
+{
+/// Define a pybind11 wrapper for a gz::sim::Link
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineSimLink(pybind11::object module);
+}  // namespace python
+}  // namespace sim
+}  // namespace gz
+
+#endif  // GZ_SIM_PYTHON__Link_HH_
diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc
@@ -20,6 +20,7 @@
 
 #include "EntityComponentManager.hh"
 #include "EventManager.hh"
+#include "Link.hh"
 #include "Model.hh"
 #include "Server.hh"
 #include "ServerConfig.hh"
@@ -33,6 +34,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) {
   m.attr("K_NULL_ENTITY") = gz::sim::kNullEntity;
   gz::sim::python::defineSimEntityComponentManager(m);
   gz::sim::python::defineSimEventManager(m);
+  gz::sim::python::defineSimLink(m);
   gz::sim::python::defineSimModel(m);
   gz::sim::python::defineSimServer(m);
   gz::sim::python::defineSimServerConfig(m);

diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+# Copyright (C) 2023 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.
+
+import os
+import unittest
+
+from gz.common import set_verbosity
+from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity
+from gz_test_deps.math import Matrix3d, Vector3d
+
+class TestModel(unittest.TestCase):
+    post_iterations = 0
+    iterations = 0
+    pre_iterations = 0
+
+    def test_model(self):
+        set_verbosity(4)
+
+        file_path = os.path.dirname(os.path.realpath(__file__))
+        fixture = TestFixture(os.path.join(file_path, 'link_test.sdf'))
+
+        def on_post_udpate_cb(_info, _ecm):
+            self.post_iterations += 1
+
+        def on_pre_udpate_cb(_info, _ecm):
+            self.pre_iterations += 1
+            world_e = world_entity(_ecm)
+            self.assertNotEqual(K_NULL_ENTITY, world_e)
+            w = World(world_e)
+            m = Model(w.model_by_name(_ecm, 'model_test'))
+            link = Link(m.link_by_name(_ecm, 'link_test'))
+            # Entity Test
+            self.assertNotEqual(K_NULL_ENTITY, link.entity())
+            # Valid Test
+            self.assertTrue(link.valid(_ecm))
+            # Name Test
+            self.assertEqual('link_test', link.name(_ecm))
+            # Parent Model Test
+            self.assertEqual(m.entity(), link.parent_model(_ecm).entity())
+            # Canonical test
+            self.assertTrue(link.is_canonical(_ecm))
+            # Wind Mode test
+            self.assertTrue(link.wind_mode(_ecm))
+            # Collisions Test
+            self.assertNotEqual(K_NULL_ENTITY, link.collision_by_name(_ecm, 'collision_test'))
+            self.assertEqual(1, link.collision_count(_ecm))
+            # Visuals Test
+            self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test'))
+            self.assertEqual(1, link.visual_count(_ecm))
+            # World Pose Test
+            self.assertEqual(None, link.world_pose(_ecm))
+            self.assertEqual(None, link.world_inertial_pose(_ecm))
+            # World Velocity Test
+            self.assertEqual(None, link.world_linear_velocity(_ecm))
+            self.assertEqual(None, link.world_angular_velocity(_ecm))
+            link.enable_velocity_checks(_ecm, True)
+            link.set_linear_velocity(_ecm, Vector3d())
+            link.set_angular_velocity(_ecm, Vector3d())
+            self.assertEqual(Vector3d(), link.world_linear_velocity(_ecm))
+            self.assertEqual(Vector3d(), link.world_angular_velocity(_ecm))
+            # World Acceleration Test
+            self.assertEqual(None, link.world_linear_acceleration(_ecm))
+            self.assertEqual(None, link.world_angular_acceleration(_ecm))
+            link.enable_acceleration_checks(_ecm, True)
+            self.assertEqual(Vector3d(), link.world_linear_acceleration(_ecm))
+            self.assertEqual(Vector3d(), link.world_angular_acceleration(_ecm))
+            # World Inertia Matrix Test
+            self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertia_matrix(_ecm))
+            # World Kinetic Energy Test
+            self.assertEqual(0, link.world_kinetic_energy(_ecm))
+            link.enable_velocity_checks(_ecm, False)
+            link.enable_acceleration_checks(_ecm, False)
+
+
+        def on_udpate_cb(_info, _ecm):
+            self.iterations += 1
+
+        fixture.on_post_update(on_post_udpate_cb)
+        fixture.on_update(on_udpate_cb)
+        fixture.on_pre_update(on_pre_udpate_cb)
+        fixture.finalize()
+
+        server = fixture.server()
+        server.run(True, 2, False)
+
+        self.assertEqual(2, self.pre_iterations)
+        self.assertEqual(2, self.iterations)
+        self.assertEqual(2, self.post_iterations)
+
+if __name__ == '__main__':
+    unittest.main()