Skip to content

Commit

Permalink
Added Plane Python interface (#941)
Browse files Browse the repository at this point in the history
Signed-off-by: ahcorde <[email protected]>

* Fix typo in documentation: _normal -> normal

Signed-off-by: Steve Peters <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
ahcorde and scpeters authored Apr 14, 2022
1 parent 24e6180 commit 1d55980
Show file tree
Hide file tree
Showing 6 changed files with 198 additions and 1 deletion.
2 changes: 1 addition & 1 deletion include/sdf/Plane.hh
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ namespace sdf
/// \return The plane normal vector.
public: ignition::math::Vector3d Normal() const;

/// \brief Set the plane normal vector. The _normal vector will be
/// \brief Set the plane normal vector. The normal vector will be
/// normalized. See ignition::math::Vector3d Normal() for more information
/// about the normal vector, such as the frame in which it is specified.
/// \param[in] _normal The plane normal vector.
Expand Down
2 changes: 2 additions & 0 deletions python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ pybind11_add_module(sdformat SHARED
src/sdf/pyError.cc
src/sdf/pyMesh.cc
src/sdf/pyParserConfig.cc
src/sdf/pyPlane.cc
src/sdf/pySphere.cc
src/sdf/pySurface.cc
)
Expand All @@ -67,6 +68,7 @@ if (BUILD_TESTING)
pyError_TEST
pyMesh_TEST
pyParserConfig_TEST
pyPlane_TEST
pySphere_TEST
pySurface_TEST
)
Expand Down
2 changes: 2 additions & 0 deletions python/src/sdf/_ignition_sdformat_pybind11.cc
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "pyError.hh"
#include "pyMesh.hh"
#include "pyParserConfig.hh"
#include "pyPlane.hh"
#include "pySphere.hh"
#include "pySurface.hh"

Expand All @@ -31,6 +32,7 @@ PYBIND11_MODULE(sdformat, m) {
sdf::python::defineError(m);
sdf::python::defineMesh(m);
sdf::python::defineParserConfig(m);
sdf::python::definePlane(m);
sdf::python::defineSphere(m);
sdf::python::defineSurface(m);
}
63 changes: 63 additions & 0 deletions python/src/sdf/pyPlane.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
/*
* Copyright (C) 2022 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 "pyPlane.hh"

#include <pybind11/pybind11.h>

#include "sdf/Plane.hh"

using namespace pybind11::literals;

namespace sdf
{
// Inline bracket to help doxygen filtering.
inline namespace SDF_VERSION_NAMESPACE {
namespace python
{
/////////////////////////////////////////////////
void definePlane(pybind11::object module)
{
pybind11::class_<sdf::Plane>(module, "Plane")
.def(pybind11::init<>())
.def(pybind11::init<sdf::Plane>())
.def("size", &sdf::Plane::Size,
"Get the plane size in meters.")
.def("set_size", &sdf::Plane::SetSize,
"Set the plane size in meters.")
.def("normal", &sdf::Plane::Normal,
"Get the plane normal vector. When a Plane is used as a geometry "
"for a Visual or Collision object, then the normal is specified in "
"the Visual or Collision frame, respectively.")
.def("set_normal", &sdf::Plane::SetNormal,
"Set the plane normal vector. The normal vector will be "
"normalized. See ignition::math::Vector3d Normal() for more "
"information about the normal vector, such as the frame in which it "
"is specified.")
.def(
"shape",
pybind11::overload_cast<>(&sdf::Plane::Shape, pybind11::const_),
pybind11::return_value_policy::reference,
"Get a mutable Ignition Math representation of this Plane.")
.def("__copy__", [](const sdf::Plane &self) {
return sdf::Plane(self);
})
.def("__deepcopy__", [](const sdf::Plane &self, pybind11::dict) {
return sdf::Plane(self);
}, "memo"_a);
}
} // namespace python
} // namespace SDF_VERSION_NAMESPACE
} // namespace sdf
41 changes: 41 additions & 0 deletions python/src/sdf/pyPlane.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
/*
* Copyright (C) 2022 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 SDFORMAT_PYTHON_PLANE_HH_
#define SDFORMAT_PYTHON_PLANE_HH_

#include <pybind11/pybind11.h>

#include "sdf/Plane.hh"

#include "sdf/config.hh"

namespace sdf
{
// Inline bracket to help doxygen filtering.
inline namespace SDF_VERSION_NAMESPACE {
namespace python
{
/// Define a pybind11 wrapper for an sdf::Plane
/**
* \param[in] module a pybind11 module to add the definition to
*/
void definePlane(pybind11::object module);
} // namespace python
} // namespace SDF_VERSION_NAMESPACE
} // namespace sdf

#endif // SDFORMAT_PYTHON_PLANE_HH_
89 changes: 89 additions & 0 deletions python/test/pyPlane_TEST.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
# Copyright (C) 2022 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 copy
from sdformat import Plane
from ignition.math import Vector3d, Vector2d, Planed
import unittest


class PlaneTEST(unittest.TestCase):

def test_default_construction(self):
plane = Plane()

self.assertEqual(Vector3d.UNIT_Z, plane.normal())
self.assertEqual(Vector2d.ONE, plane.size())

plane.set_normal(Vector3d(1, 0, 0))
self.assertEqual(Vector3d.UNIT_X, plane.normal())

plane.set_normal(Vector3d(1, 0, 1))
self.assertEqual(Vector3d(0.707107, 0, 0.707107), plane.normal())

plane.set_size(Vector2d(1.2, 3.4))
self.assertEqual(Vector2d(1.2, 3.4), plane.size())


def test_assigment(self):
plane = Plane()
plane.set_normal(Vector3d(1, 0, 0))
plane.set_size(Vector2d(1.2, 3.4))

plane2 = plane
self.assertEqual(Vector3d.UNIT_X, plane2.normal())
self.assertEqual(Vector2d(1.2, 3.4), plane2.size())

self.assertEqual(Vector3d.UNIT_X, plane2.shape().normal())
self.assertEqual(Vector2d(1.2, 3.4), plane2.shape().size())

plane.set_normal(Vector3d(0, 1, 0))
plane.set_size(Vector2d(3.3, 2.4))

self.assertEqual(Vector3d(0, 1, 0), plane2.normal())
self.assertEqual(Vector2d(3.3, 2.4), plane2.size())

self.assertEqual(Vector3d(0, 1, 0), plane.normal())
self.assertEqual(Vector2d(3.3, 2.4), plane.size())

def test_deepcopy_construction(self):
plane = Plane()
plane.set_normal(Vector3d(1, 0, 0))
plane.set_size(Vector2d(1.2, 3.4))

plane2 = copy.deepcopy(plane)
self.assertEqual(Vector3d.UNIT_X, plane2.normal())
self.assertEqual(Vector2d(1.2, 3.4), plane2.size())

plane.set_normal(Vector3d(0, 1, 0))
plane.set_size(Vector2d(2.1, 4.3))

self.assertEqual(Vector3d.UNIT_X, plane2.normal())
self.assertEqual(Vector2d(1.2, 3.4), plane2.size())
self.assertEqual(Vector2d(2.1, 4.3), plane.size())
self.assertEqual(Vector3d(0, 1, 0), plane.normal())


def test_shape(self):

plane = Plane()
self.assertEqual(Vector2d.ONE, plane.size())

plane.shape().set(plane.shape().normal(), Vector2d(1, 2),
plane.shape().offset())
self.assertEqual(Vector2d(1, 2), plane.size())


if __name__ == '__main__':
unittest.main()

0 comments on commit 1d55980

Please sign in to comment.