Skip to content

Commit e6c1399

Browse files
jorisvjorisv
authored
Merge pull request #2402 from MegMll/topic/urdf_root_joint
Update Model parsing
2 parents 57a0dcc + 566d6e5 commit e6c1399

File tree

19 files changed

+821
-203
lines changed

19 files changed

+821
-203
lines changed

‎CHANGELOG.md

+6
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
2222

2323
- Modernize python code base with ruff ([#2418](https://github.com/stack-of-tasks/pinocchio/pull/2418))
2424

25+
### Changed
26+
- Does not create a root_joint frame from parsed models (urdf, mjcf and sdf) when no root joint is provided ([#2402](https://github.com/stack-of-tasks/pinocchio/pull/2402))
27+
28+
### Added
29+
- 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))
30+
2531
## [3.2.0] - 2024-08-27
2632

2733
### Fixed

‎bindings/python/parsers/mjcf/model.cpp

+19
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,17 @@ namespace pinocchio
2828
return model;
2929
}
3030

31+
bp::tuple buildModelFromMJCF(
32+
const std::string & filename,
33+
const JointModel & root_joint,
34+
const std::string & root_joint_name)
35+
{
36+
Model model;
37+
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
38+
::pinocchio::mjcf::buildModel(filename, root_joint, root_joint_name, model, contact_models);
39+
return bp::make_tuple(model, contact_models);
40+
}
41+
3142
void exposeMJCFModel()
3243
{
3344
bp::def(
@@ -42,6 +53,14 @@ namespace pinocchio
4253
pinocchio::python::buildModelFromMJCF),
4354
bp::args("mjcf_filename", "root_joint"),
4455
"Parse the MJCF file and return a pinocchio Model with the given root Joint.");
56+
57+
bp::def(
58+
"buildModelFromMJCF",
59+
static_cast<bp::tuple (*)(const std::string &, const JointModel &, const std::string &)>(
60+
pinocchio::python::buildModelFromMJCF),
61+
bp::args("mjcf_filename", "root_joint", "root_joint_name"),
62+
"Parse the MJCF file and return a pinocchio Model with the given root Joint and its "
63+
"specified name as well as a constraint list if some are present in the MJCF file.");
4564
}
4665
} // namespace python
4766
} // namespace pinocchio

‎bindings/python/parsers/sdf/model.cpp

+25
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,21 @@ namespace pinocchio
4242
filename, root_joint, model, contact_models, root_link_name, parent_guidance);
4343
return bp::make_tuple(model, contact_models);
4444
}
45+
46+
bp::tuple buildModelFromSdf(
47+
const std::string & filename,
48+
const JointModel & root_joint,
49+
const std::string & root_link_name,
50+
const std::string & root_joint_name,
51+
const std::vector<std::string> & parent_guidance)
52+
{
53+
Model model;
54+
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
55+
pinocchio::sdf::buildModel(
56+
filename, root_joint, root_joint_name, model, contact_models, root_link_name,
57+
parent_guidance);
58+
return bp::make_tuple(model, contact_models);
59+
}
4560
#endif
4661

4762
void exposeSDFModel()
@@ -65,6 +80,16 @@ namespace pinocchio
6580
bp::arg("parent_guidance") = bp::list()),
6681
"Parse the SDF file given in input and return a pinocchio Model and constraint "
6782
"models starting with the given root joint.");
83+
84+
bp::def(
85+
"buildModelFromSdf",
86+
static_cast<bp::tuple (*)(
87+
const std::string &, const JointModel &, const std::string &, const std::string &,
88+
const std::vector<std::string> &)>(pinocchio::python::buildModelFromSdf),
89+
(bp::arg("sdf_filename"), bp::arg("root_joint"), bp::arg("root_link_name"),
90+
bp::arg("root_joint_name"), bp::arg("parent_guidance") = bp::list()),
91+
"Parse the SDF file given in input and return a pinocchio Model and constraint "
92+
"models starting with the given root joint and its specified name.");
6893
#endif
6994
}
7095
} // namespace python

‎bindings/python/parsers/urdf/model.cpp

+83-8
Original file line numberDiff line numberDiff line change
@@ -38,36 +38,75 @@ namespace pinocchio
3838
return model;
3939
}
4040

