diff --git a/include/gz/sim/Joint.hh b/include/gz/sim/Joint.hh index 8de460c544..302da37fdd 100644 --- a/include/gz/sim/Joint.hh +++ b/include/gz/sim/Joint.hh @@ -260,7 +260,7 @@ namespace gz public: std::optional> 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. diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2f25e50cd9..45d103278a 100644 --- 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/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 diff --git a/python/src/gz/sim/Joint.cc b/python/src/gz/sim/Joint.cc new file mode 100644 index 0000000000..307289ec44 --- /dev/null +++ b/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 +#include + +#include + +#include "Joint.hh" + +namespace py = pybind11; + +namespace gz +{ +namespace sim +{ +namespace python +{ +void defineSimJoint(py::object module) +{ + py::class_(module, "Joint") + .def(py::init()) + .def(py::init()) + .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 diff --git a/python/src/gz/sim/Joint.hh b/python/src/gz/sim/Joint.hh new file mode 100644 index 0000000000..4fc179eb26 --- /dev/null +++ b/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 + +#include + +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_ diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc index 5df2e17f69..60a13b93f4 100644 --- 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 "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); diff --git a/python/test/joint_TEST.py b/python/test/joint_TEST.py new file mode 100755 index 0000000000..78e15b0dc1 --- /dev/null +++ b/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() diff --git a/python/test/joint_test.sdf b/python/test/joint_test.sdf new file mode 100644 index 0000000000..f4c780c35b --- /dev/null +++ b/python/test/joint_test.sdf @@ -0,0 +1,24 @@ + + + + + + + + + + + + + link_test_1 + link_test_2 + 0 0.5 0 0 0 0 + + 0 0 1 + + + + + + +