From ae0ca8ee663fe34937a2d73097f20ee81654450e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 31 May 2024 02:12:00 +0900 Subject: [PATCH] Add python bindings for bullet and torsional friction (#1427) Signed-off-by: Ian Chen --- python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pySurface.cc | 72 +++++++++ python/src/sdf/pySurface.hh | 12 ++ python/test/pySurface_TEST.py | 202 +++++++++++++++++++++++- 4 files changed, 287 insertions(+), 1 deletion(-) diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 80caf4396..5621fd17a 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -79,6 +79,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); sdf::python::defineBox(m); + sdf::python::defineBulletFriction(m); sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); @@ -123,6 +124,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineSky(m); sdf::python::defineSphere(m); sdf::python::defineSurface(m); + sdf::python::defineTorsional(m); sdf::python::defineVisual(m); sdf::python::defineWorld(m); diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc index dc7be622b..ed8545e95 100644 --- a/python/src/sdf/pySurface.cc +++ b/python/src/sdf/pySurface.cc @@ -55,6 +55,16 @@ void defineFriction(pybind11::object module) "Get the ODE object.") .def("set_ode", &sdf::Friction::SetODE, "Set the ODE object.") + .def("bullet_friction", &sdf::Friction::BulletFriction, + pybind11::return_value_policy::reference_internal, + "Get the bullet friction object.") + .def("set_bullet_friction", &sdf::Friction::SetBulletFriction, + "Set the bullet friction object.") + .def("torsional", &sdf::Friction::Torsional, + pybind11::return_value_policy::reference_internal, + "Get the torsional friction object.") + .def("set_torsional", &sdf::Friction::SetTorsional, + "Set the torsional friction object.") .def("__copy__", [](const sdf::Friction &self) { return sdf::Friction(self); }) @@ -118,6 +128,68 @@ void defineSurface(pybind11::object module) return sdf::Surface(self); }, "memo"_a); } +///////////////////////////////////////////////// +void defineBulletFriction(pybind11::object module) +{ + pybind11::class_(module, "BulletFriction") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("friction", &sdf::BulletFriction::Friction, + "Get the friction parameter.") + .def("set_friction", &sdf::BulletFriction::SetFriction, + "Set the friction parameter.") + .def("friction2", &sdf::BulletFriction::Friction2, + "Get the friction parameter.") + .def("set_friction2", &sdf::BulletFriction::SetFriction2, + "Set the friction2 parameter.") + .def("fdir1", &sdf::BulletFriction::Fdir1, + "Get the fdir1 parameter.") + .def("set_fdir1", &sdf::BulletFriction::SetFdir1, + "Set the fdir1 parameter.") + .def("rolling_friction", &sdf::BulletFriction::RollingFriction, + "Get the rolling fricion parameter.") + .def("set_rolling_friction", &sdf::BulletFriction::SetRollingFriction, + "Set the rolling friction parameter.") + .def("__copy__", [](const sdf::BulletFriction &self) { + return sdf::BulletFriction(self); + }) + .def("__deepcopy__", [](const sdf::BulletFriction &self, pybind11::dict) { + return sdf::BulletFriction(self); + }, "memo"_a); +} +///////////////////////////////////////////////// +void defineTorsional(pybind11::object module) +{ + pybind11::class_(module, "Torsional") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("coefficient", &sdf::Torsional::Coefficient, + "Get the coefficient parameter.") + .def("set_coefficient", &sdf::Torsional::SetCoefficient, + "Set the coefficient parameter.") + .def("use_patch_radius", &sdf::Torsional::UsePatchRadius, + "Get whether to use patch radius for torsional friction calculation.") + .def("set_use_patch_radius", &sdf::Torsional::SetUsePatchRadius, + "Set whether to use patch radius for torsional friction calculation.") + .def("patch_radius", &sdf::Torsional::PatchRadius, + "Get the radius of contact patch surface.") + .def("set_patch_radius", &sdf::Torsional::SetPatchRadius, + "Set the radius of contact patch surface.") + .def("surface_radius", &sdf::Torsional::SurfaceRadius, + "Get the surface radius on the contact point.") + .def("set_surface_radius", &sdf::Torsional::SetSurfaceRadius, + "Set the surface radius on the contact point.") + .def("ode_slip", &sdf::Torsional::ODESlip, + "Get the ODE force dependent slip for torsional friction.") + .def("set_ode_slip", &sdf::Torsional::SetODESlip, + "Set the ODE force dependent slip for torsional friction.") + .def("__copy__", [](const sdf::Torsional &self) { + return sdf::Torsional(self); + }) + .def("__deepcopy__", [](const sdf::Torsional &self, pybind11::dict) { + return sdf::Torsional(self); + }, "memo"_a); +} } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/src/sdf/pySurface.hh b/python/src/sdf/pySurface.hh index 75e29f691..b45b547a2 100644 --- a/python/src/sdf/pySurface.hh +++ b/python/src/sdf/pySurface.hh @@ -52,6 +52,18 @@ void defineODE(pybind11::object module); * \param[in] module a pybind11 module to add the definition to */ void defineSurface(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::BulletFriction +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineBulletFriction(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::Torsional +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineTorsional(pybind11::object module); } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 2651b1113..ed6875c0d 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -14,7 +14,8 @@ import copy from gz_test_deps.math import Vector3d -from gz_test_deps.sdformat import Surface, Contact, Friction, ODE +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE, \ + BulletFriction, Torsional import unittest @@ -34,8 +35,24 @@ def test_assigment_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) + friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -48,6 +65,17 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -60,6 +88,20 @@ def test_assigment_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -74,6 +116,29 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1.1, 2.1, 3.1)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.03) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.7) def test_copy_construction(self): surface1 = Surface() @@ -84,8 +149,21 @@ def test_copy_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -98,6 +176,17 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -110,6 +199,20 @@ def test_copy_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -124,6 +227,29 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_deepcopy(self): surface1 = Surface() @@ -134,8 +260,21 @@ def test_deepcopy(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -148,6 +287,17 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -160,7 +310,19 @@ def test_deepcopy(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) surface1.set_friction(friction) + self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) self.assertEqual(surface1.friction().ode().slip1(), 3.1) @@ -174,6 +336,29 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_default_contact_construction(self): contact = Contact() @@ -196,6 +381,21 @@ def test_default_ode_construction(self): self.assertEqual(ode.fdir1(), Vector3d(0, 0, 0)) + def test_default_bullet_friction_construction(self): + bullet = BulletFriction() + self.assertEqual(bullet.friction(), 1.0) + self.assertEqual(bullet.friction2(), 1.0) + self.assertEqual(bullet.fdir1(), + Vector3d(0, 0, 0)) + self.assertEqual(bullet.rolling_friction(), 1.0) + + def test_default_torsional_construction(self): + torsional = Torsional() + self.assertEqual(torsional.coefficient(), 1.0) + self.assertTrue(torsional.use_patch_radius()) + self.assertEqual(torsional.patch_radius(), 0.0) + self.assertEqual(torsional.surface_radius(), 0.0) + self.assertEqual(torsional.ode_slip(), 0.0) if __name__ == '__main__': unittest.main()