From 6dd09844c4ec9d340a857f97c0e06be44d282de1 Mon Sep 17 00:00:00 2001 From: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Date: Tue, 22 Aug 2023 09:26:47 -0500 Subject: [PATCH] Adds Python bindings for the Actor convenience class (#2041) Signed-off-by: Voldivh Signed-off-by: Michael Carroll Signed-off-by: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Co-authored-by: Michael Carroll --- include/gz/sim/Actor.hh | 2 +- python/CMakeLists.txt | 2 + python/src/gz/sim/Actor.cc | 105 ++++++++++++++++++++++++++ python/src/gz/sim/Actor.hh | 40 ++++++++++ python/src/gz/sim/_gz_sim_pybind11.cc | 2 + python/test/actor_TEST.py | 94 +++++++++++++++++++++++ python/test/actor_test.sdf | 12 +++ 7 files changed, 256 insertions(+), 1 deletion(-) create mode 100644 python/src/gz/sim/Actor.cc create mode 100644 python/src/gz/sim/Actor.hh create mode 100755 python/test/actor_TEST.py create mode 100644 python/test/actor_test.sdf diff --git a/include/gz/sim/Actor.hh b/include/gz/sim/Actor.hh index 2cace92808..f9497eef14 100644 --- a/include/gz/sim/Actor.hh +++ b/include/gz/sim/Actor.hh @@ -110,7 +110,7 @@ namespace gz /// Manually setting the trajectory pose will override the scripted /// trajectory specified in SDF. /// \param[in] _ecm Entity Component manager. - /// \param[in] _name Trajectory pose w.r.t. to the trajectory origin + /// \param[in] _pose Trajectory pose w.r.t. to the trajectory origin /// \sa Pose public: void SetTrajectoryPose(EntityComponentManager &_ecm, const math::Pose3d &_pose); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 8abdd85254..2fa8ee7e91 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -42,6 +42,7 @@ endfunction() set(BINDINGS_MODULE_NAME "sim${PROJECT_VERSION_MAJOR}") pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/gz/sim/_gz_sim_pybind11.cc + src/gz/sim/Actor.cc src/gz/sim/EntityComponentManager.cc src/gz/sim/EventManager.cc src/gz/sim/Joint.cc @@ -90,6 +91,7 @@ endif() if (BUILD_TESTING) set(python_tests + actor_TEST joint_TEST link_TEST model_TEST diff --git a/python/src/gz/sim/Actor.cc b/python/src/gz/sim/Actor.cc new file mode 100644 index 0000000000..686bd46643 --- /dev/null +++ b/python/src/gz/sim/Actor.cc @@ -0,0 +1,105 @@ +/* + * 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 + +#include "Actor.hh" + +namespace py = pybind11; + +namespace gz +{ +namespace sim +{ +namespace python +{ +void defineSimActor(py::object module) +{ + py::class_(module, "Actor") + .def(py::init()) + .def(py::init()) + .def("entity", &gz::sim::Actor::Entity, + "Get the entity which this actor is related to.") + .def("reset_entity", &gz::sim::Actor::ResetEntity, + "Reset Entity to a new one.") + .def("valid", &gz::sim::Actor::Valid, + py::arg("ecm"), + "Check whether this actor correctly refers to an entity that " + "has a components::Actor.") + .def("name", &gz::sim::Actor::Name, + py::arg("ecm"), + "Get the actor's unscoped name.") + .def("pose", &gz::sim::Actor::Pose, + py::arg("ecm"), + "Get the pose of the actor." + "If the actor has a trajectory, this will only return the origin " + "pose of the trajectory and not the actual world pose of the actor.") + .def("trajectory_pose", &gz::sim::Actor::TrajectoryPose, + py::arg("ecm"), + "Get the trajectory pose of the actor. There are two " + "ways that the actor can follow a trajectory: 1) SDF script, " + "2) manually setting trajectory pose. This function retrieves 2) the " + "manual trajectory pose set by the user. The Trajectory pose is " + "given relative to the trajectory pose origin returned by Pose().") + .def("set_trajectory_pose", &gz::sim::Actor::SetTrajectoryPose, + py::arg("ecm"), + py::arg("pose"), + "Set the trajectory pose of the actor. There are two " + "ways that the actor can follow a trajectory: 1) SDF script, " + "2) manually setting trajectory pose. This function enables option 2). " + "Manually setting the trajectory pose will override the scripted " + "trajectory specified in SDF.") + .def("world_pose", &gz::sim::Actor::WorldPose, + py::arg("ecm"), + "Get the world pose of the actor." + "This returns the current world pose of the actor computed by gazebo." + "The world pose is the combination of the actor's pose and its " + "trajectory pose. The currently trajectory pose is either manually set " + "via set_trajectory_pose or interpolated from waypoints in the SDF " + "script based on the current time.") + .def("set_animation_name", &gz::sim::Actor::SetAnimationName, + py::arg("ecm"), + py::arg("name"), + "Set the name of animation to use for this actor.") + .def("set_animation_time", &gz::sim::Actor::SetAnimationTime, + py::arg("ecm"), + py::arg("time"), + "Set the time of animation to use for this actor (the time argument " + "is expected in ms).") + .def("animation_name", &gz::sim::Actor::AnimationName, + py::arg("ecm"), + "Get the name of animation used by the actor.") + .def("animation_time", &gz::sim::Actor::AnimationTime, + py::arg("ecm"), + "Get the time of animation for this actor.") + .def("__copy__", + [](const gz::sim::Actor &self) + { + return gz::sim::Actor(self); + }) + .def("__deepcopy__", + [](const gz::sim::Actor &self, pybind11::dict) + { + return gz::sim::Actor(self); + }); +} +} // namespace python +} // namespace sim +} // namespace gz diff --git a/python/src/gz/sim/Actor.hh b/python/src/gz/sim/Actor.hh new file mode 100644 index 0000000000..7095343e5f --- /dev/null +++ b/python/src/gz/sim/Actor.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__ACTOR_HH_ +#define GZ_SIM_PYTHON__ACTOR_HH_ + +#include + +#include + +namespace gz +{ +namespace sim +{ +namespace python +{ +/// Define a pybind11 wrapper for a gz::sim::Actor +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineSimActor(pybind11::object module); +} // namespace python +} // namespace sim +} // namespace gz + +#endif // GZ_SIM_PYTHON__ACTOR_HH_ diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc index c509948c2a..9ac3ae9dfe 100644 --- a/python/src/gz/sim/_gz_sim_pybind11.cc +++ b/python/src/gz/sim/_gz_sim_pybind11.cc @@ -18,6 +18,7 @@ #include "gz/sim/Entity.hh" +#include "Actor.hh" #include "EntityComponentManager.hh" #include "EventManager.hh" #include "Joint.hh" @@ -34,6 +35,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "Gazebo Sim Python Library."; m.attr("K_NULL_ENTITY") = gz::sim::kNullEntity; + gz::sim::python::defineSimActor(m); gz::sim::python::defineSimEntityComponentManager(m); gz::sim::python::defineSimEventManager(m); gz::sim::python::defineSimJoint(m); diff --git a/python/test/actor_TEST.py b/python/test/actor_TEST.py new file mode 100755 index 0000000000..2471cf2a56 --- /dev/null +++ b/python/test/actor_TEST.py @@ -0,0 +1,94 @@ +#!/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 datetime +import unittest + +from gz.common import set_verbosity +from gz_test_deps.sim import (Actor, K_NULL_ENTITY, + TestFixture, World, world_entity) +from gz_test_deps.math import Pose3d + + +class TestActor(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, 'actor_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) + actor = Actor(w.actor_by_name(_ecm, 'actor_test')) + # Entity Test + self.assertNotEqual(K_NULL_ENTITY, actor.entity()) + # Valid Test + self.assertTrue(actor.valid(_ecm)) + # Name Test + self.assertEqual('actor_test', actor.name(_ecm)) + # Pose Test + self.assertEqual(Pose3d(1, 1, 0, 0, 0, 0), actor.pose(_ecm)) + # Trajectory Pose Test + if self.pre_iterations == 0: + self.assertEqual(None, actor.trajectory_pose(_ecm)) + actor.set_trajectory_pose(_ecm, Pose3d(2, 2, 0, 0, 0, 0)) + self.assertEqual(Pose3d(2, 2, 0, 0, 0, 0), + actor.trajectory_pose(_ecm)) + # World Pose Test + # The entity doesn't have a components::WorldPose component, + # therefore, it will return None. + self.assertEqual(None, actor.world_pose(_ecm)) + # Animation Name Test + if self.pre_iterations == 0: + self.assertEqual(None, actor.animation_name(_ecm)) + actor.set_animation_name(_ecm, 'walking_test') + self.assertEqual('walking_test', actor.animation_name(_ecm)) + # Animation Time Test + if self.pre_iterations == 0: + self.assertEqual(None, actor.animation_time(_ecm)) + actor.set_animation_time(_ecm, + datetime.timedelta(milliseconds=100)) + self.assertEqual(100, + actor.animation_time(_ecm).total_seconds()*1000) + + 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/actor_test.sdf b/python/test/actor_test.sdf new file mode 100644 index 0000000000..97f9ff15c0 --- /dev/null +++ b/python/test/actor_test.sdf @@ -0,0 +1,12 @@ + + + + + + 1 1 0 0 0 0 + + + + + +