-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
Copy pathparsing_py.cc
97 lines (88 loc) · 3.9 KB
/
parsing_py.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#include "pybind11/pybind11.h"
#include "pybind11/stl.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
#include "drake/bindings/pydrake/pydrake_pybind.h"
#include "drake/multibody/parsing/package_map.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/parsing/process_model_directives.h"
using drake::geometry::SceneGraph;
using drake::multibody::MultibodyPlant;
using std::string;
namespace drake {
namespace pydrake {
PYBIND11_MODULE(parsing, m) {
PYDRAKE_PREVENT_PYTHON3_MODULE_REIMPORT(m);
m.doc() = "SDF and URDF parsing for MultibodyPlant and SceneGraph.";
// NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
using namespace drake::multibody;
constexpr auto& doc = pydrake_doc.drake.multibody;
// PackageMap
{
using Class = PackageMap;
constexpr auto& cls_doc = doc.PackageMap;
py::class_<Class>(m, "PackageMap", cls_doc.doc)
.def(py::init<>(), cls_doc.ctor.doc)
.def("Add", &Class::Add, cls_doc.Add.doc)
.def("Contains", &Class::Contains, cls_doc.Contains.doc)
.def("size", &Class::size, cls_doc.size.doc)
.def("GetPath", &Class::GetPath, cls_doc.GetPath.doc)
.def("PopulateFromFolder", &Class::PopulateFromFolder,
cls_doc.PopulateFromFolder.doc)
.def("PopulateFromEnvironment", &Class::PopulateFromEnvironment,
cls_doc.PopulateFromEnvironment.doc)
.def("PopulateUpstreamToDrake", &Class::PopulateUpstreamToDrake,
cls_doc.PopulateUpstreamToDrake.doc);
}
// Parser
{
using Class = Parser;
constexpr auto& cls_doc = doc.Parser;
py::class_<Class>(m, "Parser", cls_doc.doc)
.def(py::init<MultibodyPlant<double>*, SceneGraph<double>*>(),
py::arg("plant"), py::arg("scene_graph") = nullptr,
cls_doc.ctor.doc)
.def("package_map", &Class::package_map, py_rvp::reference_internal,
cls_doc.package_map.doc)
.def("AddAllModelsFromFile", &Class::AddAllModelsFromFile,
py::arg("file_name"), cls_doc.AddAllModelsFromFile.doc)
.def("AddModelFromFile", &Class::AddModelFromFile, py::arg("file_name"),
py::arg("model_name") = "", cls_doc.AddModelFromFile.doc)
.def("AddModelFromString", &Class::AddModelFromString,
py::arg("file_contents"), py::arg("file_type"),
py::arg("model_name") = "", cls_doc.AddModelFromString.doc);
}
// Model Directives
{
constexpr auto& parsing_doc = doc.parsing;
py::class_<parsing::ModelDirectives>(
m, "ModelDirectives", parsing_doc.ModelDirectives.doc);
m.def("LoadModelDirectives", &parsing::LoadModelDirectives,
py::arg("filename"));
py::class_<parsing::ModelInstanceInfo>(m, "ModelInstanceInfo")
.def_readonly("model_name", &parsing::ModelInstanceInfo::model_name)
.def_readonly("model_path", &parsing::ModelInstanceInfo::model_path)
.def_readonly(
"parent_frame_name", &parsing::ModelInstanceInfo::parent_frame_name)
.def_readonly(
"child_frame_name", &parsing::ModelInstanceInfo::child_frame_name)
.def_readonly("X_PC", &parsing::ModelInstanceInfo::X_PC)
.def_readonly(
"model_instance", &parsing::ModelInstanceInfo::model_instance);
m.def(
"ProcessModelDirectives",
[](const parsing::ModelDirectives& directives,
MultibodyPlant<double>* plant,
Parser* parser = nullptr) {
std::vector<parsing::ModelInstanceInfo> added_models;
parsing::ProcessModelDirectives(directives, plant, &added_models,
parser);
return added_models;
},
py::arg("directives"), py::arg("plant"), py::arg("parser"),
(std::string(parsing_doc.ProcessModelDirectives.doc) +
"\nNOTE: pydrake does not support `ModelWeldErrorFunction`.")
.c_str());
}
}
} // namespace pydrake
} // namespace drake