41+
Model buildModelFromUrdf(
42+
const std::string & filename,
43+
const JointModel & root_joint,
44+
const std::string & root_joint_name)
45+
{
46+
Model model;
47+
pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model);
48+
return model;
49+
}
50+
4151
Model &
4252
buildModelFromUrdf(const std::string & filename, const JointModel & root_joint, Model & model)
4353
{
4454
return pinocchio::urdf::buildModel(filename, root_joint, model);
4555
}
4656

47-
Model buildModelFromXML(const std::string & XMLstream, const JointModel & root_joint)
57+
Model & buildModelFromUrdf(
58+
const std::string & filename,
59+
const JointModel & root_joint,
60+
const std::string & root_joint_name,
61+
Model & model)
62+
{
63+
return pinocchio::urdf::buildModel(filename, root_joint, root_joint_name, model);
64+
}
65+
66+
Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint)
4867
{
4968
Model model;
50-
pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
69+
pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
70+
return model;
71+
}
72+
73+
Model buildModelFromXML(
74+
const std::string & xml_stream,
75+
const JointModel & root_joint,
76+
const std::string & root_joint_name)
77+
{
78+
Model model;
79+
pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
5180
return model;
5281
}
5382

5483
Model &
55-
buildModelFromXML(const std::string & XMLstream, const JointModel & root_joint, Model & model)
84+
buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model)
5685
{
57-
pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
86+
pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model);
5887
return model;
5988
}
6089

61-
Model buildModelFromXML(const std::string & XMLstream)
90+
Model & buildModelFromXML(
91+
const std::string & xml_stream,
92+
const JointModel & root_joint,
93+
const std::string & root_joint_name,
94+
Model & model)
95+
{
96+
pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model);
97+
return model;
98+
}
99+
100+
Model buildModelFromXML(const std::string & xml_stream)
62101
{
63102
Model model;
64-
pinocchio::urdf::buildModelFromXML(XMLstream, model);
103+
pinocchio::urdf::buildModelFromXML(xml_stream, model);
65104
return model;
66105
}
67106

68-
Model & buildModelFromXML(const std::string & XMLstream, Model & model)
107+
Model & buildModelFromXML(const std::string & xml_stream, Model & model)
69108
{
70-
pinocchio::urdf::buildModelFromXML(XMLstream, model);
109+
pinocchio::urdf::buildModelFromXML(xml_stream, model);
71110
return model;
72111
}
73112

@@ -86,6 +125,14 @@ namespace pinocchio
86125
"Parse the URDF file given in input and return a pinocchio Model starting with the "
87126
"given root joint.");
88127

128+
bp::def(
129+
"buildModelFromUrdf",
130+
static_cast<Model (*)(const std::string &, const JointModel &, const std::string &)>(
131+
pinocchio::python::buildModelFromUrdf),
132+
bp::args("urdf_filename", "root_joint", "root_joint_name"),
133+
"Parse the URDF file given in input and return a pinocchio Model starting with the "
134+
"given root joint with its specified name.");
135+
89136
bp::def(
90137
"buildModelFromUrdf",
91138
static_cast<Model (*)(const std::string &)>(pinocchio::python::buildModelFromUrdf),
@@ -110,6 +157,17 @@ namespace pinocchio
110157
"it is treated as operational frame and not as a joint of the model.",
111158
bp::return_internal_reference<3>());
112159

