Adds bindings for the Joint convenience class

include/gz/sim/Joint.hh

@@ -260,7 +260,7 @@ namespace gz
       public: std::optional<std::vector<double>> Position(
           const EntityComponentManager &_ecm) const;
 
-      /// \brief Get the position of the joint
+      /// \brief Get the transmitted wrench of the joint
       /// \param[in] _ecm Entity-component manager.
       /// \return Transmitted wrench of the joint or nullopt if transmitted
       /// wrench check is not enabled.

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/Joint.cc
   src/gz/sim/Link.cc
   src/gz/sim/Model.cc
   src/gz/sim/TestFixture.cc
@@ -88,6 +89,7 @@ endif()
 
 if (BUILD_TESTING)
   set(python_tests
+    joint_TEST
     link_TEST
     model_TEST
     testFixture_TEST

python/src/gz/sim/Joint.cc

@@ -0,0 +1,153 @@
+/*
+ * 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 "Joint.hh"
+
+namespace py = pybind11;
+
+namespace gz
+{
+namespace sim
+{
+namespace python
+{
+void defineSimJoint(py::object module)
+{
+  py::class_<gz::sim::Joint>(module, "Joint")
+  .def(py::init<gz::sim::Entity>())
+  .def(py::init<gz::sim::Joint>())
+  .def("entity", &gz::sim::Joint::Entity,
+      "Get the entity which this Joint is related to.")
+  .def("reset_entity", &gz::sim::Joint::ResetEntity,
+      py::arg("new_entity"),
+      "Reset Entity to a new one.")
+  .def("valid", &gz::sim::Joint::Valid,
+      py::arg("ecm"),
+      "Check whether this joint correctly refers to an entity that"
+      "has a components::Joint.")
+  .def("name", &gz::sim::Joint::Name,
+      py::arg("ecm"),
+      "Get the joint's unscoped name.")
+  .def("parent_link_name", &gz::sim::Joint::ParentLinkName,
+      py::arg("ecm"),
+      "Get the parent link name.")
+  .def("child_link_name", &gz::sim::Joint::ChildLinkName,
+      py::arg("ecm"),
+      "Get the child link name.")
+  .def("pose", &gz::sim::Joint::Pose,
+      py::arg("ecm"),
+      "Get the pose of the joint.")
+  .def("thread_pitch", &gz::sim::Joint::ThreadPitch,
+      py::arg("ecm"),
+      "Get the thread pitch of the joint.")
+  .def("axis", &gz::sim::Joint::Axis,
+      py::arg("ecm"),
+      "Get the joint axis.")
+  .def("type", &gz::sim::Joint::Type,
+      py::arg("ecm"),
+      "Get the joint type.")
+  .def("sensor_by_name", &gz::sim::Joint::SensorByName,
+      py::arg("ecm"),
+      py::arg("name"),
+      "Get the ID of a sensor entity which is an immediate child of "
+      "this joint.")
+  .def("sensors", &gz::sim::Joint::Sensors,
+      py::arg("ecm"),
+      "Get all sensors which are immediate children of this joint.")
+  .def("sensor_count", &gz::sim::Joint::SensorCount,
+      py::arg("ecm"),
+      "Get the number of sensors which are immediate children of this "
+      "joint.")
+  .def("set_velocity", &gz::sim::Joint::SetVelocity,
+      py::arg("ecm"),
+      py::arg("velocities"),
+      "Set velocity on this joint. Only applied if no forces are set.")
+  .def("set_force", &gz::sim::Joint::SetForce,
+      py::arg("ecm"),
+      py::arg("forces"),
+      "Set force on this joint. If both forces and velocities are set, "
+      "only forces are applied.")
+  .def("set_velocity_limits", &gz::sim::Joint::SetVelocityLimits,
+      py::arg("ecm"),
+      py::arg("limits"),
+      "Set the velocity limits on a joint axis.")
+  .def("set_effort_limits", &gz::sim::Joint::SetEffortLimits,
+      py::arg("ecm"),
+      py::arg("limits"),
+      "Set the effort limits on a joint axis.")
+  .def("set_position_imits", &gz::sim::Joint::SetPositionLimits,
+      py::arg("ecm"),
+      py::arg("limits"),
+      "Set the position limits on a joint axis.")
+  .def("reset_position", &gz::sim::Joint::ResetPosition,
+      py::arg("ecm"),
+      py::arg("positions"),
+      "Reset the joint positions.")
+  .def("reset_velocity", &gz::sim::Joint::ResetVelocity,
+      py::arg("ecm"),
+      py::arg("velocities"),
+      "Reset the joint velocities.")
+  .def("enable_velocity_check", &gz::sim::Joint::EnableVelocityCheck,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report velocities for a joint, so "
+      "functions like `Velocity` will return nullopt. This "
+      "function can be used to enable joint velocity check.")
+  .def("enable_position_check", &gz::sim::Joint::EnablePositionCheck,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report positions for a joint, so "
+      "functions like `Position` will return nullopt. This "
+      "function can be used to enable joint position check.")
+  .def("enable_transmitted_wrench_check",
+      &gz::sim::Joint::EnableTransmittedWrenchCheck,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report transmitted wrench for a "
+      "joint, so functions like `TransmittedWrench` will return nullopt. This "
+      "function can be used to enable joint transmitted wrench check.")
+  .def("velocity", &gz::sim::Joint::Velocity,
+      py::arg("ecm"),
+      "Get the velocity of the joint.")
+  .def("position", &gz::sim::Joint::Position,
+      py::arg("ecm"),
+      "Get the position of the joint.")
+  .def("transmitted_wrench", &gz::sim::Joint::TransmittedWrench,
+      py::arg("ecm"),
+      "Get the transmitted wrench of the joint.")
+  .def("parent_model", &gz::sim::Joint::ParentModel,
+      py::arg("ecm"),
+      "Get the parent model of the joint.")
+  .def("__copy__",
+      [](const gz::sim::Joint &self)
+      {
+        return gz::sim::Joint(self);
+      })
+  .def("__deepcopy__",
+      [](const gz::sim::Joint &self, pybind11::dict)
+      {
+        return gz::sim::Joint(self);
+      });
+}
+}  // namespace python
+}  // namespace sim
+}  // namespace gz

python/src/gz/sim/Joint.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__JOINT_HH_
+#define GZ_SIM_PYTHON__JOINT_HH_
+
+#include <pybind11/pybind11.h>
+
+#include <gz/sim/Joint.hh>
+
+namespace gz
+{
+namespace sim
+{
+namespace python
+{
+/// Define a pybind11 wrapper for a gz::sim::Joint
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+defineSimJoint(pybind11::object module);
+}  // namespace python
+}  // namespace sim
+}  // namespace gz
+
+#endif  // GZ_SIM_PYTHON__JOINT_HH_

python/src/gz/sim/_gz_sim_pybind11.cc

@@ -20,6 +20,7 @@
 
 #include "EntityComponentManager.hh"
 #include "EventManager.hh"
+#include "Joint.hh"
 #include "Link.hh"
 #include "Model.hh"
 #include "Server.hh"
@@ -34,6 +35,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::defineSimJoint(m);
   gz::sim::python::defineSimLink(m);
   gz::sim::python::defineSimModel(m);
   gz::sim::python::defineSimServer(m);

python/test/joint_TEST.py

@@ -0,0 +1,98 @@
+#!/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,
+                              Joint, Model, World, world_entity)
+from gz_test_deps.math import Pose3d
+from gz_test_deps.sdformat import JointAxis, JointType
+
+
+class TestJoint(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, 'joint_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'))
+            joint = Joint(m.joint_by_name(_ecm, 'joint_test'))
+            # Entity Test
+            self.assertNotEqual(K_NULL_ENTITY, joint.entity())
+            # Valid Test
+            self.assertTrue(joint.valid(_ecm))
+            # Name Test
+            self.assertEqual('joint_test', joint.name(_ecm))
+            # Parent Link Name Test
+            self.assertEqual('link_test_1', joint.parent_link_name(_ecm))
+            # Child Link Name Test
+            self.assertEqual('link_test_2', joint.child_link_name(_ecm))
+            # Pose Test
+            self.assertEqual(Pose3d(0, 0.5, 0, 0, 0, 0), joint.pose(_ecm))
+            # Axis Test
+            self.assertEqual(JointAxis().xyz(), joint.axis(_ecm)[0].xyz())
+            # Type Test
+            self.assertEqual(JointType.REVOLUTE, joint.type(_ecm))
+            # Sensors Test
+            self.assertNotEqual(K_NULL_ENTITY,
+                                joint.sensor_by_name(_ecm, 'sensor_test'))
+            self.assertEqual(1, joint.sensor_count(_ecm))
+            # Velocity Test.
+            joint.enable_velocity_check(_ecm, True)
+            joint.set_velocity(_ecm, [10])
+            if self.pre_iterations == 0:
+                self.assertEqual(None, joint.velocity(_ecm))
+            elif self.pre_iterations > 1:
+                self.assertAlmostEqual(10, joint.velocity(_ecm)[0])
+            # Position Test
+            if self.pre_iterations <= 1:
+                self.assertEqual(None, joint.position(_ecm))
+                joint.enable_position_check(_ecm, True)
+            else:
+                self.assertNotEqual(None, joint.position(_ecm))
+
+        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()

python/test/joint_test.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+    <world name="world_test">
+        <model name="model_test">
+
+            <link name="link_test_1">
+            </link>
+
+            <link name="link_test_2">
+            </link>
+
+            <joint name="joint_test" type="revolute">
+                <parent>link_test_1</parent>
+                <child>link_test_2</child>
+                <pose>0 0.5 0 0 0 0</pose>
+                <axis>
+                    <xyz>0 0 1</xyz>
+                </axis>
+                <sensor type="force_torque" name="sensor_test">
+                </sensor>
+            </joint>
+        </model>
+    </world>
+</sdf>