diff --git a/CHANGELOG.md b/CHANGELOG.md index 1194790ddd..47bd9861d1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add parsing of equality/connect tag for closed-loop chains for MJCF format ([#2413](https://github.com/stack-of-tasks/pinocchio/pull/2413)) - Add compatibility with NumPy 2 `__array__` API ([#2436](https://github.com/stack-of-tasks/pinocchio/pull/2436)) - Added argument to let users decide of root joint name when parsing models (urdf, mjcf, sdf) ([#2402](https://github.com/stack-of-tasks/pinocchio/pull/2402)) +- Allow use of `pathlib.Path | str` for paths in python bindings ([#2431](https://github.com/stack-of-tasks/pinocchio/pull/2431)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) diff --git a/bindings/python/parsers/mjcf/geometry.cpp b/bindings/python/parsers/mjcf/geometry.cpp index edfaf140ef..3be5995d3a 100644 --- a/bindings/python/parsers/mjcf/geometry.cpp +++ b/bindings/python/parsers/mjcf/geometry.cpp @@ -4,6 +4,7 @@ #include "pinocchio/parsers/mjcf.hpp" #include "pinocchio/bindings/python/parsers/mjcf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -15,21 +16,21 @@ namespace pinocchio namespace bp = boost::python; GeometryModel - buildGeomFromMJCF(Model & model, const std::string & filename, const GeometryType & type) + buildGeomFromMJCF(Model & model, const bp::object & filename, const GeometryType & type) { GeometryModel geometry_model; - ::pinocchio::mjcf::buildGeom(model, filename, type, geometry_model); + ::pinocchio::mjcf::buildGeom(model, path(filename), type, geometry_model); return geometry_model; } GeometryModel buildGeomFromMJCF( Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType & type, ::hpp::fcl::MeshLoaderPtr & meshLoader) { GeometryModel geometry_model; - ::pinocchio::mjcf::buildGeom(model, filename, type, geometry_model, meshLoader); + ::pinocchio::mjcf::buildGeom(model, path(filename), type, geometry_model, meshLoader); return geometry_model; } @@ -37,7 +38,7 @@ namespace pinocchio { bp::def( "buildGeomFromMJCF", - static_cast( + static_cast( pinocchio::python::buildGeomFromMJCF), bp::args("model", "mjcf_filename", "geom_type"), "Parse the Mjcf file given as input looking for the geometry of the given input model and\n" @@ -52,7 +53,7 @@ namespace pinocchio bp::def( "buildGeomFromMJCF", static_cast( + Model &, const bp::object &, const GeometryType &, ::hpp::fcl::MeshLoaderPtr &)>( pinocchio::python::buildGeomFromMJCF), bp::args("model", "mjcf_filename", "geom_type", "mesh_loader"), "Parse the Mjcf file given as input looking for the geometry of the given input model and\n" diff --git a/bindings/python/parsers/mjcf/model.cpp b/bindings/python/parsers/mjcf/model.cpp index 9cd9e6698b..6af560818e 100644 --- a/bindings/python/parsers/mjcf/model.cpp +++ b/bindings/python/parsers/mjcf/model.cpp @@ -4,6 +4,7 @@ #include "pinocchio/parsers/mjcf.hpp" #include "pinocchio/bindings/python/parsers/mjcf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -14,28 +15,29 @@ namespace pinocchio namespace bp = boost::python; - Model buildModelFromMJCF(const std::string & filename) + Model buildModelFromMJCF(const bp::object & filename) { Model model; - ::pinocchio::mjcf::buildModel(filename, model); + ::pinocchio::mjcf::buildModel(path(filename), model); return model; } - Model buildModelFromMJCF(const std::string & filename, const JointModel & root_joint) + Model buildModelFromMJCF(const bp::object & filename, const JointModel & root_joint) { Model model; - ::pinocchio::mjcf::buildModel(filename, root_joint, model); + ::pinocchio::mjcf::buildModel(path(filename), root_joint, model); return model; } bp::tuple buildModelFromMJCF( - const std::string & filename, + const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name) { Model model; PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; - ::pinocchio::mjcf::buildModel(filename, root_joint, root_joint_name, model, contact_models); + ::pinocchio::mjcf::buildModel( + path(filename), root_joint, root_joint_name, model, contact_models); return bp::make_tuple(model, contact_models); } @@ -43,20 +45,20 @@ namespace pinocchio { bp::def( "buildModelFromMJCF", - static_cast(pinocchio::python::buildModelFromMJCF), + static_cast(pinocchio::python::buildModelFromMJCF), bp::args("mjcf_filename"), "Parse the MJCF file given in input and return a pinocchio Model."); bp::def( "buildModelFromMJCF", - static_cast( + static_cast( pinocchio::python::buildModelFromMJCF), bp::args("mjcf_filename", "root_joint"), "Parse the MJCF file and return a pinocchio Model with the given root Joint."); bp::def( "buildModelFromMJCF", - static_cast( + static_cast( pinocchio::python::buildModelFromMJCF), bp::args("mjcf_filename", "root_joint", "root_joint_name"), "Parse the MJCF file and return a pinocchio Model with the given root Joint and its " diff --git a/bindings/python/parsers/sdf/geometry.cpp b/bindings/python/parsers/sdf/geometry.cpp index 0002dde537..982071f909 100644 --- a/bindings/python/parsers/sdf/geometry.cpp +++ b/bindings/python/parsers/sdf/geometry.cpp @@ -6,6 +6,7 @@ #include "pinocchio/parsers/sdf.hpp" #endif #include "pinocchio/bindings/python/parsers/sdf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -17,93 +18,87 @@ namespace pinocchio namespace bp = boost::python; #ifdef PINOCCHIO_WITH_SDFORMAT - GeometryModel - buildGeomFromSdf(const Model & model, const std::string & filename, const GeometryType type) + buildGeomFromSdf(const Model & model, const bp::object & filename, const GeometryType type) { GeometryModel geometry_model; const std::string & rootLinkName = ""; const std::string & packageDir = ""; - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, packageDir); + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, packageDir); return geometry_model; } GeometryModel buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, const std::string & rootLinkName) { GeometryModel geometry_model; const std::string & packageDir = ""; - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, packageDir); + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, packageDir); return geometry_model; } GeometryModel & buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, GeometryModel & geometry_model, const std::string & rootLinkName) { - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName); + pinocchio::sdf::buildGeom(model, path(filename), type, geometry_model, rootLinkName); return geometry_model; } GeometryModel buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, const std::string & rootLinkName, - const std::string & packageDir) + const bp::object & package_dirs) { GeometryModel geometry_model; - const std::vector dirs(1, packageDir); - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, dirs); + if (PyList_Check(package_dirs.ptr())) + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, pathList(package_dirs)); + } + else + { + const std::vector dirs(1, path(package_dirs)); + pinocchio::sdf::buildGeom(model, path(filename), type, geometry_model, rootLinkName, dirs); + } return geometry_model; } - GeometryModel buildGeomFromSdf( - const Model & model, - const std::string & filename, - const GeometryType type, - const std::string & rootLinkName, - const std::vector & package_dirs) - { - GeometryModel geometry_model; - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, package_dirs); - return geometry_model; - } - GeometryModel & buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, GeometryModel & geometry_model, const std::string & rootLinkName, - const std::vector & package_dirs) + const bp::object & package_dir) { - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, package_dirs); - return geometry_model; - } - - GeometryModel & buildGeomFromSdf( - const Model & model, - const std::string & filename, - const GeometryType type, - GeometryModel & geometry_model, - const std::string & rootLinkName, - const std::string & package_dir) - { - pinocchio::sdf::buildGeom(model, filename, type, geometry_model, rootLinkName, package_dir); + if (PyList_Check(package_dir.ptr())) + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, pathList(package_dir)); + } + else + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, path(package_dir)); + } return geometry_model; } GeometryModel buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, const std::string & rootLinkName, const hpp::fcl::MeshLoaderPtr & meshLoader) @@ -111,13 +106,13 @@ namespace pinocchio std::vector hints; GeometryModel geometry_model; pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, hints, meshLoader); + model, path(filename), type, geometry_model, rootLinkName, hints, meshLoader); return geometry_model; } GeometryModel & buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, GeometryModel & geometry_model, const std::string & rootLinkName, @@ -125,63 +120,53 @@ namespace pinocchio { std::vector hints; pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, hints, meshLoader); + model, path(filename), type, geometry_model, rootLinkName, hints, meshLoader); return geometry_model; } GeometryModel buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, const std::string & rootLinkName, - const std::vector & package_dirs, + const bp::object & package_dir, const hpp::fcl::MeshLoaderPtr & meshLoader) { GeometryModel geometry_model; - pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, package_dirs, meshLoader); + if (PyList_Check(package_dir.ptr())) + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, pathList(package_dir), + meshLoader); + } + else + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, path(package_dir), meshLoader); + } return geometry_model; } GeometryModel & buildGeomFromSdf( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, GeometryModel & geometry_model, const std::string & rootLinkName, - const std::vector & package_dirs, + const bp::object & package_dir, const hpp::fcl::MeshLoaderPtr & meshLoader) { - pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, package_dirs, meshLoader); - return geometry_model; - } - - GeometryModel buildGeomFromSdf( - const Model & model, - const std::string & filename, - const GeometryType type, - const std::string & rootLinkName, - const std::string & package_dir, - const hpp::fcl::MeshLoaderPtr & meshLoader) - { - GeometryModel geometry_model; - pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, package_dir, meshLoader); - return geometry_model; - } - - GeometryModel & buildGeomFromSdf( - const Model & model, - const std::string & filename, - const GeometryType type, - GeometryModel & geometry_model, - const std::string & rootLinkName, - const std::string & package_dir, - const hpp::fcl::MeshLoaderPtr & meshLoader) - { - pinocchio::sdf::buildGeom( - model, filename, type, geometry_model, rootLinkName, package_dir, meshLoader); + if (PyList_Check(package_dir.ptr())) + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, pathList(package_dir), + meshLoader); + } + else + { + pinocchio::sdf::buildGeom( + model, path(filename), type, geometry_model, rootLinkName, path(package_dir), meshLoader); + } return geometry_model; } @@ -193,7 +178,7 @@ namespace pinocchio bp::def( "buildGeomFromSdf", - static_cast( + static_cast( pinocchio::python::buildGeomFromSdf), bp::args("model", "sdf_filename", "geom_type"), "Parse the SDF file given as input looking for the geometry of the given input model and\n" @@ -208,8 +193,8 @@ namespace pinocchio bp::def( "buildGeomFromSdf", static_cast(pinocchio::python::buildGeomFromSdf), + const Model &, const bp::object &, const GeometryType, const std::string &, + const bp::object &)>(pinocchio::python::buildGeomFromSdf), bp::args("model", "sdf_filename", "geom_type", "root_link_name", "package_dir"), "Parse the SDF file given as input looking for the geometry of the given input model and\n" "return a GeometryModel containing either the collision geometries " @@ -219,50 +204,13 @@ namespace pinocchio "\tsdf_filename: path to the SDF file containing the model of the robot\n" "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " "or the COLLISION for collision detection).\n" - "\tpackage_dir: path pointing to the folder containing the meshes of the robot\n"); - - bp::def( - "buildGeomFromSdf", - static_cast &)>(pinocchio::python::buildGeomFromSdf), - bp::args("model", "sdf_filename", "geom_type", "root_link_name", "package_dirs"), - "Parse the SDF file given as input looking for the geometry of the given input model and\n" - "return a GeometryModel containing either the collision geometries " - "(GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n" - "Parameters:\n" - "\tmodel: model of the robot\n" - "\tsdf_filename: path to the SDF file containing the model of the robot\n" - "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " - "or the COLLISION for collision detection).\n" - "\tpackage_dirs: vector of paths pointing to the folders containing the model of the " + "\tpackage_dir: path or vector of path pointing to the folder containing the meshes of the " "robot\n"); - bp::def( - "buildGeomFromSdf", - static_cast &)>( - pinocchio::python::buildGeomFromSdf), - bp::args( - "model", "sdf_filename", "geom_type", "geom_model", "root_link_name", "package_dirs"), - "Parse the SDF file given as input looking for the geometry of the given input model and\n" - "and store either the collision geometries (GeometryType.COLLISION) or the visual " - "geometries (GeometryType.VISUAL) in the geom_model given as input.\n" - "Parameters:\n" - "\tmodel: model of the robot\n" - "\tsdf_filename: path to the SDF file containing the model of the robot\n" - "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " - "or the COLLISION for collision detection).\n" - "\tgeom_model: reference where to store the parsed information\n" - "\tpackage_dirs: vector of paths pointing to the folders containing the model of the " - "robot\n", - bp::return_internal_reference<4>()); - bp::def( "buildGeomFromSdf", static_cast( + const Model &, const bp::object &, const GeometryType, const std::string &)>( pinocchio::python::buildGeomFromSdf), bp::args("model", "sdf_filename", "geom_type", "root_link_name"), "Parse the SDF file given as input looking for the geometry of the given input model and\n" @@ -278,7 +226,7 @@ namespace pinocchio bp::def( "buildGeomFromSdf", - static_cast( pinocchio::python::buildGeomFromSdf), bp::args("model", "sdf_filename", "geom_type", "geom_model", "root_link_name"), @@ -297,8 +245,8 @@ namespace pinocchio bp::def( "buildGeomFromSdf", - static_cast( + static_cast( pinocchio::python::buildGeomFromSdf), bp::args( "model", "sdf_filename", "geom_type", "geom_model", "root_link_name", "package_dir"), @@ -311,58 +259,15 @@ namespace pinocchio "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " "or the COLLISION for collision detection).\n" "\tgeom_model: reference where to store the parsed information\n" - "\tpackage_dir: path pointing to the folder containing the meshes of the robot\n", - bp::return_internal_reference<4>()); - - bp::def( - "buildGeomFromSdf", - static_cast &, const hpp::fcl::MeshLoaderPtr &)>( - pinocchio::python::buildGeomFromSdf), - bp::args( - "model", "sdf_filename", "geom_type", "root_link_name", "package_dirs", "mesh_loader"), - "Parse the SDF file given as input looking for the geometry of the given input model and\n" - "return a GeometryModel containing either the collision geometries " - "(GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n" - "Parameters:\n" - "\tmodel: model of the robot\n" - "\tsdf_filename: path to the SDF file containing the model of the robot\n" - "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " - "or the COLLISION for collision detection).\n" - "\tpackage_dirs: vector of paths pointing to the folders containing the model of the " - "robot\n" - "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries)."); - - bp::def( - "buildGeomFromSdf", - static_cast &, - const hpp::fcl::MeshLoaderPtr &)>( - pinocchio::python::buildGeomFromSdf), - bp::args( - "model", "sdf_filename", "geom_type", "geom_model", "root_link_name", "package_dirs", - "mesh_loader"), - "Parse the SDF file given as input looking for the geometry of the given input model and\n" - "and store either the collision geometries (GeometryType.COLLISION) or the visual " - "geometries (GeometryType.VISUAL) in the geom_model given as input.\n" - "Parameters:\n" - "\tmodel: model of the robot\n" - "\tsdf_filename: path to the SDF file containing the model of the robot\n" - "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " - "or the COLLISION for collision detection).\n" - "\tgeom_model: reference where to store the parsed information\n" - "\tpackage_dirs: vector of paths pointing to the folders containing the model of the " - "robot\n" - "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).", + "\tpackage_dir: path or vector of path pointing to the folder containing the meshes of the " + "robot\n", bp::return_internal_reference<4>()); bp::def( "buildGeomFromSdf", static_cast( + const Model &, const bp::object &, const GeometryType, const std::string &, + const bp::object &, const hpp::fcl::MeshLoaderPtr &)>( pinocchio::python::buildGeomFromSdf), bp::args( "model", "sdf_filename", "geom_type", "root_link_name", "package_dir", "mesh_loader"), @@ -379,8 +284,8 @@ namespace pinocchio bp::def( "buildGeomFromSdf", - static_cast( pinocchio::python::buildGeomFromSdf), bp::args( @@ -395,14 +300,15 @@ namespace pinocchio "\tgeom_type: type of geometry to extract from the SDF file (either the VISUAL for display " "or the COLLISION for collision detection).\n" "\tgeom_model: reference where to store the parsed information\n" - "\tpackage_dir: path pointing to the folder containing the meshes of the robot\n" + "\tpackage_dir: path or vector of path pointing to the folder containing the meshes of the " + "robot\n" "\tmesh_loader: an hpp-fcl mesh loader (to load only once the related geometries).", bp::return_internal_reference<4>()); bp::def( "buildGeomFromSdf", static_cast(pinocchio::python::buildGeomFromSdf), bp::args("model", "sdf_filename", "geom_type", "root_link_name", "mesh_loader"), "Parse the SDF file given as input looking for the geometry of the given input model and\n" @@ -419,7 +325,7 @@ namespace pinocchio bp::def( "buildGeomFromSdf", - static_cast( pinocchio::python::buildGeomFromSdf), diff --git a/bindings/python/parsers/sdf/model.cpp b/bindings/python/parsers/sdf/model.cpp index 5a92c3aac7..223a54ba66 100644 --- a/bindings/python/parsers/sdf/model.cpp +++ b/bindings/python/parsers/sdf/model.cpp @@ -6,6 +6,7 @@ #include "pinocchio/parsers/sdf.hpp" #endif #include "pinocchio/bindings/python/parsers/sdf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include #include @@ -19,19 +20,19 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_SDFORMAT bp::tuple buildModelFromSdf( - const std::string & filename, + const bp::object & filename, const std::string & root_link_name, const std::vector & parent_guidance) { Model model; PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; ::pinocchio::sdf::buildModel( - filename, model, contact_models, root_link_name, parent_guidance); + path(filename), model, contact_models, root_link_name, parent_guidance); return bp::make_tuple(model, contact_models); } bp::tuple buildModelFromSdf( - const std::string & filename, + const bp::object & filename, const JointModel & root_joint, const std::string & root_link_name, const std::vector & parent_guidance) @@ -39,12 +40,12 @@ namespace pinocchio Model model; PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; pinocchio::sdf::buildModel( - filename, root_joint, model, contact_models, root_link_name, parent_guidance); + path(filename), root_joint, model, contact_models, root_link_name, parent_guidance); return bp::make_tuple(model, contact_models); } bp::tuple buildModelFromSdf( - const std::string & filename, + const bp::object & filename, const JointModel & root_joint, const std::string & root_link_name, const std::string & root_joint_name, @@ -53,7 +54,7 @@ namespace pinocchio Model model; PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; pinocchio::sdf::buildModel( - filename, root_joint, root_joint_name, model, contact_models, root_link_name, + path(filename), root_joint, root_joint_name, model, contact_models, root_link_name, parent_guidance); return bp::make_tuple(model, contact_models); } @@ -65,7 +66,7 @@ namespace pinocchio bp::def( "buildModelFromSdf", static_cast &)>( + const bp::object &, const std::string &, const std::vector &)>( pinocchio::python::buildModelFromSdf), (bp::arg("sdf_filename"), bp::arg("root_link_name"), bp::arg("parent_guidance") = bp::list()), @@ -74,7 +75,7 @@ namespace pinocchio bp::def( "buildModelFromSdf", static_cast &)>(pinocchio::python::buildModelFromSdf), (bp::arg("sdf_filename"), bp::arg("root_joint"), bp::arg("root_link_name"), bp::arg("parent_guidance") = bp::list()), @@ -84,7 +85,7 @@ namespace pinocchio bp::def( "buildModelFromSdf", static_cast &)>(pinocchio::python::buildModelFromSdf), (bp::arg("sdf_filename"), bp::arg("root_joint"), bp::arg("root_link_name"), bp::arg("root_joint_name"), bp::arg("parent_guidance") = bp::list()), diff --git a/bindings/python/parsers/srdf.cpp b/bindings/python/parsers/srdf.cpp index 2c2ec4acf0..044c05ad02 100644 --- a/bindings/python/parsers/srdf.cpp +++ b/bindings/python/parsers/srdf.cpp @@ -4,6 +4,7 @@ #include "pinocchio/parsers/srdf.hpp" #include "pinocchio/bindings/python/parsers/srdf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -14,6 +15,21 @@ namespace pinocchio namespace bp = boost::python; + void removeCollisionPairs( + const Model & model, + GeometryModel & geom_model, + const bp::object & filename, + const bool verbose = false) + { + pinocchio::srdf::removeCollisionPairs(model, geom_model, path(filename), verbose); + } + + void loadReferenceConfigurations( + Model & model, const bp::object & filename, const bool verbose = false) + { + pinocchio::srdf::loadReferenceConfigurations(model, path(filename), verbose); + } + void loadReferenceConfigurationsFromXML( Model & model, const std::string & xmlStream, const bool verbose = false) { @@ -21,13 +37,18 @@ namespace pinocchio pinocchio::srdf::loadReferenceConfigurationsFromXML(model, iss, verbose); } + bool loadRotorParameters(Model & model, const bp::object & filename, const bool verbose = false) + { + return pinocchio::srdf::loadRotorParameters(model, path(filename), verbose); + } + void exposeSRDFParser() { bp::def( "removeCollisionPairs", - static_cast( - &srdf::removeCollisionPairs), + static_cast( + &removeCollisionPairs), (bp::arg("model"), bp::arg("geom_model"), bp::arg("srdf_filename"), bp::arg("verbose") = false), "Parse an SRDF file in order to remove some collision pairs for a specific GeometryModel.\n" @@ -55,8 +76,8 @@ namespace pinocchio bp::def( "loadReferenceConfigurations", - static_cast( - &srdf::loadReferenceConfigurations), + static_cast( + &loadReferenceConfigurations), (bp::arg("model"), bp::arg("srdf_filename"), bp::arg("verbose") = false), "Retrieve all the reference configurations of a given model from the SRDF file.\n" "Parameters:\n" @@ -67,7 +88,7 @@ namespace pinocchio bp::def( "loadReferenceConfigurationsFromXML", static_cast( - &srdf::loadReferenceConfigurations), + &loadReferenceConfigurationsFromXML), (bp::arg("model"), bp::arg("srdf_xml_stream"), bp::arg("verbose") = false), "Retrieve all the reference configurations of a given model from the SRDF file.\n" "Parameters:\n" @@ -78,7 +99,7 @@ namespace pinocchio bp::def( "loadRotorParameters", - static_cast(&srdf::loadRotorParameters), + static_cast(&loadRotorParameters), (bp::arg("model"), bp::arg("srdf_filename"), bp::arg("verbose") = false), "Load the rotor parameters of a given model from a SRDF file.\n" "Results are stored in model.rotorInertia and model.rotorGearRatio." diff --git a/bindings/python/parsers/urdf/geometry.cpp b/bindings/python/parsers/urdf/geometry.cpp index 776ee4caf8..418b3a4b14 100644 --- a/bindings/python/parsers/urdf/geometry.cpp +++ b/bindings/python/parsers/urdf/geometry.cpp @@ -7,6 +7,7 @@ #endif #include "pinocchio/bindings/python/parsers/urdf.hpp" #include "pinocchio/bindings/python/utils/list.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -44,24 +45,16 @@ namespace pinocchio } std::vector pkg_dirs; - - bp::extract pkg_dir_extract(py_pkg_dirs); - bp::extract pkg_dirs_list_extract(py_pkg_dirs); - bp::extract &> pkg_dirs_vect_extract(py_pkg_dirs); - if (py_pkg_dirs.is_none()) + if (py_pkg_dirs.ptr() == Py_None) + { + } + else if (PyList_Check(py_pkg_dirs.ptr())) { - } // Provided None - else if (pkg_dir_extract.check()) // Provided a string - pkg_dirs.push_back(pkg_dir_extract()); - else if (pkg_dirs_list_extract.check()) // Provided a list of string - extract(pkg_dirs_list_extract(), pkg_dirs); - else if (pkg_dirs_vect_extract.check()) // Provided a vector of string - pkg_dirs = pkg_dirs_vect_extract(); + pkg_dirs = pathList(py_pkg_dirs); + } else - { // Did not understand the provided argument - std::string what = bp::extract(py_pkg_dirs.attr("__str__")())(); - throw std::invalid_argument( - "pkg_dirs must be either None, a string or a list of strings. Provided " + what); + { + pkg_dirs.push_back(path(py_pkg_dirs)); } pinocchio::urdf::buildGeom(model, stream, type, geometry_model, pkg_dirs, mesh_loader); @@ -121,16 +114,17 @@ namespace pinocchio GeometryModel * buildGeomFromUrdfFile( const Model & model, - const std::string & filename, + const bp::object & filename, const GeometryType type, bp::object geom_model, bp::object package_dirs, bp::object mesh_loader) { - std::ifstream stream(filename.c_str()); + const std::string filename_s = path(filename); + std::ifstream stream(filename_s.c_str()); if (!stream.is_open()) { - throw std::invalid_argument(filename + " does not seem to be a valid file."); + throw std::invalid_argument(filename_s + " does not seem to be a valid file."); } return buildGeomFromUrdfStream(model, stream, type, geom_model, package_dirs, mesh_loader); } diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index 1876026682..a1003f1191 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -7,6 +7,7 @@ #endif #include "pinocchio/bindings/python/parsers/urdf.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include @@ -19,48 +20,48 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_URDFDOM - Model buildModelFromUrdf(const std::string & filename) + Model buildModelFromUrdf(const bp::object & filename) { Model model; - pinocchio::urdf::buildModel(filename, model); + pinocchio::urdf::buildModel(path(filename), model); return model; } - Model & buildModelFromUrdf(const std::string & filename, Model & model) + Model & buildModelFromUrdf(const bp::object & filename, Model & model) { - return pinocchio::urdf::buildModel(filename, model); + return pinocchio::urdf::buildModel(path(filename), model); } - Model buildModelFromUrdf(const std::string & filename, const JointModel & root_joint) + Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint) { Model model; - pinocchio::urdf::buildModel(filename, root_joint, model); + pinocchio::urdf::buildModel(path(filename), root_joint, model); return model; } Model buildModelFromUrdf( - const std::string & filename, + const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name) { Model model; - pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model); + pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); return model; } Model & - buildModelFromUrdf(const std::string & filename, const JointModel & root_joint, Model & model) + buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model) { - return pinocchio::urdf::buildModel(filename, root_joint, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, model); } Model & buildModelFromUrdf( - const std::string & filename, + const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, Model & model) { - return pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); } Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint) @@ -119,7 +120,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint"), "Parse the URDF file given in input and return a pinocchio Model starting with the " @@ -127,7 +128,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint", "root_joint_name"), "Parse the URDF file given in input and return a pinocchio Model starting with the " @@ -135,13 +136,13 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast(pinocchio::python::buildModelFromUrdf), + static_cast(pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename"), "Parse the URDF file given in input and return a pinocchio Model."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "model"), "Append to a given model a URDF structure given by its filename.", @@ -149,7 +150,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint", "model"), "Append to a given model a URDF structure given by its filename and the root joint.\n" @@ -159,7 +160,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast(pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint", "root_joint_name", "model"), "Append to a given model a URDF structure given by its filename and the root joint with " diff --git a/bindings/python/pinocchio/visualize/base_visualizer.py b/bindings/python/pinocchio/visualize/base_visualizer.py index 1864653ea8..7d79ff399e 100644 --- a/bindings/python/pinocchio/visualize/base_visualizer.py +++ b/bindings/python/pinocchio/visualize/base_visualizer.py @@ -1,6 +1,6 @@ import abc -import os.path as osp import time +from pathlib import Path import numpy as np @@ -201,10 +201,10 @@ def create_video_ctx(self, filename=None, fps=30, directory=None, **kwargs): from tempfile import gettempdir directory = gettempdir() + directory = Path(directory) f_fmt = "%Y%m%d_%H%M%S" ext = "mp4" - filename = time.strftime(f"{f_fmt}.{ext}") - filename = osp.join(directory, filename) + filename = directory / time.strftime(f"{f_fmt}.{ext}") return VideoContext(self, fps, filename) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index dc88012f07..11784cb0d3 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -1,5 +1,6 @@ import os import warnings +from pathlib import Path from typing import ClassVar, List import numpy as np @@ -62,7 +63,7 @@ def hasMeshFileInfo(geometry_object): if geometry_object.meshPath == "": return False - _, file_extension = os.path.splitext(geometry_object.meshPath) + file_extension = Path(geometry_object.meshPath).suffix if file_extension.lower() in [".dae", ".obj", ".stl"]: return True @@ -108,14 +109,16 @@ def __init__(self, dae_path: str, cache: Optional[Set[str]] = None) -> None: # Init base class super().__init__() + dae_path = Path(dae_path) + # Attributes to be specified by the user self.path = None self.material = None self.intrinsic_transform = mg.tf.identity_matrix() # Raw file content - dae_dir = os.path.dirname(dae_path) - with open(dae_path) as text_file: + dae_dir = dae_path.parent + with dae_path.open() as text_file: self.dae_raw = text_file.read() # Parse the image resource in Collada file @@ -140,11 +143,11 @@ def __init__(self, dae_path: str, cache: Optional[Set[str]] = None) -> None: # Encode texture in base64 img_path_abs = img_path - if not os.path.isabs(img_path): - img_path_abs = os.path.normpath(os.path.join(dae_dir, img_path_abs)) - if not os.path.isfile(img_path_abs): + if not img_path.is_absolute(): + img_path_abs = os.path.normpath(dae_dir / img_path_abs) + if not img_path_abs.is_file(): raise UserWarning(f"Texture '{img_path}' not found.") - with open(img_path_abs, "rb") as img_file: + with Path(img_path_abs).open("rb") as img_file: img_data = base64.b64encode(img_file.read()) img_uri = f"data:image/png;base64,{img_data.decode('utf-8')}" self.img_resources[img_path] = img_uri @@ -744,7 +747,7 @@ def loadMeshFromFile(self, geometry_object): return None # Get file type from filename extension. - _, file_extension = os.path.splitext(geometry_object.meshPath) + file_extension = Path(geometry_object.meshPath).suffix if file_extension.lower() == ".dae": obj = DaeMeshGeometry(geometry_object.meshPath) elif file_extension.lower() == ".obj": diff --git a/bindings/python/pinocchio/windows_dll_manager.py b/bindings/python/pinocchio/windows_dll_manager.py index c414d1bb4f..360a6e6900 100644 --- a/bindings/python/pinocchio/windows_dll_manager.py +++ b/bindings/python/pinocchio/windows_dll_manager.py @@ -1,5 +1,6 @@ import contextlib import os +from pathlib import Path def get_dll_paths(): @@ -7,7 +8,7 @@ def get_dll_paths(): if pinocchio_paths is None: # Standard site-packages to bin path RELATIVE_DLL_PATH = "..\\..\\..\\bin" - return [os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH)] + return [Path(__file__).parent / RELATIVE_DLL_PATH] else: return pinocchio_paths.split(os.pathsep) diff --git a/bindings/python/utils/path.cpp b/bindings/python/utils/path.cpp new file mode 100644 index 0000000000..e50f889276 --- /dev/null +++ b/bindings/python/utils/path.cpp @@ -0,0 +1,54 @@ +// +// Copyright (c) 2024 CNRS +// + +#include "pinocchio/bindings/python/utils/path.hpp" + +namespace pinocchio +{ + namespace python + { + namespace bp = boost::python; + + std::string path(const bp::object & path) + { + + auto str_path = path; + const bp::object Path = bp::import("pathlib").attr("Path"); + + if (PyObject_IsInstance(str_path.ptr(), Path.ptr())) + str_path = str_path.attr("__str__")(); + + if (!PyObject_IsInstance(str_path.ptr(), reinterpret_cast(&PyUnicode_Type))) + { + std::string what = bp::extract(str_path.attr("__str__")())(); + throw std::invalid_argument(what + " is neither a Path nor a str."); + } + + return bp::extract(str_path); + }; + + std::vector pathList(const bp::object & path_list) + { + if (!PyList_Check(path_list.ptr())) + { + std::string what = bp::extract(path_list.attr("__str__")())(); + throw std::invalid_argument(what + " is not a list."); + } + + // Retrieve the underlying list + bp::object bp_obj(bp::handle<>(bp::borrowed(path_list.ptr()))); + bp::list bp_list(bp_obj); + bp::ssize_t list_size = bp::len(bp_list); + + std::vector path_vec; + path_vec.reserve(list_size); + // Check if all the elements contained in the current vector is of type T + for (bp::ssize_t k = 0; k < list_size; ++k) + { + path_vec.push_back(path(bp_list[k])); + } + return path_vec; + }; + } // namespace python +} // namespace pinocchio diff --git a/doc/d-practical-exercises/src/ur5x4.py b/doc/d-practical-exercises/src/ur5x4.py index 2a8459be11..1587ada25b 100644 --- a/doc/d-practical-exercises/src/ur5x4.py +++ b/doc/d-practical-exercises/src/ur5x4.py @@ -4,15 +4,15 @@ No optimization, this file is just an example of how to load the models. """ -from os.path import join +from pathlib import Path import numpy as np from pinocchio import SE3 from pinocchio.robot_wrapper import RobotWrapper from pinocchio.utils import eye, rotate, se3ToXYZQUATtuple, urdf, zero -PKG = "/opt/openrobots/share" -URDF = join(PKG, "ur5_description/urdf/ur5_gripper.urdf") +PKG = Path("/opt/openrobots/share") +URDF = PKG / "ur5_description/urdf/ur5_gripper.urdf" def loadRobot(M0, name): diff --git a/examples/append-urdf-model-with-another-model.py b/examples/append-urdf-model-with-another-model.py index 389a83f0f1..dfed28ed0d 100644 --- a/examples/append-urdf-model-with-another-model.py +++ b/examples/append-urdf-model-with-another-model.py @@ -1,6 +1,6 @@ import math import sys -from os.path import abspath, dirname, join +from pathlib import Path import hppfcl as fcl import numpy as np @@ -8,12 +8,12 @@ from pinocchio.visualize import MeshcatVisualizer as Visualizer # load model from example-robot urdf -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models/") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "ur5_robot.urdf" -urdf_model_path = join(join(model_path, "ur_description/urdf/"), urdf_filename) +urdf_model_path = model_path / "ur_description/urdf" / urdf_filename model1, collision_model1, visual_model1 = pin.buildModelsFromUrdf( urdf_model_path, package_dirs=mesh_dir diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index 144e764283..85269ae598 100644 --- a/examples/build-reduced-model.py +++ b/examples/build-reduced-model.py @@ -1,4 +1,4 @@ -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin @@ -8,11 +8,11 @@ # Load UR robot arm # This path refers to Pinocchio source code but you can define your own directory here. -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") -model_path = pinocchio_model_dir + "/example-robot-data/robots" +pinocchio_model_dir = Path(__file__).parent.parent / "models" +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir # You should change here to set up your own URDF file -urdf_filename = model_path + "/ur_description/urdf/ur5_robot.urdf" +urdf_filename = model_path / "ur_description/urdf/ur5_robot.urdf" model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir) # Check dimensions of the original model diff --git a/examples/capsule-approximation.py b/examples/capsule-approximation.py index 0b02037d70..ca513d29de 100644 --- a/examples/capsule-approximation.py +++ b/examples/capsule-approximation.py @@ -5,6 +5,8 @@ Section 3.8.1 Computing minimum bounding capsules """ +from pathlib import Path + import hppfcl import numpy as np import scipy.optimize as optimize @@ -92,8 +94,8 @@ def get_path(fn): import os for rospath in os.environ["ROS_PACKAGE_PATH"].split(":"): - abspath = os.path.join(rospath, relpath) - if os.path.isfile(abspath): + abspath = Path(rospath) / relpath + if abspath.is_file(): return abspath raise ValueError("Could not find " + fn) return fn @@ -130,9 +132,8 @@ def set_transform(origin, a, b): lMg = get_transform(origin) meshfile = get_path(mesh.attrib["filename"]) - import os - name = os.path.basename(meshfile) + name = Path(meshfile).name # Generate capsule a, b, radius = approximate_mesh(meshfile, lMg) length = np.linalg.norm(b - a) @@ -165,9 +166,8 @@ def set_transform(origin, a, b): # Example for a whole URDF model # This path refers to Pinocchio source code but you can define your own directory # here. - from os.path import abspath, dirname, join - pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") + pinocchio_model_dir = Path(__file__).parent.parent / "models" urdf_filename = ( pinocchio_model_dir + "models/example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf" diff --git a/examples/casadi/quadrotor-ocp.py b/examples/casadi/quadrotor-ocp.py index b8cd6c0da0..66c4d11af7 100644 --- a/examples/casadi/quadrotor-ocp.py +++ b/examples/casadi/quadrotor-ocp.py @@ -1,5 +1,5 @@ import sys -from os.path import abspath, dirname, join +from pathlib import Path import casadi import numpy as np @@ -8,13 +8,8 @@ # This use the example-robot-data submodule, but if you have it already properly # installed in your PYTHONPATH, there is no need for this sys.path thing -path = join( - dirname(dirname(dirname(abspath(__file__)))), - "models", - "example-robot-data", - "python", -) -sys.path.append(path) +path = Path(__file__).parent.parent.parent / "models" / "example-robot-data" / "python" +sys.path.append(str(path)) import example_robot_data # noqa: E402 # Problem parameters diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index 427b57c865..656527739a 100644 --- a/examples/collision-with-point-clouds.py +++ b/examples/collision-with-point-clouds.py @@ -4,7 +4,7 @@ # pip install --user meshcat import sys -from os.path import abspath, dirname, join +from pathlib import Path import hppfcl as fcl import numpy as np @@ -13,12 +13,12 @@ # Load the URDF model. # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "panda.urdf" -urdf_model_path = join(join(model_path, "panda_description/urdf"), urdf_filename) +urdf_model_path = model_path / "panda_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir diff --git a/examples/collisions.py b/examples/collisions.py index 449ef05ef1..1578efa4d9 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -1,13 +1,13 @@ -from os.path import abspath, dirname, join +from pathlib import Path import pinocchio as pin -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "romeo_small.urdf" -urdf_model_path = join(join(model_path, "romeo_description/urdf"), urdf_filename) +urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename # Load model model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer()) @@ -23,7 +23,7 @@ # Remove collision pairs listed in the SRDF file srdf_filename = "romeo.srdf" -srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename +srdf_model_path = model_path / "romeo_description/srdf" / srdf_filename pin.removeCollisionPairs(model, geom_model, srdf_model_path) print( diff --git a/examples/contact-cholesky.py b/examples/contact-cholesky.py index 09c135bed6..48b4ca1da0 100644 --- a/examples/contact-cholesky.py +++ b/examples/contact-cholesky.py @@ -1,11 +1,11 @@ -from os.path import abspath, dirname, join +from pathlib import Path import pinocchio as pin -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" urdf_filename = ( pinocchio_model_dir - + "/example-robot-data/robots/anymal_b_simple_description/robots/anymal.urdf" + / "example-robot-data/robots/anymal_b_simple_description/robots/anymal.urdf" ) model = pin.buildModelFromUrdf(urdf_filename) data = model.createData() diff --git a/examples/geometry-models.py b/examples/geometry-models.py index a5848a93b2..c1ae8d4579 100644 --- a/examples/geometry-models.py +++ b/examples/geometry-models.py @@ -1,16 +1,16 @@ -from os.path import abspath, dirname, join +from pathlib import Path from sys import argv import pinocchio # This path refers to Pinocchio source code but you can define your own directory here. -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = ( - join(pinocchio_model_dir, "example-robot-data/robots") if len(argv) < 2 else argv[1] +model_path = Path( + (pinocchio_model_dir / "example-robot-data/robots") if len(argv) < 2 else argv[1] ) mesh_dir = pinocchio_model_dir -urdf_model_path = join(model_path, "ur_description/urdf/ur5_robot.urdf") +urdf_model_path = model_path / "ur_description/urdf/ur5_robot.urdf" # Load the urdf model model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( diff --git a/examples/gepetto-viewer.py b/examples/gepetto-viewer.py index 3fb86e4c8c..2ba488e66f 100644 --- a/examples/gepetto-viewer.py +++ b/examples/gepetto-viewer.py @@ -2,19 +2,19 @@ # usage: launch gepetto-gui and then run this test import sys -from os.path import abspath, dirname, join +from pathlib import Path import pinocchio as pin from pinocchio.visualize import GepettoVisualizer # Load the URDF model. # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "talos_reduced.urdf" -urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename) +urdf_model_path = model_path / "talos_data/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/examples/inverse-dynamics.py b/examples/inverse-dynamics.py index 677128dcbf..9a0feb5098 100644 --- a/examples/inverse-dynamics.py +++ b/examples/inverse-dynamics.py @@ -1,23 +1,24 @@ # Copyright 2023 Inria # SPDX-License-Identifier: BSD-2-Clause + """ In this short script, we show how to compute inverse dynamics (RNEA), i.e. the vector of joint torques corresponding to a given motion. """ -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin # Load the model from a URDF file # Change to your own URDF file here, or give a path as command-line argument -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models/") -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +pinocchio_model_dir = Path(__file__).parent.parent / "models/" +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "ur5_robot.urdf" -urdf_model_path = join(join(model_path, "ur_description/urdf/"), urdf_filename) +urdf_model_path = model_path / "ur_description/urdf/" / urdf_filename model, _, _ = pin.buildModelsFromUrdf(urdf_model_path, package_dirs=mesh_dir) # Build a data frame associated with the model diff --git a/examples/meshcat-viewer-dae.py b/examples/meshcat-viewer-dae.py index 50412c76cd..e80c08e4ff 100644 --- a/examples/meshcat-viewer-dae.py +++ b/examples/meshcat-viewer-dae.py @@ -3,7 +3,7 @@ # pip install --user meshcat import sys -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin @@ -11,12 +11,12 @@ # Load the URDF model. # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "romeo_small.urdf" -urdf_model_path = join(join(model_path, "romeo_description/urdf"), urdf_filename) +urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/examples/meshcat-viewer.py b/examples/meshcat-viewer.py index 5729e5f8f9..60c375e496 100644 --- a/examples/meshcat-viewer.py +++ b/examples/meshcat-viewer.py @@ -3,7 +3,7 @@ # pip install --user meshcat import sys -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin @@ -11,14 +11,14 @@ # Load the URDF model. # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir # urdf_filename = "talos_reduced.urdf" # urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename) urdf_filename = "solo.urdf" -urdf_model_path = join(join(model_path, "solo_description/robots"), urdf_filename) +urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/examples/overview-urdf.py b/examples/overview-urdf.py index b57e4a0254..8aee4be196 100644 --- a/examples/overview-urdf.py +++ b/examples/overview-urdf.py @@ -1,16 +1,15 @@ -from os.path import abspath, dirname, join +from pathlib import Path from sys import argv import pinocchio # This path refers to Pinocchio source code but you can define your own directory here. -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" # You should change here to set up your own URDF file or just pass it as an argument of # this example. urdf_filename = ( - pinocchio_model_dir - + "/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" + pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" if len(argv) < 2 else argv[1] ) diff --git a/examples/panda3d-viewer-play.py b/examples/panda3d-viewer-play.py index fd8d9a58e9..a05101f0c5 100644 --- a/examples/panda3d-viewer-play.py +++ b/examples/panda3d-viewer-play.py @@ -5,16 +5,14 @@ import sys -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np # Add path to the example-robot-data package from git submodule. # If you have a proper install version, there is no need for this sys.path thing -path = join( - dirname(dirname(abspath(__file__))), "models", "example-robot-data", "python" -) -sys.path.append(path) +path = Path(__file__).parent.parent / "models" / "example-robot-data" / "python" +sys.path.append(str(path)) from example_robot_data.robots_loader import TalosLoader from panda3d_viewer import ViewerClosedError from pinocchio.visualize.panda3d_visualizer import Panda3dVisualizer diff --git a/examples/panda3d-viewer.py b/examples/panda3d-viewer.py index 82711543ce..0527f01004 100644 --- a/examples/panda3d-viewer.py +++ b/examples/panda3d-viewer.py @@ -4,14 +4,12 @@ # ruff: noqa: E402 import sys -from os.path import abspath, dirname, join +from pathlib import Path # Add path to the example-robot-data package from git submodule. -# If you have it properly installed, there is no need for this sys.path thing. -path = join( - dirname(dirname(abspath(__file__))), "models", "example-robot-data", "python" -) -sys.path.append(path) +# If you have it properly installed, there is no need for this sys.path thing.) +path = Path(__file__).parent.parent / "models" / "example-robot-data" / "python" +sys.path.append(str(path)) from example_robot_data.robots_loader import ( HectorLoader, HyQLoader, diff --git a/examples/reachable-workspace-with-collisions.py b/examples/reachable-workspace-with-collisions.py index 54c386778d..7239f377ba 100644 --- a/examples/reachable-workspace-with-collisions.py +++ b/examples/reachable-workspace-with-collisions.py @@ -1,7 +1,7 @@ import itertools import sys import time -from os.path import abspath, dirname, join +from pathlib import Path import meshcat.geometry as g import numpy as np @@ -17,13 +17,13 @@ def XYZRPYtoSE3(xyzrpy): # Load the URDF model. -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir -urdf_path = join(model_path, "panda_description/urdf/panda.urdf") -srdf_path = join(model_path, "panda_description/srdf/panda.srdf") +urdf_path = model_path / "panda_description/urdf/panda.urdf" +srdf_path = model_path / "panda_description/srdf/panda.srdf" robot, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir) data = robot.createData() diff --git a/examples/reachable-workspace.py b/examples/reachable-workspace.py index 717d9ba2dc..a075e899de 100644 --- a/examples/reachable-workspace.py +++ b/examples/reachable-workspace.py @@ -1,6 +1,6 @@ import sys import time -from os.path import abspath, dirname, join +from pathlib import Path import meshcat.geometry as g import numpy as np @@ -8,13 +8,13 @@ from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "panda.urdf" -urdf_model_path = join(join(model_path, "panda_description/urdf"), urdf_filename) +urdf_model_path = model_path / "panda_description/urdf" / urdf_filename robot, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir diff --git a/examples/robot-wrapper-viewer.py b/examples/robot-wrapper-viewer.py index 5eb0a076e8..5388af21de 100644 --- a/examples/robot-wrapper-viewer.py +++ b/examples/robot-wrapper-viewer.py @@ -3,7 +3,7 @@ # integrating different kinds of viewers # -from os.path import abspath, dirname, join +from pathlib import Path from sys import argv import pinocchio as pin @@ -27,12 +27,12 @@ # Load the URDF model with RobotWrapper # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "talos_reduced.urdf" -urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename) +urdf_model_path = model_path / "talos_data/robots" / urdf_filename robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()) diff --git a/examples/rviz-viewer.py b/examples/rviz-viewer.py index 284108bf4b..3e979da8cd 100644 --- a/examples/rviz-viewer.py +++ b/examples/rviz-viewer.py @@ -1,19 +1,20 @@ # NOTE: this example needs RViz to be installed # usage: start ROS master (roscore) and then run this test -from os.path import abspath, dirname, join + +from pathlib import Path import pinocchio as pin from pinocchio.visualize import RVizVisualizer # Load the URDF model. # Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "talos_reduced.urdf" -urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename) +urdf_model_path = model_path / "talos_data/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/examples/simulation-contact-dynamics.py b/examples/simulation-contact-dynamics.py index c84e070cf2..5194abacee 100644 --- a/examples/simulation-contact-dynamics.py +++ b/examples/simulation-contact-dynamics.py @@ -1,22 +1,21 @@ import math import sys import time -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer # Load the URDF model. -# Conversion with str seems to be necessary when executing this file with ipython -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "talos_reduced.urdf" -urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename) +urdf_model_path = model_path / "talos_data/robots" / urdf_filename srdf_filename = "talos.srdf" -srdf_full_path = join(join(model_path, "talos_data/srdf"), srdf_filename) +srdf_full_path = model_path / "talos_data/srdf" / srdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/examples/static-contact-dynamics.py b/examples/static-contact-dynamics.py index 29c0109bea..e6757d5596 100644 --- a/examples/static-contact-dynamics.py +++ b/examples/static-contact-dynamics.py @@ -1,4 +1,4 @@ -from os.path import abspath, dirname, join +from pathlib import Path import numpy as np import pinocchio as pin @@ -52,12 +52,12 @@ # ----- SOLUTION ------ # 0. DATA -pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +pinocchio_model_dir = Path(__file__).parent.parent / "models" -model_path = join(pinocchio_model_dir, "example-robot-data/robots") +model_path = pinocchio_model_dir / "example-robot-data/robots" mesh_dir = pinocchio_model_dir urdf_filename = "solo12.urdf" -urdf_model_path = join(join(model_path, "solo_description/robots"), urdf_filename) +urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() diff --git a/include/pinocchio/bindings/python/utils/list.hpp b/include/pinocchio/bindings/python/utils/list.hpp index 7a591698d5..b6277b242e 100644 --- a/include/pinocchio/bindings/python/utils/list.hpp +++ b/include/pinocchio/bindings/python/utils/list.hpp @@ -6,6 +6,7 @@ #define __pinocchio_python_utils_list_hpp__ #include "pinocchio/bindings/python/fwd.hpp" +#include "pinocchio/bindings/python/utils/path.hpp" #include diff --git a/include/pinocchio/bindings/python/utils/path.hpp b/include/pinocchio/bindings/python/utils/path.hpp new file mode 100644 index 0000000000..023281a462 --- /dev/null +++ b/include/pinocchio/bindings/python/utils/path.hpp @@ -0,0 +1,29 @@ +// +// Copyright (c) 2024 CNRS +// + +#ifndef __pinocchio_python_utils_path_hpp__ +#define __pinocchio_python_utils_path_hpp__ + +#include "pinocchio/bindings/python/fwd.hpp" + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; + + /// + /// \brief python pathlib.Path | str -> C++ std::string + /// + std::string path(const bp::object & path); + + /// + /// \brief python typing.List[pathlib.Path] | typing.List[str] -> C++ std::vector + /// + std::vector pathList(const bp::object & path_list); + } // namespace python +} // namespace pinocchio + +#endif // ifndef __pinocchio_python_utils_path_hpp__ diff --git a/pyproject.toml b/pyproject.toml index b70c5e0372..1b2966dfa5 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -2,4 +2,4 @@ exclude = [ "cmake/*", "models/example-robot-data/*" ] [tool.ruff.lint] -select = [ "E", "F", "I", "RUF", "UP", "W" ] +select = [ "E", "F", "I", "PTH", "RUF", "UP", "W" ] diff --git a/sources.cmake b/sources.cmake index dda8d0d52d..d3717bc865 100644 --- a/sources.cmake +++ b/sources.cmake @@ -470,6 +470,7 @@ set(${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/version.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/pickle-vector.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/macros.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/path.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/std-vector.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/printable.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/utils/dependencies.hpp @@ -568,6 +569,7 @@ set(${PROJECT_NAME}_BINDINGS_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/bindings/python/utils/version.cpp ${PROJECT_SOURCE_DIR}/bindings/python/utils/dependencies.cpp ${PROJECT_SOURCE_DIR}/bindings/python/utils/conversions.cpp + ${PROJECT_SOURCE_DIR}/bindings/python/utils/path.cpp ${PROJECT_SOURCE_DIR}/bindings/python/math/expose-linalg.cpp ${PROJECT_SOURCE_DIR}/bindings/python/math/expose-tridiagonal-matrix.cpp ${PROJECT_SOURCE_DIR}/bindings/python/math/expose-lanczos-decomposition.cpp diff --git a/unittest/python/bindings_build_geom_from_urdf_memorycheck.py b/unittest/python/bindings_build_geom_from_urdf_memorycheck.py index 2ad08bec4b..e606a84e5c 100644 --- a/unittest/python/bindings_build_geom_from_urdf_memorycheck.py +++ b/unittest/python/bindings_build_geom_from_urdf_memorycheck.py @@ -1,5 +1,5 @@ -import os import unittest +from pathlib import Path import pinocchio as pin @@ -7,15 +7,11 @@ @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") class TestBuildGeomFromUrdfMemoryCheck(unittest.TestCase): def setUp(self): - self.current_file = os.path.dirname(str(os.path.abspath(__file__))) - self.model_dir = os.path.abspath( - os.path.join(self.current_file, "../../models/") - ) - self.model_path = os.path.abspath( - os.path.join( - self.model_dir, - "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf", - ) + self.current_dir = Path(__file__).parent + self.model_dir = self.current_dir / "../../models" + self.model_path = ( + self.model_dir + / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" ) def test_load(self): diff --git a/unittest/python/bindings_contact_inverse_dynamics.py b/unittest/python/bindings_contact_inverse_dynamics.py index c610b392de..7886ed5a88 100644 --- a/unittest/python/bindings_contact_inverse_dynamics.py +++ b/unittest/python/bindings_contact_inverse_dynamics.py @@ -1,5 +1,5 @@ -import os import unittest +from pathlib import Path import numpy as np import pinocchio as pin @@ -9,28 +9,13 @@ @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") class TestContactInverseDynamics(TestCase): def setUp(self): - self.current_file = os.path.dirname(str(os.path.abspath(__file__))) - self.model_dir = os.path.abspath( - os.path.join(self.current_file, "../../models/") - ) - self.model_path = os.path.abspath( - os.path.join( - self.model_dir, - "example-robot-data/robots/talos_data", - ) - ) + self.current_dir = Path(__file__).parent + self.model_dir = self.current_dir / "../../models" + self.model_path = self.model_dir / "example-robot-data/robots/talos_data" self.urdf_filename = "talos_reduced.urdf" self.srdf_filename = "talos.srdf" - self.urdf_model_path = os.path.join( - self.model_path, - "robots", - self.urdf_filename, - ) - self.srdf_full_path = os.path.join( - self.model_path, - "srdf", - self.srdf_filename, - ) + self.urdf_model_path = self.model_path / "robots" / self.urdf_filename + self.srdf_full_path = self.model_path / "srdf" / self.srdf_filename def load_model(self): self.model = pin.buildModelFromUrdf( diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index a93372e97c..7a2c961916 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -1,4 +1,5 @@ import unittest +from pathlib import Path import pinocchio as pin from test_case import PinocchioTestCase as TestCase @@ -44,11 +45,11 @@ def test_pickle(self): import pickle data = self.data - filename = "data.pickle" - with open(filename, "wb") as f: + filename = Path("data.pickle") + with filename.open("wb") as f: pickle.dump(data, f) - with open(filename, "rb") as f: + with filename.open("rb") as f: data_copy = pickle.load(f) self.assertTrue(data == data_copy) diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index e5c80f975c..d0c545eb3b 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -1,4 +1,5 @@ import unittest +from pathlib import Path import numpy as np import pinocchio as pin @@ -73,11 +74,11 @@ def test_pickle(self): import pickle frame = pin.Frame("name", 1, 2, pin.SE3.Random(), pin.OP_FRAME) - filename = "frame.pickle" - with open(filename, "wb") as f: + filename = Path("frame.pickle") + with filename.open("wb") as f: pickle.dump(frame, f) - with open(filename, "rb") as f: + with filename.open("rb") as f: frame_copy = pickle.load(f) self.assertEqual(frame, frame_copy) diff --git a/unittest/python/bindings_geometry_model_urdf.py b/unittest/python/bindings_geometry_model_urdf.py index c8a08f3856..7ebb86b250 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -1,5 +1,6 @@ import os import unittest +from pathlib import Path import pinocchio as pin @@ -11,21 +12,15 @@ def checkGeom(geom1, geom2): @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") class TestGeometryObjectUrdfBindings(unittest.TestCase): def setUp(self): - self.current_file = os.path.dirname(str(os.path.abspath(__file__))) - self.mesh_path = os.path.abspath( - os.path.join(self.current_file, "../../models") - ) - self.model_dir = os.path.abspath( - os.path.join(self.current_file, "../../models/example-robot-data/robots") - ) - self.model_path = os.path.abspath( - os.path.join(self.model_dir, "romeo_description/urdf/romeo.urdf") - ) + self.current_dir = Path(__file__).parent + self.mesh_path = self.current_dir / "../../models" + self.model_dir = self.current_dir / "../../models/example-robot-data/robots" + self.model_path = self.model_dir / "romeo_description/urdf/romeo.urdf" def test_load(self): hint_list = [self.mesh_path, "wrong/hint"] - expected_mesh_path = os.path.join( - self.model_dir, "romeo_description/meshes/V1/collision/LHipPitch.dae" + expected_mesh_path = ( + self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) @@ -66,8 +61,7 @@ def test_self_load(self): ) self.assertTrue(checkGeom(collision_model_ref, collision_model_self)) - hint_vec = pin.StdVec_StdString() - hint_vec.append(self.mesh_path) + hint_vec = [self.mesh_path] collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf( @@ -81,11 +75,11 @@ def test_self_load(self): def test_multi_load(self): hint_list = [self.mesh_path, "wrong/hint"] - expected_collision_path = os.path.join( - self.model_dir, "romeo_description/meshes/V1/collision/LHipPitch.dae" + expected_collision_path = ( + self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) - expected_visual_path = os.path.join( - self.model_dir, "romeo_description/meshes/V1/visual/LHipPitch.dae" + expected_visual_path = ( + self.model_dir / "romeo_description/meshes/V1/visual/LHipPitch.dae" ) model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 23067b7f45..e6b9474d7e 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -1,4 +1,5 @@ import unittest +from pathlib import Path import pinocchio as pin from pinocchio.utils import np, zero @@ -106,11 +107,11 @@ def test_pickle(self): import pickle model = self.model - filename = "model.pickle" - with open(filename, "wb") as f: + filename = Path("model.pickle") + with filename.open("wb") as f: pickle.dump(model, f) - with open(filename, "rb") as f: + with filename.open("rb") as f: model_copy = pickle.load(f) self.assertTrue(model == model_copy) diff --git a/unittest/python/bindings_std_map.py b/unittest/python/bindings_std_map.py index 0165bed0dd..9031587c51 100644 --- a/unittest/python/bindings_std_map.py +++ b/unittest/python/bindings_std_map.py @@ -1,5 +1,6 @@ import pickle import unittest +from pathlib import Path import numpy as np import pinocchio as pin @@ -18,9 +19,9 @@ def test_pickle(self): keys.append(key_name) map[key_name] = np.random.rand(10) - pickle.dump(map, open("save_std_map.p", "wb")) + pickle.dump(map, Path("save_std_map.p").open("wb")) - map_loaded = pickle.load(open("save_std_map.p", "rb")) + map_loaded = pickle.load(Path("save_std_map.p").open("rb")) for key in keys: self.assertApprox(map[key], map_loaded[key]) diff --git a/unittest/python/bindings_std_vector.py b/unittest/python/bindings_std_vector.py index e4acf30dcb..9c0516ef03 100644 --- a/unittest/python/bindings_std_vector.py +++ b/unittest/python/bindings_std_vector.py @@ -1,5 +1,6 @@ import pickle import unittest +from pathlib import Path import numpy as np import pinocchio as pin @@ -15,9 +16,9 @@ def test_pickle(self): for k in range(100): vec.append(np.random.rand(3)) - pickle.dump(vec, open("save_std_vec.p", "wb")) + pickle.dump(vec, Path("save_std_vec.p").open("wb")) - vec_loaded = pickle.load(open("save_std_vec.p", "rb")) + vec_loaded = pickle.load(Path("save_std_vec.p").open("rb")) for k in range(len(vec)): self.assertApprox(vec[k], vec_loaded[k]) diff --git a/unittest/python/bindings_urdf.py b/unittest/python/bindings_urdf.py index b61502cf30..45826f08cc 100644 --- a/unittest/python/bindings_urdf.py +++ b/unittest/python/bindings_urdf.py @@ -1,5 +1,5 @@ -import os import unittest +from pathlib import Path import pinocchio as pin @@ -7,13 +7,9 @@ @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") class TestGeometryObjectUrdfBindings(unittest.TestCase): def setUp(self): - self.current_file = os.path.dirname(str(os.path.abspath(__file__))) - self.model_dir = os.path.abspath( - os.path.join(self.current_file, "../../models/example-robot-data/robots") - ) - self.model_path = os.path.abspath( - os.path.join(self.model_dir, "romeo_description/urdf/romeo.urdf") - ) + self.current_dir = Path(__file__).parent + self.model_dir = self.current_dir / "../../models/example-robot-data/robots" + self.model_path = self.model_dir / "romeo_description/urdf/romeo.urdf" def test_load(self): pin.buildModelFromUrdf(self.model_path) @@ -25,7 +21,7 @@ def test_self_load(self): pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) def test_xml(self): - with open(self.model_path) as model: + with self.model_path.open() as model: file_content = model.read() model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) @@ -40,19 +36,15 @@ def test_xml(self): def test_pickle(self): import pickle - model_dir = os.path.abspath( - os.path.join(self.current_file, "../../models/example-robot-data/robots") - ) - model_path = os.path.abspath( - os.path.join(model_dir, "ur_description/urdf/ur5_robot.urdf") - ) + model_dir = self.current_dir / "../../models/example-robot-data/robots" + model_path = model_dir / "ur_description/urdf/ur5_robot.urdf" model = pin.buildModelFromUrdf(model_path) - filename = "model.pickle" - with open(filename, "wb") as f: + filename = Path("model.pickle") + with filename.open("wb") as f: pickle.dump(model, f) - with open(filename, "rb") as f: + with filename.open("rb") as f: model_copy = pickle.load(f) self.assertTrue(model == model_copy) diff --git a/unittest/python/casadi/bindings_explog.py b/unittest/python/casadi/bindings_explog.py index 8e60389493..40c9f10bb4 100644 --- a/unittest/python/casadi/bindings_explog.py +++ b/unittest/python/casadi/bindings_explog.py @@ -1,8 +1,8 @@ -import os import sys import unittest +from pathlib import Path -sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +sys.path.append(str(Path(__file__).parent.parent)) import casadi import numpy as np diff --git a/unittest/python/casadi/bindings_main_algo.py b/unittest/python/casadi/bindings_main_algo.py index 1eba2bb79e..4409cc52e9 100644 --- a/unittest/python/casadi/bindings_main_algo.py +++ b/unittest/python/casadi/bindings_main_algo.py @@ -1,8 +1,8 @@ -import os import sys import unittest +from pathlib import Path -sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +sys.path.append(str(Path(__file__).parent.parent)) import casadi import numpy as np import pinocchio as pin diff --git a/unittest/python/robot_wrapper.py b/unittest/python/robot_wrapper.py index 91b0959bda..5e961aa146 100644 --- a/unittest/python/robot_wrapper.py +++ b/unittest/python/robot_wrapper.py @@ -2,36 +2,30 @@ # # SPDX-License-Identifier: BSD-2-Clause -import os import unittest +from pathlib import Path import pinocchio as pin class TestRobotWrapper(unittest.TestCase): def setUp(self): - self.current_file = os.path.dirname(str(os.path.abspath(__file__))) + self.current_dir = Path(__file__).parent def test_mjcf_without_root_joint(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../models/test_mjcf.xml") - ) + model_path = self.current_dir.parent / "models" / "test_mjcf.xml" robot = pin.RobotWrapper.BuildFromMJCF(model_path) self.assertEqual(robot.nq, 6) self.assertEqual(robot.nv, 5) self.assertEqual(robot.model.njoints, 4) def test_mjcf_with_root_joint(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../models/test_mjcf.xml") - ) + model_path = self.current_dir.parent / "models" / "test_mjcf.xml" robot = pin.RobotWrapper.BuildFromMJCF(model_path, pin.JointModelFreeFlyer()) self.assertEqual(robot.model.names[1], "root_joint") def test_mjcf_with_root_joint_and_root_joint_name(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../models/test_mjcf.xml") - ) + model_path = self.current_dir.parent / "models" / "test_mjcf.xml" name_ = "freeflyer_joint" robot = pin.RobotWrapper.BuildFromMJCF( model_path, pin.JointModelFreeFlyer(), name_ @@ -39,30 +33,63 @@ def test_mjcf_with_root_joint_and_root_joint_name(self): self.assertEqual(robot.model.names[1], name_) def test_urdf_with_root_joint(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../models/3DOF_planar.urdf") - ) + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" robot = pin.RobotWrapper.BuildFromURDF( model_path, [], pin.JointModelFreeFlyer() ) self.assertEqual(robot.model.names[1], "root_joint") def test_urdf_with_root_joint_and_root_joint_name(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../models/3DOF_planar.urdf") - ) + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" name_ = "freeflyer_joint" robot = pin.RobotWrapper.BuildFromURDF( model_path, [], pin.JointModelFreeFlyer(), name_ ) self.assertEqual(robot.model.names[1], name_) + def test_urdf_with_str_pkg_dirs(self): + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" + package_dir = self.current_dir.parent / "models" + robot = pin.RobotWrapper.BuildFromURDF( + model_path, str(package_dir), pin.JointModelFreeFlyer() + ) + self.assertEqual(robot.model.names[1], "root_joint") + + def test_urdf_with_path_pkg_dirs(self): + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" + package_dir = self.current_dir.parent / "models" + robot = pin.RobotWrapper.BuildFromURDF( + model_path, package_dir, pin.JointModelFreeFlyer() + ) + self.assertEqual(robot.model.names[1], "root_joint") + + def test_urdf_with_str_list_pkg_dirs(self): + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" + package_dir = self.current_dir.parent / "models" + robot = pin.RobotWrapper.BuildFromURDF( + model_path, [str(package_dir)], pin.JointModelFreeFlyer() + ) + self.assertEqual(robot.model.names[1], "root_joint") + + def test_urdf_with_path_list_pkg_dirs(self): + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" + package_dir = self.current_dir.parent / "models" + robot = pin.RobotWrapper.BuildFromURDF( + model_path, [package_dir], pin.JointModelFreeFlyer() + ) + self.assertEqual(robot.model.names[1], "root_joint") + + def test_urdf_with_None_pkg_dirs(self): + model_path = self.current_dir.parent / "models" / "3DOF_planar.urdf" + robot = pin.RobotWrapper.BuildFromURDF( + model_path, None, pin.JointModelFreeFlyer() + ) + self.assertEqual(robot.model.names[1], "root_joint") + @unittest.skipUnless(pin.WITH_SDFORMAT, "Needs SDFORMAT") def test_sdf_with_root_joint(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../../models/simple_humanoid.sdf") - ) - mesh_path = os.path.abspath(os.path.join(self.current_file, "../../models/")) + model_path = self.current_dir.parent.parent / "models" / "simple_humanoid.sdf" + mesh_path = self.current_dir.parent.parent / "models" robot = pin.RobotWrapper.BuildFromSDF( model_path, [mesh_path], pin.JointModelFreeFlyer(), verbose=True ) @@ -70,10 +97,8 @@ def test_sdf_with_root_joint(self): @unittest.skipUnless(pin.WITH_SDFORMAT, "Needs SDFORMAT") def test_sdf_with_root_joint_and_root_joint_name(self): - model_path = os.path.abspath( - os.path.join(self.current_file, "../../models/simple_humanoid.sdf") - ) - mesh_path = os.path.abspath(os.path.join(self.current_file, "../../models/")) + model_path = self.current_dir.parent.parent / "models" / "simple_humanoid.sdf" + mesh_path = self.current_dir.parent.parent / "models" name_ = "freeflyer_joint" robot = pin.RobotWrapper.BuildFromSDF( model_path, [mesh_path], pin.JointModelFreeFlyer(), root_joint_name=name_