160+
bp::def(
161+
"buildModelFromUrdf",
162+
static_cast<Model & (*)(const std::string &, const JointModel &, const std::string &,
163+
Model &)>(pinocchio::python::buildModelFromUrdf),
164+
bp::args("urdf_filename", "root_joint", "root_joint_name", "model"),
165+
"Append to a given model a URDF structure given by its filename and the root joint with "
166+
"its specified name.\n"
167+
"Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons,"
168+
"it is treated as operational frame and not as a joint of the model.",
169+
bp::return_internal_reference<3>());
170+
113171
bp::def(
114172
"buildModelFromXML",
115173
static_cast<Model (*)(const std::string &, const JointModel &)>(
@@ -118,6 +176,14 @@ namespace pinocchio
118176
"Parse the URDF XML stream given in input and return a pinocchio Model starting with "
119177
"the given root joint.");
120178

179+
bp::def(
180+
"buildModelFromXML",
181+
static_cast<Model (*)(const std::string &, const JointModel &, const std::string &)>(
182+
pinocchio::python::buildModelFromXML),
183+
bp::args("urdf_xml_stream", "root_joint", "root_joint_name"),
184+
"Parse the URDF XML stream given in input and return a pinocchio Model starting with "
185+
"the given root joint with its specified name.");
186+
121187
bp::def(
122188
"buildModelFromXML",
123189
static_cast<Model & (*)(const std::string &, const JointModel &, Model &)>(
@@ -127,6 +193,15 @@ namespace pinocchio
127193
"given interfacing joint.",
128194
bp::return_internal_reference<3>());
129195

196+
bp::def(
197+
"buildModelFromXML",
198+
static_cast<Model & (*)(const std::string &, const JointModel &, const std::string &,
199+
Model &)>(pinocchio::python::buildModelFromXML),
200+
bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"),
201+
"Parse the URDF XML stream given in input and append it to the input model with the "
202+
"given interfacing joint with its specified name.",
203+
bp::return_internal_reference<3>());
204+
130205
bp::def(
131206
"buildModelFromXML",
132207
static_cast<Model (*)(const std::string &)>(pinocchio::python::buildModelFromXML),

‎bindings/python/pinocchio/robot_wrapper.py

+18-58
Original file line numberDiff line numberDiff line change
@@ -14,24 +14,18 @@
1414

1515
class RobotWrapper:
1616
@staticmethod
17-
def BuildFromURDF(
18-
filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None
19-
):
17+
def BuildFromURDF(filename, *args, **kwargs):
2018
robot = RobotWrapper()
21-
robot.initFromURDF(filename, package_dirs, root_joint, verbose, meshLoader)
19+
20+
robot.initFromURDF(filename, *args, **kwargs)
21+
2222
return robot
2323

24-
def initFromURDF(
25-
self,
26-
filename,
27-
package_dirs=None,
28-
root_joint=None,
29-
verbose=False,
30-
meshLoader=None,
31-
):
24+
def initFromURDF(self, filename, *args, **kwargs):
3225
model, collision_model, visual_model = buildModelsFromUrdf(
33-
filename, package_dirs, root_joint, verbose, meshLoader
26+
filename, *args, **kwargs
3427
)
28+
3529
RobotWrapper.__init__(
3630
self,
3731
model=model,
@@ -40,46 +34,16 @@ def initFromURDF(
4034
)
4135

4236
@staticmethod
43-
def BuildFromSDF(
44-
filename,
45-
package_dirs=None,
46-
root_joint=None,
47-
root_link_name="",
48-
parent_guidance=[],
49-
verbose=False,
50-
meshLoader=None,
51-
):
37+
def BuildFromSDF(filename, *args, **kwargs):
5238
robot = RobotWrapper()
53-
robot.initFromSDF(
54-
filename,
55-
package_dirs,
56-
root_joint,
57-
root_link_name,
58-
parent_guidance,
59-
verbose,
60-
meshLoader,
61-
)
39+
robot.initFromSDF(filename, *args, **kwargs)
6240
return robot
6341

64-
def initFromSDF(
65-
self,
66-
filename,
67-
package_dirs=None,
68-
root_joint=None,
69-
root_link_name="",
70-
parent_guidance=[],
71-
verbose=False,
72-
meshLoader=None,
73-
):
42+
def initFromSDF(self, filename, *args, **kwargs):
7443
model, constraint_models, collision_model, visual_model = buildModelsFromSdf(
75-
filename,
76-
package_dirs,
77-
root_joint,
78-
root_link_name,
79-
parent_guidance,
80-
verbose,
81-
meshLoader,
44+
filename, *args, **kwargs
8245
)
46+
8347
RobotWrapper.__init__(
8448
self,
8549
model=model,
@@ -89,21 +53,17 @@ def initFromSDF(
8953
self.constraint_models = constraint_models
9054

9155
@staticmethod
92-
def BuildFromMJCF(filename, root_joint=None, verbose=False, meshLoader=None):
56+
def BuildFromMJCF(filename, *args, **kwargs):
9357
robot = RobotWrapper()
94-
robot.initFromMJCF(filename, root_joint, verbose, meshLoader)
58+
robot.initFromMJCF(filename, *args, **kwargs)
59+
9560
return robot
9661

97-
def initFromMJCF(
98-
self,
99-
filename,
100-
root_joint=None,
101-
verbose=False,
102-
meshLoader=None,
103-
):
62+
def initFromMJCF(self, filename, *args, **kwargs):
10463
model, collision_model, visual_model = buildModelsFromMJCF(
105-
filename, root_joint, verbose, meshLoader
64+
filename, *args, **kwargs
10665
)
66+
10767
RobotWrapper.__init__(
10868
self,
10969
model=model,

0 commit comments

Comments
 (0)